Skip to content

Commit ebf3d3e

Browse files
committed
kinematics: take the tool frame inverse request as a program writes it
The search solves to a residual of 1e-18, and a direction written to six digits is a unit vector only to 1e-7, so a request copied from a program was reported unreachable. Normalise the axis on the way in, and take the component along it off the requested tool x, refusing only a zero vector or a tool x within a millionth of a radian of the axis, since neither describes a frame.
1 parent 7f4ed9e commit ebf3d3e

4 files changed

Lines changed: 99 additions & 12 deletions

File tree

‎docs/src/motion/kinematics-conventions.adoc‎

Lines changed: 18 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -339,6 +339,24 @@ along with the number of directions left free. A caller that hands back a list
339339
of samples from a curve as though they were alternatives is telling the
340340
operator something false.
341341

342+
Which joints may take part is the caller's decision, not the module's. A
343+
rotary table turns the tool against the work as surely as a head rotary does,
344+
so a machine with a table and a two-axis head has three orientation joints and
345+
a bare tool axis leaves one of them free. Whether the table is used for that
346+
is a machining choice: Heidenhain names the axes a tilt may use with `M138`,
347+
and its `PLANE` lets the operator pick `TABLE ROT`, turn the table to place
348+
tool X, or `COORD ROT`, leave the table and rotate the coordinate system. So
349+
the request names the joints to hold. Holding the table gives the two head
350+
solutions and the turn about the tool that finishes the frame, which is what
351+
the TWP remap computes; holding nothing lets the table place tool X and the
352+
turn comes back zero.
353+
354+
The request is taken as a program carries it. A direction written to a few
355+
digits is a unit vector only to within its rounding, and a tool X written the
356+
same way is at right angles to the axis only to within it, so both are
357+
normalised on the way in and only a vector that is not a direction at all is
358+
refused.
359+
342360
=== What a module has to supply
343361

344362
Nothing, if it already reports its frames. The shared code answers the question

‎src/emc/kinematics/kinematics.h‎

Lines changed: 6 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -283,6 +283,12 @@ extern int toolFrameIsProper(const PmRotationMatrix *m);
283283
and gets the two head solutions and the spin about the tool that finishes
284284
the frame.
285285
286+
The request is normalised on the way in: axis_in_work is scaled to unit
287+
length and x_in_work has its component along the axis removed, so the
288+
rounded numbers a program carries do not make an orientation unreachable.
289+
A zero vector, or a tool x within a millionth of a radian of lying along
290+
the axis, is still refused, since neither describes a frame.
291+
286292
solutions receives max_solutions complete sets of joint values, one after
287293
another, each num_joints long. free_directions, if not NULL, receives one
288294
entry per solution: 0 where the joints are pinned down, and n where the

‎src/emc/kinematics/kins_util.c‎

Lines changed: 29 additions & 12 deletions
Original file line numberDiff line numberDiff line change
@@ -958,6 +958,7 @@ int toolFrameSolve(kinsFrameFunc work,
958958
double *tool_spin)
959959
{
960960
tfs_ctx c;
961+
double axis[3], xdir[3], len;
961962
int found, i;
962963

963964
if (!work || !tool || !seed || !solutions || !axis_in_work
@@ -969,28 +970,44 @@ int toolFrameSolve(kinsFrameFunc work,
969970
max_solutions = TOOL_FRAME_MAX_SOLUTIONS;
970971
}
971972

973+
// The request as a program carries it, a few digits of each component,
974+
// is a unit vector only to within its rounding, and the search solves
975+
// to well below that. Normalise on the way in, and refuse only what is
976+
// not a direction at all.
977+
axis[0] = axis_in_work->x;
978+
axis[1] = axis_in_work->y;
979+
axis[2] = axis_in_work->z;
980+
len = sqrt(axis[0]*axis[0] + axis[1]*axis[1] + axis[2]*axis[2]);
981+
if (len < 1e-12) { return -1; }
982+
for (i = 0; i < 3; i++) { axis[i] /= len; }
983+
972984
c.work = work;
973985
c.tool = tool;
974986
c.num_joints = num_joints;
975987
c.seed = seed;
976988
c.held = held;
977989
c.nres = x_in_work ? 6 : 3;
978-
c.want[0] = axis_in_work->x;
979-
c.want[1] = axis_in_work->y;
980-
c.want[2] = axis_in_work->z;
990+
for (i = 0; i < 3; i++) { c.want[i] = axis[i]; }
981991
if (x_in_work) {
982-
double square = axis_in_work->x * x_in_work->x
983-
+ axis_in_work->y * x_in_work->y
984-
+ axis_in_work->z * x_in_work->z;
992+
double along;
993+
994+
xdir[0] = x_in_work->x;
995+
xdir[1] = x_in_work->y;
996+
xdir[2] = x_in_work->z;
997+
len = sqrt(xdir[0]*xdir[0] + xdir[1]*xdir[1] + xdir[2]*xdir[2]);
998+
if (len < 1e-12) { return -1; }
999+
for (i = 0; i < 3; i++) { xdir[i] /= len; }
9851000

9861001
// the two vectors are two axes of one frame, so a request where they
9871002
// are not at right angles is not a frame and cannot be reached by
988-
// anything
989-
if (fabs(square) > 1e-6) { return -1; }
990-
991-
c.want[3] = x_in_work->x;
992-
c.want[4] = x_in_work->y;
993-
c.want[5] = x_in_work->z;
1003+
// anything; within rounding of right angles, the component along
1004+
// the axis is rounding and comes off
1005+
along = axis[0]*xdir[0] + axis[1]*xdir[1] + axis[2]*xdir[2];
1006+
if (fabs(along) > 1e-6) { return -1; }
1007+
for (i = 0; i < 3; i++) { xdir[i] -= along*axis[i]; }
1008+
len = sqrt(xdir[0]*xdir[0] + xdir[1]*xdir[1] + xdir[2]*xdir[2]);
1009+
if (len < 1e-12) { return -1; }
1010+
for (i = 0; i < 3; i++) { c.want[3+i] = xdir[i]/len; }
9941011
}
9951012

9961013
if (tfs_survey(&c) < 0) { return -1; }

‎tests/tool-frame/test_tool_frame.c‎

Lines changed: 46 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -535,6 +535,52 @@ int main(void)
535535
check(n == 0, "and refuses if it does not");
536536
}
537537

538+
/* the request as a program writes it: a direction to six digits is
539+
not a unit vector, and must not be unreachable for that */
540+
{
541+
PmCartesian rounded, unit;
542+
double len;
543+
544+
want = in_work(xyzacWork, identityFrame, truth);
545+
rounded.x = floor(want.z.x*1e6 + 0.5)/1e6;
546+
rounded.y = floor(want.z.y*1e6 + 0.5)/1e6;
547+
rounded.z = floor(want.z.z*1e6 + 0.5)/1e6;
548+
len = sqrt(rounded.x*rounded.x + rounded.y*rounded.y + rounded.z*rounded.z);
549+
unit.x = rounded.x/len; unit.y = rounded.y/len; unit.z = rounded.z/len;
550+
check(fabs(len - 1.0) > 1e-9, "the rounded request is off unit length");
551+
552+
n = toolFrameSolve(xyzacWork, identityFrame, 5, &rounded, NULL, seed,
553+
0, sols, TOOL_FRAME_MAX_SOLUTIONS, free_dirs, spin);
554+
check(n == 2, "a rounded axis is solved");
555+
for (i = 0; i < n; i++) {
556+
check(axis_matches(xyzacWork, identityFrame, sols + i*5, &unit),
557+
"to the direction it names");
558+
}
559+
560+
/* a tool x off right angles by rounding is taken, a zero axis is not */
561+
{
562+
double head_seed[5] = {0, 0, 0, 10, 5};
563+
PmRotationMatrix f = in_work(identityFrame, headTool, truth);
564+
PmCartesian x_off;
565+
const double c_s = cos(30*DEG), s_s = sin(30*DEG);
566+
/* an x the head cannot make, turned 30 degrees about the
567+
axis, then nudged off right angles by rounding */
568+
x_off.x = c_s*f.x.x + s_s*f.y.x + 1e-8*f.z.x;
569+
x_off.y = c_s*f.x.y + s_s*f.y.y + 1e-8*f.z.y;
570+
x_off.z = c_s*f.x.z + s_s*f.y.z + 1e-8*f.z.z;
571+
axis = f.z;
572+
n = toolFrameSolve(identityFrame, headTool, 5, &axis, &x_off,
573+
head_seed, 0, sols, TOOL_FRAME_MAX_SOLUTIONS,
574+
free_dirs, spin);
575+
check(n == 2, "a tool x off right angles by rounding is taken");
576+
check(n == 2 && spin[0] != 0.0, "and answered with a turn about the tool");
577+
}
578+
rounded.x = 0; rounded.y = 0; rounded.z = 0;
579+
check(toolFrameSolve(xyzacWork, identityFrame, 5, &rounded, NULL, seed,
580+
0, sols, TOOL_FRAME_MAX_SOLUTIONS, free_dirs, spin) == -1,
581+
"a zero vector is refused");
582+
}
583+
538584

539585
/* Asking a five axis machine for tool x as well. Its two rotaries are
540586
spent on the tool axis and the turn about that axis is not a joint,

0 commit comments

Comments
 (0)