From 985734971108e80cee008cffbdbe0db8146dddc8 Mon Sep 17 00:00:00 2001 From: Luca Toniolo <10792599+grandixximo@users.noreply.github.com> Date: Thu, 3 Sep 2026 18:21:23 +1000 Subject: [PATCH 01/60] xyzbc-trt-kins: apply the direction sign to the offsets in the inverse The inverse turns the x and z offsets through the B tilt in the conventional sense, while the forward turns them in whichever sense the conventional-directions pin selects. With the pin false, which is the default, and any of x-offset, z-offset or tool-offset set, a pose does not survive the round trip: the position comes back out by twice sin(b) times the offset. xyzac has no term of this kind. Found by tests/kins-jacobian, which multiplies the derivative of the inverse by differences of the forward and expects the identity. --- src/emc/kinematics/trtfuncs.c | 7 +++++-- 1 file changed, 5 insertions(+), 2 deletions(-) diff --git a/src/emc/kinematics/trtfuncs.c b/src/emc/kinematics/trtfuncs.c index 31c9ff1a6de..0cb4b5eb7aa 100644 --- a/src/emc/kinematics/trtfuncs.c +++ b/src/emc/kinematics/trtfuncs.c @@ -362,11 +362,14 @@ int xyzbcKinematicsInverse(const EmcPose * pos, const double dz = hal_get_real(haldata->z_offset) + dt; const double b_rad = pos->b*TO_RAD; const double c_rad = pos->c*TO_RAD; - const double dpx = -cos(b_rad)*dx + sin(b_rad)*dz + dx; - const double dpz = -sin(b_rad)*dx - cos(b_rad)*dz + dz; rtapi_real con = hal_get_bool(haldata->conventional_directions) ? 1.0 : -1.0; + // the offsets seen from the tilted table: the same rotation the + // forward applies to them, in the same sense + const double dpx = -cos(b_rad)*dx + con * sin(b_rad)*dz + dx; + const double dpz = -con * sin(b_rad)*dx - cos(b_rad)*dz + dz; + EmcPose P; // computed position P.tran.x = + cos(c_rad) * cos(b_rad) * (pos->tran.x - x_rot_point) From a71bad1ff7723ed4cb5bacae5d2a351c398b373e Mon Sep 17 00:00:00 2001 From: Luca Toniolo <10792599+grandixximo@users.noreply.github.com> Date: Thu, 3 Sep 2026 18:21:23 +1000 Subject: [PATCH 02/60] scarakins: set the elbow flag for a negative elbow angle The forward sets the flag that makes the inverse negate its arc cosine when joint 1 is below 90 degrees, so for an elbow between 0 and 90 the inverse returns the other arm and the pose does not survive the round trip. The sign of the arc cosine is the sign of the elbow angle, so the test is against zero. --- src/emc/kinematics/scarakins.c | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/src/emc/kinematics/scarakins.c b/src/emc/kinematics/scarakins.c index 8db57eb7b0c..3b0e69ee7e4 100644 --- a/src/emc/kinematics/scarakins.c +++ b/src/emc/kinematics/scarakins.c @@ -96,8 +96,9 @@ int scaraKinematicsForward(const double * joint, z = D1 + D3 - joint[2] - D5; c = a3; + // the elbow flag: which sign the inverse gives the acos of joint 1 *iflags = 0; - if (joint[1] < 90) + if (joint[1] < 0) *iflags = 1; world->tran.x = x; From 9675ed0e57f1bc3dca4934403c7af054ed5c646c Mon Sep 17 00:00:00 2001 From: Luca Toniolo <10792599+grandixximo@users.noreply.github.com> Date: Thu, 3 Sep 2026 18:21:23 +1000 Subject: [PATCH 03/60] pumakins, three21kins: compare the branch flags modulo a whole turn The forward decides the shoulder, elbow and wrist branches by comparing a joint angle with the value the inverse's formula gives, within a fuzz, and does not wrap the difference. A joint standing a whole turn from that value, which the differences of two atan2 results produce freely, fails the comparison and the inverse is sent down the other branch. The difference is brought into (-pi, pi] first. --- src/emc/kinematics/pumakins.c | 20 +++++++++++++++----- src/emc/kinematics/three21kins.c | 20 +++++++++++++++----- 2 files changed, 30 insertions(+), 10 deletions(-) diff --git a/src/emc/kinematics/pumakins.c b/src/emc/kinematics/pumakins.c index 98dc83aa4c5..049d384b424 100644 --- a/src/emc/kinematics/pumakins.c +++ b/src/emc/kinematics/pumakins.c @@ -29,6 +29,16 @@ struct haldata { hal_real_t a2, a3, d3, d4, d6; } *haldata = NULL; +/* the difference of two angles, brought into (-pi, pi] so that a joint a + whole turn from the formula still matches it */ +static double angleDiff(double a, double b) +{ + double d = a - b; + while (d > PM_PI) { d -= 2*PM_PI; } + while (d <= -PM_PI) { d += 2*PM_PI; } + return d; +} + /* The flange orientation for a joint set: the ISO 9787 mechanical interface frame, whose z points out of the interface towards the work. Shared by the forward kinematics and the tool frame so the two cannot drift apart. */ @@ -152,16 +162,16 @@ static int pumaKinematicsForward(const double * joint, *iflags = 0; /* Set shoulder-up flag if necessary */ - if (fabs(joint[0]*PM_PI/180 - atan2(hom.tran.y, hom.tran.x) + - atan2(PUMA_D3, -sqrt(sumSq))) < FLAG_FUZZ) + if (fabs(angleDiff(joint[0]*PM_PI/180, atan2(hom.tran.y, hom.tran.x) - + atan2(PUMA_D3, -sqrt(sumSq)))) < FLAG_FUZZ) { *iflags |= PUMA_SHOULDER_RIGHT; } /* Set elbow down flag if necessary */ - if (fabs(joint[2]*PM_PI/180 - atan2(PUMA_A3, PUMA_D4) + + if (fabs(angleDiff(joint[2]*PM_PI/180, atan2(PUMA_A3, PUMA_D4) - atan2(k, -sqrt(PUMA_A3 * PUMA_A3 + - PUMA_D4 * PUMA_D4 - k * k))) < FLAG_FUZZ) + PUMA_D4 * PUMA_D4 - k * k)))) < FLAG_FUZZ) { *iflags |= PUMA_ELBOW_DOWN; } @@ -177,7 +187,7 @@ static int pumaKinematicsForward(const double * joint, /* if not singular set wrist flip flag if necessary */ else{ - if (! (fabs(joint[3]*PM_PI/180 - atan2(t1, t2)) < FLAG_FUZZ)) + if (! (fabs(angleDiff(joint[3]*PM_PI/180, atan2(t1, t2))) < FLAG_FUZZ)) { *iflags |= PUMA_WRIST_FLIP; } diff --git a/src/emc/kinematics/three21kins.c b/src/emc/kinematics/three21kins.c index a5a7b5dfa8f..2a3dfe08ee5 100644 --- a/src/emc/kinematics/three21kins.c +++ b/src/emc/kinematics/three21kins.c @@ -32,6 +32,16 @@ struct haldata { hal_real_t a1, a2, a3, d1, d2, d3, d4, d6; } *haldata = NULL; +/* the difference of two angles, brought into (-pi, pi] so that a joint a + whole turn from the formula still matches it */ +static double angleDiff(double a, double b) +{ + double d = a - b; + while (d > PM_PI) { d -= 2*PM_PI; } + while (d <= -PM_PI) { d += 2*PM_PI; } + return d; +} + static int three21KinematicsForward(const double * joint, EmcPose * world, const KINEMATICS_FORWARD_FLAGS * fflags, @@ -132,8 +142,8 @@ static int three21KinematicsForward(const double * joint, *iflags = 0; /* set shoulder flag */ - if (fabs(joint[0]*PM_PI/180 - atan2(hom.tran.y, hom.tran.x) + - atan2(d23, -sqrt(sumSq))) < FLAG_FUZZ) + if (fabs(angleDiff(joint[0]*PM_PI/180, atan2(hom.tran.y, hom.tran.x) - + atan2(d23, -sqrt(sumSq)))) < FLAG_FUZZ) { *iflags |= THREE21_SHOULDER_RIGHT; } @@ -143,8 +153,8 @@ static int three21KinematicsForward(const double * joint, if (discr < 0.0) { discr = 0.0; } - if (fabs(joint[2]*PM_PI/180 - atan2(a3, d4) + - atan2(k, -sqrt(discr))) < FLAG_FUZZ) + if (fabs(angleDiff(joint[2]*PM_PI/180, atan2(a3, d4) - + atan2(k, -sqrt(discr)))) < FLAG_FUZZ) { *iflags |= THREE21_ELBOW_DOWN; } @@ -158,7 +168,7 @@ static int three21KinematicsForward(const double * joint, } else { - if (! (fabs(joint[3]*PM_PI/180 - atan2(t1, t2)) < FLAG_FUZZ)) + if (! (fabs(angleDiff(joint[3]*PM_PI/180, atan2(t1, t2))) < FLAG_FUZZ)) { *iflags |= THREE21_WRIST_FLIP; } From 680c882d3c83e4aa26c409ffd4da85f990763f14 Mon Sep 17 00:00:00 2001 From: Luca Toniolo <10792599+grandixximo@users.noreply.github.com> Date: Thu, 3 Sep 2026 18:21:24 +1000 Subject: [PATCH 04/60] userkins, millturn, matrixkins, xyzab_tdr_kins: set the kinematics pins up once, at load These four build their pins on the first kinematicsType() call behind an is_setup flag nothing sets, so every later call runs the setup again, reassigns haldata and fails to create pins already taken; motion asks twice when num_extrajoints is set. Move the setup to EXTRA_SETUP(), which halcompile runs once before the component is ready, as the two trsrn modules already do. The pins exist from load, kinematicsType() only answers, and the hal_set_unready()/hal_ready() dance goes with it. userkins is the template for out of tree modules, so its description says where the pins go. --- src/hal/components/matrixkins.comp | 14 +++++--------- src/hal/components/millturn.comp | 13 ++++++------- src/hal/components/userkins.comp | 18 +++++++++--------- src/hal/components/xyzab_tdr_kins.comp | 11 +++++------ 4 files changed, 25 insertions(+), 31 deletions(-) diff --git a/src/hal/components/matrixkins.comp b/src/hal/components/matrixkins.comp index b12dedc2fcf..aac6c04d913 100644 --- a/src/hal/components/matrixkins.comp +++ b/src/hal/components/matrixkins.comp @@ -176,6 +176,7 @@ the adjustment values should be added to the old values instead of replacing the """; see_also "kins(9)"; pin out bool dummy=1; // halcompile requires at least one pin +option extra_setup; license "GPL"; ;; @@ -191,15 +192,15 @@ static struct haldata { hal_real_t C_zz; } *haldata; -static int matrixkins_setup(void) { +EXTRA_SETUP() { + (void)__comp_inst; + (void)prefix; + (void)extra_arg; int res=0; // inherit comp_id from rtapi_main() if (comp_id < 0) goto error; - res = hal_set_unready(comp_id); - if (res) goto error; - haldata = hal_malloc(sizeof(struct haldata)); if (!haldata) goto error; @@ -215,9 +216,6 @@ static int matrixkins_setup(void) { if (res) goto error; - res = hal_ready(comp_id); - if (res) goto error; - rtapi_print("*** %s setup ok\n",__FILE__); return 0; error: @@ -235,8 +233,6 @@ EXPORT_SYMBOL(kinematicsForward); KINEMATICS_TYPE kinematicsType() { - static bool is_setup=0; - if (!is_setup) matrixkins_setup(); return KINEMATICS_BOTH; } diff --git a/src/hal/components/millturn.comp b/src/hal/components/millturn.comp index d1271de882e..45ec650ad96 100644 --- a/src/hal/components/millturn.comp +++ b/src/hal/components/millturn.comp @@ -27,9 +27,10 @@ chapter (docs/src/motion/switchkins.txt) """; // The fpin pin is not accessible in kinematics functions. -// Use the *_setup() function for pins and params used by kinematics. +// Use EXTRA_SETUP() for pins and params used by kinematics. pin out si32 fpin=0"pin to demonstrate use of a conventional (non-kinematics) function fdemo"; option period no; +option extra_setup; function fdemo; license "GPL"; author "David Mueller"; @@ -59,14 +60,15 @@ FUNCTION(fdemo) { fpin_set(fpin + 1); } -static int millturn_setup(void) { +EXTRA_SETUP() { + (void)__comp_inst; + (void)prefix; + (void)extra_arg; #define HAL_PREFIX "millturn" int res=0; // inherit comp_id from rtapi_main() if (comp_id < 0) goto error; - // set unready to allow creation of pins - if (hal_set_unready(comp_id)) goto error; haldata = hal_malloc(sizeof(*haldata)); if (!haldata) goto error; @@ -85,7 +87,6 @@ static int millturn_setup(void) { res += hal_pin_new_bool(comp_id, HAL_OUT, &haldata->kinstype_is_1, 0, "kinstype.is-1"); if (res) goto error; - hal_ready(comp_id); rtapi_print("*** %s setup ok\n",__FILE__); return 0; error: @@ -142,8 +143,6 @@ int kinematicsSwitch(int new_switchkins_type) KINEMATICS_TYPE kinematicsType() { -static bool is_setup=0; - if (!is_setup) millturn_setup(); return KINEMATICS_BOTH; // set as required // Note: If kinematics are identity, using KINEMATICS_BOTH // may be used in order to allow a gui to display diff --git a/src/hal/components/userkins.comp b/src/hal/components/userkins.comp index a2a25d88c29..a7af5d29a75 100644 --- a/src/hal/components/userkins.comp +++ b/src/hal/components/userkins.comp @@ -53,14 +53,16 @@ change all instances of `userkins` to `mykins`. * The *fpin* pin is included to satisfy the requirements of the halcompile utility but it is not accessible to kinematics functions. * HAL pins and parameters needed in kinematics functions (kinematicsForward(), - kinematicsInverse()) must be setup in a function (*userkins_setup()*) invoked - by the initial motion module call to kinematicsType(). + kinematicsInverse()) must be setup in the *EXTRA_SETUP()* function, which + halcompile runs once when the module is loaded, before the component is + made ready. """; // The fpin pin is not accessible in kinematics functions. -// Use the *_setup() function for pins and params used by kinematics. +// Use EXTRA_SETUP() for pins and params used by kinematics. pin out si32 fpin=0"pin to demonstrate use of a conventional (non-kinematics) function fdemo"; option period no; +option extra_setup; function fdemo; license "GPL"; author "Dewey Garrett"; @@ -91,14 +93,15 @@ FUNCTION(fdemo) { fpin_set(fpin + 1); } -static int userkins_setup(void) { +EXTRA_SETUP() { + (void)__comp_inst; + (void)prefix; + (void)extra_arg; #define HAL_PREFIX "userkins" int res=0; // inherit comp_id from rtapi_main() if (comp_id < 0) goto error; - // set unready to allow creation of pins - if (hal_set_unready(comp_id)) goto error; haldata = hal_malloc(sizeof(struct haldata)); if (!haldata) goto error; @@ -112,7 +115,6 @@ static int userkins_setup(void) { res += hal_param_new_real(comp_id, HAL_RO, &haldata->param_ro, 0.0, "%s.param-ro", HAL_PREFIX); if (res) goto error; - hal_ready(comp_id); rtapi_print("*** %s setup ok\n",__FILE__); return 0; error: @@ -130,8 +132,6 @@ EXPORT_SYMBOL(kinematicsForward); KINEMATICS_TYPE kinematicsType() { -static bool is_setup=0; - if (!is_setup) userkins_setup(); return KINEMATICS_IDENTITY; // set as required // Note: If kinematics are identity, using KINEMATICS_BOTH // may be used in order to allow a gui to display diff --git a/src/hal/components/xyzab_tdr_kins.comp b/src/hal/components/xyzab_tdr_kins.comp index b059f32c103..dd0350e44f7 100644 --- a/src/hal/components/xyzab_tdr_kins.comp +++ b/src/hal/components/xyzab_tdr_kins.comp @@ -31,6 +31,7 @@ chapter (docs/src/motion/switchkins.txt) """; pin out si32 dummy=0"one pin needed to satisfy halcompile requirement"; +option extra_setup; license "GPL"; author "David Mueller"; @@ -54,13 +55,14 @@ static struct haldata { hal_bool_t kinstype_is_1; } *haldata; -static int xyzab_tdr_setup(void) { +EXTRA_SETUP() { + (void)__comp_inst; + (void)prefix; + (void)extra_arg; #define HAL_PREFIX "xyzab_tdr_kins" int res=0; // inherit comp_id from rtapi_main() if (comp_id < 0) goto error; - // set unready to allow creation of pins - if (hal_set_unready(comp_id)) goto error; haldata = hal_malloc(sizeof(*haldata)); if (!haldata) goto error; @@ -80,7 +82,6 @@ static int xyzab_tdr_setup(void) { res += hal_pin_new_bool(comp_id, HAL_OUT, &haldata->kinstype_is_1, 0, "kinstype.is-1"); if (res) goto error; - hal_ready(comp_id); rtapi_print("*** %s setup ok\n",__FILE__); return 0; error: @@ -137,8 +138,6 @@ int kinematicsSwitch(int new_switchkins_type) KINEMATICS_TYPE kinematicsType() { -static bool is_setup=0; - if (!is_setup) xyzab_tdr_setup(); return KINEMATICS_BOTH; // set as required // Note: If kinematics are identity, using KINEMATICS_BOTH // may be used in order to allow a gui to display From 938b16d33950ef433bb42b21de5d87ae636d4da8 Mon Sep 17 00:00:00 2001 From: Luca Toniolo <10792599+grandixximo@users.noreply.github.com> Date: Tue, 15 Sep 2026 18:12:53 +1000 Subject: [PATCH 05/60] maxkins: make the forward and the inverse describe the same machine Two faults left kinematicsForward and kinematicsInverse disagreeing whenever the head is tilted, and nothing in tree could show it: max5 is the only config that loads the module and it ships B_PIVOT_LENGTH = 0 with six joints, so the pivot to tip distance is zero and U, V and W never leave zero. The forward took the U axis apart into xv and zv and subtracted both from the position it reports, while the inverse added xv back and subtracted zv again: X inverted and Z did not, in the same function, the shape of a typo rather than of a convention. The inverse now adds zv back. The forward also rotated the table and saddle into the workpiece frame and then subtracted the B, U and V corrections from the result, so those landed in the rotated frame; the inverse un-rotates first and adds them back in the machine frame. The vismach model says which is right: max5gui builds the head as tool, spindle, head, brotary, zslide, column, and the workpiece separately as crotary, table, saddle, so the head hangs off the Z slide and does not turn with the C table. The corrections are machine frame, they apply before the rotation, and the inverse had it right. The forward now subtracts them from the joint values and rotates the sum. A sweep of 40000 random poses, pivot lengths to 300 and tool lengths to 37.5, round trips to 2.7e-13, against 666 mm before. Reported world coordinates move for anyone running maxkins with the head tilted and the table turned; they were wrong by that amount, and the DRO and the machine now agree. Nothing moves at C0, and nothing moves at all on the max5 sim. tests/maxkins drives the machine through poses with the pivot length, the tool length and each of B, C, U, V and W away from zero and compares the position that comes back with the one commanded: motion runs the inverse to turn the pose into joints and the forward to turn the joint feedback into a position, so a pose that survives the round trip is one the two directions agree on. Both settings of conventional-directions are covered. Against the old code the pose with B 40 and U 15 came back 19.284 mm out in Z, twice u sin(b), and the pose with B 40 and C 30 landed 10.765 mm out in X and 40.174 mm out in Y. --- src/emc/kinematics/maxkins.c | 20 ++-- tests/maxkins/README | 7 ++ tests/maxkins/checkresult | 2 + tests/maxkins/core_sim.hal | 28 +++++ tests/maxkins/test-ui.py | 134 ++++++++++++++++++++++ tests/maxkins/test.ini | 210 +++++++++++++++++++++++++++++++++++ tests/maxkins/test.sh | 4 + tests/maxkins/tool.tbl | 0 8 files changed, 398 insertions(+), 7 deletions(-) create mode 100644 tests/maxkins/README create mode 100755 tests/maxkins/checkresult create mode 100644 tests/maxkins/core_sim.hal create mode 100755 tests/maxkins/test-ui.py create mode 100644 tests/maxkins/test.ini create mode 100755 tests/maxkins/test.sh create mode 100644 tests/maxkins/tool.tbl diff --git a/src/emc/kinematics/maxkins.c b/src/emc/kinematics/maxkins.c index 773525b3566..d2623e0ad62 100644 --- a/src/emc/kinematics/maxkins.c +++ b/src/emc/kinematics/maxkins.c @@ -50,10 +50,6 @@ int kinematicsForward(const double *joints, // B correction const double zb = (pivot_length + joints[8] + tool_length) * cos(d2r(joints[4])); const double xb = (pivot_length + joints[8] + tool_length) * sin(d2r(joints[4])); - - // C correction - const double xyr = hypot(joints[0], joints[1]); - const double xytheta = atan2(joints[1], joints[0]) + d2r(joints[5]); // U correction const double zv = joints[6] * sin(d2r(joints[4])); @@ -61,8 +57,18 @@ int kinematicsForward(const double *joints, // V correction is always in joint 1 only - pos->tran.x = xyr * cos(xytheta) - (con * xb) - xv; - pos->tran.y = xyr * sin(xytheta) - joints[7]; + // B, U and V are all machine frame: the head hangs off the Z slide and + // does not turn with the C table, so they apply before the rotation into + // the workpiece frame rather than after it. + const double mx = joints[0] - (con * xb) - xv; + const double my = joints[1] - joints[7]; + + // C correction + const double xyr = hypot(mx, my); + const double xytheta = atan2(my, mx) + d2r(joints[5]); + + pos->tran.x = xyr * cos(xytheta); + pos->tran.y = xyr * sin(xytheta); pos->tran.z = joints[2] - zb - (con * zv) + pivot_length + tool_length; pos->a = joints[3]; @@ -103,7 +109,7 @@ int kinematicsInverse(const EmcPose * pos, joints[0] = xyr * cos(xytheta) + (con * xb) + xv; joints[1] = xyr * sin(xytheta) + pos->v; - joints[2] = pos->tran.z + zb - (con * zv) - pivot_length - tool_length; + joints[2] = pos->tran.z + zb + (con * zv) - pivot_length - tool_length; joints[3] = pos->a; joints[4] = pos->b; diff --git a/tests/maxkins/README b/tests/maxkins/README new file mode 100644 index 00000000000..cdf964410db --- /dev/null +++ b/tests/maxkins/README @@ -0,0 +1,7 @@ +Tests that maxkins forward and inverse kinematics agree with each other. + +The machine is driven to a series of poses with the pivot length, the tool +length and every one of B, C, U, V and W away from zero, and the position +that comes back through the forward kinematics is compared with the one that +was commanded. A pose that survives the round trip is one where the two +directions of the transform describe the same machine. diff --git a/tests/maxkins/checkresult b/tests/maxkins/checkresult new file mode 100755 index 00000000000..24dc9aa53e3 --- /dev/null +++ b/tests/maxkins/checkresult @@ -0,0 +1,2 @@ +#!/bin/sh +exit 0 # test failure is indicated by test.sh exit value diff --git a/tests/maxkins/core_sim.hal b/tests/maxkins/core_sim.hal new file mode 100644 index 00000000000..daf9b8f4c25 --- /dev/null +++ b/tests/maxkins/core_sim.hal @@ -0,0 +1,28 @@ +# core HAL config file for simulation + +loadrt [KINS]KINEMATICS +loadrt [EMCMOT]EMCMOT servo_period_nsec=[EMCMOT]SERVO_PERIOD num_joints=[KINS]JOINTS + +# add motion controller functions to servo thread +addf motion-command-handler servo-thread +addf motion-controller servo-thread + +# create HAL signals for position commands from motion module +# loop position commands back to motion module feedback +net Xpos joint.0.motor-pos-cmd => joint.0.motor-pos-fb +net Ypos joint.1.motor-pos-cmd => joint.1.motor-pos-fb +net Zpos joint.2.motor-pos-cmd => joint.2.motor-pos-fb +net Apos joint.3.motor-pos-cmd => joint.3.motor-pos-fb +net Bpos joint.4.motor-pos-cmd => joint.4.motor-pos-fb +net Cpos joint.5.motor-pos-cmd => joint.5.motor-pos-fb +net Upos joint.6.motor-pos-cmd => joint.6.motor-pos-fb +net Vpos joint.7.motor-pos-cmd => joint.7.motor-pos-fb +net Wpos joint.8.motor-pos-cmd => joint.8.motor-pos-fb + +# the geometry the kinematics works from. Both are away from zero so that +# the pivot to tip distance is not degenerate. +setp maxkins.pivot-length [MAX]B_PIVOT_LENGTH +setp maxkins.tool-length [MAX]TOOL_LENGTH + +# estop loopback +net estop-loop iocontrol.0.user-enable-out iocontrol.0.emc-enable-in diff --git a/tests/maxkins/test-ui.py b/tests/maxkins/test-ui.py new file mode 100755 index 00000000000..d2b804cb83f --- /dev/null +++ b/tests/maxkins/test-ui.py @@ -0,0 +1,134 @@ +#!/usr/bin/env python3 + +import linuxcnc +import hal + +import time +import sys + + +def wait_for_linuxcnc_startup(status, timeout=10.0): + + """Poll the Status buffer waiting for it to look initialized, + rather than just allocated (all-zero). Returns on success, throws + RuntimeError on failure.""" + + start_time = time.time() + while time.time() - start_time < timeout: + status.poll() + if (status.angular_units == 0.0) \ + or (status.axis_mask == 0) \ + or (status.cycle_time == 0.0) \ + or (status.exec_state != linuxcnc.EXEC_DONE) \ + or (status.interp_state != linuxcnc.INTERP_IDLE) \ + or (status.inpos == False) \ + or (status.linear_units == 0.0) \ + or (status.max_acceleration == 0.0) \ + or (status.max_velocity == 0.0) \ + or (status.program_units == 0.0) \ + or (status.rapidrate == 0.0) \ + or (status.state != linuxcnc.RCS_DONE) \ + or (status.task_state != linuxcnc.STATE_ESTOP): + time.sleep(0.1) + else: + # looks good + return + + # timeout, throw an exception + raise RuntimeError("Timeout") + + +c = linuxcnc.command() +s = linuxcnc.stat() +e = linuxcnc.error_channel() +h = hal.component("dummy") +h.ready() + +# Wait for LinuxCNC to initialize itself so the Status buffer stabilizes. +wait_for_linuxcnc_startup(s) + +# Because the kinematics is non-trivial, a homing is needed. +# HOME_ABSOLUTE_ENCODER = 1 is used in the ini file. +c.state(linuxcnc.STATE_ESTOP_RESET) +c.state(linuxcnc.STATE_ON) +for joint in range(9): + c.home(joint) +c.wait_complete() + +LETTERS = 'XYZABCUVW' + + +def absdelta(a, b): + '''Maximum absolute difference between components of two coordinate points''' + return max(abs(a[i] - b[i]) for i in range(len(a))) + + +def test_pose(pose): + '''Command LinuxCNC to go to 'pose', then check that the position that + comes back matches it. Motion runs the inverse kinematics to turn the + pose into joint values and the forward kinematics to turn the joint + feedback back into a position, so a pose that comes back unchanged is one + the two directions agree on. + ''' + words = ' '.join('%s%0.9f' % (letter, value) + for letter, value in zip(LETTERS, pose)) + + c.mode(linuxcnc.MODE_MDI) + c.mdi('G0 ' + words) + c.wait_complete() + + # The delay seems to be needed for trajectory to fully settle, otherwise + # there is about 1e-5 error between target and actual position. + time.sleep(0.05) + s.poll() + joints = s.joint_actual_position[:9] + position = s.actual_position[:9] + + print("Commanded %s, joints %s, position %s" % (pose, joints, position)) + + # Accuracy limit is set to 1e-9 here. + # For practical purposes, a numerical accuracy of 1e-6 would be perfectly + # acceptable. Current implementation using doubles achieves about 1e-14 + # precision. + if absdelta(position, pose) > 1e-9: + raise RuntimeError( + "Forward and inverse kinematics disagree: commanded %s, joints %s, got %s" + % (pose, joints, position)) + + +# X Y Z A B C U V W +POSES = [ + # The pose everything starts from. + ( 0, 0, 0, 0, 0, 0, 0, 0, 0), + # Linear axes only, with the head upright. + ( 10, 20, 30, 0, 0, 0, 0, 0, 0), + # B alone tilts the head, which moves the tip in X and Z. + ( 10, 20, 30, 0, 40, 0, 0, 0, 0), + # C alone turns the table under the tip. + ( 10, 20, 30, 0, 0, 30, 0, 0, 0), + # B and C together: the head correction is in the machine frame and the + # table rotation is not, so the order the two are applied in matters. + ( 10, 20, 30, 0, 40, 30, 0, 0, 0), + # U alone shifts the head along its own axis, which with B tilted has a + # component in both X and Z. + ( 10, 20, 30, 0, 40, 0, 15, 0, 0), + # V is a plain shift of the saddle. + ( 10, 20, 30, 0, 40, 0, 0, 8, 0), + # W extends the pivot to tip distance. + ( 10, 20, 30, 0, 40, 0, 0, 0, 12), + # Everything at once. + ( 50, 50, 50, 5, 40, 30, 15, 8, 12), + # And with the signs the other way round. + ( -50, -50, -50, -5, -40, -30, -15, -8, -12), + # Back to the origin, where the pose is the same whichever direction the + # axes are taken to run in. + ( 0, 0, 0, 0, 0, 0, 0, 0, 0), +] + +for conventional in (0, 1): + hal.set_p('maxkins.conventional-directions', str(conventional)) + print("conventional-directions %d" % conventional) + for pose in POSES: + test_pose(pose) + +sys.exit(0) diff --git a/tests/maxkins/test.ini b/tests/maxkins/test.ini new file mode 100644 index 00000000000..7cf23c05738 --- /dev/null +++ b/tests/maxkins/test.ini @@ -0,0 +1,210 @@ +[EMC] +# The version string for this INI file. +VERSION = 1.1 + +DEBUG = 0 + +[MAX] +# distance from the end of the reference tool to the centre of the head +# tilt axis, and the length of the tool on top of it +B_PIVOT_LENGTH = 100 +TOOL_LENGTH = 25 + +[DISPLAY] +DISPLAY = ./test-ui.py + +[FILTER] +#No Content + +[RS274NGC] +PARAMETER_FILE = sim.var + +[EMCMOT] +EMCMOT = motmod +COMM_TIMEOUT = 4.0 +BASE_PERIOD = 0 +SERVO_PERIOD = 1000000 + +[TASK] +TASK = milltask +CYCLE_TIME = 0.001 + +[HAL] +HALUI = halui +HALFILE = core_sim.hal + +[HALUI] +#No Content + +[TRAJ] +COORDINATES = X Y Z A B C U V W +HOME = 0 0 0 0 0 0 0 0 0 +LINEAR_UNITS = mm +ANGULAR_UNITS = degree +DEFAULT_LINEAR_VELOCITY = 10 +MAX_LINEAR_VELOCITY = 100 +MAX_LINEAR_ACCELERATION = 1000 +DEFAULT_ANGULAR_VELOCITY = 10 +MAX_ANGULAR_VELOCITY = 100 +MAX_ANGULAR_ACCELERATION = 1000 + +[EMCIO] +TOOL_TABLE = tool.tbl +RANDOM_TOOLCHANGER = 0 + +[KINS] +KINEMATICS = maxkins +JOINTS = 9 + +[AXIS_X] +MIN_LIMIT = -1000 +MAX_LIMIT = 1000 +MAX_VELOCITY = 100 +MAX_ACCELERATION = 1000 + +[JOINT_0] +TYPE = LINEAR +HOME = 0.000 +HOME_ABSOLUTE_ENCODER = 1 +MAX_VELOCITY = 100 +MAX_ACCELERATION = 1000 +BACKLASH = 0.000 +MIN_LIMIT = -1000 +MAX_LIMIT = 1000 +FERROR = 0.001 + +[AXIS_Y] +MIN_LIMIT = -1000 +MAX_LIMIT = 1000 +MAX_VELOCITY = 100 +MAX_ACCELERATION = 1000 + +[JOINT_1] +TYPE = LINEAR +HOME = 0.000 +HOME_ABSOLUTE_ENCODER = 1 +MAX_VELOCITY = 100 +MAX_ACCELERATION = 1000 +BACKLASH = 0.000 +MIN_LIMIT = -1000 +MAX_LIMIT = 1000 +FERROR = 0.001 + +[AXIS_Z] +MIN_LIMIT = -1000 +MAX_LIMIT = 1000 +MAX_VELOCITY = 100 +MAX_ACCELERATION = 1000 + +[JOINT_2] +TYPE = LINEAR +HOME = 0.000 +HOME_ABSOLUTE_ENCODER = 1 +MAX_VELOCITY = 100 +MAX_ACCELERATION = 1000 +BACKLASH = 0.000 +MIN_LIMIT = -1000 +MAX_LIMIT = 1000 +FERROR = 0.001 + +[AXIS_A] +MIN_LIMIT = -360 +MAX_LIMIT = 360 +MAX_VELOCITY = 100 +MAX_ACCELERATION = 1000 + +[JOINT_3] +TYPE = ANGULAR +HOME = 0.000 +HOME_ABSOLUTE_ENCODER = 1 +MAX_VELOCITY = 100 +MAX_ACCELERATION = 1000 +BACKLASH = 0.000 +MIN_LIMIT = -360 +MAX_LIMIT = 360 +FERROR = 0.001 + +[AXIS_B] +MIN_LIMIT = -360 +MAX_LIMIT = 360 +MAX_VELOCITY = 100 +MAX_ACCELERATION = 1000 + +[JOINT_4] +TYPE = ANGULAR +HOME = 0.000 +HOME_ABSOLUTE_ENCODER = 1 +MAX_VELOCITY = 100 +MAX_ACCELERATION = 1000 +BACKLASH = 0.000 +MIN_LIMIT = -360 +MAX_LIMIT = 360 +FERROR = 0.001 + +[AXIS_C] +MIN_LIMIT = -360 +MAX_LIMIT = 360 +MAX_VELOCITY = 100 +MAX_ACCELERATION = 1000 + +[JOINT_5] +TYPE = ANGULAR +HOME = 0.000 +HOME_ABSOLUTE_ENCODER = 1 +MAX_VELOCITY = 100 +MAX_ACCELERATION = 1000 +BACKLASH = 0.000 +MIN_LIMIT = -360 +MAX_LIMIT = 360 +FERROR = 0.001 + +[AXIS_U] +MIN_LIMIT = -1000 +MAX_LIMIT = 1000 +MAX_VELOCITY = 100 +MAX_ACCELERATION = 1000 + +[JOINT_6] +TYPE = LINEAR +HOME = 0.000 +HOME_ABSOLUTE_ENCODER = 1 +MAX_VELOCITY = 100 +MAX_ACCELERATION = 1000 +BACKLASH = 0.000 +MIN_LIMIT = -1000 +MAX_LIMIT = 1000 +FERROR = 0.001 + +[AXIS_V] +MIN_LIMIT = -1000 +MAX_LIMIT = 1000 +MAX_VELOCITY = 100 +MAX_ACCELERATION = 1000 + +[JOINT_7] +TYPE = LINEAR +HOME = 0.000 +HOME_ABSOLUTE_ENCODER = 1 +MAX_VELOCITY = 100 +MAX_ACCELERATION = 1000 +BACKLASH = 0.000 +MIN_LIMIT = -1000 +MAX_LIMIT = 1000 +FERROR = 0.001 + +[AXIS_W] +MIN_LIMIT = -1000 +MAX_LIMIT = 1000 +MAX_VELOCITY = 100 +MAX_ACCELERATION = 1000 + +[JOINT_8] +TYPE = LINEAR +HOME = 0.000 +HOME_ABSOLUTE_ENCODER = 1 +MAX_VELOCITY = 100 +MAX_ACCELERATION = 1000 +BACKLASH = 0.000 +MIN_LIMIT = -1000 +MAX_LIMIT = 1000 +FERROR = 0.001 diff --git a/tests/maxkins/test.sh b/tests/maxkins/test.sh new file mode 100755 index 00000000000..6edf34d77ca --- /dev/null +++ b/tests/maxkins/test.sh @@ -0,0 +1,4 @@ +#!/bin/bash + +rm -f sim.var +linuxcnc -r test.ini diff --git a/tests/maxkins/tool.tbl b/tests/maxkins/tool.tbl new file mode 100644 index 00000000000..e69de29bb2d From 21aef29fdbe5c029c3add96e948352cb60be9777 Mon Sep 17 00:00:00 2001 From: Luca Toniolo <10792599+grandixximo@users.noreply.github.com> Date: Tue, 15 Sep 2026 18:31:31 +1000 Subject: [PATCH 06/60] motion: a kinematics switch whose forward cannot solve the joints is refused handle_kinematicsSwitch() switched the module, then ran the new forward kinematics on the joints and, when that failed, reported it and kept the position it knew. The module stayed in the new type, so from the next cycle the inverse ran the joints to wherever that kept position lands in the new kinematics: on the hexapod that is a jump of twenty units on every strut, with no move commanded. The joints are where they are, and a kinematics whose forward cannot solve them is not one the machine can run in from there. Switch the module back before anything else sees the new type, report which type is still in force, and abort as before. Nothing moves and a later switch is taken on its own merits. tests/kins-switch-unsolved loads the hexapod with identity first, homes at zero, asks for the hexapod and checks that the type, the joints and the position are all as they were, that the failure was reported, that a move afterwards follows the identity inverse and that a switch to the type in force is accepted. --- src/emc/motion/control.c | 20 ++-- tests/kins-switch-unsolved/README | 9 ++ tests/kins-switch-unsolved/checkresult | 3 + tests/kins-switch-unsolved/sim.hal | 16 +++ tests/kins-switch-unsolved/test-ui.py | 112 +++++++++++++++++++++ tests/kins-switch-unsolved/test.ini | 130 +++++++++++++++++++++++++ tests/kins-switch-unsolved/test.sh | 4 + tests/kins-switch-unsolved/tool.tbl | 1 + 8 files changed, 288 insertions(+), 7 deletions(-) create mode 100644 tests/kins-switch-unsolved/README create mode 100755 tests/kins-switch-unsolved/checkresult create mode 100644 tests/kins-switch-unsolved/sim.hal create mode 100755 tests/kins-switch-unsolved/test-ui.py create mode 100644 tests/kins-switch-unsolved/test.ini create mode 100755 tests/kins-switch-unsolved/test.sh create mode 100644 tests/kins-switch-unsolved/tool.tbl diff --git a/src/emc/motion/control.c b/src/emc/motion/control.c index 50ddadc6c07..b509a133d3e 100644 --- a/src/emc/motion/control.c +++ b/src/emc/motion/control.c @@ -363,10 +363,6 @@ static void handle_kinematicsSwitch(void) { return; // the kinematics in force is unchanged } - switchkins_type = requested_type; - hal_set_real(emcmot_hal_data->kins_type, (double)switchkins_type); - emcmotStatus->switchkins_type = switchkins_type; - KINEMATICS_FORWARD_FLAGS tmpFFlags = fflags; KINEMATICS_INVERSE_FLAGS tmpIFlags = iflags; #ifdef SWITCHKINS_DEBUG @@ -376,15 +372,25 @@ static void handle_kinematicsSwitch(void) { beforePose[anum] = *pcmd_p[anum]; } #endif + /* the joints stay where they are, so a kinematics whose forward cannot + solve them is one the machine cannot run in from here: put the old + one back, or the inverse would run the joints to wherever the pose + we know lands in the new one */ EmcPose poseKinsSwitch = emcmotStatus->carte_pos_cmd; if (kinematicsForward(joint_posKinsSwitch, &poseKinsSwitch, &tmpFFlags, &tmpIFlags)) { - reportError(_("kinematicsForward failed for kinematics type %d"), - switchkins_type); + kinematicsSwitch(switchkins_type); + reportError(_("kinematicsForward failed for kinematics type %d," + " type %d is still in force"), + requested_type, switchkins_type); SET_MOTION_ERROR_FLAG(1); // abort - return; // keep the position we know rather than an unsolved one + return; // the kinematics in force and the position are unchanged } emcmotStatus->carte_pos_cmd = poseKinsSwitch; + + switchkins_type = requested_type; + hal_set_real(emcmot_hal_data->kins_type, (double)switchkins_type); + emcmotStatus->switchkins_type = switchkins_type; #ifdef SWITCHKINS_DEBUG fprintf(stderr,"kswitch type=%d (%s:%d)\n",switchkins_type,__FUNCTION__,__LINE__); for (anum = 0; anum < EMCMOT_MAX_AXIS; anum++) { diff --git a/tests/kins-switch-unsolved/README b/tests/kins-switch-unsolved/README new file mode 100644 index 00000000000..c3f263e4f7f --- /dev/null +++ b/tests/kins-switch-unsolved/README @@ -0,0 +1,9 @@ +A kinematics switch whose forward kinematics cannot solve the current +joint position must leave the type in force, the joints and the position +alone, report the failure and abort; motion goes on working in the +kinematics it kept. + +The hexapod module starts in identity kinematics here, so the joints are +put at a few units each as coordinates. Six struts that short are not a +platform position the hexapod forward can converge to, so G12.1 P1 asks +for exactly that switch. diff --git a/tests/kins-switch-unsolved/checkresult b/tests/kins-switch-unsolved/checkresult new file mode 100755 index 00000000000..9d48d3f180e --- /dev/null +++ b/tests/kins-switch-unsolved/checkresult @@ -0,0 +1,3 @@ +#!/bin/sh +# the test script counts its own failures +grep -q "^Exiting with 0 errors" "$1" diff --git a/tests/kins-switch-unsolved/sim.hal b/tests/kins-switch-unsolved/sim.hal new file mode 100644 index 00000000000..e92c60eb526 --- /dev/null +++ b/tests/kins-switch-unsolved/sim.hal @@ -0,0 +1,16 @@ +loadrt [KINS]KINEMATICS +loadrt [EMCMOT]EMCMOT servo_period_nsec=[EMCMOT]SERVO_PERIOD num_joints=[KINS]JOINTS + +addf motion-command-handler servo-thread +addf motion-controller servo-thread + +net J0 joint.0.motor-pos-cmd => joint.0.motor-pos-fb +net J1 joint.1.motor-pos-cmd => joint.1.motor-pos-fb +net J2 joint.2.motor-pos-cmd => joint.2.motor-pos-fb +net J3 joint.3.motor-pos-cmd => joint.3.motor-pos-fb +net J4 joint.4.motor-pos-cmd => joint.4.motor-pos-fb +net J5 joint.5.motor-pos-cmd => joint.5.motor-pos-fb + +net estop-loop iocontrol.0.user-enable-out iocontrol.0.emc-enable-in +net tool-prep-loop iocontrol.0.tool-prepare iocontrol.0.tool-prepared +net tool-change-loop iocontrol.0.tool-change iocontrol.0.tool-changed diff --git a/tests/kins-switch-unsolved/test-ui.py b/tests/kins-switch-unsolved/test-ui.py new file mode 100755 index 00000000000..4335f689ac0 --- /dev/null +++ b/tests/kins-switch-unsolved/test-ui.py @@ -0,0 +1,112 @@ +#!/usr/bin/env python3 +# A kinematics switch the new forward cannot solve. Motion must keep the +# position it knows, say so, abort, and take a switch back afterwards. +import hal +import linuxcnc +import sys +import time + +c = linuxcnc.command() +s = linuxcnc.stat() +e = linuxcnc.error_channel() + +errors = 0 + +def error(what): + global errors + errors += 1 + print("*** ERROR %s" % what) + +def kins_type(): + return int(round(hal.get_value("motion.kins-type"))) + +def position(): + s.poll() + return tuple(round(v, 6) for v in s.position[:6]) + +def drain(): + said = [] + while True: + m = e.poll() + if not m: + return said + said.append(m[1]) + +def wait_idle(): + deadline = time.time() + 30 + while time.time() < deadline: + s.poll() + if s.interp_state == linuxcnc.INTERP_IDLE and not s.queue: + return + time.sleep(0.05) + error("timed out waiting for the interpreter") + +def mdi(cmd): + c.mdi(cmd) + c.wait_complete(30) + wait_idle() + time.sleep(0.2) # let the error channel and the status catch up + +c.state(linuxcnc.STATE_ESTOP_RESET) +c.state(linuxcnc.STATE_ON) +c.wait_complete(30) +c.home(-1) +c.wait_complete(60) +c.mode(linuxcnc.MODE_MDI) +c.wait_complete(30) +drain() + +if kins_type() != 0: + error("starts in kinematics type %d, expected identity (0)" % kins_type()) + +def joints(): + s.poll() + return tuple(round(v, 6) for v in s.joint_position[:6]) + +# a known position, in identity: joints and coordinates are the same thing +mdi("G0 X1 Y2 Z3 A4 B5 C6") +before = position() +if before != (1, 2, 3, 4, 5, 6): + error("position before the switch is %s, expected (1, 2, 3, 4, 5, 6)" % (before,)) +drain() + +# the switch the hexapod forward cannot solve: six struts of a few units +# are no platform position. Nothing may change but the report. +mdi("G12.1 P1") +said = drain() +if not any("kinematicsForward failed" in m for m in said): + error("no report of the failed forward kinematics, got %s" % said) +if kins_type() != 0: + error("kinematics type %d after the failed switch, expected 0 still" % kins_type()) +if position() != before: + error("position after the failed switch is %s, expected %s unchanged" % (position(), before)) +if joints() != before: + error("joints after the failed switch are %s, expected %s unchanged" % (joints(), before)) +s.poll() +if s.task_state != linuxcnc.STATE_ON: + error("task state %d after the failed switch, expected still ON" % s.task_state) + +# motion goes on working in the kinematics it kept, and the module is +# back in it too: the joints follow the identity inverse, not the hexapod's +c.mode(linuxcnc.MODE_MDI) +c.wait_complete(30) +mdi("G0 X30 Y30 Z30 A30 B30 C30") +if joints() != (30, 30, 30, 30, 30, 30): + error("joints after the move are %s, expected all 30" % (joints(),)) +said = drain() +if said: + error("unexpected messages after the failed switch: %s" % said) + +# a switch to the kinematics in force is nothing to do +mdi("G13.1") +if kins_type() != 0: + error("kinematics type %d after G13.1, expected identity (0)" % kins_type()) +if position() != (30, 30, 30, 30, 30, 30): + error("position after G13.1 is %s, expected all 30" % (position(),)) +said = drain() +if said: + error("unexpected messages after the switch back: %s" % said) + +print("Exiting with %d errors" % errors) +c.state(linuxcnc.STATE_ESTOP) +sys.exit(1 if errors else 0) diff --git a/tests/kins-switch-unsolved/test.ini b/tests/kins-switch-unsolved/test.ini new file mode 100644 index 00000000000..c93bc09563f --- /dev/null +++ b/tests/kins-switch-unsolved/test.ini @@ -0,0 +1,130 @@ +[EMC] +VERSION = 1.1 +DEBUG = 0 + +[DISPLAY] +DISPLAY = ./test-ui.py + +[RS274NGC] +RS274NGC_STARTUP_CODE = G17 G21 G40 G49 G54 G64 P0.001 G80 G90 G92.1 G94 G97 G98 +PARAMETER_FILE = sim.var + +[KINS] +# switchkins-type 0 is identity, 1 is the hexapod, 2 is the userk template +KINEMATICS = genhexkins sparm=identityfirst +JOINTS = 6 + +[HAL] +HALFILE = sim.hal + +[TRAJ] +COORDINATES = XYZABC +LINEAR_UNITS = mm +ANGULAR_UNITS = deg +DEFAULT_LINEAR_VELOCITY = 200 +MAX_LINEAR_VELOCITY = 346 +MAX_LINEAR_ACCELERATION = 800 +DEFAULT_LINEAR_ACCELERATION = 800 +MAX_ANGULAR_VELOCITY = 360 + +[EMCMOT] +EMCMOT = motmod +SERVO_PERIOD = 1000000 +COMM_TIMEOUT = 4 + +[TASK] +TASK = milltask +CYCLE_TIME = 0.010 + +[EMCIO] +TOOL_TABLE = tool.tbl + +[AXIS_X] +MIN_LIMIT = -500 +MAX_LIMIT = 500 +MAX_VELOCITY = 200 +MAX_ACCELERATION = 800 + +[AXIS_Y] +MIN_LIMIT = -500 +MAX_LIMIT = 500 +MAX_VELOCITY = 200 +MAX_ACCELERATION = 800 + +[AXIS_Z] +MIN_LIMIT = -500 +MAX_LIMIT = 500 +MAX_VELOCITY = 200 +MAX_ACCELERATION = 800 + +[AXIS_A] +MIN_LIMIT = -500 +MAX_LIMIT = 500 +MAX_VELOCITY = 60 +MAX_ACCELERATION = 200 + +[AXIS_B] +MIN_LIMIT = -500 +MAX_LIMIT = 500 +MAX_VELOCITY = 60 +MAX_ACCELERATION = 200 + +[AXIS_C] +MIN_LIMIT = -500 +MAX_LIMIT = 500 +MAX_VELOCITY = 60 +MAX_ACCELERATION = 200 + +[JOINT_0] +TYPE = LINEAR +MIN_LIMIT = -500 +MAX_LIMIT = 500 +MAX_VELOCITY = 200 +MAX_ACCELERATION = 800 +HOME_SEARCH_VEL = 0 +HOME_SEQUENCE = 0 + +[JOINT_1] +TYPE = LINEAR +MIN_LIMIT = -500 +MAX_LIMIT = 500 +MAX_VELOCITY = 200 +MAX_ACCELERATION = 800 +HOME_SEARCH_VEL = 0 +HOME_SEQUENCE = 0 + +[JOINT_2] +TYPE = LINEAR +MIN_LIMIT = -500 +MAX_LIMIT = 500 +MAX_VELOCITY = 200 +MAX_ACCELERATION = 800 +HOME_SEARCH_VEL = 0 +HOME_SEQUENCE = 0 + +[JOINT_3] +TYPE = ANGULAR +MIN_LIMIT = -500 +MAX_LIMIT = 500 +MAX_VELOCITY = 60 +MAX_ACCELERATION = 200 +HOME_SEARCH_VEL = 0 +HOME_SEQUENCE = 0 + +[JOINT_4] +TYPE = ANGULAR +MIN_LIMIT = -500 +MAX_LIMIT = 500 +MAX_VELOCITY = 60 +MAX_ACCELERATION = 200 +HOME_SEARCH_VEL = 0 +HOME_SEQUENCE = 0 + +[JOINT_5] +TYPE = ANGULAR +MIN_LIMIT = -500 +MAX_LIMIT = 500 +MAX_VELOCITY = 60 +MAX_ACCELERATION = 200 +HOME_SEARCH_VEL = 0 +HOME_SEQUENCE = 0 diff --git a/tests/kins-switch-unsolved/test.sh b/tests/kins-switch-unsolved/test.sh new file mode 100755 index 00000000000..765cf14fed6 --- /dev/null +++ b/tests/kins-switch-unsolved/test.sh @@ -0,0 +1,4 @@ +#!/bin/bash -e +# a failed run leaves the var file behind, and it carries offsets +rm -f sim.var sim.var.bak +linuxcnc -r test.ini diff --git a/tests/kins-switch-unsolved/tool.tbl b/tests/kins-switch-unsolved/tool.tbl new file mode 100644 index 00000000000..d793e2d60ed --- /dev/null +++ b/tests/kins-switch-unsolved/tool.tbl @@ -0,0 +1 @@ +T1 P1 D0.0 Z12.5 ; From 13837763955fae4cc14306dec412ca11859df5d8 Mon Sep 17 00:00:00 2001 From: david mueller Date: Sun, 23 Aug 2026 13:32:45 +1000 Subject: [PATCH 07/60] twp: split the machine maths out of the tilted work plane remap The remap carried the geometry of every supported machine as branches on the (primary, secondary) letter pair inside its four kinematics functions, so adding a machine meant adding a branch to each. The generic half is remap.py now and the machine half a remap_funcs_twp.py beside each config, imported by name: eleven functions the generic side asks a machine, which joint angles reach a tool orientation, how to build the transformation, the default tool x, what to write on the module pins. The split and the generic file are David Mueller's, from https://github.com/Sigma1912/LinuxCNC_Demo_Configs/tree/main/5axis-twp. Adapted: the ini is read through linuxcnc.ini, the kinematics switch is G12.1 rather than the deprecated motion.switchkins-type pin, the angles reach the module pins in degrees, pin names unchanged. Three fixes come with it: kins_calc_primary collected only the last candidate's primary angle, so P1 and P2 failed where a solution existed; candidate angles were compared in radians against limits in degrees; on xyzbca-trsrn G53.6, G68.3 and one G53.3 case reported success without activating the plane. Both configs verified over eight orientations under every code against the run before the change, tool vectors identical to 1e-9. --- .../python/remap.py | 1476 ++++++++--------- .../xyzacb-trsrn_twp/remap_funcs_twp.py | 347 ++++ .../xyzacb-trsrn_twp/xyzacb-trsrn.ini | 2 +- .../xyzbca-trsrn_twp/remap_funcs_twp.py | 350 ++++ .../xyzbca-trsrn_twp/xyzbca-trsrn.ini | 2 +- 5 files changed, 1386 insertions(+), 791 deletions(-) create mode 100644 configs/sim/axis/vismach/5axis/table-rotary_spindle-rotary-nutating/xyzacb-trsrn_twp/remap_funcs_twp.py create mode 100644 configs/sim/axis/vismach/5axis/table-rotary_spindle-rotary-nutating/xyzbca-trsrn_twp/remap_funcs_twp.py diff --git a/configs/sim/axis/vismach/5axis/table-rotary_spindle-rotary-nutating/python/remap.py b/configs/sim/axis/vismach/5axis/table-rotary_spindle-rotary-nutating/python/remap.py index f4f9506a846..05fc53261a1 100755 --- a/configs/sim/axis/vismach/5axis/table-rotary_spindle-rotary-nutating/python/remap.py +++ b/configs/sim/axis/vismach/5axis/table-rotary_spindle-rotary-nutating/python/remap.py @@ -1,7 +1,7 @@ # This is a python remap for LinuxCNC implementing 'Tilted Work Plane' # G68.2, G68.3, G68.4 and related Gcodes G53.1, G53.3, G53.6, G69 # -# Copyright ()c) 2023 David Mueller +# Copyright ()c) 2025 David Mueller # # This program is free software: you can redistribute it and/or modify # it under the terms of the GNU General Public License as published by @@ -13,7 +13,22 @@ # MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the # GNU General Public License for more details. # -# +''' +The remap does the following: + +- Parses the G68.[2,4] gcodes and constructs the requested tool orientation vectors (x,z). +- Writes and reads hal pins created and updated by'twp-helper-comp.py' (mostly for updating the gui). +- Parses the G53.[1,3,6] and uses the functions in 'remap_funcs_twp.py' to calculate all rotary joint position that result in the correct tool orientation (there may be more than just one). +- Selects the appropriate rotary angles that will respect rotary limits set in the ini file and also follow any orientation strategy requested by the operator using the 'P' word. +- Sets the kinematic modes +- Calculates new work offset values so the WCS origin after switching to TWP mode is in the requested physical position. +- Used MDI commands to: + - Move the rotary joints to the calculated positions + - Switch the WCS system to 'G59' and set the values of G59, G59.[1,2.3] to the calculated coordinates +- Parses the G69 gcodes, resets the relevant parameters and switches back to Identity kinematic mode +''' + + import sys import traceback import numpy as np @@ -23,7 +38,6 @@ from util import lineno, call_pydevd import hal - # logging import logging # this name will be printed first on each log message @@ -33,22 +47,41 @@ formatter = logging.Formatter('%(name)s %(levelname)s: %(message)s') handler.setFormatter(formatter) log.addHandler(handler) -# Manually force the log level for this module -log.setLevel(logging.ERROR) # One of DEBUG, INFO, WARNING, ERROR, CRITICAL - # set up parsing of the inifile import os import linuxcnc # get the path for the ini file used to start this config inifile = os.environ.get("INI_FILE_NAME") + +# adding the remap_funcs folder to the system path. The machine specific +# functions live beside the ini file, which is the working directory, and the +# parent is searched too so a config may keep them one level up and share them +# between variants. +cwd = os.getcwd() +parent = os.path.abspath(os.path.join(cwd, os.pardir)) +sys.path.insert(0, parent) +sys.path.insert(0, cwd) +from remap_funcs_twp import * + # instantiate the LinuxCNC ini-parser config = linuxcnc.ini(inifile) -## SPINDLE ROTARY JOINT LETTERS -# spindle primary joint +# debug setting +try: + debug_setting = config.getint('TWP', 'LOG_LEVEL', fallback=1) + if debug_setting > 4: debug_setting = 4 + if debug_setting < 0: debug_setting = 0 +except Exception as error: + debug_setting = 1 + log.warning("Unable to parse debug setting given in INI. Setting it to 1.") +debug_levels = (logging.CRITICAL, logging.ERROR, logging.WARNING, logging.INFO, logging.DEBUG) +log.setLevel(debug_levels[debug_setting]) + +## ROTARY JOINT LETTERS +# primary rotary joint (independent of the secondary joint) joint_letter_primary = config.getstring('TWP', 'PRIMARY', fallback="").capitalize() -# spindle secondary joint (ie the one closer to the tool) +# secondary rotary joint (dependent on the primary joint) joint_letter_secondary = config.getstring('TWP', 'SECONDARY', fallback="").capitalize() if not joint_letter_primary in ('A','B','C') or not joint_letter_secondary in ('A','B','C'): @@ -58,32 +91,28 @@ else: # get the MIN/MAX limits of the respective rotary joint letters category = 'AXIS_' + joint_letter_primary - primary_min_limit = config.getreal(category, 'MIN_LIMIT', fallback=0.0) - primary_max_limit = config.getreal(category, 'MAX_LIMIT', fallback=0.0) - log.info('Joint letter for primary is %s with MIN/MAX limits: %s,%s', joint_letter_primary, primary_min_limit, primary_max_limit) + primary_min_limit = radians(config.getreal(category, 'MIN_LIMIT', fallback=0.0)) + primary_max_limit = radians(config.getreal(category, 'MAX_LIMIT', fallback=0.0)) + log.info('Joint letter for primary is %s with MIN/MAX limits: %s,%s', + joint_letter_primary, degrees(primary_min_limit), degrees(primary_max_limit)) category = 'AXIS_' + joint_letter_secondary - secondary_min_limit = config.getreal(category, 'MIN_LIMIT', fallback=0.0) - secondary_max_limit = config.getreal(category, 'MAX_LIMIT', fallback=0.0) - log.info('Joint letter for secondary is %s with MIN/MAX Limits: %s,%s', joint_letter_secondary, secondary_min_limit, secondary_max_limit) - - -## CONNECTIONS TO THE KINEMATIC COMPONENT -# get the name of the kinematic component -kins_comp = config.getstring('KINS', 'KINEMATICS', fallback="") -# name of the hal pin that represents the nutation-angle -kins_nutation_angle = kins_comp + '_kins.nut-angle' -# name of the hal pin that represents the pre-rotation -kins_pre_rotation = kins_comp + '_kins.pre-rot' -# name of the hal pin that represents the primary joint orientation angle -kins_primary_rotation = kins_comp + '_kins.primary-angle' -# name of the hal pin that represents the secondary joint orientation angle -kins_secondary_rotation = kins_comp + '_kins.secondary-angle' + secondary_min_limit = radians(config.getreal(category, 'MIN_LIMIT', fallback=0.0)) + secondary_max_limit = radians(config.getreal(category, 'MAX_LIMIT', fallback=0.0)) + log.info('Joint letter for secondary is %s with MIN/MAX Limits: %s,%s', + joint_letter_secondary, degrees(secondary_min_limit), degrees(secondary_max_limit)) + ## CONNECTIONS TO THE HELPER COMPONENT twp_comp = 'twp-helper-comp.' twp_is_defined = twp_comp + 'twp-is-defined' twp_is_active = twp_comp + 'twp-is-active' +# Which rotary joint should be prioritized when calculating optimal joint rotation angles +try: + optimization_priority = config.getint('TWP', 'PRIORITY', fallback=1) +except Exception as error: + log.warning("Unable to parse orientation priority given in INI. Setting it to 1.") + optimization_priority = 1 # raise InterpreterException if execute() or read() fail throw_exceptions = 1 @@ -93,7 +122,7 @@ twp_matrix = np.asmatrix(np.identity(4)) # some g68.2 p-word modes require several calls to enter all the required parameters so we -# need a flag that indicates when the twp has been defined and is ready for g53.x +# need a flag that indicates when the twp has been defined and is ready for G53.n # [current p-word, number of calls required, (state of calls required for that p mode added by g68.2)] # note that we use string since boolean True == 1, which gives wrong results if we want # to count the elements that are True because it is counted as integer '1' @@ -105,592 +134,391 @@ current_work_offset_number = 1 saved_work_offset = [0,0,0] # orientation mode refers to the strategy used to choose from the different rotary angles for a given -# tool-z vector. The optimization is applied to the primary axis only with mode 0 (shortest path) being +# z-vector vector. The optimization is applied to the primary axis only with mode 0 (shortest path) being # the default. (0=shortest_path , 1=positive_rotation only, 2=negative_rotation only, ) orient_mode = 0 -# defines the kinematic model for (world <-> tool) coordinates of the machine at hand -# returns 4x4 transformation matrix for given angles and 4x4 input matrix -# NOTE: these matrices must be the same as the ones used to derive the kinematic model -def kins_tool_transformation(theta_1, theta_2, pre_rot, matrix_in, direction='fwd'): - global joint_letter_primary, joint_letter_secondary - global kins_nutation_angle - T_in = matrix_in - - ## Define 4x4 transformation for virtual rotation around tool-z to orient tool-x and -y - Stc = sin(pre_rot) - Ctc = cos(pre_rot) - Rtc=np.matrix([[ Ctc, -Stc, 0, 0], - [ Stc, Ctc, 0, 0], - [ 0 , 0 , 1, 0], - [ 0, 0 , 0, 1]]) - - ## Define 4x4 transformation for the primary joint - # get the basic 3x3 rotation matrix (returns array) - if joint_letter_primary == 'A': - Rp = Rx(theta_1) - elif joint_letter_primary == 'B': - Rp = Ry(theta_1) - elif joint_letter_primary == 'C': - Rp = Rz(theta_1) - # add fourth column on the right - Rp = np.hstack((Rp, [[0],[0],[0]])) - # expand to 4x4 array and make into a matrix - row_4 = [0,0,0,1] - Rp = np.vstack((Rp, row_4)) - Rp = np.asmatrix(Rp) - - ## Define 4x4 transformation matrix for the secondary joint - # get the basic 3x3 rotation matrix (returns array) - if joint_letter_secondary == 'A': - Rs = Rx(theta_2) - elif joint_letter_secondary == 'B': - Rs = Ry(theta_2) - elif joint_letter_secondary == 'C': - Rs = Rz(theta_2) - # add fourth column on the right - Rs = np.hstack((Rs, [[0],[0],[0]])) - # expand to 4x4 array and make into a matrix - row_4 = [0,0,0,1] - Rs = np.vstack((Rs, row_4)) - Rs = np.asmatrix(Rs) - - if (joint_letter_primary, joint_letter_secondary)== ('C', 'B'): - # Additional definitions for nutating joint - v = radians(hal.get_value(kins_nutation_angle)) - Sv = sin(v) - Cv = cos(v) - Ss = sin(theta_2) - Cs = cos(theta_2) - r = Cs + Sv*Sv*(1-Cs) - s = Cs + Cv*Cv*(1-Cs) - t = Sv*Cv*(1-Cs) - # define rotation matrix for the secondary spindle joint - Rs=np.matrix([[ Cs, -Cv*Ss, Sv*Ss, 0], - [ Cv*Ss, r, t, 0], - [ -Sv*Ss, t, s, 0], - [ 0, 0, 0, 1]]) - - elif (joint_letter_primary, joint_letter_secondary)== ('C', 'A'): - # Additional definitions for nutating joint - v = radians(hal.get_value(kins_nutation_angle)) - Sv = sin(v) - Cv = cos(v) - Ss = sin(theta_2) - Cs = cos(theta_2) - r = Cs + Sv*Sv*(1-Cs) - s = Cs + Cv*Cv*(1-Cs) - t = Sv*Cv*(1-Cs) - # define rotation matrix for the secondary spindle joint - Rs=np.matrix([[ r, -Cv*Ss, t, 0], - [ Cv*Ss, Cs, -Sv*Ss, 0], - [ t, Sv*Ss, s, 0], - [ 0, 0, 0, 1]]) - else: - log.error('No formula for this spindle kinematic (primary, secondary) %s, %s', joint_letter_primary, joint_letter_secondary) - - # calculate the transformation matrix for the forward tool kinematic - matrix_tool_fwd = np.transpose(Rtc)*np.transpose(Rs)*np.transpose(Rp)*T_in - # calculate the transformation matrix for the inverse tool kinematic - matrix_tool_inv = Rp*Rs*Rtc*T_in - if direction == 'fwd': - #log.debug("matrix tool fwd: \n", matrix_tool_fwd) - #log.debug("inv would have been: \n", matrix_tool_inv) - return matrix_tool_fwd - elif direction == 'inv': - #log.debug("matrix tool inv: \n", matrix_tool_inv) - #log.debug("fwd would have been: \n", matrix_tool_fwd) - return matrix_tool_inv - else: - return 0 +# define the basic rotation matrices +def Rx(th): + return np.array([[1, 0 , 0 ], + [0, cos(th), -sin(th)], + [0, sin(th), cos(th)]]) +def Ry(th): + return np.array([[ cos(th), 0, sin(th)], + [ 0 , 1, 0 ], + [-sin(th), 0, cos(th)]]) -# returns angle 'tc' required to rotate the x-axis of the tool-coords parallelto the machine-xy plane -# for given machine joint position angles. -# For G68.3 this is the default tool-x direction -# NOTE: this uses formulas derived from the transformation matrix in the inverse tool kinematic -def kins_calc_tool_rot_c_for_horizontal_x(self, theta_1, theta_2): - global joint_letter_primary, joint_letter_secondary - # The idea is that the tool-x vector is parallel to the machine xy-plane when the - # z component of the x-direction vector is equal to zero - # Mathematically we take the symbolic formula found in row 3, column 1 of the transformation - # matrix from the inverse tool-kinematics, equal that to zero and solve for 'tc'. - # this makes the x orientation of the tool coords horizontal and the user can set the - # rotation from there using g68.3 r - global kins_nutation_angle - v = radians(hal.get_value(kins_nutation_angle)) - Cv = cos(v) - Sv = sin(v) - Cs = cos(theta_2) - Ss = sin(theta_2) - Cp = cos(theta_1) - Sp = sin(theta_1) - if (joint_letter_primary, joint_letter_secondary)== ('C', 'B'): - t = Sv*Cv*(1-Cs) - tc = atan2((Sv*Ss),t) - elif (joint_letter_primary, joint_letter_secondary)== ('C', 'A'): - t = Sv*Cv*(1-Cs) - tc = atan2(-t,(Sv*Ss)) - else: - log.error('No formula for this spindle kinematic (primary, secondary) %s, %s', joint_letter_primary, joint_letter_secondary) - # note: tool-c rotation is done using a halpin that feeds into the kinematic component and the - # vismach model. In contrast to a gcode command where 'c' refers to a physical machine joint) - return tc +def Rz(th): + return np.array([[cos(th), -sin(th), 0], + [sin(th), cos(th), 0], + [0 , 0 , 1]]) -# calculates the secondary joint position for a given tool-vector -# secondary being the joint closest to the tool -# Note: this uses functions derived from the custom kinematic -def kins_calc_secondary(self, tool_z_req): - global joint_letter_primary, joint_letter_secondary - global secondary_min_limit, secondary_max_limit - global kins_nutation_angle - epsilon = 0.000001 - theta_2_list=[] - (Kzx, Kzy, Kzz) = (tool_z_req[0], tool_z_req[1], tool_z_req[2]) - - if (joint_letter_primary, joint_letter_secondary)== ('C', 'B'): - # This kinmatic has infinite results for the vertical tool orientation - # so we explicitly define the angles for that specific case - if Kzz > 1 - epsilon: - return [0] - else: - v = radians(hal.get_value(kins_nutation_angle)) - Sv = sin(v) - Cv = cos(v) - theta_2 = acos((Kzz - Cv*Cv)/(1 - Cv*Cv)) - elif (joint_letter_primary, joint_letter_secondary)== ('C', 'A'): - # This kinmatic has infinite results for the vertical tool orientation - # so we explicitly define the angles for that specific case - if Kzz > 1 - epsilon: - return [0] - else: - v = radians(hal.get_value(kins_nutation_angle)) - Sv = sin(v) - Cv = cos(v) - theta_2 = acos((Kzz - Cv*Cv)/(1 - Cv*Cv)) - else: - log.error('No formula for this spindle kinematic (primary, secondary) %s', (joint_letter_primary, joint_letter_secondary)) - # since we are using acos() we really have two solutions theta_2 and -theta_2 - for theta in [theta_2, -theta_2]: - log.debug('Checking if result %s is within secondary joint limits of %s and %s.', - degrees(theta), secondary_min_limit, secondary_max_limit) - if theta > secondary_min_limit and theta < secondary_max_limit: - log.debug('Adding %s to valid angles list.', degrees(theta)) - theta_2_list.append(theta) - log.debug('List of possible secondary angles: %s\n', theta_2_list) - return theta_2_list - - -# calculates the primary joint position for a given tool-vector -# Note: this uses functions derived from the custom kinematic -def kins_calc_primary(self, tool_z_req, theta_2_list): - global joint_letter_primary, joint_letter_secondary - global primary_min_limit, primary_max_limit - global kins_nutation_angle - epsilon = 0.000001 - theta_1_list=[] - (Kzx, Kzy, Kzz) = (tool_z_req[0], tool_z_req[1], tool_z_req[2]) - if (joint_letter_primary, joint_letter_secondary)== ('C', 'B'): - # This kinmatic has infinite results for the vertical tool orientation - # so we explicitly define the angles for that specific case - if Kzz > 1 - epsilon: - return [0] - else: - v = radians(hal.get_value(kins_nutation_angle)) - Sv = sin(v) - Cv = cos(v) - for i in range(len(theta_2_list)): - theta_2 = theta_2_list[i] - Ss = sin(theta_2) - Cs = cos(theta_2) - t = Sv*Cv*(1-Cs) - p = Sv * Ss - - theta_1 = asin((p*Kzy - t*Kzx)/(t*t + p*p)) - elif (joint_letter_primary, joint_letter_secondary)== ('C', 'A'): - # This kinmatic has infinite results for the vertical tool orientation - # so we explicitly define the angles for that specific case - if Kzz > 1 - epsilon: - return [0] - else: - v = radians(hal.get_value(kins_nutation_angle)) - Sv = sin(v) - Cv = cos(v) - for i in range(len(theta_2_list)): - theta_2 = theta_2_list[i] - Ss = sin(theta_2) - Cs = cos(theta_2) - t = Sv*Cv*(1-Cs) - p = Sv * Ss - q = (t*Kzy - p*Kzx)/(t*t + p*p) - theta_1 = asin(q) - else: - log.error('No formula for this spindle kinematic (primary, secondary) %s', (joint_letter_primary, joint_letter_secondary)) - # since we are using asin() we really have two solutions theta_1 and pi-theta_2 - for theta in [theta_1, transform_to_pipi(pi - theta_1)]: - log.debug('Checking if result %s is within secondary joint limits of %s and %s.', - degrees(theta), secondary_min_limit, secondary_max_limit) - if theta > secondary_min_limit and theta < secondary_max_limit: - log.debug('Adding %s to valid angles list.', degrees(theta)) - theta_1_list.append(theta) - log.debug('List of possible secondary angles: %s\n', theta_2_list) - return theta_1_list - - -# this is from 'mika-s.github.io' -# transforms a given angle to the interval of [-pi,pi] -def transform_to_pipi(input_angle): - revolutions = int((input_angle + np.sign(input_angle) * pi) / (2 * pi)) - p1 = truncated_remainder(input_angle + np.sign(input_angle) * pi, 2 * pi) - p2 = (np.sign(np.sign(input_angle) - + 2 * (np.sign(fabs((truncated_remainder(input_angle + pi, 2 * pi)) / (2 * pi))) - 1))) * pi - output_angle = p1 - p2 - return output_angle - - -# this is from 'mika-s.github.io' -# used by 'transform_to_pipi()' -def truncated_remainder(dividend, divisor): - divided_number = dividend / divisor - divided_number = -int(-divided_number) if divided_number < 0 else int(divided_number) - remainder = dividend - divisor * divided_number - return remainder - - -# returns a list of valid primary/secondary spindle joint positions for a given tool-orientation vector -# or 'None','None' if no valid position could be found -def kins_calc_jnt_angles(self, tool_z_req): - log.debug('tool_z_requested: %s', tool_z_req) + +def calc_euler_rot_matrix(th1, th2, th3, order): # expects radians + # returns the rotation matrices for given order and angles + log.debug('Entering: %s', sys._getframe( ).f_code.co_name) + debug_msg = (f' Euler order {order} requested with angles: ' + f'{degrees(th1):.4f}, {degrees(th2):.4f}, {degrees(th3):.4f}') + log.debug(debug_msg) + if order == '131': + matrix = np.dot(np.dot(Rx(th1), Rz(th2)), Rx(th3)) + elif order=='121': + matrix = np.dot(np.dot(Rx(th1), Ry(th2)), Rx(th3)) + elif order=='212': + matrix = np.dot(np.dot(Ry(th1), Rx(th2)), Ry(th3)) + elif order=='232': + matrix = np.dot(np.dot(Ry(th1), Rz(th2)), Ry(th3)) + elif order=='323': + matrix = np.dot(np.dot(Rz(th1), Ry(th2)), Rz(th3)) + elif order=='313': + matrix = np.dot(np.dot(Rz(th1), Rx(th2)), Rz(th3)) + elif order=='123': + matrix = np.dot(np.dot(Rx(th1), Ry(th2)), Rz(th3)) + elif order=='132': + matrix = np.dot(np.dot(Rx(th1), Rz(th2)), Ry(th3)) + elif order=='213': + matrix = np.dot(np.dot(Ry(th1), Rx(th2)), Rz(th3)) + elif order=='231': + matrix = np.dot(np.dot(Ry(th1), Rz(th2)), Rx(th3)) + elif order=='321': + matrix = np.dot(np.dot(Rz(th1), Ry(th2)), Rx(th3)) + elif order=='312': + matrix = np.dot(np.dot(Rz(th1), Rx(th2)), Ry(th3)) + #log.debug(' Returning euler rotation as matrix: \n %s', matrix) + return matrix + + +def calc_joint_angles(z_vector_req, x_vector_req): + # returns a list of valid primary/secondary rotary joint positions in radians for a given orientation vector + # returns an empty list if no valid position could be found + log.debug('Entering: %s', sys._getframe( ).f_code.co_name) + log.debug(' z_vector_requested: %s', z_vector_req) + log.debug(' x_vector_requested: %s', x_vector_req) # set the tolerance value epsilon = 0.0001 # create np.array so we can easily calculate differences and check elements - tool_z_req = np.array([tool_z_req[0], tool_z_req[1], tool_z_req[2]]) - # calculate secondary joint values using kinematic specific formula - theta_2_pair = kins_calc_secondary(self, tool_z_req) - # calculate primary joint values using kinematic specific formula - theta_1_pair = kins_calc_primary(self, tool_z_req, theta_2_pair) + z_vector_req = np.array([z_vector_req[0], z_vector_req[1], z_vector_req[2]]) + # calculate joint values using kinematic specific formula + try: + (theta_1_calcd, theta_2_calcd) = kins_calc_possible_joint_angles(log, z_vector_req, x_vector_req) + except Exception as error: + log.error('Remap_funcs: kins_calc_possible_joint_angles failure, %s', error) + + # remove any duplicate values from the results + theta_1_calcd = tuple(set(theta_1_calcd)) + theta_2_calcd = tuple(set(theta_2_calcd)) + log.debug(' Got possible angles theta_1: ' + ' '.join("{:.4f}°".format(degrees(theta)) for theta in theta_1_calcd)) + log.debug(' Got possible angles theta_2: ' + ' '.join("{:.4f}°".format(degrees(theta)) for theta in theta_2_calcd)) + if theta_1_calcd == None or theta_2_calcd == None: + return [] + angle_pairs_list = [] + # create a list of paired combinations of returned angles (theta_1 , theta_2) + for i in range(len(theta_1_calcd)): + for j in range(len(theta_2_calcd)): + angle_pairs_list.append((theta_1_calcd[i], theta_2_calcd[j])) + angle_pairs_list = list(set(angle_pairs_list)) + # iterate through the list and check if a particular pair actually produces the requested z-vector orientation joint_angles_list = [] - # iterate through all the possible combinations of (theta_1 , theta_2) - for i in range(len(theta_1_pair)): - for j in range(len(theta_2_pair)): - # rotate an identity matrix using the custom tool kinematic model and the (theta_1, theta_2) - matrix_in = np.asmatrix(np.identity(4)) - t_out = kins_tool_transformation(theta_1_pair[i], theta_2_pair[j], 0, matrix_in,'inv') - # the resulting tool-z vector for this pair of (theta_1, theta_2) is found in the third column - tool_z_would_be = np.array([t_out[0,2], t_out[1,2], t_out[2,2]]) - log.debug('tool_z_would_be: %s', tool_z_would_be) - # calculate the difference of the respective elements - tool_z_diff = tool_z_req - tool_z_would_be - # and check if all elements are within [-epsilon,epsilon] - match = np.all((tool_z_diff > -epsilon) & (tool_z_diff < epsilon)) - log.debug('Is the tool-Z-vector close enough ? %s', match) - if match: - # check if we already have this particular pair in the list - if not (theta_1_pair[i], theta_2_pair[j]) in joint_angles_list: - log.debug('Appending (theta_1_pair, theta_2_pair) %s', (degrees(theta_1_pair[i]), degrees(theta_2_pair[j]))) - joint_angles_list.append((theta_1_pair[i], theta_2_pair[j])) - log.info('Found valid joint angles: %s', joint_angles_list) - if joint_angles_list: - return joint_angles_list - #return joint_angles_list[-1] - else: - return None, None + for i in range(len(angle_pairs_list)): + debug_msg = (f' Checking angle pair {i}: ({angle_pairs_list[i][0]:.4f}, {angle_pairs_list[i][1]:.4f}) ' + f'({degrees(angle_pairs_list[i][0]):.4f}°, {degrees(angle_pairs_list[i][1]):.4f}°)') + log.debug(debug_msg) + # we start with an identity matrix (ie oriented to world) + matrix_in = np.asmatrix(np.identity(4)) + try: + direction = kins_calc_transformation_get_direction() + except Exception as error: + log.error('kins_calc_transformation_get_direction, %s', error) + try: + matrix_out = kins_calc_transformation_matrix(angle_pairs_list[i][0], angle_pairs_list[i][1], 0, matrix_in, direction) + except Exception as error: + log.error('kins_calc_transformation_matrix, %s', error) + # the resulting z-vector for this pair of (theta_1, theta_2) is found in the third column + z_vector_would_be = np.array([matrix_out[0,2], matrix_out[1,2], matrix_out[2,2]]) + # calculate the difference of the respective elements + z_vector_diff = z_vector_req - z_vector_would_be + log.debug(' z_vector_diff: %s', z_vector_diff) + # and check if all elements are within [-epsilon,epsilon] + match_z = np.all((z_vector_diff > -epsilon) & (z_vector_diff < epsilon)) + log.debug(' Is the z-vector-vector close enough ? %s', match_z) + if match_z: + joint_angles_list.append((angle_pairs_list[i][0], angle_pairs_list[i][1])) + for (theta_1, theta_2) in joint_angles_list: + log.debug(f'Returning valid joint angles found: {degrees(theta_1):.4f}°, {degrees(theta_2):.4f}°') + return joint_angles_list # returns radians + def calc_shortest_distance(pos, trgt, mode): - # calculate the shortest distance in [-180°, 180°] - # eg if pos=170° and trgt=-170° then dist will be 20° - # If the operator requests positive or negative rotation - # we may need to return the long distance instead - log.debug('Got (pos, trgt): %s', (pos, trgt)) + pos = degrees(pos) + trgt = degrees(trgt) + # calculate the shortest distance in [-180°, 180°] eg if pos=170° and trgt=-170° then dist will be 20° + # If the operator requests positive or negative rotation we may need to return the long distance instead + log.debug('Entering: %s', sys._getframe( ).f_code.co_name) dist_short = (trgt - pos + 180) % 360 - 180 # calculate short and long distance if dist_short >= 0: # ie dist_long should be negative dist_long = -(360 - dist_short) else: dist_long = 360 + dist_short - log.debug('Calculated (dist_short, dist_long): %s', (dist_short, dist_long)) + log.debug(f' Calculated dist_short: {dist_short:.4f}°, dist_long: {dist_long:.4f}°') if mode == 1: # positive rotation only, ie we want a positive distance if dist_short >= 0: # ie we want this one dist = dist_short else: # ie we need to go the other way dist = dist_long - if mode == 2: # negative rotation only ie we want a positive distance - if dist_short >= 0: # ie we need to go the other way + elif mode == 2: # negative rotation only ie we want a positive distance + if dist_short > 0: # ie we need to go the other way dist = dist_long else: # ie we want this one dist = dist_short else: # mode = 0 ie we want the shortest distance either way dist = dist_short - log.debug('Distance returned: %s', dist) - return dist + log.debug(f'Returning distance: {dist:.4f}°') + return radians(dist) -# this takes a target angle in [-pi,pi] and finds the closest move within [min_limit, max_limit] -# from a given position in [min_limit, max_limit], returns the optimized target angle and the distance -# from the given position to that target angle -def calc_rotary_move_with_joint_limits(position, target, max_limit, min_limit, mode): - pos = degrees(position) - trgt = degrees(target) - log.debug('(Current_pos, target): %s', (pos, trgt)) +def calc_rotary_move_with_joint_limits(pos, trgt, max_limit, min_limit, mode): # expects radians + # this takes a target angle in [-pi,pi] and finds the closest move within [min_limit, max_limit] + # from a given position in [min_limit, max_limit], returns the optimized target angle and the distance + # from the given position to that target angle + log.debug('Entering: %s', sys._getframe( ).f_code.co_name) + log.debug(f' Current position: {degrees(pos):.4f}°, target position: {degrees(trgt):.4f}°') # calculate the shortest distance from position to target for the strategy given by # the operator (ie shortest (= default), positive rotation only, negative rotation only ) dist = calc_shortest_distance(pos, trgt, mode) # check that the result is within the rotary axis limits defined in the ini file if dist >= 0: # shortest way is in the positive direction if (pos + dist) <= max_limit: # if the limits allow we rotate the joint in the positive sense - log.debug('Max_limit OK, target changed to: %s', (pos + dist)) + log.debug(f' Max_limit OK, setting target to: {degrees(pos + dist):.4f}°') theta = pos + dist - else: # if positive limits would be exceeded we need to go the longer wey in the other direction + else: # if positive limits would be exceeded we need to go the longer way in the other direction + log.debug(f' Maximum axis limit of {degrees(max_limit):.4f} would be violated.') if mode == 0: - log.debug('Max_limit reached, target remains: %s', trgt) + dist = dist - 2*pi + log.debug(f' Changing target to: {degrees(trgt):.4f}°, distance to: {degrees(dist):.4f}°') theta = trgt else: # if the rotation direction was set by the operator then we can not change direction + log.debug(f' Unable to change direction because orient mode is set to {mode:.0f}.\n') theta = None - dist = None else: # shortest way is in the negative direction if (pos + dist) >= min_limit: # if the limits allow we rotate the joint in the negative sense - log.debug('Min_limit OK, target changed to: %s', (pos + dist)) + log.debug(f' Min_limit OK, setting target to: {degrees(pos + dist):.4f}°') theta = pos + dist else: # if negative limits would be exceeded we need to go the longer way int the other direction + log.debug(f' Minimum axis limit of {degrees(min_limit):.4f} would be violated.') if mode == 0: - log.debug('Min_limit reached, target remains: %s', trgt) + dist = dist + 2*pi + log.debug(f' Changing target to: {degrees(trgt):.4f}°, distance to: {degrees(dist):.4f}°') theta = trgt else: # if the rotation direction was set by the operator then we can not change direction + log.debug(f' Unable to change direction because orient mode is set to {mode:.0f}.\n') theta = None - dist = None + if theta is not None: + log.debug(f'Returning: angle {degrees(theta):.4f}° with distance {degrees(dist):.4f}° for requested mode {mode:.0f}\n') # we also attach the distance for this particular move and mode - log.debug('Angle and distance returned: %s, %s', theta, dist) - return theta, dist + return theta, dist # returns radians -# this takes a list of joint angle pairs in [-pi,pi] and optimizes them for shortest moves -# in (min_limit, max_linit) from the current joint positions using the orient_mode set by -# the operator: 0=shortest (default), 1=positive rotation only, 2=negative rotation only -def calc_angle_pairs_and_distances(self, possible_prim_sec_angle_pairs): +def calc_angle_pairs_and_distances(self, possible_prim_sec_angle_pairs): # expects radians + # this takes a list of joint angle pairs in [-pi,pi] and optimizes them for shortest moves + # in (min_limit, max_linit) from the current joint positions using the orient_mode set by + # the operator: 0=shortest (default), 1=positive rotation only, 2=negative rotation only + log.debug('Entering: %s', sys._getframe( ).f_code.co_name) global primary_min_limit, primary_max_limit, secondary_min_limit, secondary_max_limit global orient_mode # get the current joint positions - prim_pos, sec_pos = get_current_rotary_positions(self) + prim_pos, sec_pos = get_current_rotary_positions(self) # returns radians # we want to return a list of angles that are optimized for the orient_mode and the # rotary axes limits as set in the ini file target_dist_list= [] for prim_trgt, sec_trgt in possible_prim_sec_angle_pairs: - # primary joint, here we apply the orient mode requested by the operator + # For the priortized joint we apply the orient mode requested by the operator + # the other we optimize for shortest move + if optimization_priority == 2: + primary_strategy = 0 + secondary_strategy = orient_mode + else: + primary_strategy = orient_mode + secondary_strategy = 0 + # primary joint prim_move, prim_dist = calc_rotary_move_with_joint_limits(prim_pos, prim_trgt, primary_max_limit, primary_min_limit, - orient_mode) - # secondary joint, here we want the shortest move (although we could also apply a strategy here) + primary_strategy) + # secondary joint sec_move, sec_dist = calc_rotary_move_with_joint_limits(sec_pos, sec_trgt, secondary_max_limit, secondary_min_limit, - 0) + secondary_strategy) # if a solution has been found for this particular pair then we add it to the list if not (prim_move == None) and not (sec_move == None): target_dist_list.append(((prim_move, sec_move),(prim_dist, sec_dist))) - log.debug('Assembled target_dist_list: %s',target_dist_list) - return target_dist_list + for ((prim_move, sec_move),(prim_dist, sec_dist)) in target_dist_list: + debug_msg = (f'Returning prim_move: {degrees(prim_move):.4f}°, sec_move: {degrees(sec_move):.4f}°, ' + f'prim_dist: {degrees(prim_dist):.4f}°, sec_dist: {degrees(sec_dist):.4f}°') + log.debug(debug_msg) + return target_dist_list # returns radians -# find the optimal joint move from current to target positions in the list -# for this we look at the primary joint move only -# orient_mode is 0=shortest, 1=positive rotation only, 2=negative rotation only -# For orient_mode=(1,2): If no move can be found within joint limits we return None def calc_optimal_joint_move(self, possible_prim_sec_angle_pairs): + # find the optimal joint move from current to target positions in the list + # orient_mode is 0=shortest, 1=positive rotation only, 2=negative rotation only + # For orient_mode=(1,2): If no move can be found within joint limits we return None + log.debug('Entering: %s', sys._getframe( ).f_code.co_name) global orient_mode # this returns a list with all moves ((prim_move, sec_move),(prim_dist, sec_dist)) that # will result in correct tool orientation, stay within the rotary axis limits and respect the # orient_mode if set by the operator valid_joint_moves_and_distances = calc_angle_pairs_and_distances(self, possible_prim_sec_angle_pairs) + if len(valid_joint_moves_and_distances) < 1: + log.error(f' No valid joint moves found.') + return (None, None) # now we need to pick and return the (primary angle, secondary angle) that results in the - # shortest move of the primary joint + # shortest move of the prioritized joint (theta_1, theta_2) = (None, None) - dist = 3600 + joint = optimization_priority - 1 + dist = 10 # some large initial value for trgt_angles, dists in valid_joint_moves_and_distances: - if orient_mode == 0 and fabs(dists[0]) < fabs(dist): # shortest move requested + if orient_mode == 0 and fabs(dists[joint]) < fabs(dist): # shortest move requested (theta_1, theta_2) = trgt_angles dist = dists[0] - elif orient_mode == 1 and fabs(dists[0]) < fabs(dist) and dists[0] >= 0: # positive primary rotation only + elif orient_mode == 1 and fabs(dists[joint]) < fabs(dist) and dists[joint] >= 0: # positive primary rotation only (theta_1, theta_2) = trgt_angles dist = dists[0] - elif orient_mode == 2 and fabs(dists[0]) < fabs(dist) and dists[0] <= 0: # negative primary rotation only + elif orient_mode == 2 and fabs(dists[joint]) < fabs(dist) and dists[joint] <= 0: # negative primary rotation only (theta_1, theta_2) = trgt_angles dist = dists[0] - log.debug('Shortest move selected for (orient_mode, theta_1, theta_2): %s', (orient_mode, theta_1, theta_2)) - return theta_1, theta_2 - - -# calculates the required pre-rotation around tool-z so the tool-x matches the requested -# orientation after rotation of the spindle joints -def kins_calc_pre_rot(self, theta_1, theta_2, tool_x_req, tool_z_requested): - # tolerance setting for check if tool-x-vector needs to be rotated at all + if theta_1 is not None: + debug_msg = (f'Returning shortest move selected for orient_mode {orient_mode:.0f}: ' + f'primary: {degrees(theta_1):.4f}°, secondary: {degrees(theta_2):.4f}°\n') + log.debug(debug_msg) + return theta_1, theta_2 # returns radians + + +def calc_virtual_rotation(theta_1, theta_2, x_vector_req, z_vector_req, matrix_in, direction): # expects radians + # calculates a required virtual-rotation around tool- or work-z so the x-vector matches the requested + # orientation after rotation + log.debug('Entering: %s', sys._getframe( ).f_code.co_name) + # tolerance setting for check if x-vector-vector needs to be rotated at all epsilon = 0.00000001 - log.info("Tool-x-requested: %s", tool_x_req) - # we need to calculate the current tool-x vector with the given rotations using - # the transformation matrix from our custom tool kinematic - log.debug("joint angles (secondary, primary) in radians given: %s", (theta_2, theta_1)) - log.debug("joint angles (secondary, primary) in degrees given: %s", (theta_2*180/pi, theta_1*180/pi)) - # run the identity matrix through the tool kinematic transformation in the requested direction - # using the given joint angles and pre-rotation zero - matrix_in = np.asmatrix(np.identity(4)) - t_out = kins_tool_transformation(theta_1, theta_2, 0, matrix_in,'inv') - # the tool-x vector for the given machine joint rotations is found directly in the first column - tool_x_is = [t_out[0,0], t_out[1,0], t_out[2,0]] - log.debug("tool-x after machine rotation would be: %s", tool_x_is) - # we calculate the angular difference between the two vectors so we can 'pre-rotate' - # around tool-z to get the requested tool-x vector after machine rotation + log.info(" x-vector-requested: %s", x_vector_req) + debug_msg = (f' got joint angles: primary {theta_1:.4f} {degrees(theta_1):.4f}°, ' + f'secondary {theta_2:.4f}° {degrees(theta_2):.4f}°') + log.debug(debug_msg) + # run matrix_in through the kinematic transformation in the requested direction + # using the given joint angles and zero virtual-rotation + try: + matrix_out = kins_calc_transformation_matrix(theta_1, theta_2, 0, matrix_in, direction) + except Exception as error: + log.error('calc_virtual_rotation, %s', error) + # the x-vector for the given machine joint rotations is found directly in the first column + x_vector_is = [matrix_out[0,0], matrix_out[1,0], matrix_out[2,0]] + log.debug(" X-vector after machine rotation would be: %s", x_vector_is) + # we calculate the angular difference between the two vectors so we can add a virtual rotation + # around z-vector or work-z to match the requested x orientation after machine rotation # just to be sure we normalize the two vectors - tool_x_is = tool_x_is / np.linalg.norm(tool_x_is) - tool_x_req = tool_x_req / np.linalg.norm(tool_x_req) + x_vector_is = x_vector_is / np.linalg.norm(x_vector_is) + x_vector_req = x_vector_req / np.linalg.norm(x_vector_req) # check if the x-vector is already in the required orientation (ie parallel) - log.debug("check if vectors are parallel: %s", np.dot(tool_x_is,tool_x_req)) - if np.dot(tool_x_is,tool_x_req) > 1 - epsilon: - log.info("Tool x-vector already oriented, setting pre-rotation = 0") - # if we are already parallel then we don't need to pre-rotate - pre_rot = 0 + log.debug(" checking if vectors are parallel: %s", np.dot(x_vector_is,x_vector_req)) + if np.dot(x_vector_is, x_vector_req) > 1 - epsilon: + log.info(" X-vector already oriented, setting virtual-rotation = 0") + # if we are already parallel then we don't need to add a virtual rotation + virtual_rot = 0 else: # we can use the cross product to determine the direction we need to rotate - cross = np.cross(tool_x_req, tool_x_is) - log.debug("cross product (tool_x_req, tool_x_is): %s", cross) - log.info("Tool_z_requested: %s", tool_z_requested) - pre_rot = np.arccos(np.dot(tool_x_req, tool_x_is)) - log.debug('base pre_rot: %s', pre_rot) + cross = np.cross(x_vector_req, x_vector_is) + log.debug(" cross product (x_vector_req, x_vector_is): %s", cross) + virtual_rot = np.arccos(np.dot(x_vector_req, x_vector_is)) + log.debug(f' raw virtual_rot: {virtual_rot:.4f} {degrees(virtual_rot):.4f}°') # To find out which quadrant we need the angle to be in we create a list of them all - pre_rot_list = [pre_rot, -pre_rot, 2*pi-pre_rot, -(2*pi-pre_rot)] - log.debug('pre_rot_list: %s',pre_rot_list) - # then we run all of them through the kinematic model and see which gives us - # the requested tool-x-vector - for pre_rot in pre_rot_list: + virtual_rot_list = [virtual_rot, -virtual_rot, 2*pi-virtual_rot, -(2*pi-virtual_rot)] + log.debug(' Got possible virtual_rot angles: ' + ' '.join("{:.4f}°".format(degrees(angle)) for angle in virtual_rot_list)) + # then we run all of them through the kinematic model and see which gives us the requested x-vector-vector + for virtual_rot in virtual_rot_list: + log.debug(f' Checking virtual_rot = {degrees(virtual_rot):.4f}°') zeta = 0.0001 - # run the identity matrix through the tool kinematic transformation in the requested direction - # using the given joint angles and pre-rotation angle in the list - matrix_in = np.asmatrix(np.identity(4)) - t_out = kins_tool_transformation(theta_1, theta_2, pre_rot, matrix_in,'inv') - # the tool-x vector for the given primary and secondary rotations is found directly in the first column - tool_x_would_be = [t_out[0,0], t_out[1,0], t_out[2,0]] - log.debug('tool_x_would_be: %s', tool_x_would_be) + # run the identity matrix through the kinematic transformation in the requested direction + # using the given joint angles and virtual-rotation angle in the list + try: + matrix_out = kins_calc_transformation_matrix(theta_1, theta_2, virtual_rot, matrix_in, direction) + except Exception as error: + log.error('calc_virtual_rotation, %s', error) + # the oriented x-vector is found directly in the first column + x_vector_would_be = [matrix_out[0,0], matrix_out[1,0], matrix_out[2,0]] + log.debug(' x_vector_would_be: %s', x_vector_would_be) # calculate the difference of the respective elements - tool_x_diff = tool_x_req - tool_x_would_be + x_vector_diff = x_vector_req - x_vector_would_be # and check if all elements are within [-epsilon,epsilon] - match = np.all((tool_x_diff > -zeta) & (tool_x_diff < zeta)) - log.debug('Is the tool-X-vector close enough ? %s', match) + match = np.all((x_vector_diff > -zeta) & (x_vector_diff < zeta)) + log.debug(' Is the X-vector close enough ? %s', match) if match: # if we have a match we leave the loop and use this angle break - log.info("Pre-rotation calculated [deg]: %s", degrees(pre_rot)) - # return pre_rot in radians - return pre_rot - - -# transforms a 4x4 input matrix using the current tool transformation matrix -# (forward or inverse) using the kinematic model of the machine -def kins_calc_tool_transformation(self, matrix_in, theta_1=None, theta_2=None, pre_rot=None, direction='fwd'): - global kins_pre_rotation - # if no angle values have been passed we get the current joint positions - if theta_2 == None or theta_1 == None: - # read current spindle rotary angles and convert to radians - theta_1, theta_2 = get_current_rotary_positions(self) - else: - log.debug("got for secondary joint: %s", theta_2) - log.debug("got for primary joint: %s", theta_1) - # pre-rot is the virtual rotary axis around the tool-z axis to align the tool-x axis - # if no pre-rot angle is passed then we use the currently active value - if pre_rot == None: - pre_rot = hal.get_value(kins_pre_rotation ) - log.debug("current pre-rot: %s", pre_rot) - else: - log.debug("requested pre-rot value [DEG]): %s", degrees(pre_rot)) - # run the input matrix through the tool kinematic transformation in the requested direction - # using the current joint angles and pre-rotation as requested - matrix_out = kins_tool_transformation(theta_1, theta_2, pre_rot, matrix_in, direction) - return matrix_out - - -# define the basic rotation matrices, used for euler twp modes -def Rx(th): - return np.array([[1, 0 , 0 ], - [0, cos(th), -sin(th)], - [0, sin(th), cos(th)]]) - -def Ry(th): - return np.array([[ cos(th), 0, sin(th)], - [ 0 , 1, 0 ], - [-sin(th), 0, cos(th)]]) - -def Rz(th): - return np.array([[cos(th), -sin(th), 0], - [sin(th), cos(th), 0], - [0 , 0 , 1]]) + log.info(f'Returning virtual-rotation calculated {degrees(virtual_rot):.4f}°') + return virtual_rot # returns radians -# returns the rotation matrices for given order and angles -def twp_calc_euler_rot_matrix(th1, th2, th3, order): - log.debug("euler order requested: %s", order) - log.debug("angles given (th1, th2 , th3): %s", (th1, th2, th3)) - th1 = radians(th1) - th2 = radians(th2) - th3 = radians(th3) - if order == '131': - matrix = np.dot(np.dot(Rx(th1), Rz(th2)), Rx(th3)) - elif order=='121': - matrix = np.dot(np.dot(Rx(th1), Ry(th2)), Rx(th3)) - elif order=='212': - matrix = np.dot(np.dot(Ry(th1), Rx(th2)), Ry(th3)) - elif order=='232': - matrix = np.dot(np.dot(Ry(th1), Rz(th2)), Ry(th3)) - elif order=='323': - matrix = np.dot(np.dot(Rz(th1), Ry(th2)), Rz(th3)) - elif order=='313': - matrix = np.dot(np.dot(Rz(th1), Rx(th2)), Rz(th3)) - elif order=='123': - matrix = np.dot(np.dot(Rx(th1), Ry(th2)), Rz(th3)) - elif order=='132': - matrix = np.dot(np.dot(Rx(th1), Rz(th2)), Ry(th3)) - elif order=='213': - matrix = np.dot(np.dot(Ry(th1), Rx(th2)), Rz(th3)) - elif order=='231': - matrix = np.dot(np.dot(Ry(th1), Rz(th2)), Rx(th3)) - elif order=='321': - matrix = np.dot(np.dot(Rz(th1), Ry(th2)), Rx(th3)) - elif order=='312': - matrix = np.dot(np.dot(Rz(th1), Rx(th2)), Ry(th3)) - log.debug('euler rotation as matrix: \n %s', matrix) - return matrix +def calc_twp_matrix_from_joint_position(self, matrix_in, virtual_rot, direction): # expects radians + # transforms a 4x4 input matrix using the current transformation matrix + # (forward or inverse) using the kinematic model of the machine + log.debug('Entering: %s', sys._getframe( ).f_code.co_name) + global kins_virtual_rotation + # read current spindle rotary angles (radians) + theta_1, theta_2 = get_current_rotary_positions(self) + # virtual-rot is the virtual rotary axis around the z-vector or work-z axis to align the x-vector + log.debug(f" requested virtual-rot value {degrees(virtual_rot):.4f}°") + # run matrix_in through the kinematic transformation in the requested direction + # using the current joint angles and virtual-rotation as requested + try: + twp_matrix = kins_calc_transformation_matrix(theta_1, theta_2, virtual_rot, matrix_in, direction) + except Exception as error: + log.error('calc_twp_matrix_from_joint_position, %s', error) + return twp_matrix -# The tilted-work-plane is created in identity mode and must NOT be updated after a switch -def gui_update_twp(self): +def gui_update_twp(): + # The tilted-work-plane is created in identity mode and must NOT be updated after a switch + log.debug('Entering: %s', sys._getframe( ).f_code.co_name) global twp_matrix, saved_work_offset # twp origin as vector (in world coords) from current work-offset to the origin of the twp - hal.set_p("twp-helper-comp.twp-ox-in",str(twp_matrix[0,3])) - hal.set_p("twp-helper-comp.twp-oy-in",str(twp_matrix[1,3])) - hal.set_p("twp-helper-comp.twp-oz-in",str(twp_matrix[2,3])) - # twp x-vector - hal.set_p("twp-helper-comp.twp-xx-in",str(twp_matrix[0,0])) - hal.set_p("twp-helper-comp.twp-xy-in",str(twp_matrix[1,0])) - hal.set_p("twp-helper-comp.twp-xz-in",str(twp_matrix[2,0])) - # twp z-vector - hal.set_p("twp-helper-comp.twp-zx-in",str(twp_matrix[0,2])) - hal.set_p("twp-helper-comp.twp-zy-in",str(twp_matrix[1,2])) - hal.set_p("twp-helper-comp.twp-zz-in",str(twp_matrix[2,2])) + try: + hal.set_p("twp-helper-comp.twp-ox-in",str(twp_matrix[0,3])) + hal.set_p("twp-helper-comp.twp-oy-in",str(twp_matrix[1,3])) + hal.set_p("twp-helper-comp.twp-oz-in",str(twp_matrix[2,3])) + # twp x-vector + hal.set_p("twp-helper-comp.twp-xx-in",str(twp_matrix[0,0])) + hal.set_p("twp-helper-comp.twp-xy-in",str(twp_matrix[1,0])) + hal.set_p("twp-helper-comp.twp-xz-in",str(twp_matrix[2,0])) + # twp z-vector + hal.set_p("twp-helper-comp.twp-zx-in",str(twp_matrix[0,2])) + hal.set_p("twp-helper-comp.twp-zy-in",str(twp_matrix[1,2])) + hal.set_p("twp-helper-comp.twp-zz-in",str(twp_matrix[2,2])) + except Exception as error: + log.error('gui_update_twp failed, %s', error) # publish the twp offset coordinates in world coordinates (ie identity) [work_offset_x, work_offset_y, work_offset_z] = saved_work_offset - log.debug("Setting work_offsets in the simulation: %s", (work_offset_x, work_offset_y, work_offset_z)) + log.debug(" Setting work_offsets in the simulation: %s", (work_offset_x, work_offset_y, work_offset_z)) # this is used to translate the rotated twp to the correct position # care must be taken that only the work_offsets in identity mode are sent as that is - # what the model uses. The visuals for the offsets are created then rotated according to - # the rotary joint position and then translated. + # what the model uses. The visuals for the offsets are created in the origin, + # then rotated according to the rotary joint position and then translated. # The twp has to be rotated out of the machine xy plane using the g68.2 parameters and is then # translated by the offset values of the identity mode. - hal.set_p("twp-helper-comp.twp-ox-world-in",str(work_offset_x)) - hal.set_p("twp-helper-comp.twp-oy-world-in",str(work_offset_y)) - hal.set_p("twp-helper-comp.twp-oz-world-in",str(work_offset_z)) + try: + hal.set_p("twp-helper-comp.twp-ox-world-in",str(work_offset_x)) + hal.set_p("twp-helper-comp.twp-oy-world-in",str(work_offset_y)) + hal.set_p("twp-helper-comp.twp-oz-world-in",str(work_offset_z)) + except Exception as error: + log.error('gui_update_twp failed, %s', error) # NOTE: Due to easier abort handling we currently restrict the use of twp to G54 # as LinuxCNC seems to revert to G54 as the default system def get_current_work_offset(self): + log.debug('Entering: %s', sys._getframe( ).f_code.co_name) # get which offset is active (g54=1 .. g59.3=9) active_offset = int(self.params[5220]) current_work_offset_number = active_offset @@ -702,11 +530,12 @@ def get_current_work_offset(self): co_x = self.params[work_offset_x] co_y = self.params[work_offset_y] co_z = self.params[work_offset_z] - current_work_offset = [co_x, co_y, co_z] + current_work_offset = (co_x, co_y, co_z) return [current_work_offset_number, current_work_offset] def get_current_rotary_positions(self): + log.debug('Entering: %s', sys._getframe( ).f_code.co_name) global joint_letter_primary, joint_letter_secondary if joint_letter_primary == 'A': theta_1 = radians(self.AA_current) @@ -714,7 +543,7 @@ def get_current_rotary_positions(self): theta_1 = radians(self.BB_current) elif joint_letter_primary == 'C': theta_1 = radians(self.CC_current) - log.debug('Current position Primary joint: %s', degrees(theta_1)) + log.debug(f' Current position Primary joint: {degrees(theta_1):.4f}°') # read current spindle rotary angles and convert to radians if joint_letter_secondary == 'A': theta_2 = radians(self.AA_current) @@ -722,45 +551,32 @@ def get_current_rotary_positions(self): theta_2 = radians(self.BB_current) elif joint_letter_secondary == 'C': theta_2 = radians(self.CC_current) - log.debug('Current position Secondary joint: %s', degrees(theta_2)) + log.debug(f' Current position Secondary joint: {degrees(theta_2):.4f}°') return theta_1, theta_2 -# forms a 4x4 transformation matrix from a given 1x3 point vector [x,y,z] -def point_to_matrix(point): - # start with a 4x4 identity matrix and add the point vector to the 4th column - matrix = np.identity(4) - [matrix[0,3], matrix[1,3], matrix[2,3]] = point - matrix = np.asmatrix(matrix) - return matrix - - -# extracts the point vector form a given 4x4 transformation matrix -def matrix_to_point(matrix): - point = (matrix[0,3],matrix[1,3],matrix[2,3]) - return point - - -def reset_twp_params(self): - global pre_rot, twp_matrix, twp_flag, twp_build_params - pre_rot = 0 +def reset_twp_params(): + log.debug('Entering: %s', sys._getframe( ).f_code.co_name) + global virtual_rot, twp_matrix, twp_flag, twp_build_params + virtual_rot = 0 # we must not change tool kins parameters when TOOL kins are active or we get sudden joint position changes - # ie don't do this: kins_comp_set_pre_rot(self,0)! + # ie don't do this: kins_comp_set_virtual_rot(0)! twp_flag = [] twp_build_params = {} - log.info("Resetting TWP-matrix") + log.info(" Resetting TWP-matrix") twp_matrix = np.asmatrix(np.identity(4)) -# Orient the tool to the current twp (with TCP for G53.1 or IDENTITY for G53.6) -# (some controllers offer an optional P-word to give preferred rotation directions this is not implemented yet) -# Note: To avoid that this python code is run prematurely by the read ahead we need a quebuster at the beginning but -# because we need self.execute() to switch the WCS properly this remap needs to be called from -# an ngc reamp that contains a quebuster before calling this code -# IMPORTANT: -# The correct kinematic mode (ie TCP for 53.1 / IDENTITY for G53.6) must be active when this code is called -# (ie do it in the ngc remap mentioned above!) -def g53x_core(self): - global saved_work_offset, twp_matrix, twp_flag, pre_rot + +def g53n_core(self): + log.debug('Entering: %s', sys._getframe( ).f_code.co_name) + # Orient the tool to the current twp (with TCP for G53.1 or IDENTITY for G53.6) + # Note: To avoid that this python code is run prematurely by the read ahead we need a quebuster at the + # beginning but because we need self.execute() to switch the WCS properly this remap needs to be called from + # an ngc reamp that contains a quebuster before calling this code. + # IMPORTANT: + # The correct kinematic mode (ie TCP for 53.1 / IDENTITY for G53.6) must be active when this code is called + # (ie do it in the ngc remap mentioned above!) + global saved_work_offset, twp_matrix, twp_flag, virtual_rot global joint_letter_primary, joint_letter_secondary, twp_error_status global orient_mode if self.task == 0: # ignore the preview interpreter @@ -769,112 +585,142 @@ def g53x_core(self): if not hal.get_value(twp_is_defined): # reset the twp parameters - reset_twp_params(self) - msg = "G53.x: No TWP defined." - log.debug(msg) + reset_twp_params() + msg = "G53.n: No TWP defined." + log.debug(' ' + msg) emccanon.CANON_ERROR(msg) yield INTERP_EXECUTE_FINISH # w/o this the error message is not displayed yield INTERP_EXIT # w/o this the error does not abort a running gcode program return INTERP_ERROR - elif hal.get_value(twp_is_active): # reset the twp parameters - reset_twp_params(self) - msg = "G53.x: TWP already active" - log.debug(msg) + reset_twp_params() + msg = "G53.n: TWP already active" + log.debug(' ' + msg) emccanon.CANON_ERROR(msg) yield INTERP_EXECUTE_FINISH # w/o this the error message is not displayed yield INTERP_EXIT # w/o this the error does not abort a running gcode program return INTERP_ERROR - # Check if any words have been passed with the respective G53.x command + # Check if any words have been passed with the respective G53.n command c = self.blocks[self.remap_level] p = c.p_number if c.p_flag else 0 x = c.i_number if c.i_flag else None y = c.j_number if c.j_flag else None z = c.k_number if c.k_flag else None - log.debug('G53.x Words passed: (P, X,Y,Z): %s', (p,x,y,z)) + log.debug(' G53.n Words passed: (P, X,Y,Z): %s', (p,x,y,z)) + if p not in [0,1,2]: - # reset the twp parameters - reset_twp_params(self) - msg = "G53.x : unrecognised P-Word found." - log.debug(msg) + # reset the twp parameters + reset_twp_params() + msg = "G53.n : unrecognised P-Word found." + log.debug(' ' + msg) emccanon.CANON_ERROR(msg) yield INTERP_EXECUTE_FINISH # w/o this the error message is not displayed yield INTERP_EXIT # w/o this the error does not abort a running gcode program return INTERP_ERROR orient_mode = p - # calculate the required rotary joint positions and pre_rotation for the requested tool-orientation + z_vector_requested = [twp_matrix[0,2],twp_matrix[1,2],twp_matrix[2,2]] + x_vector_requested = [twp_matrix[0,0],twp_matrix[1,0],twp_matrix[2,0]] + # calculate all possible pairs of (primary, secondary) angles to matches the requested orientation try: - tool_z_requested = [twp_matrix[0,2],twp_matrix[1,2],twp_matrix[2,2]] - # calculate all possible pairs of (primary, secondary) angles so our tool-z vector matches the requested tool-z # angles are returned in [-pi,pi] - possible_prim_sec_angle_pairs = kins_calc_jnt_angles(self, tool_z_requested) - # An excepton will occur if the requested tool orientation cannot be achieved with the kinematic at hand + possible_prim_sec_angle_pairs = calc_joint_angles(z_vector_requested, x_vector_requested) # returns radians except Exception as error: - log.error('G53.x: Calculation failed, %s', error) - possible_prim_sec_angle_pairs = [] - if not possible_prim_sec_angle_pairs: - # reset the twp parameters - reset_twp_params(self) - msg = "G53.x ERROR: Requested tool orientation not reachable -> aborting G53.x" - log.debug(msg) + log.error('calc_joint_angles, %s', error) + # reset the twp parameters + reset_twp_params() + msg = ("G53.n ERROR: Calculation of joint angles has failed. -> aborting G53.n") + log.debug(' ' + msg) + emccanon.CANON_ERROR(msg) + yield INTERP_EXECUTE_FINISH # w/o this the error message is not displayed + yield INTERP_EXIT # w/o this the error does not abort a running gcode program + return INTERP_ERROR + + if possible_prim_sec_angle_pairs == []: + # reset the twp parameters + log.error('G53.n: No possible primary/secondary angle pairs found.') + reset_twp_params() + msg = "G53.n ERROR: Requested tool orientation not reachable -> aborting G53.n" + log.debug(' ' + msg) emccanon.CANON_ERROR(msg) yield INTERP_EXECUTE_FINISH # w/o this the error message is not displayed yield INTERP_EXIT # w/o this the error does not abort a running gcode program return INTERP_ERROR # this returns one pair of optimized angles in degrees, or (None, None) if no solution could be found - theta_1, theta_2 = calc_optimal_joint_move(self, possible_prim_sec_angle_pairs) - if theta_1 == None: + try: + theta_1, theta_2 = calc_optimal_joint_move(self, possible_prim_sec_angle_pairs) # returns radians + except Exception as error: + log.error('G53.n: Calculation of optimal joint move failed, %s', error) + if theta_1 == None or theta_2 == None: # reset the twp parameters - reset_twp_params(self) - msg = ("G53.x ERROR: Requested tool orientation not reachable -> aborting G53.x") - log.debug(msg) + reset_twp_params() + msg = ("G53.n ERROR: Requested tool orientation not reachable -> aborting G53.n") + log.debug(' ' + msg) emccanon.CANON_ERROR(msg) yield INTERP_EXECUTE_FINISH # w/o this the error message is not displayed yield INTERP_EXIT # w/o this the error does not abort a running gcode program return INTERP_ERROR - theta_1 = radians(theta_1) - theta_2 = radians(theta_2) - # calculate the pre-rotation needed so our tool-x vector matches the requested tool-x vector - tool_x_requested = [twp_matrix[0,0],twp_matrix[1,0],twp_matrix[2,0]] - pre_rot = kins_calc_pre_rot(self,theta_1, theta_2, tool_x_requested, tool_z_requested) - log.debug("Calculated pre-rotation (pre_rot) to match requested tool-x): %s", pre_rot) + # get the particular conditions to be met for the kinematic at hand + try: + (x_vector_requested, z_vector_requested, matrix_in, direction) = kins_calc_virtual_rot_get_values(x_vector_requested, + z_vector_requested, + twp_matrix) + except Exception as error: + log.error('G53.n: kins_calc_virtual_rot_get_values failed, %s', error) + # calculate the virtual-rotation needed + virtual_rot = calc_virtual_rotation(theta_1, + theta_2, + x_vector_requested, + z_vector_requested, + matrix_in, + direction) # returns radians + log.debug(f" Calculated virtual-rotation to match requested x-vector: {degrees(virtual_rot):.4f}°") + # mark twp-flag as active twp_flag = [0, 'active'] - gui_update_twp(self) - # set the pre-rotation value in the kinematic component - log.debug("G53.x: setting primary, secondary and pre_rotation angles in kinematic component: %s", (degrees(theta_1), degrees(theta_2), degrees(pre_rot))) - hal.set_p(kins_pre_rotation, str(pre_rot)) - hal.set_p(kins_primary_rotation, str(degrees(theta_1))) - hal.set_p(kins_secondary_rotation, str(degrees(theta_2))) - - # calculate the work offset in tool-coords - P = matrix_to_point(kins_calc_tool_transformation(self, point_to_matrix(saved_work_offset), theta_1, theta_2, pre_rot)) - # get the current twp_origin + gui_update_twp() + + # set the virtual-rotation value in the kinematic component + debug_msg = (f' G53.n: Setting angle values in kins comp to theta1: {degrees(theta_1):.4f}°, ' + f'theta2: {degrees(theta_2):.4f}°, virtual_rot: {degrees(virtual_rot):.4f}°') + log.debug(debug_msg) + try: + kins_set_values(theta_1, theta_2, virtual_rot) + except Exception as error: + log.error('G53.n: kins_set_values failed, %s', error) + + # calculate the work offset in transformed-coordinatess + log.debug(" G53.n: Saved work offset: %s", saved_work_offset) twp_offset = (twp_matrix[0,3],twp_matrix[1,3],twp_matrix[2,3]) - # calculate the twp offset in tool-coords - Q = matrix_to_point(kins_calc_tool_transformation(self, point_to_matrix(twp_offset), theta_1, theta_2, pre_rot)) - log.debug("G53.x: Setting transformed work-offsets for tool-kins in G59, G59.1, G59.2 and G59.3 to: %s ", P) + try: + new_offset = kins_calc_transformed_work_offset(saved_work_offset, twp_offset, theta_1, theta_2, virtual_rot) + except Exception as error: + log.error('G53.n: Calculation of kins_calc_transformed_work_offset failed, %s', error) + debug_msg = (f' G53.n: Setting transformed work-offsets for twp-kins in G59, G59.1, ' + f'G59.2 and G59.3 to: {new_offset[0]:.4f}, {new_offset[1]:.4f}, {new_offset[2]:.4f}') + log.debug(debug_msg) # set the dedicated TWP work offset values (G53, G53.1, G53.2, G53.3) - self.execute("G10 L2 P6 X%f Y%f Z%f " % (P[0]+Q[0], P[1]+Q[1], P[2]+Q[2]), lineno()) - self.execute("G10 L2 P7 X%f Y%f Z%f " % (P[0]+Q[0], P[1]+Q[1], P[2]+Q[2]), lineno()) - self.execute("G10 L2 P8 X%f Y%f Z%f " % (P[0]+Q[0], P[1]+Q[1], P[2]+Q[2]), lineno()) - self.execute("G10 L2 P9 X%f Y%f Z%f " % (P[0]+Q[0], P[1]+Q[1], P[2]+Q[2]), lineno()) - log.debug("G53.x: Moving (secondary and primary) joints to: %s", (degrees(theta_2), degrees(theta_1))) + self.execute("G10 L2 P6 X%f Y%f Z%f" % (new_offset[0], new_offset[1], new_offset[2]), lineno()) + self.execute("G10 L2 P7 X%f Y%f Z%f" % (new_offset[0], new_offset[1], new_offset[2]), lineno()) + self.execute("G10 L2 P8 X%f Y%f Z%f" % (new_offset[0], new_offset[1], new_offset[2]), lineno()) + self.execute("G10 L2 P9 X%f Y%f Z%f" % (new_offset[0], new_offset[1], new_offset[2]), lineno()) + + log.debug(f" G53.n: Moving primary joint to {degrees(theta_1):.4f}° and secondary joint to {degrees(theta_2):.4f}° ") if (x,y,z) == (None,None,None): - # Move rotary joints to align the tool with the requested twp - self.execute("G0 %s%f %s%f" % (joint_letter_secondary, degrees(theta_2), joint_letter_primary, degrees(theta_1)), lineno()) + # Move rotary joints to align the tool and the requested work plane + self.execute("G0 %s%f %s%f" % (joint_letter_primary, degrees(theta_1), joint_letter_secondary, degrees(theta_2)), lineno()) # switch to the dedicated TWP work offsets self.execute("G59", lineno()) - # activate TOOL kinematics + # activate TWP kinematics self.execute("G12.1 P2") if (x,y,z) != (None,None,None): - log.debug('G53.3 called') - self.execute("G0 X%s Y%s Z%s %s%f %s%f" % (x, y, z, joint_letter_secondary, degrees(theta_2), joint_letter_primary, degrees(theta_1)), lineno()) + log.debug(' G53.3 called') + self.execute("G0 X%s Y%s Z%s %s%f %s%f" % + (x, y, z, joint_letter_primary, degrees(theta_1), joint_letter_secondary, degrees(theta_2)), lineno()) # set twp-state to 'active' (2) self.execute("M68 E2 Q2") yield INTERP_EXECUTE_FINISH @@ -886,24 +732,25 @@ def g53x_core(self): # because we need self.execute() to switch the WCS properly this remap needs to be called from # an ngc that contains a quebuster before calling this code def g69_core(self): + log.debug('Entering: %s', sys._getframe( ).f_code.co_name) global twp_flag, saved_work_offset_number, saved_work_offset if self.task == 0: # ignore the preview interpreter yield INTERP_EXECUTE_FINISH return INTERP_OK log.info('G69 called') # reset the twp parameters - reset_twp_params(self) - gui_update_twp(self) + reset_twp_params() + gui_update_twp() # set twp-state to 'undefined' (0) self.execute("M68 E2 Q0") yield INTERP_EXECUTE_FINISH return INTERP_OK -# define a virtual tilted-work-plane (twp) that is perpendicular to the current -# tool-orientation +# define a virtual tilted-work-plane (twp) that is perpendicular to the current tool-orientation def g683(self, **words): - global twp_matrix, pre_rot, twp_flag, saved_work_offset_number, saved_work_offset + log.debug('Entering: %s', sys._getframe( ).f_code.co_name) + global twp_matrix, virtual_rot, twp_flag, saved_work_offset_number, saved_work_offset if self.task == 0: # ignore the preview interpreter yield INTERP_EXECUTE_FINISH @@ -917,7 +764,7 @@ def g683(self, **words): if hal.get_value(twp_is_defined): # reset the twp parameters - reset_twp_params(self) + reset_twp_params() msg =("G68.3 ERROR: TWP already defined.") log.debug(msg) emccanon.CANON_ERROR(msg) @@ -931,7 +778,7 @@ def g683(self, **words): (n, offsets) = get_current_work_offset(self) if n != 1: # reset the twp parameters - reset_twp_params(self) + reset_twp_params() msg = "G68.3 ERROR: Must be in G54 to define TWP." log.debug(msg) emccanon.CANON_ERROR(msg) @@ -944,23 +791,31 @@ def g683(self, **words): x = c.x_number if c.x_flag else 0 y = c.y_number if c.y_flag else 0 z = c.z_number if c.z_flag else 0 - # parse the requested rotation of tool-x around the origin - r = c.r_number if c.r_flag else 0 + # parse the requested rotation of x-vector around the origin + r = radians(c.r_number) if c.r_flag else 0 twp_flag = [0, 1, 'empty'] # one call to define the twp in this mode - theta_1, theta_2 = get_current_rotary_positions(self) - # calculate tool-prerotation necessary to have tool-x vector in machine xy-plane - pre_rot = kins_calc_tool_rot_c_for_horizontal_x(self, theta_1, theta_2 ) - log.info("G68.3: Pre-Rotation calculated for x-vector in machine-xy plane [deg]: %s", pre_rot*180/pi) - # then we need the tool transformation matrix of the current tool orientation with the - # calculated pre-rotation to get the tool-x vector in the machine xy-plane - # for this we take the 4x4 identity matrix and pass it through the inverse tool kinematic - # transformation using the current rotary joint positions and calculated pre-rotation angle - # plus the requested angle of rotation for tool-x from the machine-xy plane + theta_1, theta_2 = get_current_rotary_positions(self) # radians + # calculate virtual rotation to have the oriented x-vector in the direction required for the kinematic at hand + try: + virtual_rot = kins_calc_virtual_rot_for_g683(theta_1, theta_2 ) + except Exception as error: + log.error('remap_func: kins_calc_virtual_rot_for_g683 failed, %s', error) + log.info("G68.3: virtual-Rotation calculated for x-vector in machine-xy plane [deg]: %s", degrees(virtual_rot)) + # then we need to calculate the transformation matrix of the current orientation with the including the + # calculated virtual-rotation. + # for this we take the 4x4 identity matrix and pass it through the kinematic transformation using the + # current rotary joint positions and the calculated virtual-rotation angle plus any additional angle + # passed in the R word of the G68.3 command start_matrix = np.asmatrix(np.identity(4)) - log.info('G68.3: Requested origin rotation [deg]: %s', r) - twp_matrix = kins_calc_tool_transformation(self, start_matrix, None, None, pre_rot + radians(r), 'inv') - log.debug("G68.3: Tool matrix with x-vector in machine xy-plane: \n%s", twp_matrix) + log.info('G68.3: Requested R-word rotation [deg]: %s', degrees(r)) + # the required transformation direction may depend on the kinematic at hand + try: + direction = kins_calc_transformation_get_direction() + except Exception as error: + log.error('kins_calc_transformation_get_direction, %s', error) + twp_matrix = calc_twp_matrix_from_joint_position(self, start_matrix, virtual_rot + r, direction) + log.debug("G68.3: TWP matrix with oriented x-vector: \n%s", twp_matrix) # put the requested origin into the twp_matrix (twp_matrix[0,3], twp_matrix[1,3], twp_matrix[2,3]) = (x, y, z) # update the build state of the twp call @@ -974,13 +829,14 @@ def g683(self, **words): self.execute("M68 E2 Q1") yield INTERP_EXECUTE_FINISH - gui_update_twp(self) + gui_update_twp() return INTERP_OK # definition of a virtual work-plane (twp) using different methods set by the 'p'-word def g682(self, **words): - global twp_matrix, pre_rot, twp_flag, twp_build_params, saved_work_offset_number, saved_work_offset + log.debug('Entering: %s', sys._getframe( ).f_code.co_name) + global twp_matrix, virtual_rot, twp_flag, twp_build_params, saved_work_offset_number, saved_work_offset if self.task == 0: # ignore the preview interpreter yield INTERP_EXECUTE_FINISH @@ -994,9 +850,9 @@ def g682(self, **words): if hal.get_value(twp_is_defined): # ie TWP has already been defined # reset the twp parameters - reset_twp_params(self) + reset_twp_params() msg = ("G68.2: TWP already defined.") - log.debug(msg) + log.debug(' ' + msg) emccanon.CANON_ERROR(msg) yield INTERP_EXECUTE_FINISH # w/o this the error message is not displayed yield INTERP_EXIT # w/o this the error does not abort a running gcode program @@ -1007,9 +863,9 @@ def g682(self, **words): (n, offsets) = get_current_work_offset(self) if n != 1: # reset the twp parameters - reset_twp_params(self) + reset_twp_params() msg = "G68.2 ERROR: Must be in G54 to define TWP." - log.debug(msg) + log.debug(' ' + msg) emccanon.CANON_ERROR(msg) yield INTERP_EXECUTE_FINISH # w/o this the error message is not displayed yield INTERP_EXIT # w/o this the error does not abort a running gcode program @@ -1018,7 +874,7 @@ def g682(self, **words): # collect the currently active work offset values (ie g54, g55 or other) saved_work_offset_number = n saved_work_offset = offsets - log.debug("G68.2: Saved work offsets %s", (n, saved_work_offset)) + log.debug(" G68.2: Saved work offsets %s", (n, saved_work_offset)) c = self.blocks[self.remap_level] p = c.p_number if c.p_flag else 0 @@ -1028,9 +884,9 @@ def g682(self, **words): q = str(int(c.q_number if c.q_flag else 313)) if q not in ['121','131','212','232','313','323']: # reset the twp parameters - reset_twp_params(self) + reset_twp_params() msg = ("G68.2 (P0): No recognised Q-Word found.") - log.debug(msg) + log.debug(' ' + msg) emccanon.CANON_ERROR(msg) yield INTERP_EXECUTE_FINISH # w/o this the error message is not displayed yield INTERP_EXIT # w/o this the error does not abort a running gcode program @@ -1040,21 +896,24 @@ def g682(self, **words): x = c.x_number if c.x_flag else 0 y = c.y_number if c.y_flag else 0 z = c.z_number if c.z_flag else 0 - # parse the requested rotation of tool-x around the origin - r = c.r_number if c.r_flag else 0 + # parse the requested xy-rotation around the origin + r = radians(c.r_number) if c.r_flag else 0 # parse the requested euler rotation angles - th1 = c.i_number if c.i_flag else 0 - th2 = c.j_number if c.j_flag else 0 - th3 = c.k_number if c.k_flag else 0 + th1 = radians(c.i_number) if c.i_flag else 0 + th2 = radians(c.j_number) if c.j_flag else 0 + th3 = radians(c.k_number) if c.k_flag else 0 # build the translation vector of the twp_matrix twp_origin = [[x], [y], [z]] - # we use xzx-euler rotation to create the rotation matrix for the requested origin rotation - twp_origin_rotation = twp_calc_euler_rot_matrix(0, r, 0, '131') - log.debug('G68.2 (P0): Twp_origin_rotation \n%s',twp_origin_rotation) + # create the rotation matrix for the requested origin rotation + try: + twp_origin_rotation = kins_calc_twp_origin_rot_matrix(r) + except Exception as error: + log.error('remap_func: kins_calc_twp_origin_rot_matrix failed, %s', error) + log.debug(' G68.2 (P0): Twp_origin_rotation \n%s',twp_origin_rotation) # build the rotation matrix for the requested euler rotation - twp_euler_rotation = twp_calc_euler_rot_matrix(th1, th2, th3, q) - log.debug('G68.2 (P0): Twp_euler_rotation \n%s',twp_euler_rotation) + twp_euler_rotation = calc_euler_rot_matrix(th1, th2, th3, q) + log.debug(' G68.2 (P0): Twp_euler_rotation \n%s',twp_euler_rotation) # calculate the total twp_rotation using matrix multiplication twp_rotation = np.asmatrix(twp_origin_rotation) * np.asmatrix(twp_euler_rotation) # combine rotation and translation and form the 4x4 twp-transformation matrix @@ -1072,34 +931,36 @@ def g682(self, **words): if q not in ['123','132','213','231','312','321']: # reset the twp parameters - reset_twp_params(self) + reset_twp_params() msg = ("G68.2 P1: No recognised Q-Word found.") - log.debug(msg) + log.debug(' ' + msg) emccanon.CANON_ERROR(msg) yield INTERP_EXECUTE_FINISH # w/o this the error message is not displayed yield INTERP_EXIT # w/o this the error does not abort a running gcode program return INTERP_ERROR - # parse the requested origin x = c.x_number if c.x_flag else 0 y = c.y_number if c.y_flag else 0 z = c.z_number if c.z_flag else 0 - # parse the requested rotation of tool-x around the origin - r = c.r_number if c.r_flag else 0 + # parse the requested xy-rotation around the origin + r = radians(c.r_number) if c.r_flag else 0 # parse the requested euler rotation angles - th1 = c.i_number if c.i_flag else 0 - th2 = c.j_number if c.j_flag else 0 - th3 = c.k_number if c.k_flag else 0 + th1 = radians(c.i_number) if c.i_flag else 0 + th2 = radians(c.j_number) if c.j_flag else 0 + th3 = radians(c.k_number) if c.k_flag else 0 # build the translation vector of the twp_matrix twp_origin = [[x], [y], [z]] - # we use xzx-euler rotation to create the rotation matrix for the requested origin rotation - twp_origin_rotation = twp_calc_euler_rot_matrix(0, r, 0, '131') - log.debug('G68.2 P1: Twp_origin_rotation \n%s',twp_origin_rotation) + # create the rotation matrix for the requested origin rotation + try: + twp_origin_rotation = kins_calc_twp_origin_rot_matrix(r) + except Exception as error: + log.error('remap_func: kins_calc_twp_origin_rot_matrix failed, %s', error) + log.debug(' G68.2 P1: Twp_origin_rotation \n%s',twp_origin_rotation) # build the rotation matrix for the requested euler rotation - twp_euler_rotation = twp_calc_euler_rot_matrix(th1, th2, th3, q) - log.debug('G68.2 P1: Twp_euler_rotation \n%s',twp_euler_rotation) + twp_euler_rotation = calc_euler_rot_matrix(th1, th2, th3, q) + log.debug(' G68.2 P1: Twp_euler_rotation \n%s',twp_euler_rotation) # calculate the total twp_rotation using matrix multiplication twp_rotation = np.asmatrix(twp_origin_rotation) * np.asmatrix(twp_euler_rotation) # combine rotation and translation and form the 4x4 twp-transformation matrix @@ -1111,21 +972,28 @@ def g682(self, **words): twp_flag[2] = 'done' elif p == 2: # twp defined py 3 points on the plane + # TODO implement operator errors as outlined in the twp README + #- G68.2 P2 (Q0),Q1,Q2,Q3 commands are not entered consecutively + #- two to the points entered in Q1,Q2,Q3 are identical + #- all three points entered in Q1,Q2,Q3 are on a line + #- the distance between a line defined by any two points entered in (Q1,Q2,Q3) and + #the remaining point is less than 10mm or 0.5inch (just some arbitrary values for now) + # if this is the first call for this mode reset the twp_flag flag if not twp_flag: twp_flag = [int(p), 4 , 'empty', 'empty', 'empty', 'empty'] # four calls needed twp_build_params = {'q0':[], 'q1':[], 'q2':[], 'q3':[]} # Point 1: defines the origin of the twp - # Point 2: direction from P1 to P2 defines the positive x direction on the twp (tool-x) - # Point 3: defines the positive y side and with P1 and P2 defines the xy work plane (tool-z) + # Point 2: direction from P1 to P2 defines the positive x direction on the twp (x-vector) + # Point 3: defines the positive y side and with P1 and P2 defines the xy work plane (z-vector) q = int(c.q_number if c.q_flag else 0) # this mode needs four calls to fill all required parameters if q == 0: # define new origin and rotation x = c.x_number if c.x_flag else 0 y = c.y_number if c.y_flag else 0 z = c.z_number if c.z_flag else 0 - # parse the requested rotation of tool-x around the origin - r = c.r_number if c.r_flag else 0 + # parse the requested xy-rotation around the origin + r = radians(c.r_number) if c.r_flag else 0 twp_build_params['q0'] = [x,y,z,r] twp_flag[2] = 'done' elif q == 1: # define point 1 @@ -1148,9 +1016,9 @@ def g682(self, **words): twp_flag[5] = 'done' else: # reset the twp parameters - reset_twp_params(self) + reset_twp_params() msg = ("G68.2 P2: No recognised Q-Word found.") - log.debug(msg) + log.debug(' ' + msg) emccanon.CANON_ERROR(msg) yield INTERP_EXECUTE_FINISH # w/o this the error message is not displayed yield INTERP_EXIT # w/o this the error does not abort a running gcode program @@ -1164,36 +1032,38 @@ def g682(self, **words): p1 = twp_build_params['q1'][0:3] p2 = twp_build_params['q2'] p3 = twp_build_params['q3'] - log.debug("G68.2 P2: Point 1: %s",p1) - log.debug("G68.2 P2: Point 2: %s",p2) - log.debug("G68.2 P2: Point 3: %s",p3) + log.debug(" G68.2 P2: Point 1: %s",p1) + log.debug(" G68.2 P2: Point 2: %s",p2) + log.debug(" G68.2 P2: Point 3: %s",p3) # build vectors x:P1->P2 and v2:P1->P3 twp_vect_x = [p2[0]-p1[0], p2[1]-p1[1], p2[2]-p1[2]] - log.debug("G68.2 P2: Twp_vect_x: \n%s",twp_vect_x) + log.debug(" G68.2 P2: Twp_vect_x: \n%s",twp_vect_x) v2 = [p3[0]-p1[0], p3[1]-p1[1], p3[2]-p1[2]] - log.debug("G68.2 P2 (v2): %s",v2) + log.debug(" G68.2 P2 (v2): %s",v2) # normalize the two vectors twp_vect_x = twp_vect_x / np.linalg.norm(twp_vect_x) v2 = v2 / np.linalg.norm(v2) - # we can use the cross product to calculate the tool-z vector + # we can use the cross product to calculate the z-vector vector # note: if P3 is on the right side of the vector P1->P2 - # then the tool-z will be below the twp (ie tool-z will be downwards) + # then the z-vector will be below the twp (ie z-vector will be downwards) twp_vect_z = np.cross(twp_vect_x , v2) - log.debug("G68.2 P2: Twp_vect_z %s",twp_vect_z) - # we can use the cross product to calculate the tool-y vector + log.debug(" G68.2 P2: Twp_vect_z %s",twp_vect_z) + # we can use the cross product to calculate the y vector twp_vect_y = np.cross(twp_vect_z, twp_vect_x) - log.debug("G68.2 P2: Twp_vect_y %s",twp_vect_y) - # build the rotation matrix of the twp_matrix from the calculated tool-vectors + log.debug(" G68.2 P2: Twp_vect_y %s",twp_vect_y) + # build the rotation matrix of the twp_matrix from the calculated vectors # first stack the vectors (lists) and then flip diagonally (transpose) # so the vectors are now vertical twp_vect_rotation_t = np.vstack((twp_vect_x, twp_vect_y)) twp_vect_rotation_t = np.vstack((twp_vect_rotation_t, twp_vect_z)) twp_vect_rotation = np.transpose(twp_vect_rotation_t) - log.debug("G68.2 P2: Built the twp-rotation-matrix: \n%s", twp_vect_rotation) - # convert requested origin rotation to radians - # we use xzx-euler rotation to create the rotation matrix for the requested origin rotation - twp_origin_rotation = twp_calc_euler_rot_matrix(0, r, 0, '131') - log.debug('G68.2 P2: Twp-origin-rotation-matrix \n%s',twp_origin_rotation) + log.debug(" G68.2 P2: Built the twp-rotation-matrix: \n%s", twp_vect_rotation) + # create the rotation matrix for the requested origin rotation + try: + twp_origin_rotation = kins_calc_twp_origin_rot_matrix(r) + except Exception as error: + log.error('remap_func: kins_calc_twp_origin_rot_matrix failed, %s', error) + log.debug(' G68.2 P2: Twp-origin-rotation-matrix \n%s',twp_origin_rotation) # calculate the total twp_rotation using matrix multiplication twp_rotation = np.asmatrix(twp_origin_rotation) * np.asmatrix(twp_vect_rotation) # add the origin translation on the right @@ -1202,22 +1072,26 @@ def g682(self, **words): twp_row_4 = [0,0,0,1] twp_matrix = np.vstack((twp_matrix, twp_row_4)) twp_matrix = np.asmatrix(twp_matrix) - log.debug("G68.2 P2: Built twp-transformation-matrix: \n%s", twp_matrix) + log.debug(" G68.2 P2: Built twp-transformation-matrix: \n%s", twp_matrix) - elif p == 3: # two vectors (vector 1 defines the tool-x and vector 2 defines the tool-z) + elif p == 3: # two vectors (vector 1 defines the x-vector and vector 2 defines the z-vector) + # TODO implement operator errors as outlined in the twp README + #- G68.2 P3 Q1 and Q2 commands are not entered consecutively + #- one of the vectors is the zero vector + #- the enclosed angle between the 1. and 2. vector is <85° or >95° (re fanuc twp pdf) q = int(c.q_number if c.q_flag else 0) # if this is the first call for this mode reset the twp_flag flag if not twp_flag: - log.info('first call') + log.info(' first call') twp_flag = [int(p), 2 , 'empty', 'empty'] # two calls needed twp_build_params = {'q0':[], 'q1':[]} - log.debug('twp_build_params: %s', twp_build_params) + log.debug(' twp_build_params: %s', twp_build_params) if q == 0: # define new origin of the twp x = c.x_number if c.x_flag else 0 y = c.y_number if c.y_flag else 0 z = c.z_number if c.z_flag else 0 - # parse the requested rotation of tool-x around the origin - r = c.r_number if c.r_flag else 0 + # parse the requested xy-rotation around the origin + r = radians(c.r_number) if c.r_flag else 0 # first vector (direction of x in the twp) i = c.i_number if c.i_flag else 0 j = c.j_number if c.j_flag else 0 @@ -1232,9 +1106,9 @@ def g682(self, **words): twp_flag[3] = 'done' else: # reset the twp parameters - reset_twp_params(self) + reset_twp_params() msg = ("G68.2 P3: No recognised Q-Word found.") - log.debug(msg) + log.debug(' ' + msg) emccanon.CANON_ERROR(msg) yield INTERP_EXECUTE_FINISH # w/o this the error message is not displayed yield INTERP_EXIT # w/o this the error does not abort a running gcode program @@ -1249,39 +1123,39 @@ def g682(self, **words): log.debug("(x, y, z): %s", (x, y, z)) log.debug("(i, j, k): %s", (i, j, k)) log.debug("(i1, j1, k1): %s", (i1, j1, k1)) - # build unit vector defining tool-x direction + # build unit vector defining x-vector direction twp_vect_x = [i-x, j-y, k-z] twp_vect_x = twp_vect_x / np.linalg.norm(twp_vect_x) twp_vect_z = [i1, j1, k1] twp_vect_z = twp_vect_z / np.linalg.norm(twp_vect_z) orth = np.dot(twp_vect_x, twp_vect_z) - log.debug("orth check: %s", orth) + log.debug(" orth check: %s", orth) # the two vectors must be orthogonal - if orth != 0: - reset_twp_params(self) + if orth > 0.001: + reset_twp_params() msg = ("G68.2 P3: Vectors are not orthogonal.") - log.debug(msg) + log.debug(' ' + msg) emccanon.CANON_ERROR(msg) yield INTERP_EXECUTE_FINISH # w/o this the error message is not displayed yield INTERP_EXIT # w/o this the error does not abort a running gcode program return INTERP_ERROR - # we can use the cross product to calculate the tool-y vector + # we can use the cross product to calculate the y vector twp_vect_y = np.cross(twp_vect_z, twp_vect_x) - log.debug("G68.2 P3: twp_vect_y %s",twp_vect_y) - # build the rotation matrix of the twp_matrix from the calculated tool-vectors + log.debug(" G68.2 P3: twp_vect_y %s",twp_vect_y) + # build the rotation matrix of the twp_matrix from the calculated vectors # first stack the vectors (lists) and then flip diagonally (transpose) # so the vectors are now vertical twp_vect_rotation_t = np.vstack((twp_vect_x, twp_vect_y)) twp_vect_rotation_t = np.vstack((twp_vect_rotation_t, twp_vect_z)) twp_vect_rotation = np.transpose(twp_vect_rotation_t) - log.debug("G68.2 P3: Built twp-rotation-matrix: \n%s", twp_vect_rotation) - # we use xzx-euler rotation to create the rotation matrix for the requested origin rotation + log.debug(" G68.2 P3: Built twp-rotation-matrix: \n%s", twp_vect_rotation) + # create the rotation matrix for the requested origin rotation try: - twp_origin_rotation = twp_calc_euler_rot_matrix(0, r, 0, '131') - except Exception as e: - log.info('G68.2 P3: twp_origin_rotation failed, %s', e) - log.debug('G68.2 P3: Twp-origin-rotation-matrix \n%s',twp_origin_rotation) + twp_origin_rotation = kins_calc_twp_origin_rot_matrix(r) + except Exception as error: + log.error('remap_func: kins_calc_twp_origin_rot_matrix failed, %s', error) + log.debug(' G68.2 P3: Twp-origin-rotation-matrix \n%s',twp_origin_rotation) # calculate the total twp_rotation using matrix multiplication twp_rotation = np.asmatrix(twp_origin_rotation) * np.asmatrix(twp_vect_rotation) # add the origin translation on the right @@ -1291,41 +1165,45 @@ def g682(self, **words): twp_row_4 = [0,0,0,1] twp_matrix = np.vstack((twp_matrix, twp_row_4)) twp_matrix = np.asmatrix(twp_matrix) - log.debug("G68.2 P3: Built twp-transformation-matrix: \n%s", twp_matrix) + log.debug(" G68.2 P3: Built twp-transformation-matrix: \n%s", twp_matrix) + + # TODO implement G68.2 P4 as outlined in the fanuc twp pdf (the exact meaning of which is unclear to me) else: # reset the twp parameters - reset_twp_params(self) + reset_twp_params() msg = ("G68.2: No recognised P-Word found.") - log.debug(msg) + log.debug(' ' + msg) emccanon.CANON_ERROR(msg) yield INTERP_EXECUTE_FINISH # w/o this the error message is not displayed yield INTERP_EXIT # w/o this the error does not abort a running gcode program return INTERP_ERROR - log.debug("G68.2: twp_flag: %s", twp_flag) - log.debug("G68.2: calls required: %s", twp_flag.count('done')) - log.debug("G68.2: number of calls made: %s", twp_flag.count('done')) + log.debug(" G68.2: twp_flag: %s", twp_flag) + log.debug(" G68.2: calls required: %s", twp_flag.count('done')) + log.debug(" G68.2: number of calls made: %s", twp_flag.count('done')) if twp_flag.count('done') == twp_flag[1]: - log.info('G68.2: requested rotation: %s', radians(r)) - log.info("G68.2: twp-tranformation-matrix: \n%s",twp_matrix) + log.info(' G68.2: requested rotation (degrees): %s', degrees(r)) + log.info(" G68.2: twp-tranformation-matrix: \n%s",twp_matrix) twp_origin = [twp_matrix[0,3],twp_matrix[1,3],twp_matrix[2,3]] - log.info("G68.2: twp origin: %s", twp_origin) + log.info(" G68.2: twp origin: %s", twp_origin) twp_vect_x = [twp_matrix[0,0],twp_matrix[1,0],twp_matrix[2,0]] - log.info("G68.2: twp vector-x: %s", twp_vect_x) + log.info(" G68.2: twp vector-x: %s", twp_vect_x) twp_vect_z = [twp_matrix[0,2],twp_matrix[1,2],twp_matrix[2,2]] - log.info("G68.2: twp vector-z: %s", twp_vect_z) + log.info(" G68.2: twp vector-z: %s", twp_vect_z) # set twp-state to 'defined' (1) self.execute("M68 E2 Q1") yield INTERP_EXECUTE_FINISH - gui_update_twp(self) + gui_update_twp() return INTERP_OK + # incremental definition of a virtual work-plane (twp) using different methods set by the 'p'-word def g684(self, **words): - global twp_matrix, pre_rot, twp_flag, twp_build_params, saved_work_offset_number, saved_work_offset + log.debug('Entering: %s', sys._getframe( ).f_code.co_name) + global twp_matrix, virtual_rot, twp_flag, twp_build_params, saved_work_offset_number, saved_work_offset if self.task == 0: # ignore the preview interpreter yield INTERP_EXECUTE_FINISH @@ -1339,9 +1217,9 @@ def g684(self, **words): if not hal.get_value(twp_is_active): # ie there is currently no TWP defined # reset the twp parameters - reset_twp_params(self) + reset_twp_params() msg = ("G68.4: No TWP active to increment from. Run G68.2 or G68.3 first.") - log.debug(msg) + log.debug(' ' + msg) emccanon.CANON_ERROR(msg) yield INTERP_EXECUTE_FINISH # w/o this the error message is not displayed yield INTERP_EXIT # w/o this the error does not abort a running gcode program @@ -1352,18 +1230,16 @@ def g684(self, **words): # Must be in one of the dedicated offset systems for TWP if False: #n < 6: # reset the twp parameters - reset_twp_params(self) + reset_twp_params() msg = ("G68.4 ERROR: Must be in G59, G59.x to increment TWP.") - log.debug(msg) + log.debug(' ' + msg) emccanon.CANON_ERROR(msg) yield INTERP_EXECUTE_FINISH # w/o this the error message is not displayed yield INTERP_EXIT # w/o this the error does not abort a running gcode program return INTERP_ERROR - # store the current TWP to twp_matrix_current = np.matrix.copy(twp_matrix) - c = self.blocks[self.remap_level] p = c.p_number if c.p_flag else 0 @@ -1374,9 +1250,9 @@ def g684(self, **words): if q not in ['121','131','212','232','313','323']: # reset the twp parameters - reset_twp_params(self) + reset_twp_params() msg = ("G68.4 (P0): No recognised Q-Word found.") - log.debug(msg) + log.debug(' ' + msg) emccanon.CANON_ERROR(msg) yield INTERP_EXECUTE_FINISH # w/o this the error message is not displayed yield INTERP_EXIT # w/o this the error does not abort a running gcode program @@ -1386,21 +1262,24 @@ def g684(self, **words): x = c.x_number if c.x_flag else 0 y = c.y_number if c.y_flag else 0 z = c.z_number if c.z_flag else 0 - # parse the requested rotation of tool-x around the origin - r = c.r_number if c.r_flag else 0 - # parse requested euler angles - th1 = c.i_number if c.i_flag else 0 - th2 = c.j_number if c.j_flag else 0 - th3 = c.k_number if c.k_flag else 0 + # parse the requested xy-rotation around the origin + r = radians(c.r_number) if c.r_flag else 0 + # parse the requested euler rotation angles + th1 = radians(c.i_number) if c.i_flag else 0 + th2 = radians(c.j_number) if c.j_flag else 0 + th3 = radians(c.k_number) if c.k_flag else 0 # build the translation vector of the twp_matrix twp_origin = [[x], [y], [z]] - # we use xzx-euler rotation to create the rotation matrix for the requested origin rotation - twp_origin_rotation = twp_calc_euler_rot_matrix(0, r, 0, '131') - log.debug('G68.4 (P0): Twp_origin_rotation \n%s',twp_origin_rotation) + # create the rotation matrix for the requested origin rotation + try: + twp_origin_rotation = kins_calc_twp_origin_rot_matrix(r) + except Exception as error: + log.error('remap_func: kins_calc_twp_origin_rot_matrix failed, %s', error) + log.debug(' G68.4 (P0): Twp_origin_rotation \n%s',twp_origin_rotation) # build the rotation matrix for the requested euler rotation - twp_euler_rotation = twp_calc_euler_rot_matrix(th1, th2, th3, q) - log.debug('G68.4 (P0): Twp_euler_rotation \n%s',twp_euler_rotation) + twp_euler_rotation = calc_euler_rot_matrix(th1, th2, th3, q) + log.debug(' G68.4 (P0): Twp_euler_rotation \n%s',twp_euler_rotation) # calculate the total twp_rotation using matrix multiplication twp_rotation = np.asmatrix(twp_origin_rotation) * np.asmatrix(twp_euler_rotation) # combine rotation and translation and form the 4x4 twp-transformation matrix @@ -1418,9 +1297,9 @@ def g684(self, **words): if q not in ['123','132','213','231','312','321']: # reset the twp parameters - reset_twp_params(self) + reset_twp_params() msg = ("G68.4 P1: No recognised Q-Word found.") - log.debug(msg) + log.debug(' ' + msg) emccanon.CANON_ERROR(msg) yield INTERP_EXECUTE_FINISH # w/o this the error message is not displayed yield INTERP_EXIT # w/o this the error does not abort a running gcode program @@ -1430,21 +1309,24 @@ def g684(self, **words): x = c.x_number if c.x_flag else 0 y = c.y_number if c.y_flag else 0 z = c.z_number if c.z_flag else 0 - # parse the requested rotation of tool-x around the origin - r = c.r_number if c.r_flag else 0 + # parse the requested xy-rotation around the origin + r = radians(c.r_number) if c.r_flag else 0 # parse the requested euler rotation angles - th1 = c.i_number if c.i_flag else 0 - th2 = c.j_number if c.j_flag else 0 - th3 = c.k_number if c.k_flag else 0 + th1 = radians(c.i_number) if c.i_flag else 0 + th2 = radians(c.j_number) if c.j_flag else 0 + th3 = radians(c.k_number) if c.k_flag else 0 # build the translation vector of the twp_matrix twp_origin = [[x], [y], [z]] - # we use xzx-euler rotation to create the rotation matrix for the requested origin rotation - twp_origin_rotation = twp_calc_euler_rot_matrix(0, r, 0, '131') - log.debug('G68.4 P1: Twp_origin_rotation \n%s',twp_origin_rotation) + # create the rotation matrix for the requested origin rotation + try: + twp_origin_rotation = kins_calc_twp_origin_rot_matrix(r) + except Exception as error: + log.error('remap_func: kins_calc_twp_origin_rot_matrix failed, %s', error) + log.debug(' G68.4 P1: Twp_origin_rotation \n%s',twp_origin_rotation) # build the rotation matrix for the requested euler rotation - twp_euler_rotation = twp_calc_euler_rot_matrix(th1, th2, th3, q) - log.debug('G68.4 P1: Twp_euler_rotation \n%s',twp_euler_rotation) + twp_euler_rotation = calc_euler_rot_matrix(th1, th2, th3, q) + log.debug(' G68.4 P1: Twp_euler_rotation \n%s',twp_euler_rotation) # calculate the total twp_rotation using matrix multiplication twp_rotation = np.asmatrix(twp_origin_rotation) * np.asmatrix(twp_euler_rotation) # combine rotation and translation and form the 4x4 twp-transformation matrix @@ -1456,21 +1338,28 @@ def g684(self, **words): twp_flag[2] = 'done' elif p == 2: # twp defined py 3 points on the plane + # TODO implement operator errors as outlined in the twp README + #- G68.2 P2 (Q0),Q1,Q2,Q3 commands are not entered consecutively + #- two to the points entered in Q1,Q2,Q3 are identical + #- all three points entered in Q1,Q2,Q3 are on a line + #- the distance between a line defined by any two points entered in (Q1,Q2,Q3) and + #the remaining point is less than 10mm or 0.5inch (just some arbitrary values for now) + # if this is the first call for this mode reset the twp_flag flag if not twp_flag: twp_flag = [int(p), 4 , 'empty', 'empty', 'empty', 'empty'] # four calls needed twp_build_params = {'q0':[], 'q1':[], 'q2':[], 'q3':[]} # Point 1: defines the origin of the twp - # Point 2: direction from P1 to P2 defines the positive x direction on the twp (tool-x) - # Point 3: defines the positive y side and with P1 and P2 defines the xy work plane (tool-z) + # Point 2: direction from P1 to P2 defines the positive x direction on the twp (x-vector) + # Point 3: defines the positive y side and with P1 and P2 defines the xy work plane (z-vector) q = int(c.q_number if c.q_flag else 0) # this mode needs four calls to fill all required parameters if q == 0: # define new origin and rotation x = c.x_number if c.x_flag else 0 y = c.y_number if c.y_flag else 0 z = c.z_number if c.z_flag else 0 - # parse the requested rotation of tool-x around the origin - r = c.r_number if c.r_flag else 0 + # parse the requested xy-rotation around the origin + r = radians(c.r_number) if c.r_flag else 0 twp_build_params['q0'] = [x,y,z,r] twp_flag[2] = 'done' elif q == 1: # define point 1 @@ -1493,9 +1382,9 @@ def g684(self, **words): twp_flag[5] = 'done' else: # reset the twp parameters - reset_twp_params(self) + reset_twp_params() msg = ("G68.4 P2: No recognised Q-Word found.") - log.debug(msg) + log.debug(' ' + msg) emccanon.CANON_ERROR(msg) yield INTERP_EXECUTE_FINISH # w/o this the error message is not displayed yield INTERP_EXIT # w/o this the error does not abort a running gcode program @@ -1509,38 +1398,38 @@ def g684(self, **words): p1 = twp_build_params['q1'][0:3] p2 = twp_build_params['q2'] p3 = twp_build_params['q3'] - log.debug("G68.4 P2: Point 1: %s",p1) - log.debug("G68.4 P2: Point 2: %s",p2) - log.debug("G68.4 P2: Point 3: %s",p3) + log.debug(" G68.4 P2: Point 1: %s",p1) + log.debug(" G68.4 P2: Point 2: %s",p2) + log.debug(" G68.4 P2: Point 3: %s",p3) # build vectors x:P1->P2 and v2:P1->P3 twp_vect_x = [p2[0]-p1[0], p2[1]-p1[1], p2[2]-p1[2]] - log.debug("G68.4 P2: Twp_vect_x: \n%s",twp_vect_x) + log.debug(" G68.4 P2: Twp_vect_x: \n%s",twp_vect_x) v2 = [p3[0]-p1[0], p3[1]-p1[1], p3[2]-p1[2]] - log.debug("G68.4 P2: (v2) %s", v2) + log.debug(" G68.4 P2: (v2) %s", v2) # normalize the two vectors twp_vect_x = twp_vect_x / np.linalg.norm(twp_vect_x) v2 = v2 / np.linalg.norm(v2) - # we can use the cross product to calculate the tool-z vector + # we can use the cross product to calculate the z-vector vector # note: if P3 is on the right side of the vector P1->P2 - # then the tool-z will be below the twp (ie tool-z will be downwards) + # then the z-vector will be below the twp (ie z-vector will be downwards) twp_vect_z = np.cross(twp_vect_x , v2) - log.debug("G68.4 P2: Twp_vect_z %s",twp_vect_z) - # we can use the cross product to calculate the tool-y vector + log.debug(" G68.4 P2: Twp_vect_z %s",twp_vect_z) + # we can use the cross product to calculate the y vector twp_vect_y = np.cross(twp_vect_z, twp_vect_x) - log.debug("G68.4 P2: Twp_vect_y %s",twp_vect_y) - # build the rotation matrix of the twp_matrix from the calculated tool-vectors + log.debug(" G68.4 P2: Twp_vect_y %s",twp_vect_y) + # build the rotation matrix of the twp_matrix from the calculated vectors # first stack the vectors (lists) and then flip diagonally (transpose) # so the vectors are now vertical twp_vect_rotation_t = np.vstack((twp_vect_x, twp_vect_y)) twp_vect_rotation_t = np.vstack((twp_vect_rotation_t, twp_vect_z)) twp_vect_rotation = np.transpose(twp_vect_rotation_t) - log.debug("G68.4 P2: Built the twp-rotation-matrix: \n%s", twp_vect_rotation) - # we use xzx-euler rotation to create the rotation matrix for the requested origin rotation + log.debug(" G68.4 P2: Built the twp-rotation-matrix: \n%s", twp_vect_rotation) + # create the rotation matrix for the requested origin rotation try: - twp_origin_rotation = twp_calc_euler_rot_matrix(0, r, 0, '131') - except Exception as e: - log.debug('G68.4 P2: twp_origin_rotation failed ', e) - log.debug('G68.4 P2: Twp-origin-rotation-matrix \n%s',twp_origin_rotation) + twp_origin_rotation = kins_calc_twp_origin_rot_matrix(r) + except Exception as error: + log.error('remap_func: kins_calc_twp_origin_rot_matrix failed, %s', error) + log.debug(' G68.4 P2: Twp-origin-rotation-matrix \n%s',twp_origin_rotation) # calculate the total twp_rotation using matrix multiplication twp_rotation = np.asmatrix(twp_origin_rotation) * np.asmatrix(twp_vect_rotation) # add the origin translation on the right @@ -1549,9 +1438,13 @@ def g684(self, **words): twp_row_4 = [0,0,0,1] twp_matrix = np.vstack((twp_matrix, twp_row_4)) twp_matrix = np.asmatrix(twp_matrix) - log.debug("G68.4 P2: Built twp-transformation-matrix: \n%s", twp_matrix) + log.debug(" G68.4 P2: Built twp-transformation-matrix: \n%s", twp_matrix) - elif p == 3: # two vectors (vector 1 defines the tool-x and vector 2 defines the tool-z) + elif p == 3: # two vectors (vector 1 defines the x-vector and vector 2 defines the z-vector) + # TODO implement operator errors as outlined in the twp README + #- G68.2 P3 Q1 and Q2 commands are not entered consecutively + #- one of the vectors is the zero vector + #- the enclosed angle between the 1. and 2. vector is <85° or >95° (re fanuc twp pdf) q = int(c.q_number if c.q_flag else 0) # if this is the first call for this mode reset the twp_flag flag if not twp_flag: @@ -1561,8 +1454,8 @@ def g684(self, **words): x = c.x_number if c.x_flag else 0 y = c.y_number if c.y_flag else 0 z = c.z_number if c.z_flag else 0 - # parse the requested rotation of tool-x around the origin - r = c.r_number if c.r_flag else 0 + # parse the requested xy-rotation around the origin + r = radians(c.r_number) if c.r_flag else 0 # first vector (direction of x in the twp) i = c.i_number if c.i_flag else 0 j = c.j_number if c.j_flag else 0 @@ -1577,9 +1470,9 @@ def g684(self, **words): twp_flag[3] = 'done' else: # reset the twp parameters - reset_twp_params(self) + reset_twp_params() msg = ("G68.4 P3: No recognised Q-Word found.") - log.debug(msg) + log.debug(' ' + msg) emccanon.CANON_ERROR(msg) yield INTERP_EXECUTE_FINISH # w/o this the error message is not displayed yield INTERP_EXIT # w/o this the error does not abort a running gcode program @@ -1594,40 +1487,43 @@ def g684(self, **words): log.debug("(x, y, z) %s", (x, y, z)) log.debug("(i, j, k) %s", (i, j, k)) log.debug("(i1, j1, k1) %s", (i1, j1, k1)) - # build unit vector defining tool-x direction + # build unit vector defining x-vector direction twp_vect_x = [i-x, j-y, k-z] twp_vect_x = twp_vect_x / np.linalg.norm(twp_vect_x) twp_vect_z = [i1, j1, k1] twp_vect_z = twp_vect_z / np.linalg.norm(twp_vect_z) orth = np.dot(twp_vect_x, twp_vect_z) - log.debug("orth check: %s", orth) + log.debug(" orth check: %s", orth) # the two vectors must be orthogonal if orth != 0: # reset the twp parameters - reset_twp_params(self) + reset_twp_params() ## reset the parameter values #twp_flag = [int(p), 2 , 'empty', 'empty'] # two calls needed #twp_build_params = {'q0':[], 'q1':[]} msg = ("G68.4 P3: Vectors are not orthogonal.") - log.debug(msg) + log.debug(' ' + msg) emccanon.CANON_ERROR(msg) yield INTERP_EXECUTE_FINISH # w/o this the error message is not displayed yield INTERP_EXIT # w/o this the error does not abort a running gcode program return INTERP_ERROR - # we can use the cross product to calculate the tool-y vector + # we can use the cross product to calculate the y vector twp_vect_y = np.cross(twp_vect_z, twp_vect_x) - log.debug("G68.4 P3: twp_vect_y %s",twp_vect_y) - # build the rotation matrix of the twp_matrix from the calculated tool-vectors + log.debug(" G68.4 P3: twp_vect_y %s",twp_vect_y) + # build the rotation matrix of the twp_matrix from the calculated vectors # first stack the vectors (lists) and then flip diagonally (transpose) # so the vectors are now vertical twp_vect_rotation_t = np.vstack((twp_vect_x, twp_vect_y)) twp_vect_rotation_t = np.vstack((twp_vect_rotation_t, twp_vect_z)) twp_vect_rotation = np.transpose(twp_vect_rotation_t) - log.debug("G68.4 P3: Built twp-rotation-matrix: \n%s", twp_vect_rotation) - # we use xzx-euler rotation to create the rotation matrix for the requested origin rotation - twp_origin_rotation = twp_calc_euler_rot_matrix(0, r, 0, '131') - log.debug('G68.4 P3: Twp-origin-rotation-matrix \n%s',twp_origin_rotation) + log.debug(" G68.4 P3: Built twp-rotation-matrix: \n%s", twp_vect_rotation) + # create the rotation matrix for the requested origin rotation + try: + twp_origin_rotation = kins_calc_twp_origin_rot_matrix(r) + except Exception as error: + log.error('remap_func: kins_calc_twp_origin_rot_matrix failed, %s', error) + log.debug(' G68.4 P3: Twp-origin-rotation-matrix \n%s',twp_origin_rotation) # calculate the total twp_rotation using matrix multiplication twp_rotation = np.asmatrix(twp_origin_rotation) * np.asmatrix(twp_vect_rotation) # add the origin translation on the right @@ -1637,40 +1533,42 @@ def g684(self, **words): twp_row_4 = [0,0,0,1] twp_matrix = np.vstack((twp_matrix, twp_row_4)) twp_matrix = np.asmatrix(twp_matrix) - log.debug("G68.4 P3: Built twp-transformation-matrix: \n%s", twp_matrix) + log.debug(" G68.4 P3: Built twp-transformation-matrix: \n%s", twp_matrix) + + # TODO implement G68.4 P4 as outlined in the fanuc twp pdf (the exact meaning of which is unclear to me) else: # reset the twp parameters - reset_twp_params(self) + reset_twp_params() msg = ("G68.4: No recognised P-Word found.") - log.debug(msg) + log.debug(' ' + msg) emccanon.CANON_ERROR(msg) yield INTERP_EXECUTE_FINISH # w/o this the error message is not displayed yield INTERP_EXIT # w/o this the error does not abort a running gcode program return INTERP_ERROR - log.debug("G68.4: twp_flag: %s", twp_flag) - log.debug("G68.4: calls required: %s", twp_flag.count('done')) - log.debug("G68.4: number of calls made: %s", twp_flag.count('done')) + log.debug(" G68.4: twp_flag: %s", twp_flag) + log.debug(" G68.4: calls required: %s", twp_flag.count('done')) + log.debug(" G68.4: number of calls made: %s", twp_flag.count('done')) if twp_flag.count('done') == twp_flag[1]: - log.info('G68.4: requested rotation %s', radians(r)) - log.info("G68.4: twp_matrix_current: \n%s", twp_matrix_current) - log.info("G68.4: incremental twp_matrix requested: \n%s",twp_matrix) - log.info("G68.4: calculating new twp_matrix...") + log.info(' G68.4: requested rotation (degrees) %s', degrees(r)) + log.info(" G68.4: twp_matrix_current: \n%s", twp_matrix_current) + log.info(" G68.4: incremental twp_matrix requested: \n%s",twp_matrix) + log.info(" G68.4: calculating new twp_matrix...") twp_matrix_new = twp_matrix_current * twp_matrix - log.info("G68.4: twp_matrix_new: \n%s",twp_matrix_new) + log.info(" G68.4: twp_matrix_new: \n%s",twp_matrix_new) twp_origin = [twp_matrix[0,3],twp_matrix[1,3],twp_matrix[2,3]] - log.info("G68.4: twp origin: %s", twp_origin) + log.info(" G68.4: twp origin: %s", twp_origin) twp_vect_x = [twp_matrix[0,0],twp_matrix[1,0],twp_matrix[2,0]] - log.info("G68.4: twp vector-x: %s", twp_vect_x) + log.info(" G68.4: twp vector-x: %s", twp_vect_x) twp_vect_z = [twp_matrix[0,2],twp_matrix[1,2],twp_matrix[2,2]] - log.info("G68.4: twp vector-z: %s", twp_vect_z) - log.info("G68.4: incremented twp_matrix: \n%s", twp_matrix_new) + log.info(" G68.4: twp vector-z: %s", twp_vect_z) + log.info(" G68.4: incremented twp_matrix: \n%s", twp_matrix_new) twp_matrix = twp_matrix_new # set twp-state to 'defined' (1) self.execute("M68 E2 Q1") yield INTERP_EXECUTE_FINISH - gui_update_twp(self) + gui_update_twp() return INTERP_OK diff --git a/configs/sim/axis/vismach/5axis/table-rotary_spindle-rotary-nutating/xyzacb-trsrn_twp/remap_funcs_twp.py b/configs/sim/axis/vismach/5axis/table-rotary_spindle-rotary-nutating/xyzacb-trsrn_twp/remap_funcs_twp.py new file mode 100644 index 00000000000..7c2b498b7b6 --- /dev/null +++ b/configs/sim/axis/vismach/5axis/table-rotary_spindle-rotary-nutating/xyzacb-trsrn_twp/remap_funcs_twp.py @@ -0,0 +1,347 @@ +# This is imported by remap.py and contains twp functionality specific to the +# xyzacb-trsrn config, a machine with primary rotary C and secondary rotary B +# +# +# Copyright ()c) 2025 David Mueller +# +# This program is free software: you can redistribute it and/or modify +# it under the terms of the GNU General Public License as published by +# the Free Software Foundation, either version 2 of the License, or +# (at your option) any later version. +# +# This program is distributed in the hope that it will be useful, +# but WITHOUT ANY WARRANTY; without even the implied warranty of +# MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +# GNU General Public License for more details. +# +# +import sys +import numpy as np +from math import sin,cos,tan,asin,acos,atan,atan2,sqrt,pi,degrees,radians,fabs +import hal + + +# set up parsing of the inifile +import os +import linuxcnc +# get the path for the ini file used to start this config +inifile = os.environ.get("INI_FILE_NAME") +# instantiate the LinuxCNC ini-parser +config = linuxcnc.ini(inifile) + +## ROTARY JOINT LETTERS +# primary joint +joint_letter_primary = config.getstring('TWP', 'PRIMARY', fallback="").capitalize() +# secondary joint (ie the one closer to the tool) +joint_letter_secondary = config.getstring('TWP', 'SECONDARY', fallback="").capitalize() +# get the MIN/MAX limits of the respective rotary joint letters +category = 'AXIS_' + joint_letter_primary +primary_min_limit = config.getreal(category, 'MIN_LIMIT', fallback=0.0) +primary_max_limit = config.getreal(category, 'MAX_LIMIT', fallback=0.0) +category = 'AXIS_' + joint_letter_secondary +secondary_min_limit = config.getreal(category, 'MIN_LIMIT', fallback=0.0) +secondary_max_limit = config.getreal(category, 'MAX_LIMIT', fallback=0.0) + +## CONNECTIONS TO THE KINEMATIC COMPONENT +# the module is named for the kinematics, its hal pins carry a "_kins" suffix +kins_comp = config.getstring('KINS', 'KINEMATICS', fallback="") + '_kins' +kins_nutation_angle = kins_comp + '.nut-angle' +kins_virtual_rotation = kins_comp + '.pre-rot' +kins_primary_rotation = kins_comp + '.primary-angle' +kins_secondary_rotation = kins_comp + '.secondary-angle' + + +# defines the kinematic model for (world <-> tool) coordinates of the machine at hand +# returns 4x4 transformation matrix for given angles and 4x4 input matrix +# NOTE: these matrices must be the same as the ones used to derive the kinematic model +def kins_calc_transformation_matrix(theta_1, theta_2, virtual_rot, matrix_in, direction='fwd'): # expects radians + global kins_nutation_angle + T_in = matrix_in + ## Define 4x4 transformation for virtual rotation around tool-z to orient tool-x and -y + Stc = sin(virtual_rot) + Ctc = cos(virtual_rot) + Rtc=np.matrix([[ Ctc, -Stc, 0, 0], + [ Stc, Ctc, 0, 0], + [ 0 , 0 , 1, 0], + [ 0, 0 , 0, 1]]) + + ## Define 4x4 transformation for the primary joint + # get the basic 3x3 rotation matrix (returns array) + Rp = Rz(theta_1) + # add fourth column on the right + Rp = np.hstack((Rp, [[0],[0],[0]])) + # expand to 4x4 array and make into a matrix + row_4 = [0,0,0,1] + Rp = np.vstack((Rp, row_4)) + Rp = np.asmatrix(Rp) + + ## Define 4x4 transformation matrix for the secondary joint + # get the basic 3x3 rotation matrix (returns array) + Rs = Ry(theta_2) + # add fourth column on the right + Rs = np.hstack((Rs, [[0],[0],[0]])) + # expand to 4x4 array and make into a matrix + row_4 = [0,0,0,1] + Rs = np.vstack((Rs, row_4)) + Rs = np.asmatrix(Rs) + + # Additional definitions for nutating joint + v = radians(hal.get_value(kins_nutation_angle)) + Sv = sin(v) + Cv = cos(v) + Ss = sin(theta_2) + Cs = cos(theta_2) + r = Cs + Sv*Sv*(1-Cs) + s = Cs + Cv*Cv*(1-Cs) + t = Sv*Cv*(1-Cs) + # define rotation matrix for the secondary joint + Rs=np.matrix([[ Cs, -Cv*Ss, Sv*Ss, 0], + [ Cv*Ss, r, t, 0], + [ -Sv*Ss, t, s, 0], + [ 0, 0, 0, 1]]) + + # calculate the transformation matrix for the forward tool kinematic + matrix_tool_fwd = np.transpose(Rtc)*np.transpose(Rs)*np.transpose(Rp)*T_in + # calculate the transformation matrix for the inverse tool kinematic + matrix_tool_inv = Rp*Rs*Rtc*T_in + if direction == 'fwd': + #log.debug("matrix tool fwd: \n", matrix_tool_fwd) + #log.debug("inv would have been: \n", matrix_tool_inv) + return matrix_tool_fwd + elif direction == 'inv': + #log.debug("matrix tool inv: \n", matrix_tool_inv) + #log.debug("fwd would have been: \n", matrix_tool_fwd) + return matrix_tool_inv + else: + return 0 + + +# calculates the primary joint position for a given tool-vector +# Note: this uses functions derived from the custom kinematic +def kins_calc_primary(log, z_vector_req, x_vector_req, theta_2_list=[]): + global primary_min_limit, primary_max_limit + global kins_nutation_angle + epsilon = 0.000001 + theta_1_list=[] + (Kzx, Kzy, Kzz) = (z_vector_req[0], z_vector_req[1], z_vector_req[2]) + # This kinmatic has infinite results for the vertical tool orientation + # so we explicitly define the angles for that specific case + if Kzz > 1 - epsilon: + return [0] + else: + v = radians(hal.get_value(kins_nutation_angle)) + Sv = sin(v) + Cv = cos(v) + for i in range(len(theta_2_list)): + theta_2 = theta_2_list[i] + Ss = sin(theta_2) + Cs = cos(theta_2) + t = Sv*Cv*(1-Cs) + p = Sv * Ss + theta_1 = asin((p*Kzy - t*Kzx)/(t*t + p*p)) + # since we are using asin() we really have two solutions theta_1 and pi-theta_2 + for theta in [theta_1, transform_to_pipi(pi - theta_1)]: + log.debug(f' Checking possible primary angle {degrees(theta):.4f}° for limit violations.') + if degrees(theta) > primary_min_limit and degrees(theta) < primary_max_limit: + theta_1_list.append(theta) + return theta_1_list # returns radians + + +# calculates the secondary joint position for a given tool-vector +# secondary being the joint closest to the tool +# Note: this uses functions derived from the custom kinematic +def kins_calc_secondary(log, z_vector_req, x_vector_req): + global secondary_min_limit, secondary_max_limit + global kins_nutation_angle + epsilon = 0.000001 + theta_2_list=[] + (Kzx, Kzy, Kzz) = (z_vector_req[0], z_vector_req[1], z_vector_req[2]) + v = radians(hal.get_value(kins_nutation_angle)) + Sv = sin(v) + Cv = cos(v) + # This kinmatic has infinite results for the vertical tool orientation + # so we explicitly define the angles for that specific case + if Kzz > 1 - epsilon: + theta_2 = 0 + # This kinematics nutation angle restricts the negative range of Kzz + elif Kzz < 2*Cv*Cv - 1: + log.error('remap_funcs: Requested orientation not reachable with the current nutation angle.') + return None + else: + theta_2 = acos((Kzz - Cv*Cv)/(1 - Cv*Cv)) + for theta in [theta_2, -theta_2]: + log.debug(f' Checking possible secondary angle {degrees(theta):.4f}° for limit violations.') + if degrees(theta) > secondary_min_limit and degrees(theta) < secondary_max_limit: + theta_2_list.append(theta) + return theta_2_list # returns radians + + +# define the order in which the joint angles need to be calculated +def kins_calc_possible_joint_angles(log, z_vector_req, x_vector_req): + try: + theta_2_calcd = kins_calc_secondary(log, z_vector_req, x_vector_req) + except Exception as error: + log.error('kins_calc_jnt_angles, kins_calc_secondary, %s', error) + if theta_2_calcd == None: + return (None, None) + try: + theta_1_calcd = kins_calc_primary(log, z_vector_req, x_vector_req, theta_2_calcd) + except Exception as error: + log.error('kins_calc_jnt_angles, kins_calc_primary, %s', error) + return (theta_1_calcd, theta_2_calcd) # returns radians + + +# calculate the transformed work offset used after 53.n +def kins_calc_transformed_work_offset(current_offset, twp_offset, theta_1, theta_2, virtual_rot): + P = matrix_to_point(kins_calc_transformation_matrix(theta_1, theta_2, virtual_rot, point_to_matrix(current_offset))) + # calculate the twp offset in transformed-coordinates + Q = matrix_to_point(kins_calc_transformation_matrix(theta_1, theta_2, virtual_rot, point_to_matrix(twp_offset))) + transformed_offset = (P[0]+Q[0], P[1]+Q[1], P[2]+Q[2]) + return transformed_offset + +# pass required values to the kinematics component +# the module takes the virtual rotation in radians and the two joint angles in +# degrees, the same units the joints themselves are in +def kins_set_values(theta_1, theta_2, virtual_rot): # expects radians + hal.set_p(kins_virtual_rotation, str(virtual_rot)) + hal.set_p(kins_primary_rotation, str(degrees(theta_1))) + hal.set_p(kins_secondary_rotation, str(degrees(theta_2))) + + +# returns angle required to orient the x-vector parallel to the machine-xy plane +# for given machine joint position angles. +# For G68.3 this is the default tool-x direction +# NOTE: this uses formulas derived from the transformation matrix in the inverse tool kinematic +# TODO I don't actually know if this is the correct x orientation for G68.3' +def kins_calc_virtual_rot_for_g683(theta_1, theta_2): + # The idea is that the oriented x-vector is parallel to the machine xy-plane when the + # z component of the x-direction vector is equal to zero + # Mathematically we take the symbolic formula found in row 3, column 1 of the transformation + # matrix from the inverse tool-kinematics, equal that to zero and solve for 'tc'. + # this makes the x-vector of the oriented coords horizontal and the user can set the + # rotation from there using g68.3 r + global kins_nutation_angle + v = radians(hal.get_value(kins_nutation_angle)) + Cv = cos(v) + Sv = sin(v) + Cs = cos(theta_2) + Ss = sin(theta_2) + Cp = cos(theta_1) + Sp = sin(theta_1) + t = Sv*Cv*(1-Cs) + tc = atan2((Sv*Ss),t) + # note: rotation is done using a halpin that feeds into the kinematic component and the + # vismach model. In contrast to a gcode command where 'c' refers to a physical machine joint) + return tc # returns radians + + +# return the start values required to calculate the virtual rotation +def kins_calc_virtual_rot_get_values(x_vector_requested, z_vector_requested, twp_matrix): + x_vector_requested = [twp_matrix[0,0],twp_matrix[1,0],twp_matrix[2,0]] + z_vector_requested = [twp_matrix[0,2],twp_matrix[1,2],twp_matrix[2,2]] + matrix_in = np.asmatrix(np.identity(4)) + direction = 'inv' + return (x_vector_requested, z_vector_requested, matrix_in, direction) + + +# If the operator has requested a rotation by passing an R word in the 68.n command we need to +# create a rotation matrix that represents a rotation around the Z-axis of the TWP plane +def kins_calc_twp_origin_rot_matrix(r): # expects radians + # we use xzx-euler rotation to create the rotation matrix for the requested origin rotation + twp_origin_rot_matrix = calc_euler_rot_matrix(0, r, 0, '131') + + return twp_origin_rot_matrix + + +# This returns which transformation to use when checking calculated angles +# and when calculating the twp_matrix for G68.3 +def kins_calc_transformation_get_direction(): + return 'inv' + + +# returns the pin name for the virtual rotation in the kinematics component +def kins_get_current_virtual_rot(): + current_virtual_rot = hal.get_value(kins_virtual_rotation) + return current_virtual_rot # returns radians + + + + + + + +# forms a 4x4 transformation matrix from a given 1x3 point vector [x,y,z] +def point_to_matrix(point): + # start with a 4x4 identity matrix and add the point vector to the 4th column + matrix = np.identity(4) + [matrix[0,3], matrix[1,3], matrix[2,3]] = point + matrix = np.asmatrix(matrix) + return matrix + +# extracts the point vector form a given 4x4 transformation matrix +def matrix_to_point(matrix): + point = (matrix[0,3],matrix[1,3],matrix[2,3]) + return point + + +# this is from 'mika-s.github.io' +# transforms a given angle to the interval of [-pi,pi] +def transform_to_pipi(input_angle): + def truncated_remainder(dividend, divisor): + divided_number = dividend / divisor + divided_number = -int(-divided_number) if divided_number < 0 else int(divided_number) + remainder = dividend - divisor * divided_number + return remainder + + revolutions = int((input_angle + np.sign(input_angle) * pi) / (2 * pi)) + p1 = truncated_remainder(input_angle + np.sign(input_angle) * pi, 2 * pi) + p2 = (np.sign(np.sign(input_angle) + + 2 * (np.sign(fabs((truncated_remainder(input_angle + pi, 2 * pi)) / (2 * pi))) - 1))) * pi + output_angle = p1 - p2 + return output_angle + + +# define the basic rotation matrices, used for euler twp modes +def Rx(th): + return np.array([[1, 0 , 0 ], + [0, cos(th), -sin(th)], + [0, sin(th), cos(th)]]) + +def Ry(th): + return np.array([[ cos(th), 0, sin(th)], + [ 0 , 1, 0 ], + [-sin(th), 0, cos(th)]]) + +def Rz(th): + return np.array([[cos(th), -sin(th), 0], + [sin(th), cos(th), 0], + [0 , 0 , 1]]) + + +# returns the rotation matrices for given order and angles +def calc_euler_rot_matrix(th1, th2, th3, order): + if order == '131': + matrix = np.dot(np.dot(Rx(th1), Rz(th2)), Rx(th3)) + elif order=='121': + matrix = np.dot(np.dot(Rx(th1), Ry(th2)), Rx(th3)) + elif order=='212': + matrix = np.dot(np.dot(Ry(th1), Rx(th2)), Ry(th3)) + elif order=='232': + matrix = np.dot(np.dot(Ry(th1), Rz(th2)), Ry(th3)) + elif order=='323': + matrix = np.dot(np.dot(Rz(th1), Ry(th2)), Rz(th3)) + elif order=='313': + matrix = np.dot(np.dot(Rz(th1), Rx(th2)), Rz(th3)) + elif order=='123': + matrix = np.dot(np.dot(Rx(th1), Ry(th2)), Rz(th3)) + elif order=='132': + matrix = np.dot(np.dot(Rx(th1), Rz(th2)), Ry(th3)) + elif order=='213': + matrix = np.dot(np.dot(Ry(th1), Rx(th2)), Rz(th3)) + elif order=='231': + matrix = np.dot(np.dot(Ry(th1), Rz(th2)), Rx(th3)) + elif order=='321': + matrix = np.dot(np.dot(Rz(th1), Ry(th2)), Rx(th3)) + elif order=='312': + matrix = np.dot(np.dot(Rz(th1), Rx(th2)), Ry(th3)) + return matrix diff --git a/configs/sim/axis/vismach/5axis/table-rotary_spindle-rotary-nutating/xyzacb-trsrn_twp/xyzacb-trsrn.ini b/configs/sim/axis/vismach/5axis/table-rotary_spindle-rotary-nutating/xyzacb-trsrn_twp/xyzacb-trsrn.ini index 06b9cd5d23f..2be585e6ef8 100644 --- a/configs/sim/axis/vismach/5axis/table-rotary_spindle-rotary-nutating/xyzacb-trsrn_twp/xyzacb-trsrn.ini +++ b/configs/sim/axis/vismach/5axis/table-rotary_spindle-rotary-nutating/xyzacb-trsrn_twp/xyzacb-trsrn.ini @@ -39,7 +39,7 @@ SUBROUTINE_PATH = ../remap_subs:../demos REMAP = G53.1 modalgroup=1 argspec=p ngc=g531remap REMAP = G53.3 modalgroup=1 argspec=pxyz ngc=g533remap REMAP = G53.6 modalgroup=1 argspec=p ngc=g536remap - REMAP = M530 modalgroup=10 python=g53x_core + REMAP = M530 modalgroup=10 python=g53n_core REMAP = G68.2 modalgroup=1 argspec=pqxyzijkr python=g682 REMAP = G68.3 modalgroup=1 argspec=xyzr python=g683 diff --git a/configs/sim/axis/vismach/5axis/table-rotary_spindle-rotary-nutating/xyzbca-trsrn_twp/remap_funcs_twp.py b/configs/sim/axis/vismach/5axis/table-rotary_spindle-rotary-nutating/xyzbca-trsrn_twp/remap_funcs_twp.py new file mode 100644 index 00000000000..b1629e7d012 --- /dev/null +++ b/configs/sim/axis/vismach/5axis/table-rotary_spindle-rotary-nutating/xyzbca-trsrn_twp/remap_funcs_twp.py @@ -0,0 +1,350 @@ +# This is imported by remap.py and contains twp functionality specific to the +# xyzbca-trsrn config, a machine with primary rotary C and secondary rotary A +# +# +# Copyright ()c) 2025 David Mueller +# +# This program is free software: you can redistribute it and/or modify +# it under the terms of the GNU General Public License as published by +# the Free Software Foundation, either version 2 of the License, or +# (at your option) any later version. +# +# This program is distributed in the hope that it will be useful, +# but WITHOUT ANY WARRANTY; without even the implied warranty of +# MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +# GNU General Public License for more details. +# +# +import sys +import numpy as np +from math import sin,cos,tan,asin,acos,atan,atan2,sqrt,pi,degrees,radians,fabs +import hal + + +# set up parsing of the inifile +import os +import linuxcnc +# get the path for the ini file used to start this config +inifile = os.environ.get("INI_FILE_NAME") +# instantiate the LinuxCNC ini-parser +config = linuxcnc.ini(inifile) + +## ROTARY JOINT LETTERS +# primary joint +joint_letter_primary = config.getstring('TWP', 'PRIMARY', fallback="").capitalize() +# secondary joint (ie the one closer to the tool) +joint_letter_secondary = config.getstring('TWP', 'SECONDARY', fallback="").capitalize() +# get the MIN/MAX limits of the respective rotary joint letters +category = 'AXIS_' + joint_letter_primary +primary_min_limit = config.getreal(category, 'MIN_LIMIT', fallback=0.0) +primary_max_limit = config.getreal(category, 'MAX_LIMIT', fallback=0.0) +category = 'AXIS_' + joint_letter_secondary +secondary_min_limit = config.getreal(category, 'MIN_LIMIT', fallback=0.0) +secondary_max_limit = config.getreal(category, 'MAX_LIMIT', fallback=0.0) + +## CONNECTIONS TO THE KINEMATIC COMPONENT +# the module is named for the kinematics, its hal pins carry a "_kins" suffix +kins_comp = config.getstring('KINS', 'KINEMATICS', fallback="") + '_kins' +kins_nutation_angle = kins_comp + '.nut-angle' +kins_virtual_rotation = kins_comp + '.pre-rot' +kins_primary_rotation = kins_comp + '.primary-angle' +kins_secondary_rotation = kins_comp + '.secondary-angle' + + +# defines the kinematic model for (world <-> tool) coordinates of the machine at hand +# returns 4x4 transformation matrix for given angles and 4x4 input matrix +# NOTE: these matrices must be the same as the ones used to derive the kinematic model +def kins_calc_transformation_matrix(theta_1, theta_2, virtual_rot, matrix_in, direction='fwd'): # expects radians + global kins_nutation_angle + T_in = matrix_in + ## Define 4x4 transformation for virtual rotation around tool-z to orient tool-x and -y + Stc = sin(virtual_rot) + Ctc = cos(virtual_rot) + Rtc=np.matrix([[ Ctc, -Stc, 0, 0], + [ Stc, Ctc, 0, 0], + [ 0 , 0 , 1, 0], + [ 0, 0 , 0, 1]]) + + ## Define 4x4 transformation for the primary joint + # get the basic 3x3 rotation matrix (returns array) + Rp = Rz(theta_1) + # add fourth column on the right + Rp = np.hstack((Rp, [[0],[0],[0]])) + # expand to 4x4 array and make into a matrix + row_4 = [0,0,0,1] + Rp = np.vstack((Rp, row_4)) + Rp = np.asmatrix(Rp) + + ## Define 4x4 transformation matrix for the secondary joint + # get the basic 3x3 rotation matrix (returns array) + Rs = Rx(theta_2) + # add fourth column on the right + Rs = np.hstack((Rs, [[0],[0],[0]])) + # expand to 4x4 array and make into a matrix + row_4 = [0,0,0,1] + Rs = np.vstack((Rs, row_4)) + Rs = np.asmatrix(Rs) + + # Additional definitions for nutating joint + v = radians(hal.get_value(kins_nutation_angle)) + Sv = sin(v) + Cv = cos(v) + Ss = sin(theta_2) + Cs = cos(theta_2) + r = Cs + Sv*Sv*(1-Cs) + s = Cs + Cv*Cv*(1-Cs) + t = Sv*Cv*(1-Cs) + # define rotation matrix for the secondary joint + Rs=np.matrix([[ r, -Cv*Ss, t, 0], + [ Cv*Ss, Cs, -Sv*Ss, 0], + [ t, Sv*Ss, s, 0], + [ 0, 0, 0, 1]]) + + # calculate the transformation matrix for the forward tool kinematic + matrix_tool_fwd = np.transpose(Rtc)*np.transpose(Rs)*np.transpose(Rp)*T_in + # calculate the transformation matrix for the inverse tool kinematic + matrix_tool_inv = Rp*Rs*Rtc*T_in + if direction == 'fwd': + #log.debug("matrix tool fwd: \n", matrix_tool_fwd) + #log.debug("inv would have been: \n", matrix_tool_inv) + return matrix_tool_fwd + elif direction == 'inv': + #log.debug("matrix tool inv: \n", matrix_tool_inv) + #log.debug("fwd would have been: \n", matrix_tool_fwd) + return matrix_tool_inv + else: + return 0 + + +# calculates the primary joint position for a given tool-vector +# Note: this uses functions derived from the custom kinematic +def kins_calc_primary(log, z_vector_req, x_vector_req, theta_2_list=[]): + global primary_min_limit, primary_max_limit + global kins_nutation_angle + epsilon = 0.000001 + theta_1_list=[] + (Kzx, Kzy, Kzz) = (z_vector_req[0], z_vector_req[1], z_vector_req[2]) + # This kinmatic has infinite results for the vertical tool orientation + # so we explicitly define the angles for that specific case + if Kzz > 1 - epsilon: + return [0] + else: + v = radians(hal.get_value(kins_nutation_angle)) + Sv = sin(v) + Cv = cos(v) + for i in range(len(theta_2_list)): + theta_2 = theta_2_list[i] + Ss = sin(theta_2) + Cs = cos(theta_2) + t = Sv*Cv*(1-Cs) + p = Sv * Ss + q = (t*Kzy - p*Kzx)/(t*t + p*p) + theta_1 = asin(q) + # since we are using asin() we really have two solutions theta_1 and pi-theta_2 + for theta in [theta_1, transform_to_pipi(pi - theta_1)]: + if degrees(theta) > primary_min_limit and degrees(theta) < primary_max_limit: + theta_1_list.append(theta) + + return theta_1_list + + +# calculates the secondary joint position for a given tool-vector +# secondary being the joint closest to the tool +# Note: this uses functions derived from the custom kinematic +def kins_calc_secondary(log, z_vector_req, x_vector_req): + global secondary_min_limit, secondary_max_limit + global kins_nutation_angle + epsilon = 0.000001 + theta_2_list=[] + (Kzx, Kzy, Kzz) = (z_vector_req[0], z_vector_req[1], z_vector_req[2]) + v = radians(hal.get_value(kins_nutation_angle)) + Sv = sin(v) + Cv = cos(v) + # This kinmatic has infinite results for the vertical tool orientation + # so we explicitly define the angles for that specific case + if Kzz > 1 - epsilon: + theta_2 = 0 + # This kinematics nutation angle restricts the negative range of Kzz + elif Kzz < 2*Cv*Cv - 1: + log.error('remap_funcs: Requested orientation not reachable with the current nutation angle.') + return None + else: + theta_2 = acos((Kzz - Cv*Cv)/(1 - Cv*Cv)) + # since we are using acos() we really have two solutions theta_1 and -theta_1 + for theta in [theta_2, -theta_2]: + log.debug(f' Checking possible secondary angle {degrees(theta):.4f}° for limit violations.') + if degrees(theta) > secondary_min_limit and degrees(theta) < secondary_max_limit: + theta_2_list.append(theta) + + return theta_2_list # returns radians + + +# define the order in which the joint angles need to be calculated +def kins_calc_possible_joint_angles(log, z_vector_req, x_vector_req): + try: + theta_2_calcd = kins_calc_secondary(log, z_vector_req, x_vector_req) + except Exception as error: + log.error('kins_calc_jnt_angles, kins_calc_secondary, %s', error) + if theta_2_calcd == None: + return (None, None) + try: + theta_1_calcd = kins_calc_primary(log, z_vector_req, x_vector_req, theta_2_calcd) + except Exception as error: + log.error('kins_calc_jnt_angles, kins_calc_primary, %s', error) + return (theta_1_calcd, theta_2_calcd) # returns radians + + +# calculate the transformed work offset used after 53.n +def kins_calc_transformed_work_offset(current_offset, twp_offset, theta_1, theta_2, virtual_rot): + P = matrix_to_point(kins_calc_transformation_matrix(theta_1, theta_2, virtual_rot, point_to_matrix(current_offset))) + # calculate the twp offset in transformed-coordinates + Q = matrix_to_point(kins_calc_transformation_matrix(theta_1, theta_2, virtual_rot, point_to_matrix(twp_offset))) + transformed_offset = (P[0]+Q[0], P[1]+Q[1], P[2]+Q[2]) + return transformed_offset + +# pass required values to the kinematics component +# the module takes the virtual rotation in radians and the two joint angles in +# degrees, the same units the joints themselves are in +def kins_set_values(theta_1, theta_2, virtual_rot): # expects radians + hal.set_p(kins_virtual_rotation, str(virtual_rot)) + hal.set_p(kins_primary_rotation, str(degrees(theta_1))) + hal.set_p(kins_secondary_rotation, str(degrees(theta_2))) + + +# returns angle required to orient the x-vector parallel to the machine-xy plane +# for given machine joint position angles. +# For G68.3 this is the default tool-x direction +# NOTE: this uses formulas derived from the transformation matrix in the inverse tool kinematic +# TODO I don't actually know if this is the correct x orientation for G68.3' +def kins_calc_virtual_rot_for_g683(theta_1, theta_2): + # The idea is that the oriented x-vector is parallel to the machine xy-plane when the + # z component of the x-direction vector is equal to zero + # Mathematically we take the symbolic formula found in row 3, column 1 of the transformation + # matrix from the inverse tool-kinematics, equal that to zero and solve for 'tc'. + # this makes the x-vector of the oriented coords horizontal and the user can set the + # rotation from there using g68.3 r + global kins_nutation_angle + v = radians(hal.get_value(kins_nutation_angle)) + Cv = cos(v) + Sv = sin(v) + Cs = cos(theta_2) + Ss = sin(theta_2) + Cp = cos(theta_1) + Sp = sin(theta_1) + t = Sv*Cv*(1-Cs) + tc = atan2(-t,(Sv*Ss)) + # note: rotation is done using a halpin that feeds into the kinematic component and the + # vismach model. In contrast to a gcode command where 'c' refers to a physical machine joint) + return tc # returns radians + + +# return the start values required to calculate the virtual rotation +def kins_calc_virtual_rot_get_values(x_vector_requested, z_vector_requested, twp_matrix): + x_vector_requested = [twp_matrix[0,0],twp_matrix[1,0],twp_matrix[2,0]] + z_vector_requested = [twp_matrix[0,2],twp_matrix[1,2],twp_matrix[2,2]] + matrix_in = np.asmatrix(np.identity(4)) + direction = 'inv' + return (x_vector_requested, z_vector_requested, matrix_in, direction) + + +# If the operator has requested a rotation by passing an R word in the 68.n command we need to +# create a rotation matrix that represents a rotation around the Z-axis of the TWP plane +def kins_calc_twp_origin_rot_matrix(r): # expects radians + # we use xzx-euler rotation to create the rotation matrix for the requested origin rotation + twp_origin_rot_matrix = calc_euler_rot_matrix(0, r, 0, '131') + + return twp_origin_rot_matrix + + +# This returns which transformation to use when checking calculated angles +# and when calculating the twp_matrix for G68.3 +def kins_calc_transformation_get_direction(): + return 'inv' + + +# returns the pin name for the virtual rotation in the kinematics component +def kins_get_current_virtual_rot(): + current_virtual_rot = hal.get_value(kins_virtual_rotation) + return current_virtual_rot # returns radians + + + + + + + +# forms a 4x4 transformation matrix from a given 1x3 point vector [x,y,z] +def point_to_matrix(point): + # start with a 4x4 identity matrix and add the point vector to the 4th column + matrix = np.identity(4) + [matrix[0,3], matrix[1,3], matrix[2,3]] = point + matrix = np.asmatrix(matrix) + return matrix + +# extracts the point vector form a given 4x4 transformation matrix +def matrix_to_point(matrix): + point = (matrix[0,3],matrix[1,3],matrix[2,3]) + return point + + +# this is from 'mika-s.github.io' +# transforms a given angle to the interval of [-pi,pi] +def transform_to_pipi(input_angle): + def truncated_remainder(dividend, divisor): + divided_number = dividend / divisor + divided_number = -int(-divided_number) if divided_number < 0 else int(divided_number) + remainder = dividend - divisor * divided_number + return remainder + + revolutions = int((input_angle + np.sign(input_angle) * pi) / (2 * pi)) + p1 = truncated_remainder(input_angle + np.sign(input_angle) * pi, 2 * pi) + p2 = (np.sign(np.sign(input_angle) + + 2 * (np.sign(fabs((truncated_remainder(input_angle + pi, 2 * pi)) / (2 * pi))) - 1))) * pi + output_angle = p1 - p2 + return output_angle + + +# define the basic rotation matrices, used for euler twp modes +def Rx(th): + return np.array([[1, 0 , 0 ], + [0, cos(th), -sin(th)], + [0, sin(th), cos(th)]]) + +def Ry(th): + return np.array([[ cos(th), 0, sin(th)], + [ 0 , 1, 0 ], + [-sin(th), 0, cos(th)]]) + +def Rz(th): + return np.array([[cos(th), -sin(th), 0], + [sin(th), cos(th), 0], + [0 , 0 , 1]]) + + +# returns the rotation matrices for given order and angles +def calc_euler_rot_matrix(th1, th2, th3, order): + if order == '131': + matrix = np.dot(np.dot(Rx(th1), Rz(th2)), Rx(th3)) + elif order=='121': + matrix = np.dot(np.dot(Rx(th1), Ry(th2)), Rx(th3)) + elif order=='212': + matrix = np.dot(np.dot(Ry(th1), Rx(th2)), Ry(th3)) + elif order=='232': + matrix = np.dot(np.dot(Ry(th1), Rz(th2)), Ry(th3)) + elif order=='323': + matrix = np.dot(np.dot(Rz(th1), Ry(th2)), Rz(th3)) + elif order=='313': + matrix = np.dot(np.dot(Rz(th1), Rx(th2)), Rz(th3)) + elif order=='123': + matrix = np.dot(np.dot(Rx(th1), Ry(th2)), Rz(th3)) + elif order=='132': + matrix = np.dot(np.dot(Rx(th1), Rz(th2)), Ry(th3)) + elif order=='213': + matrix = np.dot(np.dot(Ry(th1), Rx(th2)), Rz(th3)) + elif order=='231': + matrix = np.dot(np.dot(Ry(th1), Rz(th2)), Rx(th3)) + elif order=='321': + matrix = np.dot(np.dot(Rz(th1), Ry(th2)), Rx(th3)) + elif order=='312': + matrix = np.dot(np.dot(Rz(th1), Rx(th2)), Ry(th3)) + return matrix diff --git a/configs/sim/axis/vismach/5axis/table-rotary_spindle-rotary-nutating/xyzbca-trsrn_twp/xyzbca-trsrn.ini b/configs/sim/axis/vismach/5axis/table-rotary_spindle-rotary-nutating/xyzbca-trsrn_twp/xyzbca-trsrn.ini index d9ae382fefc..d3032855aee 100644 --- a/configs/sim/axis/vismach/5axis/table-rotary_spindle-rotary-nutating/xyzbca-trsrn_twp/xyzbca-trsrn.ini +++ b/configs/sim/axis/vismach/5axis/table-rotary_spindle-rotary-nutating/xyzbca-trsrn_twp/xyzbca-trsrn.ini @@ -39,7 +39,7 @@ SUBROUTINE_PATH = ../remap_subs:../demos REMAP = G53.1 modalgroup=1 argspec=p ngc=g531remap REMAP = G53.3 modalgroup=1 argspec=pxyz ngc=g533remap REMAP = G53.6 modalgroup=1 argspec=p ngc=g536remap - REMAP = M530 modalgroup=10 python=g53x_core + REMAP = M530 modalgroup=10 python=g53n_core REMAP = G68.2 modalgroup=1 argspec=pqxyzijkr python=g682 REMAP = G68.3 modalgroup=1 argspec=xyzr python=g683 From a1884050c1ab374234ebe82a2bdabf87400efb45 Mon Sep 17 00:00:00 2001 From: Luca Toniolo <10792599+grandixximo@users.noreply.github.com> Date: Tue, 25 Aug 2026 01:03:48 +1000 Subject: [PATCH 08/60] twp: keep the arc functions inside their domain A tool vector in a principal plane raised "math domain error" from asin() in kins_calc_primary: the vector is a column of a product of rotations, a unit vector only to rounding, and a zero component lands the argument a rounding error outside plus or minus one. Sweeping every reachable orientation in five degree steps gave 47 failures of 5184 at the configured nutation, and every failure is a vector with a zero component, what G68.2 with I0 or J0 asks for. Clamp an argument within rounding of the limit and leave anything further out to raise, since that is an unreachable orientation rather than an artefact. kins_calc_possible_joint_angles then fell through to an unassigned variable on that failure; it returns no solution, which the caller handles. --- .../xyzacb-trsrn_twp/remap_funcs_twp.py | 32 +++++++++++++++++-- .../xyzbca-trsrn_twp/remap_funcs_twp.py | 32 +++++++++++++++++-- 2 files changed, 60 insertions(+), 4 deletions(-) diff --git a/configs/sim/axis/vismach/5axis/table-rotary_spindle-rotary-nutating/xyzacb-trsrn_twp/remap_funcs_twp.py b/configs/sim/axis/vismach/5axis/table-rotary_spindle-rotary-nutating/xyzacb-trsrn_twp/remap_funcs_twp.py index 7c2b498b7b6..7c2ebd39961 100644 --- a/configs/sim/axis/vismach/5axis/table-rotary_spindle-rotary-nutating/xyzacb-trsrn_twp/remap_funcs_twp.py +++ b/configs/sim/axis/vismach/5axis/table-rotary_spindle-rotary-nutating/xyzacb-trsrn_twp/remap_funcs_twp.py @@ -21,6 +21,29 @@ import hal +# asin() and acos() take a value that the trigonometry guarantees is within +# [-1, 1] and that floating point does not. The tool vector reaching here is +# a column of a product of rotation matrices, so it is a unit vector only to +# within rounding, and one ulp of slack in it is enough to put the argument +# outside the domain. A nutation angle of 90 degrees makes that certain +# rather than unlucky: Cv is zero, so t vanishes, and the ratio below reduces +# to Kzy/Ss with nothing left to absorb the slop. +# +# Anything within a rounding error of the limit is pulled back to it. Beyond +# that the request really is out of range and is left to raise, because that +# is a machine that cannot reach the orientation and not an arithmetic +# artefact. +UNIT_EPSILON = 1e-9 + +def clamp_unit(value): + if -1.0 - UNIT_EPSILON <= value <= -1.0: + return -1.0 + if 1.0 <= value <= 1.0 + UNIT_EPSILON: + return 1.0 + return value + + + # set up parsing of the inifile import os import linuxcnc @@ -138,7 +161,7 @@ def kins_calc_primary(log, z_vector_req, x_vector_req, theta_2_list=[]): Cs = cos(theta_2) t = Sv*Cv*(1-Cs) p = Sv * Ss - theta_1 = asin((p*Kzy - t*Kzx)/(t*t + p*p)) + theta_1 = asin(clamp_unit((p*Kzy - t*Kzx)/(t*t + p*p))) # since we are using asin() we really have two solutions theta_1 and pi-theta_2 for theta in [theta_1, transform_to_pipi(pi - theta_1)]: log.debug(f' Checking possible primary angle {degrees(theta):.4f}° for limit violations.') @@ -168,7 +191,7 @@ def kins_calc_secondary(log, z_vector_req, x_vector_req): log.error('remap_funcs: Requested orientation not reachable with the current nutation angle.') return None else: - theta_2 = acos((Kzz - Cv*Cv)/(1 - Cv*Cv)) + theta_2 = acos(clamp_unit((Kzz - Cv*Cv)/(1 - Cv*Cv))) for theta in [theta_2, -theta_2]: log.debug(f' Checking possible secondary angle {degrees(theta):.4f}° for limit violations.') if degrees(theta) > secondary_min_limit and degrees(theta) < secondary_max_limit: @@ -182,12 +205,17 @@ def kins_calc_possible_joint_angles(log, z_vector_req, x_vector_req): theta_2_calcd = kins_calc_secondary(log, z_vector_req, x_vector_req) except Exception as error: log.error('kins_calc_jnt_angles, kins_calc_secondary, %s', error) + # an orientation this machine cannot reach is 'no solution', which the + # caller already handles. Falling through would raise a second and + # less informative error over the top of this one. + return (None, None) if theta_2_calcd == None: return (None, None) try: theta_1_calcd = kins_calc_primary(log, z_vector_req, x_vector_req, theta_2_calcd) except Exception as error: log.error('kins_calc_jnt_angles, kins_calc_primary, %s', error) + return (None, None) return (theta_1_calcd, theta_2_calcd) # returns radians diff --git a/configs/sim/axis/vismach/5axis/table-rotary_spindle-rotary-nutating/xyzbca-trsrn_twp/remap_funcs_twp.py b/configs/sim/axis/vismach/5axis/table-rotary_spindle-rotary-nutating/xyzbca-trsrn_twp/remap_funcs_twp.py index b1629e7d012..abb24726b72 100644 --- a/configs/sim/axis/vismach/5axis/table-rotary_spindle-rotary-nutating/xyzbca-trsrn_twp/remap_funcs_twp.py +++ b/configs/sim/axis/vismach/5axis/table-rotary_spindle-rotary-nutating/xyzbca-trsrn_twp/remap_funcs_twp.py @@ -21,6 +21,29 @@ import hal +# asin() and acos() take a value that the trigonometry guarantees is within +# [-1, 1] and that floating point does not. The tool vector reaching here is +# a column of a product of rotation matrices, so it is a unit vector only to +# within rounding, and one ulp of slack in it is enough to put the argument +# outside the domain. A nutation angle of 90 degrees makes that certain +# rather than unlucky: Cv is zero, so t vanishes, and the ratio below reduces +# to Kzy/Ss with nothing left to absorb the slop. +# +# Anything within a rounding error of the limit is pulled back to it. Beyond +# that the request really is out of range and is left to raise, because that +# is a machine that cannot reach the orientation and not an arithmetic +# artefact. +UNIT_EPSILON = 1e-9 + +def clamp_unit(value): + if -1.0 - UNIT_EPSILON <= value <= -1.0: + return -1.0 + if 1.0 <= value <= 1.0 + UNIT_EPSILON: + return 1.0 + return value + + + # set up parsing of the inifile import os import linuxcnc @@ -138,7 +161,7 @@ def kins_calc_primary(log, z_vector_req, x_vector_req, theta_2_list=[]): Cs = cos(theta_2) t = Sv*Cv*(1-Cs) p = Sv * Ss - q = (t*Kzy - p*Kzx)/(t*t + p*p) + q = clamp_unit((t*Kzy - p*Kzx)/(t*t + p*p)) theta_1 = asin(q) # since we are using asin() we really have two solutions theta_1 and pi-theta_2 for theta in [theta_1, transform_to_pipi(pi - theta_1)]: @@ -169,7 +192,7 @@ def kins_calc_secondary(log, z_vector_req, x_vector_req): log.error('remap_funcs: Requested orientation not reachable with the current nutation angle.') return None else: - theta_2 = acos((Kzz - Cv*Cv)/(1 - Cv*Cv)) + theta_2 = acos(clamp_unit((Kzz - Cv*Cv)/(1 - Cv*Cv))) # since we are using acos() we really have two solutions theta_1 and -theta_1 for theta in [theta_2, -theta_2]: log.debug(f' Checking possible secondary angle {degrees(theta):.4f}° for limit violations.') @@ -185,12 +208,17 @@ def kins_calc_possible_joint_angles(log, z_vector_req, x_vector_req): theta_2_calcd = kins_calc_secondary(log, z_vector_req, x_vector_req) except Exception as error: log.error('kins_calc_jnt_angles, kins_calc_secondary, %s', error) + # an orientation this machine cannot reach is 'no solution', which the + # caller already handles. Falling through would raise a second and + # less informative error over the top of this one. + return (None, None) if theta_2_calcd == None: return (None, None) try: theta_1_calcd = kins_calc_primary(log, z_vector_req, x_vector_req, theta_2_calcd) except Exception as error: log.error('kins_calc_jnt_angles, kins_calc_primary, %s', error) + return (None, None) return (theta_1_calcd, theta_2_calcd) # returns radians From 9978bf07c23377aec7c5051e42ffde5ff201349c Mon Sep 17 00:00:00 2001 From: Luca Toniolo <10792599+grandixximo@users.noreply.github.com> Date: Thu, 3 Sep 2026 18:21:24 +1000 Subject: [PATCH 09/60] kinematics: add an optional Jacobian entry point A feed is a speed in the work frame and the machine delivers a speed at each joint; on any kinematics but the identity the two are related by where the machine is, and nothing in the interface answered that, so a limit taken from the joints had nowhere to get it. kinematicsJacobian() answers it: jac[j][a] is how joint j responds to a unit rate of pose coordinate a, rows joints, columns in EmcPose order. It is the derivative of the inverse, since that is what every consumer multiplies by and every module has one, in joint units per pose unit so nothing is converted. A module supplies nothing: kinsJacobianFromInverse() takes central differences of its inverse, eighteen calls a pose. switchkins answers for every type, exactly for an identity type and by differences for one that registers nothing; switchkinsRegisterJacobian() takes a closed form. kinsJacobianFromMappedAxes() turns the derivative of a computed position into rows for the modules that finish in position_to_mapped_joints(). Nothing in motion calls it yet; that is the realtime seam of the limits work, and it waits on the closed forms. --- docs/src/motion/kinematics-conventions.adoc | 99 ++++++++++++++- src/emc/kinematics/kinematics.h | 95 +++++++++++++++ src/emc/kinematics/kins_util.c | 126 ++++++++++++++++++++ src/emc/kinematics/switchkins.c | 40 +++++++ src/emc/kinematics/switchkins.h | 11 ++ src/emc/kinematics/trivkins.c | 9 ++ 6 files changed, 374 insertions(+), 6 deletions(-) diff --git a/docs/src/motion/kinematics-conventions.adoc b/docs/src/motion/kinematics-conventions.adoc index 063ee63e794..1825d8af4d4 100644 --- a/docs/src/motion/kinematics-conventions.adoc +++ b/docs/src/motion/kinematics-conventions.adoc @@ -279,12 +279,11 @@ The joint values that reach a requested orientation, through question a tilted work plane asks when it has to orient the machine. <> says what it answers. -The Jacobian, relating commanded velocity to joint velocity at a given pose, so -that a feed can be checked against the joint velocity, acceleration and limit -values it will actually demand, and so that proximity to a singularity is a -number rather than a surprise. A module with a closed form can supply it -directly. Otherwise it can be obtained by differencing `kinematicsInverse()` -about the pose, which needs no change to the module at all. +How joint motion follows world motion, through `kinematicsJacobian()`, so +that a feed can be checked against the joint velocity and acceleration it will +actually demand, and so that proximity to a singularity is a number rather +than a surprise. <> says what it answers and in +which units. All of these are functions of the joint values and the module's own geometry. None needs state carried between calls, and none needs the module to be running @@ -377,6 +376,88 @@ The search is not a realtime routine. How long it takes depends on the machine and on the request, and the callers that want it, orienting a tilted work plane and previewing a program, are not in the servo loop. +[[sec:jacobian]] +== The Jacobian + +A feed is a speed in the work frame. What the machine has to deliver is a +speed at each joint, and on any kinematics that is not the identity the two +are related by where the machine is. The Jacobian is that relation at one +pose: how each joint responds to a unit rate of each pose coordinate. + + jac[j][a] = d joint[j] / d pose[a] + +Rows are joints. Columns are the pose coordinates in `EmcPose` order, X Y Z A +B C U V W. It is the derivative of `kinematicsInverse()`: multiplied by a pose +velocity it gives the joint velocity motion will command, which is what a feed +limit compares with the joint limits. Joint `j` binds when + + |jac[j] . tangent| * F + +exceeds that joint's velocity limit, `tangent` being the direction of the move +in pose coordinates and `F` the feed along it. The acceleration limit follows +from a second Jacobian taken further along the path, with no more from the +module. A row that grows without bound is a pose approaching a singularity, +where no world speed is slow enough for the joints to follow. + +=== Units + +Each entry is in joint units per pose unit, whatever units the module's own +forward and inverse already use. Nothing is converted: a caller that feeds +pose rates in `EmcPose` units gets joint rates in the units motion already +commands, and never has to know which unit a rotary joint is in. On every +module in the tree both are degrees, so a table rotary's own row is a 1 in its +own column, and a robot's rotary rows carry degrees per millimetre against the +linear columns. + +This is why the Jacobian, unlike the orientation inverse, does not need the +interface to name the rotary joint unit. Every number in it is a ratio of +quantities that already pass through `kinematicsForward()` and +`kinematicsInverse()`, and the caller never combines it with anything measured +in another unit. + +=== Frame + +The columns are pose coordinates, so the answer lives in the work frame, where +`kinematicsForward()` reports positions. The A, B and C columns are rates of +the pose words, the wrapped linear axes the planner already treats as +coordinates, and not an angular velocity vector: on a machine that carries the +work the forward writes the rotary joint into the pose word, and that column +says exactly that, a 1 for its own joint. + +That makes this a different object from the frames of +<>, and the two rules are kept apart deliberately. A frame +is an orientation, and a renderer placing two bodies needs each against +something fixed, so frames are reported against the machine. A Jacobian is a +derivative of the pose, and everything that uses it multiplies it by a pose +rate, so it is reported where the pose is. A module whose maths produces a +twist in the machine frame, which is what the Denavit-Hartenberg modules +produce, turns it into pose word rates through the matrix of the axes each +pose word turns about, once, inside the module. `genserkins` does this, and +having it written once there is worth more than the closed form itself, since +every consumer would otherwise guess it. + +=== What a module has to supply + +Nothing. The shared code takes central differences of the module's inverse +about the pose, eighteen inverse calls on the solution branch the inverse +flags select. That costs a few microseconds on a closed form inverse and +milliseconds on one that iterates, and it answers to the inverse's own +precision, which for an iterating inverse is its convergence tolerance divided +by the step. Modules built on `switchkins.c` answer this way for every type +that registers nothing; an identity type answers exactly. + +A module with a closed form registers it with `switchkinsRegisterJacobian()`. +It is exact, it costs what the inverse costs, and it knows its own singular +poses rather than discovering them as an inverse that fails a step away from +the pose. Every module in the tree whose inverse is written out supplies one. +The two arms whose inverse is a chain of arc tangents, `pumakins` and +`three21kins`, answer through the differences. + +A module reading its rotary angles from the joint argument of the inverse +rather than from the pose, which the nutating heads do, has an inverse whose +derivative about the pose is not the coupling the machine has. Such a module +supplies the closed form, taken against the pose. + [[sec:writing-a-module]] == Writing a Module @@ -406,6 +487,12 @@ Orientation inverse:: do nothing. Register a closed form only where one exists, and where it does, say which poses it treats as degenerate. +Jacobian:: + Rows are joints, columns are pose coordinates, entries in the units the + forward and inverse already use, reported where the pose is. A module with + a closed form inverse differentiates it and registers the result; one + without lets the shared code difference the inverse. + Geometry stays in the module:: Whatever a consumer needs to know about the machine's shape is answered by the module. A consumer that restates it has taken a copy that nothing keeps diff --git a/src/emc/kinematics/kinematics.h b/src/emc/kinematics/kinematics.h index ba285cb8861..900fe5aa474 100644 --- a/src/emc/kinematics/kinematics.h +++ b/src/emc/kinematics/kinematics.h @@ -16,6 +16,7 @@ #define __LINUXCNC_KINEMATICS_H #include "emcpos.h" /* EmcPose */ +#include "emcmotcfg.h" /* EMCMOT_MAX_JOINTS, EMCMOT_MAX_AXIS */ #include "rtapi_bool.h" /* @@ -360,6 +361,90 @@ extern int toolFrameSolve(kinsFrameFunc work, int *free_directions, double *tool_spin); +/* How each joint responds to a unit rate of each pose coordinate: + + jac[j][a] = d joint[j] / d pose[a] + + Rows are joints, columns are pose coordinates in EmcPose order, x y z a b + c u v w. This is the derivative of kinematicsInverse(): multiply it by a + pose velocity and the result is the joint velocity that motion will + command, which is what a feed limit checks against the joint limits. A + row that grows without bound is a pose approaching a singularity, where + the joints cannot keep up with any world speed at all. + + Each entry is in joint units per pose unit, whatever units the module's + own forward and inverse already use. Nothing is converted here: a caller + that feeds pose rates in EmcPose units gets joint rates in the units + motion already commands, and never has to know which unit a rotary joint + is in. On every module in the tree both are degrees, so a table rotary's + own row is a plain 1 in its own column. + + The columns are pose coordinates, so the answer lives in the work frame, + where kinematicsForward() reports positions. The a, b and c columns are + rates of the pose words, the wrapped linear axes the planner already + treats as coordinates, and not an angular velocity vector. That makes + this a different object from the frames above, which are orientations + and are given against the machine; see the Kinematics Conventions + chapter. + + joint and world are one pose in both descriptions: world is what + kinematicsForward() reports for joint under these flags. Both are given + because a closed form differentiates at the joints while the generic + default perturbs the pose, and iflags keeps every inverse the default + calls on the same solution branch. Rows past the module's joint count + are zero. + + Optional, like the frames. Modules built on switchkins.c export it + always and answer for every type, since it can always be obtained from + the inverse where a frame cannot; other modules need not export it, and + a caller that resolves it dynamically and finds nothing can call + kinsJacobianFromInverse() itself with the module's inverse. + + Returns 0, or -1 if the module cannot answer at this pose. */ +extern int kinematicsJacobian(const double *joint, + const EmcPose *world, + double jac[EMCMOT_MAX_JOINTS][EMCMOT_MAX_AXIS], + const KINEMATICS_INVERSE_FLAGS *iflags); + +typedef int (*kinsInverseFunc)(const EmcPose *world, + double *joint, + const KINEMATICS_INVERSE_FLAGS *iflags, + KINEMATICS_FORWARD_FLAGS *fflags); + +/* The generic Jacobian, by central differences of an inverse about world: + two inverse calls per pose coordinate, eighteen in all, on the solution + branch iflags selects. The joint array handed to every call starts from + joint, so a module that reads its joint argument sees the machine where + it is. + + The answer is as good as the inverse: a closed form gives it to rounding, + an inverse that iterates to a tolerance gives it to that tolerance over + the step, and should supply its own. num_joints is the module's joint + count. Returns 0, or -1 if any inverse fails. */ +#define KINS_JACOBIAN_STEP 1e-3 /* pose units, either kind */ + +extern int kinsJacobianFromInverse(kinsInverseFunc inverse, + int num_joints, + const double *joint, + const EmcPose *world, + const KINEMATICS_INVERSE_FLAGS *iflags, + double jac[EMCMOT_MAX_JOINTS][EMCMOT_MAX_AXIS]); + +/* For a module whose inverse computes a position P and then hands it to + position_to_mapped_joints(): given dP[axis][pose], how each coordinate of + P responds to each pose coordinate, fill in jac so that every joint gets + the row of the letter it is mapped to. Duplicate letters get duplicate + rows, which is the gantry case. */ +extern int kinsJacobianFromMappedAxes(int max_joints, + const double dP[EMCMOT_MAX_AXIS][EMCMOT_MAX_AXIS], + double jac[EMCMOT_MAX_JOINTS][EMCMOT_MAX_AXIS]); + +/* joints are axes: a 1 per joint in the column of its letter */ +extern int identityKinematicsJacobian(const double *joint, + const EmcPose *world, + double jac[EMCMOT_MAX_JOINTS][EMCMOT_MAX_AXIS], + const KINEMATICS_INVERSE_FLAGS *iflags); + extern int kinematicsSwitchable(void); extern int kinematicsSwitch(int switchkins_type); //NOTE: switchable kinematics may require Interp::Synch @@ -414,6 +499,11 @@ extern int xyzacKinematicsWorkFrame(const double *joints, PmRotationMatrix *rot, const KINEMATICS_FORWARD_FLAGS *fflags); +extern int xyzacKinematicsJacobian(const double *joints, + const EmcPose *pos, + double jac[EMCMOT_MAX_JOINTS][EMCMOT_MAX_AXIS], + const KINEMATICS_INVERSE_FLAGS *iflags); + extern int xyzbcKinematicsForward(const double *joints, EmcPose * pos, @@ -433,4 +523,9 @@ extern int xyzbcKinematicsWorkFrame(const double *joints, PmRotationMatrix *rot, const KINEMATICS_FORWARD_FLAGS *fflags); +extern int xyzbcKinematicsJacobian(const double *joints, + const EmcPose *pos, + double jac[EMCMOT_MAX_JOINTS][EMCMOT_MAX_AXIS], + const KINEMATICS_INVERSE_FLAGS *iflags); + //********************************************************************* diff --git a/src/emc/kinematics/kins_util.c b/src/emc/kinematics/kins_util.c index fa20010d1d2..2e30969fd90 100644 --- a/src/emc/kinematics/kins_util.c +++ b/src/emc/kinematics/kins_util.c @@ -1040,3 +1040,129 @@ int toolFrameSolve(kinsFrameFunc work, } return found; } + +//---------------------------------------------------------------------- +// The Jacobian. See kinematics.h for what it is and which way it points. +//---------------------------------------------------------------------- + +static void kj_zero(double jac[EMCMOT_MAX_JOINTS][EMCMOT_MAX_AXIS]) +{ + int j, a; + for (j = 0; j < EMCMOT_MAX_JOINTS; j++) { + for (a = 0; a < EMCMOT_MAX_AXIS; a++) { jac[j][a] = 0; } + } +} + +// pose coordinate a of p, in EmcPose order +static double *kj_coord(EmcPose *p, int a) +{ + switch (a) { + case 0: return &p->tran.x; + case 1: return &p->tran.y; + case 2: return &p->tran.z; + case 3: return &p->a; + case 4: return &p->b; + case 5: return &p->c; + case 6: return &p->u; + case 7: return &p->v; + default: return &p->w; + } +} + +int kinsJacobianFromInverse(kinsInverseFunc inverse, + int num_joints, + const double *joint, + const EmcPose *world, + const KINEMATICS_INVERSE_FLAGS *iflags, + double jac[EMCMOT_MAX_JOINTS][EMCMOT_MAX_AXIS]) +{ + double qp[EMCMOT_MAX_JOINTS], qm[EMCMOT_MAX_JOINTS]; + KINEMATICS_INVERSE_FLAGS ifl = iflags ? *iflags : 0; + KINEMATICS_FORWARD_FLAGS ffl = 0; + EmcPose p; + int j, a; + + if (!inverse || !joint || !world || !jac + || num_joints <= 0 || num_joints > EMCMOT_MAX_JOINTS) { + return -1; + } + + kj_zero(jac); + + for (a = 0; a < EMCMOT_MAX_AXIS; a++) { + p = *world; + // the joint array every call sees starts at the machine's own + // position, for a module that reads it before writing it + for (j = 0; j < EMCMOT_MAX_JOINTS; j++) { qp[j] = qm[j] = joint[j]; } + + *kj_coord(&p, a) += KINS_JACOBIAN_STEP; + if (inverse(&p, qp, &ifl, &ffl)) { return -1; } + + *kj_coord(&p, a) -= 2 * KINS_JACOBIAN_STEP; + if (inverse(&p, qm, &ifl, &ffl)) { return -1; } + + for (j = 0; j < num_joints; j++) { + jac[j][a] = (qp[j] - qm[j]) / (2 * KINS_JACOBIAN_STEP); + } + } + return 0; +} // kinsJacobianFromInverse() + +int kinsJacobianFromMappedAxes(int max_joints, + const double dP[EMCMOT_MAX_AXIS][EMCMOT_MAX_AXIS], + double jac[EMCMOT_MAX_JOINTS][EMCMOT_MAX_AXIS]) +{ + int jno, a; + + if (!map_initialized) { + rtapi_print_msg(RTAPI_MSG_ERR, + "kinsJacobianFromMappedAxes before map_initialized\n"); + return -1; + } + if (max_joints <= 0 || max_joints > EMCMOT_MAX_JOINTS) { return -1; } + + kj_zero(jac); + + for (jno = 0; jno < max_joints; jno++) { + int bit = 1<= kins_count) { + return -1; + } + // a closed form is exact and knows its own singular poses + if (kjacs[switchkins_type]) { + return kjacs[switchkins_type](joint, world, jac, iflags); + } + // otherwise the type's own inverse, differenced. The type function + // rather than the dispatch, so this cannot recurse through a switch. + if (!kinvs[switchkins_type]) { return -1; } + return kinsJacobianFromInverse(kinvs[switchkins_type], kp.max_joints, + joint, world, iflags, jac); +} // kinematicsJacobian() + KINEMATICS_TYPE kinematicsType() { return KINEMATICS_BOTH; @@ -354,6 +374,20 @@ int switchkinsRegisterFrames(int ktype, KT kwork, KT ktool, return 0; } // switchkinsRegisterFrames() +int switchkinsRegisterJacobian(int ktype, KJ kjac) +{ + if (ktype < 0 || ktype >= SWITCHKINS_MAX_TYPES) { + rtapi_print_msg(RTAPI_MSG_ERR, + "switchkinsRegisterJacobian: BAD switchkins_type" + " <%d> (must be 0..%d)\n", + ktype, SWITCHKINS_MAX_TYPES - 1); + register_error = 1; + return -1; + } + kjacs[ktype] = kjac; + return 0; +} // switchkinsRegisterJacobian() + int switchkinsRegisterToolFrameInverse(int ktype, KTI kinv) { if (ktype < 0 || ktype >= SWITCHKINS_MAX_TYPES) { @@ -402,11 +436,13 @@ EXPORT_SYMBOL(kinematicsInverse); EXPORT_SYMBOL(kinematicsToolFrame); EXPORT_SYMBOL(kinematicsWorkFrame); EXPORT_SYMBOL(kinematicsToolFrameInverse); +EXPORT_SYMBOL(kinematicsJacobian); EXPORT_SYMBOL(switchkinsRegister); EXPORT_SYMBOL(switchkinsRegisterFrames); EXPORT_SYMBOL(switchkinsRegisterToolFrameInverse); EXPORT_SYMBOL(switchkinsDeclare); EXPORT_SYMBOL(kinematicsTypeFlags); +EXPORT_SYMBOL(switchkinsRegisterJacobian); MODULE_LICENSE("GPL"); static int comp_id; @@ -443,6 +479,10 @@ int rtapi_app_main(void) ktools[i] = identityKinematicsToolFrame; knative[i] = TOOL_FRAME_SPINDLE; } + // and its Jacobian is exact, so do not difference for it + if (!kjacs[i] && kfwds[i] == identityKinematicsForward) { + kjacs[i] = identityKinematicsJacobian; + } } // the highest type provided by either route sets the count diff --git a/src/emc/kinematics/switchkins.h b/src/emc/kinematics/switchkins.h index d325ce75e48..77caca90629 100644 --- a/src/emc/kinematics/switchkins.h +++ b/src/emc/kinematics/switchkins.h @@ -73,4 +73,15 @@ extern int switchkinsRegisterToolFrameInverse(int ktype, KTI kinv); // never calls it leaves its types numeric-only: G12.1 P still works, // G13.1 refuses to guess which type is identity. extern int switchkinsDeclare(int ktype, int flags); + +// KinematicsJACOBIAN function (optional, see kinematics.h) +typedef int (*KJ)(const double *joint, + const EmcPose *world, + double jac[EMCMOT_MAX_JOINTS][EMCMOT_MAX_AXIS], + const KINEMATICS_INVERSE_FLAGS *iflags); + +// called from switchkinsSetup() only by a type with a closed form. A type +// that does not gets the exact answer if it is an identity type, and +// otherwise the generic differences of its own inverse. +extern int switchkinsRegisterJacobian(int ktype, KJ kjac); #endif // } diff --git a/src/emc/kinematics/trivkins.c b/src/emc/kinematics/trivkins.c index 4b3685dc6d6..f04d9642622 100644 --- a/src/emc/kinematics/trivkins.c +++ b/src/emc/kinematics/trivkins.c @@ -52,6 +52,14 @@ int kinematicsWorkFrame(const double *joints, return identityKinematicsWorkFrame(joints, rot, fflags); } +int kinematicsJacobian(const double *joints, + const EmcPose *pos, + double jac[EMCMOT_MAX_JOINTS][EMCMOT_MAX_AXIS], + const KINEMATICS_INVERSE_FLAGS *iflags) +{ + return identityKinematicsJacobian(joints, pos, jac, iflags); +} + static KINEMATICS_TYPE ktype = -1; KINEMATICS_TYPE kinematicsType() @@ -72,6 +80,7 @@ EXPORT_SYMBOL(kinematicsForward); EXPORT_SYMBOL(kinematicsInverse); EXPORT_SYMBOL(kinematicsToolFrame); EXPORT_SYMBOL(kinematicsWorkFrame); +EXPORT_SYMBOL(kinematicsJacobian); MODULE_LICENSE("GPL"); static int comp_id; From e8b46a391531091815af7392bbdca5ea5c4f4f11 Mon Sep 17 00:00:00 2001 From: Luca Toniolo <10792599+grandixximo@users.noreply.github.com> Date: Thu, 3 Sep 2026 18:21:24 +1000 Subject: [PATCH 10/60] trtfuncs, 5axiskins, maxkins: supply the Jacobian Each inverse is a rotation of the pose about the table or the pivot plus offsets, so the derivative is the same rotation for the linear columns and the rotation advanced a quarter turn, times the lever, for the rotary ones. The tables and 5axiskins build the position and hand it to position_to_mapped_joints(), so they fill a matrix of the position's derivative and let kinsJacobianFromMappedAxes() place the rows, which keeps duplicate letters right. maxkins has fixed joint numbers and fills its rows directly. --- src/emc/kinematics/5axiskins.c | 44 ++++++++++++ src/emc/kinematics/maxkins.c | 45 ++++++++++++ src/emc/kinematics/trtfuncs.c | 105 ++++++++++++++++++++++++++++ src/emc/kinematics/xyzac-trt-kins.c | 2 + src/emc/kinematics/xyzbc-trt-kins.c | 2 + 5 files changed, 198 insertions(+) diff --git a/src/emc/kinematics/5axiskins.c b/src/emc/kinematics/5axiskins.c index aa0af3cfb8b..966a79479ee 100644 --- a/src/emc/kinematics/5axiskins.c +++ b/src/emc/kinematics/5axiskins.c @@ -162,6 +162,48 @@ static int fiveaxis_KinematicsInverse(const EmcPose * pos, return 0; } // fiveaxis_kinematicsInverse() +static int fiveaxis_KinematicsJacobian(const double *joints, + const EmcPose * pos, + double jac[EMCMOT_MAX_JOINTS][EMCMOT_MAX_AXIS], + const KINEMATICS_INVERSE_FLAGS * iflags) +{ + (void)joints; + (void)iflags; + rtapi_real pivot_length = hal_get_real(haldata->pivot_length); + const double R = pivot_length + pos->w; + const double sb = sin(TO_RAD*pos->b), cb = cos(TO_RAD*pos->b); + const double sc = sin(TO_RAD*pos->c), cc = cos(TO_RAD*pos->c); + double dP[EMCMOT_MAX_AXIS][EMCMOT_MAX_AXIS]; + int a, b; + + for (a = 0; a < EMCMOT_MAX_AXIS; a++) { + for (b = 0; b < EMCMOT_MAX_AXIS; b++) { dP[a][b] = 0; } + } + + // the computed position of the inverse is the pose less the pivot + // vector r = s2r(R, c, 180 - b), which is (R sin b cos c, R sin b sin c, + // -R cos b); each row is that coordinate differentiated + dP[0][0] = 1; + dP[0][4] = -R * cb * cc * TO_RAD; + dP[0][5] = R * sb * sc * TO_RAD; + dP[0][8] = -sb * cc; + + dP[1][1] = 1; + dP[1][4] = -R * cb * sc * TO_RAD; + dP[1][5] = -R * sb * cc * TO_RAD; + dP[1][8] = -sb * sc; + + dP[2][2] = 1; + dP[2][4] = -R * sb * TO_RAD; + dP[2][8] = cb; + + for (a = 3; a < EMCMOT_MAX_AXIS; a++) { dP[a][a] = 1; } + + return kinsJacobianFromMappedAxes(fiveaxis_max_joints, + (const double (*)[EMCMOT_MAX_AXIS])dP, + jac); +} // fiveaxis_KinematicsJacobian() + int fiveaxis_KinematicsSetup(const int comp_id, const char* coordinates, kparms* kp) @@ -264,11 +306,13 @@ int switchkinsSetup(kparms* kp, *kinv1 = fiveaxis_KinematicsInverse; switchkinsDeclare(0, KINSTYPE_IDENTITY); switchkinsDeclare(1, KINSTYPE_PRIMARY); + switchkinsRegisterJacobian(1, fiveaxis_KinematicsJacobian); } else { rtapi_print("\n!!! switchkins-type 0 is %s\n",kp->kinsname); *kset0 = fiveaxis_KinematicsSetup; *kfwd0 = fiveaxis_KinematicsForward; *kinv0 = fiveaxis_KinematicsInverse; + switchkinsRegisterJacobian(0, fiveaxis_KinematicsJacobian); *kset1 = identityKinematicsSetup; *kfwd1 = identityKinematicsForward; diff --git a/src/emc/kinematics/maxkins.c b/src/emc/kinematics/maxkins.c index d2623e0ad62..2da69858e44 100644 --- a/src/emc/kinematics/maxkins.c +++ b/src/emc/kinematics/maxkins.c @@ -121,6 +121,50 @@ int kinematicsInverse(const EmcPose * pos, return 0; } +int kinematicsJacobian(const double *joints, + const EmcPose * pos, + double jac[EMCMOT_MAX_JOINTS][EMCMOT_MAX_AXIS], + const KINEMATICS_INVERSE_FLAGS * iflags) +{ + rtapi_real con = hal_get_bool(haldata->conventional_directions) ? 1.0 : -1.0; + rtapi_real pivot_length = hal_get_real(haldata->pivot_length); + const double k = M_PI/180; + const double sb = sin(d2r(pos->b)), cb = cos(d2r(pos->b)); + const double sc = sin(d2r(pos->c)), cc = cos(d2r(pos->c)); + const double x = pos->tran.x, y = pos->tran.y; + const double R = pivot_length + pos->w; + int j, a; + + (void)joints; + (void)iflags; + for (j = 0; j < EMCMOT_MAX_JOINTS; j++) { + for (a = 0; a < EMCMOT_MAX_AXIS; a++) { jac[j][a] = 0; } + } + + // kinematicsInverse() with the polar form expanded: rotating (x, y) + // by -c is x*cos(c) + y*sin(c) and y*cos(c) - x*sin(c), and the + // B and U corrections are what they are written as + jac[0][0] = cc; + jac[0][1] = sc; + jac[0][4] = (con * R * cb - pos->u * sb) * k; + jac[0][5] = (-x * sc + y * cc) * k; + jac[0][6] = cb; + jac[0][8] = con * sb; + + jac[1][0] = -sc; + jac[1][1] = cc; + jac[1][5] = (-x * cc - y * sc) * k; + jac[1][7] = 1; + + jac[2][2] = 1; + jac[2][4] = (-R * sb + con * pos->u * cb) * k; + jac[2][6] = con * sb; + jac[2][8] = cb; + + for (j = 3; j < 9; j++) { jac[j][j] = 1; } + return 0; +} + KINEMATICS_TYPE kinematicsType() { return KINEMATICS_BOTH; @@ -130,6 +174,7 @@ KINS_NOT_SWITCHABLE EXPORT_SYMBOL(kinematicsType); EXPORT_SYMBOL(kinematicsInverse); EXPORT_SYMBOL(kinematicsForward); +EXPORT_SYMBOL(kinematicsJacobian); MODULE_LICENSE("GPL"); static int comp_id; diff --git a/src/emc/kinematics/trtfuncs.c b/src/emc/kinematics/trtfuncs.c index 0cb4b5eb7aa..6f77bfd92ad 100644 --- a/src/emc/kinematics/trtfuncs.c +++ b/src/emc/kinematics/trtfuncs.c @@ -299,6 +299,59 @@ int xyzacKinematicsToolFrame(const double *joints, return 0; } // xyzacKinematicsToolFrame() +int xyzacKinematicsJacobian(const double *joints, + const EmcPose *pos, + double jac[EMCMOT_MAX_JOINTS][EMCMOT_MAX_AXIS], + const KINEMATICS_INVERSE_FLAGS *iflags) +{ + (void)joints; + (void)iflags; + const double x_rot_point = hal_get_real(haldata->x_rot_point); + const double y_rot_point = hal_get_real(haldata->y_rot_point); + const double z_rot_point = hal_get_real(haldata->z_rot_point); + const double dy = hal_get_real(haldata->y_offset); + const double dt = hal_get_real(haldata->tool_offset); + const double dz = hal_get_real(haldata->z_offset) + dt; + const double sa = sin(pos->a*TO_RAD), ca = cos(pos->a*TO_RAD); + const double sc = sin(pos->c*TO_RAD), cc = cos(pos->c*TO_RAD); + const double X = pos->tran.x - x_rot_point; + const double Y = pos->tran.y - y_rot_point; + const double Z = pos->tran.z - z_rot_point; + double dP[EMCMOT_MAX_AXIS][EMCMOT_MAX_AXIS]; + int a, b; + + rtapi_real con = hal_get_bool(haldata->conventional_directions) ? 1.0 : -1.0; + + for (a = 0; a < EMCMOT_MAX_AXIS; a++) { + for (b = 0; b < EMCMOT_MAX_AXIS; b++) { dP[a][b] = 0; } + } + + // the computed position P of xyzacKinematicsInverse(), differentiated: + // its coefficients for x, y and z, and the same expressions with the + // rotation taken a quarter turn on for a and for c + dP[0][0] = cc; + dP[0][1] = con * sc; + dP[0][5] = (-sc*X + con*cc*Y) * TO_RAD; + + dP[1][0] = - con * sc * ca; + dP[1][1] = cc * ca; + dP[1][2] = con * sa; + dP[1][3] = (con*sc*sa*X - cc*sa*Y + con*ca*Z + sa*dy - con*ca*dz) * TO_RAD; + dP[1][5] = (-con*cc*ca*X - sc*ca*Y) * TO_RAD; + + dP[2][0] = sc * sa; + dP[2][1] = - con * cc * sa; + dP[2][2] = ca; + dP[2][3] = (sc*ca*X - con*cc*ca*Y - sa*Z + con*ca*dy + sa*dz) * TO_RAD; + dP[2][5] = (cc*sa*X + con*sc*sa*Y) * TO_RAD; + + for (a = 3; a < EMCMOT_MAX_AXIS; a++) { dP[a][a] = 1; } + + return kinsJacobianFromMappedAxes(trtfuncs_max_joints, + (const double (*)[EMCMOT_MAX_AXIS])dP, + jac); +} // xyzacKinematicsJacobian() + int xyzbcKinematicsForward(const double *joints, EmcPose * pos, const KINEMATICS_FORWARD_FLAGS * fflags, @@ -443,3 +496,55 @@ int xyzbcKinematicsToolFrame(const double *joints, *rot = TOOL_FRAME_SPINDLE; return 0; } // xyzbcKinematicsToolFrame() + +int xyzbcKinematicsJacobian(const double *joints, + const EmcPose *pos, + double jac[EMCMOT_MAX_JOINTS][EMCMOT_MAX_AXIS], + const KINEMATICS_INVERSE_FLAGS *iflags) +{ + (void)joints; + (void)iflags; + const double x_rot_point = hal_get_real(haldata->x_rot_point); + const double y_rot_point = hal_get_real(haldata->y_rot_point); + const double z_rot_point = hal_get_real(haldata->z_rot_point); + const double dx = hal_get_real(haldata->x_offset); + const double dt = hal_get_real(haldata->tool_offset); + const double dz = hal_get_real(haldata->z_offset) + dt; + const double sb = sin(pos->b*TO_RAD), cb = cos(pos->b*TO_RAD); + const double sc = sin(pos->c*TO_RAD), cc = cos(pos->c*TO_RAD); + const double X = pos->tran.x - x_rot_point; + const double Y = pos->tran.y - y_rot_point; + const double Z = pos->tran.z - z_rot_point; + double dP[EMCMOT_MAX_AXIS][EMCMOT_MAX_AXIS]; + int a, b; + + rtapi_real con = hal_get_bool(haldata->conventional_directions) ? 1.0 : -1.0; + + for (a = 0; a < EMCMOT_MAX_AXIS; a++) { + for (b = 0; b < EMCMOT_MAX_AXIS; b++) { dP[a][b] = 0; } + } + + // see the comment in xyzacKinematicsJacobian(); dpx and dpz of the + // inverse depend on b as well + dP[0][0] = cc * cb; + dP[0][1] = con * sc * cb; + dP[0][2] = - con * sb; + dP[0][4] = (-cc*sb*X - con*sc*sb*Y - con*cb*Z + sb*dx + con*cb*dz) * TO_RAD; + dP[0][5] = (-sc*cb*X + con*cc*cb*Y) * TO_RAD; + + dP[1][0] = - con * sc; + dP[1][1] = cc; + dP[1][5] = (-con*cc*X - sc*Y) * TO_RAD; + + dP[2][0] = con * cc * sb; + dP[2][1] = sc * sb; + dP[2][2] = cb; + dP[2][4] = (con*cc*cb*X + sc*cb*Y - sb*Z - con*cb*dx + sb*dz) * TO_RAD; + dP[2][5] = (-con*sc*sb*X + cc*sb*Y) * TO_RAD; + + for (a = 3; a < EMCMOT_MAX_AXIS; a++) { dP[a][a] = 1; } + + return kinsJacobianFromMappedAxes(trtfuncs_max_joints, + (const double (*)[EMCMOT_MAX_AXIS])dP, + jac); +} // xyzbcKinematicsJacobian() diff --git a/src/emc/kinematics/xyzac-trt-kins.c b/src/emc/kinematics/xyzac-trt-kins.c index 13fd6bd79ec..b8bb47bbc1f 100644 --- a/src/emc/kinematics/xyzac-trt-kins.c +++ b/src/emc/kinematics/xyzac-trt-kins.c @@ -43,6 +43,7 @@ int switchkinsSetup(kparms* kp, &TOOL_FRAME_SPINDLE); switchkinsDeclare(0, KINSTYPE_IDENTITY); switchkinsDeclare(1, KINSTYPE_PRIMARY); + switchkinsRegisterJacobian(1, xyzacKinematicsJacobian); } else { rtapi_print("\n!!! switchkins-type 0 is %s\n",kp->kinsname); *kset0 = trtKinematicsSetup; // trt: xyzac,xyzbc @@ -51,6 +52,7 @@ int switchkinsSetup(kparms* kp, switchkinsRegisterFrames(0, xyzacKinematicsWorkFrame, xyzacKinematicsToolFrame, &TOOL_FRAME_SPINDLE); + switchkinsRegisterJacobian(0, xyzacKinematicsJacobian); *kset1 = identityKinematicsSetup; *kfwd1 = identityKinematicsForward; diff --git a/src/emc/kinematics/xyzbc-trt-kins.c b/src/emc/kinematics/xyzbc-trt-kins.c index 73ea69bf820..7b61a69e301 100644 --- a/src/emc/kinematics/xyzbc-trt-kins.c +++ b/src/emc/kinematics/xyzbc-trt-kins.c @@ -43,6 +43,7 @@ int switchkinsSetup(kparms* kp, &TOOL_FRAME_SPINDLE); switchkinsDeclare(0, KINSTYPE_IDENTITY); switchkinsDeclare(1, KINSTYPE_PRIMARY); + switchkinsRegisterJacobian(1, xyzbcKinematicsJacobian); } else { rtapi_print("\n!!! switchkins-type 0 is %s\n",kp->kinsname); *kset0 = trtKinematicsSetup; // trt: xyzac,xyzbc @@ -51,6 +52,7 @@ int switchkinsSetup(kparms* kp, switchkinsRegisterFrames(0, xyzbcKinematicsWorkFrame, xyzbcKinematicsToolFrame, &TOOL_FRAME_SPINDLE); + switchkinsRegisterJacobian(0, xyzbcKinematicsJacobian); *kset1 = identityKinematicsSetup; *kfwd1 = identityKinematicsForward; From 854c813ce65361b0923c38e741823c06ee66687c Mon Sep 17 00:00:00 2001 From: Luca Toniolo <10792599+grandixximo@users.noreply.github.com> Date: Thu, 3 Sep 2026 18:21:25 +1000 Subject: [PATCH 11/60] corexykins, rotatekins, rosekins, matrixkins, millturn, userkins: supply the Jacobian The belt sum and difference, the rotation and its quarter turn, the polar radius and angle, the calibration matrix itself, and the two templates' joint to axis assignments. These are the short ones; they are here so that no module in the tree answers by differencing when its inverse is a few lines. --- src/emc/kinematics/corexykins.c | 20 +++++++++++++++++++ src/emc/kinematics/rosekins.c | 24 ++++++++++++++++++++++ src/emc/kinematics/rotatekins.c | 24 ++++++++++++++++++++++ src/hal/components/matrixkins.comp | 28 ++++++++++++++++++++++++++ src/hal/components/millturn.comp | 32 ++++++++++++++++++++++++++++++ src/hal/components/userkins.comp | 22 ++++++++++++++++++++ 6 files changed, 150 insertions(+) diff --git a/src/emc/kinematics/corexykins.c b/src/emc/kinematics/corexykins.c index f592ff52e9f..473a2ceede1 100644 --- a/src/emc/kinematics/corexykins.c +++ b/src/emc/kinematics/corexykins.c @@ -49,6 +49,25 @@ int kinematicsInverse(const EmcPose *pos return 0; } +int kinematicsJacobian(const double *joints, + const EmcPose *pos, + double jac[EMCMOT_MAX_JOINTS][EMCMOT_MAX_AXIS], + const KINEMATICS_INVERSE_FLAGS *iflags) +{ + int j, a; + (void)joints; + (void)pos; + (void)iflags; + for (j = 0; j < EMCMOT_MAX_JOINTS; j++) { + for (a = 0; a < EMCMOT_MAX_AXIS; a++) { jac[j][a] = 0; } + } + // the two belt motors each carry x and y, in opposite senses for y + jac[0][0] = 1; jac[0][1] = 1; + jac[1][0] = 1; jac[1][1] = -1; + for (j = 2; j < 9; j++) { jac[j][j] = 1; } + return 0; +} + int kinematicsHome(EmcPose *world ,double *joint ,KINEMATICS_FORWARD_FLAGS *fflags @@ -65,6 +84,7 @@ KINS_NOT_SWITCHABLE EXPORT_SYMBOL(kinematicsType); EXPORT_SYMBOL(kinematicsForward); EXPORT_SYMBOL(kinematicsInverse); +EXPORT_SYMBOL(kinematicsJacobian); MODULE_LICENSE("GPL"); static int comp_id; diff --git a/src/emc/kinematics/rosekins.c b/src/emc/kinematics/rosekins.c index adfe763a33a..9f73fbc3f9d 100644 --- a/src/emc/kinematics/rosekins.c +++ b/src/emc/kinematics/rosekins.c @@ -26,6 +26,7 @@ KINS_NOT_SWITCHABLE EXPORT_SYMBOL(kinematicsType); EXPORT_SYMBOL(kinematicsInverse); EXPORT_SYMBOL(kinematicsForward); +EXPORT_SYMBOL(kinematicsJacobian); MODULE_LICENSE("GPL"); #ifndef hypot @@ -112,6 +113,29 @@ int kinematicsInverse(const EmcPose * pos, return 0; } +int kinematicsJacobian(const double *joints, + const EmcPose *pos, + double jac[EMCMOT_MAX_JOINTS][EMCMOT_MAX_AXIS], + const KINEMATICS_INVERSE_FLAGS *iflags) +{ + double x = pos->tran.x, y = pos->tran.y; + double r2 = x*x + y*y; + double r = sqrt(r2); + int j, a; + (void)joints; + (void)iflags; + // on the axis the angle is undefined and its rate unbounded + if (r2 <= 0) { return -1; } + for (j = 0; j < EMCMOT_MAX_JOINTS; j++) { + for (a = 0; a < EMCMOT_MAX_AXIS; a++) { jac[j][a] = 0; } + } + jac[0][0] = x/r; jac[0][1] = y/r; + jac[1][2] = 1; + jac[2][0] = -y/r2 * TO_DEG; + jac[2][1] = x/r2 * TO_DEG; + return 0; +} + KINEMATICS_TYPE kinematicsType() { return KINEMATICS_BOTH; diff --git a/src/emc/kinematics/rotatekins.c b/src/emc/kinematics/rotatekins.c index 838c9178154..b5b648b4b38 100644 --- a/src/emc/kinematics/rotatekins.c +++ b/src/emc/kinematics/rotatekins.c @@ -60,6 +60,29 @@ int kinematicsInverse(const EmcPose * pos, return 0; } +int kinematicsJacobian(const double *joints, + const EmcPose *pos, + double jac[EMCMOT_MAX_JOINTS][EMCMOT_MAX_AXIS], + const KINEMATICS_INVERSE_FLAGS *iflags) +{ + double c_rad = pos->c*M_PI/180; + double cc = cos(c_rad), sc = sin(c_rad); + int j, a; + (void)joints; + (void)iflags; + for (j = 0; j < EMCMOT_MAX_JOINTS; j++) { + for (a = 0; a < EMCMOT_MAX_AXIS; a++) { jac[j][a] = 0; } + } + // the inverse above, differentiated: the rotation itself for x and y, + // and the rotated point turned a quarter turn for c + jac[0][0] = cc; jac[0][1] = -sc; + jac[0][5] = (-pos->tran.x*sc - pos->tran.y*cc) * (M_PI/180); + jac[1][0] = sc; jac[1][1] = cc; + jac[1][5] = ( pos->tran.x*cc - pos->tran.y*sc) * (M_PI/180); + for (j = 2; j < 9; j++) { jac[j][j] = 1; } + return 0; +} + /* implemented for these kinematics as giving joints preference */ int kinematicsHome(EmcPose * world, double *joint, @@ -81,6 +104,7 @@ KINS_NOT_SWITCHABLE EXPORT_SYMBOL(kinematicsType); EXPORT_SYMBOL(kinematicsForward); EXPORT_SYMBOL(kinematicsInverse); +EXPORT_SYMBOL(kinematicsJacobian); MODULE_LICENSE("GPL"); int comp_id; diff --git a/src/hal/components/matrixkins.comp b/src/hal/components/matrixkins.comp index aac6c04d913..8bf76899c8e 100644 --- a/src/hal/components/matrixkins.comp +++ b/src/hal/components/matrixkins.comp @@ -229,6 +229,7 @@ error: KINS_NOT_SWITCHABLE EXPORT_SYMBOL(kinematicsType); EXPORT_SYMBOL(kinematicsInverse); +EXPORT_SYMBOL(kinematicsJacobian); EXPORT_SYMBOL(kinematicsForward); KINEMATICS_TYPE kinematicsType() @@ -321,3 +322,30 @@ int kinematicsInverse(const EmcPose * pos, return 0; } + +int kinematicsJacobian(const double *j, + const EmcPose * pos, + double jac[EMCMOT_MAX_JOINTS][EMCMOT_MAX_AXIS], + const KINEMATICS_INVERSE_FLAGS * iflags) +{ + int r, c; + (void)j; + (void)pos; + (void)iflags; + for (r = 0; r < EMCMOT_MAX_JOINTS; r++) { + for (c = 0; c < EMCMOT_MAX_AXIS; c++) { jac[r][c] = 0; } + } + // the inverse is the calibration matrix itself, so its derivative is + // that matrix, and the pass-through axes are ones + jac[0][0] = hal_get_real(haldata->C_xx); + jac[0][1] = hal_get_real(haldata->C_xy); + jac[0][2] = hal_get_real(haldata->C_xz); + jac[1][0] = hal_get_real(haldata->C_yx); + jac[1][1] = hal_get_real(haldata->C_yy); + jac[1][2] = hal_get_real(haldata->C_yz); + jac[2][0] = hal_get_real(haldata->C_zx); + jac[2][1] = hal_get_real(haldata->C_zy); + jac[2][2] = hal_get_real(haldata->C_zz); + for (r = 3; r < 9; r++) { jac[r][r] = 1; } + return 0; +} diff --git a/src/hal/components/millturn.comp b/src/hal/components/millturn.comp index 45ec650ad96..abb217f7a3a 100644 --- a/src/hal/components/millturn.comp +++ b/src/hal/components/millturn.comp @@ -100,6 +100,7 @@ EXPORT_SYMBOL(kinematicsSwitchable); EXPORT_SYMBOL(kinematicsTypeFlags); EXPORT_SYMBOL(kinematicsSwitch); EXPORT_SYMBOL(kinematicsInverse); +EXPORT_SYMBOL(kinematicsJacobian); EXPORT_SYMBOL(kinematicsForward); static rtapi_u32 switchkins_type; @@ -224,3 +225,34 @@ int kinematicsInverse(const EmcPose * pos, return 0; } // kinematicsInverse() + +int kinematicsJacobian(const double *j, + const EmcPose * pos, + double jac[EMCMOT_MAX_JOINTS][EMCMOT_MAX_AXIS], + const KINEMATICS_INVERSE_FLAGS * iflags) +{ + int r, c; + (void)j; + (void)pos; + (void)iflags; + for (r = 0; r < EMCMOT_MAX_JOINTS; r++) { + for (c = 0; c < EMCMOT_MAX_AXIS; c++) { jac[r][c] = 0; } + } + // the derivative of kinematicsInverse() for each type: which joint + // follows which pose coordinate, and in which sense + switch (switchkins_type) { + case 0: + jac[0][0] = 1; + jac[1][1] = 1; + jac[2][2] = 1; + jac[3][3] = 1; + break; + case 1: + jac[2][0] = 1; + jac[1][1] = -1; + jac[0][2] = 1; + jac[3][3] = 1; + break; + } + return 0; +} // kinematicsJacobian() diff --git a/src/hal/components/userkins.comp b/src/hal/components/userkins.comp index a7af5d29a75..ac0c003369d 100644 --- a/src/hal/components/userkins.comp +++ b/src/hal/components/userkins.comp @@ -128,6 +128,7 @@ KINS_NOT_SWITCHABLE EXPORT_SYMBOL(kinematicsType); EXPORT_SYMBOL(kinematicsInverse); +EXPORT_SYMBOL(kinematicsJacobian); EXPORT_SYMBOL(kinematicsForward); KINEMATICS_TYPE kinematicsType() @@ -194,3 +195,24 @@ int kinematicsInverse(const EmcPose * pos, return 0; } // kinematicsInverse() + +int kinematicsJacobian(const double *j, + const EmcPose * pos, + double jac[EMCMOT_MAX_JOINTS][EMCMOT_MAX_AXIS], + const KINEMATICS_INVERSE_FLAGS * iflags) +{ + int r, c; + (void)j; + (void)pos; + (void)iflags; + // How each joint responds to each pose coordinate, the derivative of + // kinematicsInverse(): for this template joint 0 follows x, joint 1 + // follows y and joint 2 follows z, each one for one. See kinematics.h. + for (r = 0; r < EMCMOT_MAX_JOINTS; r++) { + for (c = 0; c < EMCMOT_MAX_AXIS; c++) { jac[r][c] = 0; } + } + jac[0][0] = 1; + jac[1][1] = 1; + jac[2][2] = 1; + return 0; +} // kinematicsJacobian() From 791fdc934ef7a201ee9c0a0a09e57d2e3fa8f8a3 Mon Sep 17 00:00:00 2001 From: Luca Toniolo <10792599+grandixximo@users.noreply.github.com> Date: Thu, 3 Sep 2026 18:21:25 +1000 Subject: [PATCH 12/60] xyzab_tdr_kins, xyzacb_trsrn, xyzbca_trsrn: supply the Jacobian 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. --- src/hal/components/xyzab_tdr_kins.comp | 62 +++++++++++ src/hal/components/xyzacb_trsrn.comp | 136 +++++++++++++++++++++++++ src/hal/components/xyzbca_trsrn.comp | 136 +++++++++++++++++++++++++ 3 files changed, 334 insertions(+) diff --git a/src/hal/components/xyzab_tdr_kins.comp b/src/hal/components/xyzab_tdr_kins.comp index dd0350e44f7..2ee61e6a9b8 100644 --- a/src/hal/components/xyzab_tdr_kins.comp +++ b/src/hal/components/xyzab_tdr_kins.comp @@ -95,6 +95,7 @@ EXPORT_SYMBOL(kinematicsSwitchable); EXPORT_SYMBOL(kinematicsSwitch); EXPORT_SYMBOL(kinematicsTypeFlags); EXPORT_SYMBOL(kinematicsInverse); +EXPORT_SYMBOL(kinematicsJacobian); EXPORT_SYMBOL(kinematicsForward); static rtapi_u32 switchkins_type; @@ -265,3 +266,64 @@ int kinematicsInverse(const EmcPose * pos, return 0; } // kinematicsInverse() + +int kinematicsJacobian(const double *j, + const EmcPose * pos, + double jac[EMCMOT_MAX_JOINTS][EMCMOT_MAX_AXIS], + const KINEMATICS_INVERSE_FLAGS * iflags) +{ + (void)j; + (void)iflags; + double x_rot_point = hal_get_real(haldata->x_rot_point); + double y_rot_point = hal_get_real(haldata->y_rot_point); + double z_rot_point = hal_get_real(haldata->z_rot_point); + double dx = hal_get_real(haldata->x_offset); + double dz = hal_get_real(haldata->z_offset); + double dt = hal_get_real(haldata->tool_offset_z); + double sa = sin(pos->a*TO_RAD); + double ca = cos(pos->a*TO_RAD); + double sb = sin(pos->b*TO_RAD); + double cb = cos(pos->b*TO_RAD); + double qx = pos->tran.x - x_rot_point - dx; + double qy = pos->tran.y - y_rot_point; + double qz = pos->tran.z - z_rot_point - dz - dt; + int r, c; + + for (r = 0; r < EMCMOT_MAX_JOINTS; r++) { + for (c = 0; c < EMCMOT_MAX_AXIS; c++) { jac[r][c] = 0; } + } + + switch (switchkins_type) { + case 0: // ====================== IDENTITY kinematics JACOBIAN ==================== + jac[0][0] = 1; + jac[1][1] = 1; + jac[2][2] = 1; + jac[3][3] = 1; + jac[4][4] = 1; + break; + case 1: // ========================= TCP kinematics JACOBIAN ====================== + // the TCP inverse above differentiated: its coefficients of + // qx, qy and qz for the linear columns, and the same terms + // with a or b advanced a quarter turn for the rotary columns + jac[0][0] = cb; + jac[0][1] = sa*sb; + jac[0][2] = -ca*sb; + jac[0][3] = ( ca*sb*qy + sa*sb*qz) * TO_RAD; + jac[0][4] = (-sb*qx + sa*cb*qy - ca*cb*qz - sb*dx - cb*dz) * TO_RAD; + + jac[1][1] = ca; + jac[1][2] = sa; + jac[1][3] = (-sa*qy + ca*qz) * TO_RAD; + + jac[2][0] = sb; + jac[2][1] = -sa*cb; + jac[2][2] = ca*cb; + jac[2][3] = (-ca*cb*qy - sa*cb*qz) * TO_RAD; + jac[2][4] = ( cb*qx + sa*sb*qy - ca*sb*qz + cb*dx - sb*dz) * TO_RAD; + + jac[3][3] = 1; + jac[4][4] = 1; + break; + } + return 0; +} // kinematicsJacobian() diff --git a/src/hal/components/xyzacb_trsrn.comp b/src/hal/components/xyzacb_trsrn.comp index 321d45584e5..67fd97715a8 100644 --- a/src/hal/components/xyzacb_trsrn.comp +++ b/src/hal/components/xyzacb_trsrn.comp @@ -91,6 +91,7 @@ EXPORT_SYMBOL(kinematicsSwitchable); EXPORT_SYMBOL(kinematicsSwitch); EXPORT_SYMBOL(kinematicsTypeFlags); EXPORT_SYMBOL(kinematicsInverse); +EXPORT_SYMBOL(kinematicsJacobian); EXPORT_SYMBOL(kinematicsForward); EXPORT_SYMBOL(kinematicsToolFrame); EXPORT_SYMBOL(kinematicsWorkFrame); @@ -555,3 +556,138 @@ int kinematicsInverse(const EmcPose * pos, return 0; } // kinematicsInverse() + +int kinematicsJacobian(const double *j, + const EmcPose * pos, + double jac[EMCMOT_MAX_JOINTS][EMCMOT_MAX_AXIS], + const KINEMATICS_INVERSE_FLAGS * iflags) +{ + (void)j; + (void)iflags; + + // the same geometry as kinematicsInverse(), read the same way + double Ly = hal_get_real(haldata->y_pivot); + double Lz = hal_get_real(haldata->z_pivot); + double Dx = hal_get_real(haldata->x_offset); + double Dy = hal_get_real(haldata->y_offset); + double Dray = hal_get_real(haldata->y_rot_axis) - (Dy + Ly); + double Draz = hal_get_real(haldata->z_rot_axis) - Lz; + double tc = hal_get_real(haldata->pre_rot); + double nu = hal_get_real(haldata->nut_angle); // degrees + double theta_1 = hal_get_real(haldata->prim_angle); // degrees + double theta_2 = hal_get_real(haldata->sec_angle); // degrees + double Dt = hal_get_real(haldata->tool_offset_z); + + double Sv = sin(nu*TO_RAD); + double Cv = cos(nu*TO_RAD); + double Stc = sin(tc); + double Ctc = cos(tc); + + // The TCP inverse reads the rotary angles from its joint argument, + // where the machine is, and its own pose words for the same angles + // are the same numbers once the move is done. Its derivative is taken + // against the pose, which is what a consumer multiplies by. + double Sw = sin(pos->a*TO_RAD); + double Cw = cos(pos->a*TO_RAD); + double Ss = 0, Cs = 0, Sp = 0, Cp = 0; + double CvSs = 0, SvSs = 0, r = 0, s = 0, t = 0; + // derivatives of the above over the secondary angle (Ss, r, s, t, CvSs, + // SvSs) and the primary angle (Sp, Cp), per degree + double dSs = 0, dr = 0, ds = 0, dt_ = 0, dCvSs = 0, dSvSs = 0; + double dSp = 0, dCp = 0; + + double Qy = pos->tran.y; + double Qz = pos->tran.z; + double Ay, Az; // the two lever arms the table turns about + int R, C; + + for (R = 0; R < EMCMOT_MAX_JOINTS; R++) { + for (C = 0; C < EMCMOT_MAX_AXIS; C++) { jac[R][C] = 0; } + } + + switch (switchkins_type) { + + case 0: // ========================= IDENTITY kinematics JACOBIAN ==================== + for (R = 0; R < 6; R++) { jac[R][R] = 1; } + break; + + case 1: // ========================= TCP kinematics JACOBIAN + Ss = sin(pos->b*TO_RAD); + Cs = cos(pos->b*TO_RAD); + Sp = sin(pos->c*TO_RAD); + Cp = cos(pos->c*TO_RAD); + CvSs = Cv*Ss; + SvSs = Sv*Ss; + r = Cs + Sv*Sv*(1-Cs); + s = Cs + Cv*Cv*(1-Cs); + t = Sv*Cv*(1-Cs); + + dSs = Cs*TO_RAD; + dr = -Ss*Cv*Cv*TO_RAD; + ds = -Ss*Sv*Sv*TO_RAD; + dt_ = Sv*Cv*Ss*TO_RAD; + dCvSs = Cv*dSs; + dSvSs = Sv*dSs; + dSp = Cp*TO_RAD; + dCp = -Sp*TO_RAD; + + Ay = Dray + Dy + Ly - Qy; + Az = Draz + Dt + Lz - Qz; + + // j[0]: Qx plus terms in the head angles only + jac[0][0] = 1; + jac[0][4] = (Cp*dSvSs - Sp*dt_)*(Dt + Lz) - (Cp*dCvSs + Sp*dr)*Ly; + jac[0][5] = (dCp*SvSs - dSp*t)*(Dt + Lz) + dCp*Dx + - (dCp*CvSs + dSp*r)*Ly - Dy*dSp; + + // j[1]: -Cw*Ay - Az*Sw plus head terms + jac[1][1] = Cw; + jac[1][2] = Sw; + jac[1][3] = ( Sw*Ay - Az*Cw)*TO_RAD; + jac[1][4] = (Sp*dSvSs + Cp*dt_)*(Dt + Lz) - (dCvSs*Sp - Cp*dr)*Ly; + jac[1][5] = dCp*Dy + Dx*dSp + (dSp*SvSs + dCp*t)*(Dt + Lz) + - (CvSs*dSp - dCp*r)*Ly; + + // j[2]: -Cw*Az + Ay*Sw plus head terms + jac[2][1] = -Sw; + jac[2][2] = Cw; + jac[2][3] = ( Sw*Az + Ay*Cw)*TO_RAD; + jac[2][4] = (Dt + Lz)*ds + Ly*dt_; + + jac[3][3] = 1; + jac[4][4] = 1; + jac[5][5] = 1; + break; + + case 2: // ========================= TOOL kinematics JACOBIAN + // the head angles come from pins, so the inverse is linear in + // the pose and the rows are its coefficients + Ss = sin(theta_2*TO_RAD); + Cs = cos(theta_2*TO_RAD); + Sp = sin(theta_1*TO_RAD); + Cp = cos(theta_1*TO_RAD); + CvSs = Cv*Ss; + SvSs = Sv*Ss; + r = Cs + Sv*Sv*(1-Cs); + s = Cs + Cv*Cv*(1-Cs); + t = Sv*Cv*(1-Cs); + + jac[0][0] = ((Cp*Cs - CvSs*Sp)*Ctc - (Cp*CvSs + Sp*r)*Stc); + jac[0][1] = -((Cp*CvSs + Sp*r)*Ctc + (Cp*Cs - CvSs*Sp)*Stc); + jac[0][2] = (Cp*SvSs - Sp*t); + + jac[1][0] = ((Cp*CvSs + Cs*Sp)*Ctc - (CvSs*Sp - Cp*r)*Stc); + jac[1][1] = -((CvSs*Sp - Cp*r)*Ctc + (Cp*CvSs + Cs*Sp)*Stc); + jac[1][2] = (Sp*SvSs + Cp*t); + + jac[2][0] = -(Ctc*SvSs - Stc*t); + jac[2][1] = (Stc*SvSs + Ctc*t); + jac[2][2] = s; + + jac[3][3] = 1; + jac[4][4] = 1; + jac[5][5] = 1; + break; + } + return 0; +} // kinematicsJacobian() diff --git a/src/hal/components/xyzbca_trsrn.comp b/src/hal/components/xyzbca_trsrn.comp index 371349d2365..10126165eb1 100644 --- a/src/hal/components/xyzbca_trsrn.comp +++ b/src/hal/components/xyzbca_trsrn.comp @@ -93,6 +93,7 @@ EXPORT_SYMBOL(kinematicsSwitchable); EXPORT_SYMBOL(kinematicsSwitch); EXPORT_SYMBOL(kinematicsTypeFlags); EXPORT_SYMBOL(kinematicsInverse); +EXPORT_SYMBOL(kinematicsJacobian); EXPORT_SYMBOL(kinematicsForward); EXPORT_SYMBOL(kinematicsToolFrame); EXPORT_SYMBOL(kinematicsWorkFrame); @@ -560,3 +561,138 @@ int kinematicsInverse(const EmcPose * pos, return 0; } // kinematicsInverse() + +int kinematicsJacobian(const double *j, + const EmcPose * pos, + double jac[EMCMOT_MAX_JOINTS][EMCMOT_MAX_AXIS], + const KINEMATICS_INVERSE_FLAGS * iflags) +{ + (void)j; + (void)iflags; + + // the same geometry as kinematicsInverse(), read the same way + double Lx = hal_get_real(haldata->x_pivot); + double Lz = hal_get_real(haldata->z_pivot); + double Dx = hal_get_real(haldata->x_offset); + double Dy = hal_get_real(haldata->y_offset); + double Drax = hal_get_real(haldata->x_rot_axis) - Lx - Dx; + double Draz = hal_get_real(haldata->z_rot_axis) - Lz; + double tc = hal_get_real(haldata->pre_rot); + double nu = hal_get_real(haldata->nut_angle); // degrees + double theta_1 = hal_get_real(haldata->prim_angle); // degrees + double theta_2 = hal_get_real(haldata->sec_angle); // degrees + double Dt = hal_get_real(haldata->tool_offset_z); + + double Sv = sin(nu*TO_RAD); + double Cv = cos(nu*TO_RAD); + double Stc = sin(tc); + double Ctc = cos(tc); + + // The TCP inverse reads the rotary angles from its joint argument, + // where the machine is, and its own pose words for the same angles + // are the same numbers once the move is done. Its derivative is taken + // against the pose, which is what a consumer multiplies by. + double Sw = sin(pos->b*TO_RAD); + double Cw = cos(pos->b*TO_RAD); + double Ss = 0, Cs = 0, Sp = 0, Cp = 0; + double CvSs = 0, SvSs = 0, r = 0, s = 0, t = 0; + // derivatives of the above over the secondary angle (Ss, r, s, t, CvSs, + // SvSs) and the primary angle (Sp, Cp), per degree + double dSs = 0, dr = 0, ds = 0, dt_ = 0, dCvSs = 0, dSvSs = 0; + double dSp = 0, dCp = 0; + + double Qx = pos->tran.x; + double Qz = pos->tran.z; + double Ax, Az; // the two lever arms the table turns about + int R, C; + + for (R = 0; R < EMCMOT_MAX_JOINTS; R++) { + for (C = 0; C < EMCMOT_MAX_AXIS; C++) { jac[R][C] = 0; } + } + + switch (switchkins_type) { + + case 0: // ========================= IDENTITY kinematics JACOBIAN ==================== + for (R = 0; R < 6; R++) { jac[R][R] = 1; } + break; + + case 1: // ========================= TCP kinematics JACOBIAN + Ss = sin(pos->a*TO_RAD); + Cs = cos(pos->a*TO_RAD); + Sp = sin(pos->c*TO_RAD); + Cp = cos(pos->c*TO_RAD); + CvSs = Cv*Ss; + SvSs = Sv*Ss; + r = Cs + Sv*Sv*(1-Cs); + s = Cs + Cv*Cv*(1-Cs); + t = Sv*Cv*(1-Cs); + + dSs = Cs*TO_RAD; + dr = -Ss*Cv*Cv*TO_RAD; + ds = -Ss*Sv*Sv*TO_RAD; + dt_ = Sv*Cv*Ss*TO_RAD; + dCvSs = Cv*dSs; + dSvSs = Sv*dSs; + dSp = Cp*TO_RAD; + dCp = -Sp*TO_RAD; + + Ax = Drax + Dx + Lx - Qx; + Az = Draz + Dt + Lz - Qz; + + // j[0]: -Cw*Ax + Az*Sw plus head terms + jac[0][0] = Cw; + jac[0][2] = -Sw; + jac[0][3] = (Sp*dSvSs + Cp*dt_)*(Dt + Lz) - (dCvSs*Sp - Cp*dr)*Lx; + jac[0][4] = ( Sw*Ax + Az*Cw)*TO_RAD; + jac[0][5] = dCp*Dx - Dy*dSp + (dSp*SvSs + dCp*t)*(Dt + Lz) + - (CvSs*dSp - dCp*r)*Lx; + + // j[1]: Qy plus head terms + jac[1][1] = 1; + jac[1][3] = -(Cp*dSvSs - Sp*dt_)*(Dt + Lz) + (Cp*dCvSs + Sp*dr)*Lx; + jac[1][5] = -(dCp*SvSs - dSp*t)*(Dt + Lz) + dCp*Dy + + (dCp*CvSs + dSp*r)*Lx + Dx*dSp; + + // j[2]: -Cw*Az - Ax*Sw plus head terms + jac[2][0] = Sw; + jac[2][2] = Cw; + jac[2][3] = (Dt + Lz)*ds + Lx*dt_; + jac[2][4] = ( Sw*Az - Ax*Cw)*TO_RAD; + + jac[3][3] = 1; + jac[4][4] = 1; + jac[5][5] = 1; + break; + + case 2: // ========================= TOOL kinematics JACOBIAN + // the head angles come from pins, so the inverse is linear in + // the pose and the rows are its coefficients + Ss = sin(theta_2*TO_RAD); + Cs = cos(theta_2*TO_RAD); + Sp = sin(theta_1*TO_RAD); + Cp = cos(theta_1*TO_RAD); + CvSs = Cv*Ss; + SvSs = Sv*Ss; + r = Cs + Sv*Sv*(1-Cs); + s = Cs + Cv*Cv*(1-Cs); + t = Sv*Cv*(1-Cs); + + jac[0][0] = -((CvSs*Sp - Cp*r)*Ctc + (Cp*CvSs + Cs*Sp)*Stc); + jac[0][1] = -((Cp*CvSs + Cs*Sp)*Ctc - (CvSs*Sp - Cp*r)*Stc); + jac[0][2] = (Sp*SvSs + Cp*t); + + jac[1][0] = ((Cp*CvSs + Sp*r)*Ctc + (Cp*Cs - CvSs*Sp)*Stc); + jac[1][1] = ((Cp*Cs - CvSs*Sp)*Ctc - (Cp*CvSs + Sp*r)*Stc); + jac[1][2] = -(Cp*SvSs - Sp*t); + + jac[2][0] = (Stc*SvSs + Ctc*t); + jac[2][1] = (Ctc*SvSs - Stc*t); + jac[2][2] = s; + + jac[3][3] = 1; + jac[4][4] = 1; + jac[5][5] = 1; + break; + } + return 0; +} // kinematicsJacobian() From 5d454a8a465df0fb433ea588c53cff45c2d1cc6e Mon Sep 17 00:00:00 2001 From: Luca Toniolo <10792599+grandixximo@users.noreply.github.com> Date: Thu, 3 Sep 2026 18:21:25 +1000 Subject: [PATCH 13/60] tripodkins, lineardeltakins, rotarydeltakins, genhexkins, pentakins: 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. --- src/emc/kinematics/genhexkins.c | 71 ++++++++++++++++++++++++++ src/emc/kinematics/lineardeltakins.c | 27 ++++++++++ src/emc/kinematics/pentakins.c | 75 ++++++++++++++++++++++++++++ src/emc/kinematics/rotarydeltakins.c | 53 ++++++++++++++++++++ src/emc/kinematics/tripodkins.c | 32 ++++++++++++ 5 files changed, 258 insertions(+) diff --git a/src/emc/kinematics/genhexkins.c b/src/emc/kinematics/genhexkins.c index 2966c377852..3493d58d5f3 100644 --- a/src/emc/kinematics/genhexkins.c +++ b/src/emc/kinematics/genhexkins.c @@ -544,6 +544,75 @@ static int genhexKinematicsInverse(const EmcPose * pos, return 0; } //genhexKinematicsInverse() +/************************ genhexKinematicsJacobian() ***********************/ +/* A strut length changes by the component of its platform end's motion + along the strut. That end moves with the platform, dP + w x (R a), so + the row for strut i is [u_i, (R a_i x u_i) . E] with u_i the unit strut + vector and E the matrix taking the rates of the roll, pitch and yaw + words to the angular velocity w for R = Rz(c) Ry(b) Rx(a). The forward + kinematics builds the same rows for its Newton step, in radians. */ + +static int genhexKinematicsJacobian(const double * joints, + const EmcPose * pos, + double jac[EMCMOT_MAX_JOINTS][EMCMOT_MAX_AXIS], + const KINEMATICS_INVERSE_FLAGS * iflags) +{ + PmCartesian aw, RMatrix_a, strut, u, moment; + PmRotationMatrix RMatrix; + PmRpy rpy; + PmCartesian E[3]; + double sb, cb, sc, cc; + int i, j, col, m; + + genhex_read_hal_pins(); + + /* the screw lead correction is a function of the pose too, and this + does not differentiate it; difference the inverse instead */ + if (hal_get_real(haldata->screw_lead) != 0.0) { + return kinsJacobianFromInverse(genhexKinematicsInverse, NUM_STRUTS, + joints, pos, iflags, jac); + } + + for (j = 0; j < EMCMOT_MAX_JOINTS; j++) { + for (col = 0; col < EMCMOT_MAX_AXIS; col++) { jac[j][col] = 0; } + } + + rpy.r = pos->a * PM_PI / 180.0; + rpy.p = pos->b * PM_PI / 180.0; + rpy.y = pos->c * PM_PI / 180.0; + pmRpyMatConvert(&rpy, &RMatrix); + + /* w = E [da db dc]: the roll axis carried by pitch and yaw, the pitch + axis carried by yaw, and the yaw axis fixed */ + sb = sin(rpy.p); cb = cos(rpy.p); + sc = sin(rpy.y); cc = cos(rpy.y); + E[0].x = cb*cc; E[0].y = cb*sc; E[0].z = -sb; + E[1].x = -sc; E[1].y = cc; E[1].z = 0; + E[2].x = 0; E[2].y = 0; E[2].z = 1; + + for (i = 0; i < NUM_STRUTS; i++) { + double len; + + pmMatCartMult(&RMatrix, &a[i], &RMatrix_a); + pmCartCartAdd(&pos->tran, &RMatrix_a, &aw); + pmCartCartSub(&aw, &b[i], &strut); + pmCartMag(&strut, &len); + if (len <= 0) { return -1; } + pmCartScalMult(&strut, 1.0/len, &u); + pmCartCartCross(&RMatrix_a, &u, &moment); + + jac[i][0] = u.x; + jac[i][1] = u.y; + jac[i][2] = u.z; + for (m = 0; m < 3; m++) { + double dot; + pmCartCartDot(&moment, &E[m], &dot); + jac[i][3+m] = dot * PM_PI / 180.0; + } + } + return 0; +} // genhexKinematicsJacobian() + // HAL pin initializaion values. In small arrays so we can easily // address them in the pin creation loop. static const rtapi_real init_basex[NUM_STRUTS] = { @@ -707,6 +776,7 @@ int switchkinsSetup(kparms* kp, *kinv1 = genhexKinematicsInverse; switchkinsDeclare(0, KINSTYPE_IDENTITY); switchkinsDeclare(1, KINSTYPE_PRIMARY); + switchkinsRegisterJacobian(1, genhexKinematicsJacobian); } else { rtapi_print("\n!!! switchkins-type 0 is %s\n",kp->kinsname); kp->fwd_iterates_mask = 0x1; //genhexkins switchkins_type==0 @@ -715,6 +785,7 @@ int switchkinsSetup(kparms* kp, *kset0 = genhexKinematicsSetup; *kfwd0 = genhexKinematicsForward; *kinv0 = genhexKinematicsInverse; + switchkinsRegisterJacobian(0, genhexKinematicsJacobian); *kset1 = identityKinematicsSetup; *kfwd1 = identityKinematicsForward; diff --git a/src/emc/kinematics/lineardeltakins.c b/src/emc/kinematics/lineardeltakins.c index 353e9234562..541643fef74 100644 --- a/src/emc/kinematics/lineardeltakins.c +++ b/src/emc/kinematics/lineardeltakins.c @@ -48,6 +48,32 @@ int kinematicsInverse(const EmcPose *pos, double *joints, return kinematics_inverse(pos, joints); } +int kinematicsJacobian(const double *joints, + const EmcPose *pos, + double jac[EMCMOT_MAX_JOINTS][EMCMOT_MAX_AXIS], + const KINEMATICS_INVERSE_FLAGS *iflags) { + double x = pos->tran.x, y = pos->tran.y, z = pos->tran.z; + int i, j, a; + (void)iflags; + set_geometry(hal_get_real(haldata->r), hal_get_real(haldata->l)); + for (j = 0; j < EMCMOT_MAX_JOINTS; j++) { + for (a = 0; a < EMCMOT_MAX_AXIS; a++) { jac[j][a] = 0; } + } + // each carriage is the platform height plus the rise of its rod, and + // the rise changes with the horizontal offset from the tower + for (i = 0; i < 3; i++) { + double tx = (i == 0) ? Ax : (i == 1) ? Bx : Cx; + double ty = (i == 0) ? Ay : (i == 1) ? By : Cy; + double rise = joints[i] - z; + if (rise <= 0) { return -1; } + jac[i][0] = (tx - x)/rise; + jac[i][1] = (ty - y)/rise; + jac[i][2] = 1; + } + for (j = 3; j < 9; j++) { jac[j][j] = 1; } + return 0; +} + KINEMATICS_TYPE kinematicsType() { return KINEMATICS_BOTH; @@ -85,4 +111,5 @@ KINS_NOT_SWITCHABLE EXPORT_SYMBOL(kinematicsType); EXPORT_SYMBOL(kinematicsForward); EXPORT_SYMBOL(kinematicsInverse); +EXPORT_SYMBOL(kinematicsJacobian); MODULE_LICENSE("GPL"); diff --git a/src/emc/kinematics/pentakins.c b/src/emc/kinematics/pentakins.c index 18487be3134..f8415b4112c 100644 --- a/src/emc/kinematics/pentakins.c +++ b/src/emc/kinematics/pentakins.c @@ -399,6 +399,80 @@ int kinematicsInverse(const EmcPose * pos, return 0; } +int kinematicsJacobian(const double * joints, + const EmcPose * pos, + double jac[EMCMOT_MAX_JOINTS][EMCMOT_MAX_AXIS], + const KINEMATICS_INVERSE_FLAGS * iflags) +{ + PmRotationMatrix R; + PmRpy rpy; + PmCartesian P, d, xyz, wa, wb, dxyz[5]; + int i, j, a, col; + + (void)joints; + (void)iflags; + pentakins_read_hal_pins(); + for (j = 0; j < EMCMOT_MAX_JOINTS; j++) { + for (a = 0; a < EMCMOT_MAX_AXIS; a++) { jac[j][a] = 0; } + } + + /* InvKins() differentiated. The effector end of each strut is found in + effector coordinates as xyz = R^T (b - P) with R = Ry(b) Rx(a), so a + pose translation moves it by -R^T and a pose rotation about w moves + it by -R^T (w x (b - P)); the strut length is then the distance from + that point to the strut's pivot circle of radius ra at height za. */ + P = pos->tran; + rpy.r = pos->a * PM_PI / 180.0; + rpy.p = pos->b * PM_PI / 180.0; + rpy.y = 0; + pmRpyMatConvert(&rpy, &R); + + /* rotation axes for a and b, in world coordinates */ + wa.x = cos(rpy.p); wa.y = 0; wa.z = -sin(rpy.p); + wb.x = 0; wb.y = 1; wb.z = 0; + + for (i = 0; i < NUM_STRUTS; i++) { + double rho, A, B, len; + + pmCartCartSub(&b[i], &P, &d); + /* R^T d, written out since pmMatCartMult applies R */ + xyz.x = R.x.x*d.x + R.x.y*d.y + R.x.z*d.z; + xyz.y = R.y.x*d.x + R.y.y*d.y + R.y.z*d.z; + xyz.z = R.z.x*d.x + R.z.y*d.y + R.z.z*d.z; + + /* d xyz / d pose, one PmCartesian per pose column x y z a b */ + for (col = 0; col < 3; col++) { + /* -R^T e_col, which is minus row col of R^T, i.e. minus column + col of R read as a row of R^T */ + PmCartesian e = {0, 0, 0}, w; + if (col == 0) e.x = 1; else if (col == 1) e.y = 1; else e.z = 1; + w.x = -(R.x.x*e.x + R.x.y*e.y + R.x.z*e.z); + w.y = -(R.y.x*e.x + R.y.y*e.y + R.y.z*e.z); + w.z = -(R.z.x*e.x + R.z.y*e.y + R.z.z*e.z); + dxyz[col] = w; + } + for (col = 3; col < 5; col++) { + PmCartesian cr, w; + pmCartCartCross(col == 3 ? &wa : &wb, &d, &cr); + w.x = -(R.x.x*cr.x + R.x.y*cr.y + R.x.z*cr.z) * (PM_PI/180.0); + w.y = -(R.y.x*cr.x + R.y.y*cr.y + R.y.z*cr.z) * (PM_PI/180.0); + w.z = -(R.z.x*cr.x + R.z.y*cr.y + R.z.z*cr.z) * (PM_PI/180.0); + dxyz[col] = w; + } + + rho = sqrt(sqr(xyz.x) + sqr(xyz.y)); + A = xyz.z - za[i]; + B = rho - ra[i]; + len = sqrt(sqr(A) + sqr(B)); + if (len <= 0 || rho <= 0) { return -1; } + for (col = 0; col < 5; col++) { + jac[i][col] = (A*dxyz[col].z + + B*(xyz.x*dxyz[col].x + xyz.y*dxyz[col].y)/rho) / len; + } + } + return 0; +} + KINEMATICS_TYPE kinematicsType() { return KINEMATICS_BOTH; @@ -408,6 +482,7 @@ KINS_NOT_SWITCHABLE EXPORT_SYMBOL(kinematicsType); EXPORT_SYMBOL(kinematicsForward); EXPORT_SYMBOL(kinematicsInverse); +EXPORT_SYMBOL(kinematicsJacobian); MODULE_LICENSE("GPL"); diff --git a/src/emc/kinematics/rotarydeltakins.c b/src/emc/kinematics/rotarydeltakins.c index 8c83ebdec4f..a2f52c10c1c 100644 --- a/src/emc/kinematics/rotarydeltakins.c +++ b/src/emc/kinematics/rotarydeltakins.c @@ -51,6 +51,58 @@ int kinematicsInverse(const EmcPose *pos, double *joints, return kinematics_inverse(pos, joints); } +int kinematicsJacobian(const double *joints, + const EmcPose *pos, + double jac[EMCMOT_MAX_JOINTS][EMCMOT_MAX_AXIS], + const KINEMATICS_INVERSE_FLAGS *iflags) { + int i, j, a; + (void)iflags; + set_geometry(hal_get_real(haldata->pfr), hal_get_real(haldata->tl), hal_get_real(haldata->sl), hal_get_real(haldata->fr)); + for (j = 0; j < EMCMOT_MAX_JOINTS; j++) { + for (a = 0; a < EMCMOT_MAX_AXIS; a++) { jac[j][a] = 0; } + } + // The foot stays a shin length from each knee, so along a leg the + // motion of the foot and the motion of the knee agree: + // (P - K) . dP = (P - K) . dK/dq dq + // K is the knee less the foot offset, written as kinematics_forward() + // writes it, and q the hip angle that swings it. + for (i = 0; i < 3; i++) { + double q = D2R(joints[i]); + double reach = platformradius - footradius + thighlength * cos(q); + double kx, ky, kz, dkx, dky, dkz, px, py, pz, denom; + switch (i) { + case 0: + kx = 0; ky = -reach; + dkx = 0; dky = thighlength * sin(q); + break; + case 1: + kx = reach * 0.5 * sqrt(3); ky = reach * 0.5; + dkx = -thighlength * sin(q) * 0.5 * sqrt(3); + dky = -thighlength * sin(q) * 0.5; + break; + default: + kx = -reach * 0.5 * sqrt(3); ky = reach * 0.5; + dkx = thighlength * sin(q) * 0.5 * sqrt(3); + dky = -thighlength * sin(q) * 0.5; + break; + } + kz = -thighlength * sin(q); + dkz = -thighlength * cos(q); + px = pos->tran.x - kx; + py = pos->tran.y - ky; + pz = pos->tran.z - kz; + denom = (px*dkx + py*dky + pz*dkz) * (M_PI/180.); + // the shin at right angles to the thigh's swing: the knee cannot + // move the foot, so no finite hip rate follows the foot + if (fabs(denom) < 1e-12) { return -1; } + jac[i][0] = px/denom; + jac[i][1] = py/denom; + jac[i][2] = pz/denom; + } + for (j = 3; j < 9; j++) { jac[j][j] = 1; } + return 0; +} + KINEMATICS_TYPE kinematicsType() { return KINEMATICS_BOTH; @@ -92,4 +144,5 @@ KINS_NOT_SWITCHABLE EXPORT_SYMBOL(kinematicsType); EXPORT_SYMBOL(kinematicsForward); EXPORT_SYMBOL(kinematicsInverse); +EXPORT_SYMBOL(kinematicsJacobian); MODULE_LICENSE("GPL"); diff --git a/src/emc/kinematics/tripodkins.c b/src/emc/kinematics/tripodkins.c index 990b7997297..c58d726dd46 100644 --- a/src/emc/kinematics/tripodkins.c +++ b/src/emc/kinematics/tripodkins.c @@ -218,6 +218,37 @@ int kinematicsInverse(const EmcPose * pos, #undef Dz } +int kinematicsJacobian(const double * joints, + const EmcPose * pos, + double jac[EMCMOT_MAX_JOINTS][EMCMOT_MAX_AXIS], + const KINEMATICS_INVERSE_FLAGS * iflags) +{ + rtapi_real Bx = hal_get_real(haldata->bx); + rtapi_real Cx = hal_get_real(haldata->cx); + rtapi_real Cy = hal_get_real(haldata->cy); + /* the three strut base points, in the order of the joints */ + const double base[3][2] = { {0, 0}, {Bx, 0}, {Cx, Cy} }; + int i, j, a; + + (void)iflags; + for (j = 0; j < EMCMOT_MAX_JOINTS; j++) { + for (a = 0; a < EMCMOT_MAX_AXIS; a++) { jac[j][a] = 0; } + } + /* a strut length changes by the component of the motion along the + strut, so each row is the unit vector from base to D */ + for (i = 0; i < 3; i++) { + double dx = pos->tran.x - base[i][0]; + double dy = pos->tran.y - base[i][1]; + double dz = pos->tran.z; + double len = joints[i]; + if (len <= 0) { return -1; } + jac[i][0] = dx/len; + jac[i][1] = dy/len; + jac[i][2] = dz/len; + } + return 0; +} + KINEMATICS_TYPE kinematicsType() { return KINEMATICS_BOTH; @@ -356,6 +387,7 @@ KINS_NOT_SWITCHABLE EXPORT_SYMBOL(kinematicsType); EXPORT_SYMBOL(kinematicsForward); EXPORT_SYMBOL(kinematicsInverse); +EXPORT_SYMBOL(kinematicsJacobian); MODULE_LICENSE("GPL"); From a052cf104fd055e8eaecfb3e1fc720afee902a4f Mon Sep 17 00:00:00 2001 From: Luca Toniolo <10792599+grandixximo@users.noreply.github.com> Date: Thu, 3 Sep 2026 18:21:25 +1000 Subject: [PATCH 14/60] scarakins, scorbot-kins: supply the Jacobian 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. --- src/emc/kinematics/scarakins.c | 54 +++++++++++++++++++++++ src/emc/kinematics/scorbot-kins.c | 71 +++++++++++++++++++++++++++++++ 2 files changed, 125 insertions(+) diff --git a/src/emc/kinematics/scarakins.c b/src/emc/kinematics/scarakins.c index 3b0e69ee7e4..fa237e62b31 100644 --- a/src/emc/kinematics/scarakins.c +++ b/src/emc/kinematics/scarakins.c @@ -179,6 +179,58 @@ static int scaraKinematicsInverse(const EmcPose * world, return (0); } // scaraKinematicsInverse() +static int scaraKinematicsJacobian(const double * joint, + const EmcPose * world, + double jac[EMCMOT_MAX_JOINTS][EMCMOT_MAX_AXIS], + const KINEMATICS_INVERSE_FLAGS * iflags) +{ + (void)iflags; + rtapi_real D2 = hal_get_real(haldata->d2); + rtapi_real D4 = hal_get_real(haldata->d4); + rtapi_real D6 = hal_get_real(haldata->d6); + const double a3 = world->c * (PM_PI / 180); + const double q1 = joint[1] * (PM_PI / 180); + const double xt = world->tran.x - D6*cos(a3); + const double yt = world->tran.y - D6*sin(a3); + const double rsq = xt*xt + yt*yt; + /* gradients over (x, y, c) of the quantities the inverse builds */ + double d_xt[3] = { 1, 0, D6*sin(a3) * (PM_PI/180) }; + double d_yt[3] = { 0, 1, -D6*cos(a3) * (PM_PI/180) }; + double d_q1[3], d_q0[3], dphi_dq1; + int i, j, a; + + if (rsq <= 0 || fabs(sin(q1)) < 1e-12) { + /* the arm folded or straight out: the elbow rate is unbounded */ + return -1; + } + for (j = 0; j < EMCMOT_MAX_JOINTS; j++) { + for (a = 0; a < EMCMOT_MAX_AXIS; a++) { jac[j][a] = 0; } + } + + /* rsq = D2^2 + D4^2 + 2 D2 D4 cos(q1), so q1 follows rsq; q0 is the + bearing of the end effector less the angle the outer arm subtends, + whose rate over q1 is (D2 D4 cos(q1) + D4^2) / rsq */ + dphi_dq1 = (D2*D4*cos(q1) + D4*D4) / rsq; + for (i = 0; i < 3; i++) { + double d_rsq = 2*xt*d_xt[i] + 2*yt*d_yt[i]; + d_q1[i] = -d_rsq / (2*D2*D4*sin(q1)); + d_q0[i] = (xt*d_yt[i] - yt*d_xt[i]) / rsq - dphi_dq1 * d_q1[i]; + } + + /* columns x, y, c; the rest of the pose does not reach these joints */ + for (i = 0; i < 3; i++) { + int col = (i == 2) ? 5 : i; + jac[0][col] = d_q0[i] * (180 / PM_PI); + jac[1][col] = d_q1[i] * (180 / PM_PI); + jac[3][col] = -(jac[0][col] + jac[1][col]); + } + jac[3][5] += 1; + jac[2][2] = -1; + jac[4][3] = 1; + jac[5][4] = 1; + return 0; +} // scaraKinematicsJacobian() + #define DEFAULT_D1 490 #define DEFAULT_D2 340 #define DEFAULT_D3 50 @@ -233,11 +285,13 @@ int switchkinsSetup(kparms* kp, *kinv1 = scaraKinematicsInverse; switchkinsDeclare(0, KINSTYPE_IDENTITY); switchkinsDeclare(1, KINSTYPE_PRIMARY); + switchkinsRegisterJacobian(1, scaraKinematicsJacobian); } else { rtapi_print("\n!!! switchkins-type 0 is %s\n",kp->kinsname); *kset0 = scaraKinematicsSetup; *kfwd0 = scaraKinematicsForward; *kinv0 = scaraKinematicsInverse; + switchkinsRegisterJacobian(0, scaraKinematicsJacobian); *kset1 = identityKinematicsSetup; *kfwd1 = identityKinematicsForward; diff --git a/src/emc/kinematics/scorbot-kins.c b/src/emc/kinematics/scorbot-kins.c index bd8868a063d..b7f933a3b71 100644 --- a/src/emc/kinematics/scorbot-kins.c +++ b/src/emc/kinematics/scorbot-kins.c @@ -294,6 +294,76 @@ int kinematicsInverse( } +int kinematicsJacobian( + const double *joints, + const EmcPose *pose, + double jac[EMCMOT_MAX_JOINTS][EMCMOT_MAX_AXIS], + const KINEMATICS_INVERSE_FLAGS *iflags +) { + // kinematicsInverse() above, differentiated step by step in the same + // order, each quantity carried as its gradient over (x, y, z) + const double x = pose->tran.x, y = pose->tran.y; + const double rho2 = x*x + y*y; + const double rho = sqrt(rho2); + double r_cp, z_cp, dist, angle_to_cp, j1_angle, j1, z_j2, u; + double d_r_cp[3], d_z_cp[3], d_dist[3], d_angle[3], d_j1a[3], d_j1[3], d_j2[3]; + double q; + int i, j, a; + + (void)joints; + (void)iflags; + if (rho2 <= 0) { return -1; } + for (j = 0; j < EMCMOT_MAX_JOINTS; j++) { + for (a = 0; a < EMCMOT_MAX_AXIS; a++) { jac[j][a] = 0; } + } + + // j0 = atan2(y, x) + jac[0][0] = -y/rho2 * TO_DEG; + jac[0][1] = x/rho2 * TO_DEG; + + r_cp = rho - L0_HORIZONTAL_DISTANCE; + z_cp = pose->tran.z - L0_VERTICAL_DISTANCE; + d_r_cp[0] = x/rho; d_r_cp[1] = y/rho; d_r_cp[2] = 0; + d_z_cp[0] = 0; d_z_cp[1] = 0; d_z_cp[2] = 1; + + dist = sqrt(r_cp*r_cp + z_cp*z_cp); + if (dist <= 0 || dist >= 2*L1_LENGTH) { return -1; } + for (i = 0; i < 3; i++) { + d_dist[i] = (r_cp*d_r_cp[i] + z_cp*d_z_cp[i]) / dist; + } + + // the signed acos in the inverse is atan2(z_cp, r_cp) + angle_to_cp = TO_DEG * atan2(z_cp, r_cp); + for (i = 0; i < 3; i++) { + d_angle[i] = TO_DEG * (r_cp*d_z_cp[i] - z_cp*d_r_cp[i]) / (dist*dist); + } + + q = dist / (2*L1_LENGTH); + j1_angle = TO_DEG * acos(q); + for (i = 0; i < 3; i++) { + d_j1a[i] = -TO_DEG / sqrt(1 - q*q) * d_dist[i] / (2*L1_LENGTH); + } + + j1 = angle_to_cp + j1_angle; + for (i = 0; i < 3; i++) { + d_j1[i] = d_angle[i] + d_j1a[i]; + jac[1][i] = d_j1[i]; + } + + z_j2 = L1_LENGTH * sin(TO_RAD * j1); + u = (z_j2 - z_cp) / L2_LENGTH; + if (fabs(u) >= 1) { return -1; } + for (i = 0; i < 3; i++) { + double d_z_j2 = L1_LENGTH * cos(TO_RAD * j1) * TO_RAD * d_j1[i]; + d_j2[i] = -TO_DEG / sqrt(1 - u*u) * (d_z_j2 - d_z_cp[i]) / L2_LENGTH; + jac[2][i] = d_j2[i]; + } + + jac[3][3] = 1; + jac[4][4] = 1; + return 0; +} + KINEMATICS_TYPE kinematicsType(void) { return KINEMATICS_BOTH; } @@ -302,6 +372,7 @@ KINS_NOT_SWITCHABLE EXPORT_SYMBOL(kinematicsType); EXPORT_SYMBOL(kinematicsForward); EXPORT_SYMBOL(kinematicsInverse); +EXPORT_SYMBOL(kinematicsJacobian); MODULE_LICENSE("GPL"); static int comp_id; From 84bc963df2527d57ed8d5c6a1fd7e2c660bd0afc Mon Sep 17 00:00:00 2001 From: Luca Toniolo <10792599+grandixximo@users.noreply.github.com> Date: Thu, 3 Sep 2026 18:21:25 +1000 Subject: [PATCH 15/60] genserkins: supply the Jacobian from its geometric one 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. --- src/emc/kinematics/genserfuncs.c | 107 +++++++++++++++++++++++++++++++ src/emc/kinematics/genserkins.c | 2 + src/emc/kinematics/genserkins.h | 5 ++ 3 files changed, 114 insertions(+) diff --git a/src/emc/kinematics/genserfuncs.c b/src/emc/kinematics/genserfuncs.c index 5600ab2be1f..d8432cdec3d 100644 --- a/src/emc/kinematics/genserfuncs.c +++ b/src/emc/kinematics/genserfuncs.c @@ -313,6 +313,113 @@ int genser_kin_jac_fwd(void *kins, return GO_RESULT_OK; } +/* The Jacobian in the terms of kinematics.h: joints in degrees per pose + word in EmcPose units, the derivative of genserKinematicsInverse(). + + compute_jinv() gives the geometric inverse Jacobian, radians of joint per + unit of base-frame twist. A pose word rate is not a twist: the roll, + pitch and yaw rates reach the angular velocity through E, the matrix of + the axes each one turns about, for the RPY convention of go_rpy_mat_convert, + R = Rz(yaw) Ry(pitch) Rx(roll). So + + dq/dp = unrotate . deg . Jinv . blockdiag(I, E . rad) + + with the unit conversions and the unrotate coupling applied in the order + the inverse applies them. */ +int genserKinematicsJacobian(const double *joint, + const EmcPose *world, + double jac[EMCMOT_MAX_JOINTS][EMCMOT_MAX_AXIS], + const KINEMATICS_INVERSE_FLAGS *iflags) +{ + (void)iflags; + genser_struct *genser = KINS_PTR; + GO_MATRIX_DECLARE(Jfwd, Jfwd_stg, 6, GENSER_MAX_JOINTS); + GO_MATRIX_DECLARE(Jinv, Jinv_stg, GENSER_MAX_JOINTS, 6); + go_pose T_L_0; + go_link linkout[GENSER_MAX_JOINTS] = {}; + go_real jest[GENSER_MAX_JOINTS]; + double E[3][3]; + double sb, cb, sc, cc; + int link, i, j, a, m, retval; + +#ifndef ULAPI + genser_kin_init(); + if (!genser_hal_inited) { + rtapi_print_msg(RTAPI_MSG_ERR, + "genserKinematicsJacobian: not initialized\n"); + return -1; + } +#endif + + for (j = 0; j < EMCMOT_MAX_JOINTS; j++) { + for (a = 0; a < EMCMOT_MAX_AXIS; a++) { jac[j][a] = 0; } + } + + // the kinematic joint angles, in radians and with the unrotate + // coupling removed, exactly as the forward prepares them + for (link = 0; link < genser->link_num; link++) { + rtapi_s32 unrotate = hal_get_si32(haldata->unrotate[link]); + jest[link] = joint[link] * (PM_PI / 180); + if (link && unrotate) + jest[link] -= unrotate * jest[link-1]; + } + + go_matrix_init(Jfwd, Jfwd_stg, 6, genser->link_num); + go_matrix_init(Jinv, Jinv_stg, genser->link_num, 6); + + for (link = 0; link < genser->link_num; link++) { + retval = go_link_joint_set(&genser->links[link], jest[link], &linkout[link]); + if (GO_RESULT_OK != retval) + return -1; + } + retval = compute_jfwd(linkout, genser->link_num, &Jfwd, &T_L_0); + if (GO_RESULT_OK != retval) + return -1; + retval = compute_jinv(&Jfwd, &Jinv); + if (GO_RESULT_OK != retval) + return -1; // singular: no finite joint rate follows the pose + + // E columns: the roll axis carried by pitch and yaw, the pitch axis + // carried by yaw, and the yaw axis fixed + sb = sin(world->b * PM_PI / 180); cb = cos(world->b * PM_PI / 180); + sc = sin(world->c * PM_PI / 180); cc = cos(world->c * PM_PI / 180); + E[0][0] = cb*cc; E[1][0] = cb*sc; E[2][0] = -sb; + E[0][1] = -sc; E[1][1] = cc; E[2][1] = 0; + E[0][2] = 0; E[1][2] = 0; E[2][2] = 1; + + for (i = 0; i < genser->link_num; i++) { + // linear pose words: the twist column is the pose column, and the + // joint comes out in radians + for (a = 0; a < 3; a++) { + jac[i][a] = Jinv.el[i][a] * (180 / PM_PI); + } + // angular pose words: through E, radians of pose word per degree + // of pose word and degrees of joint per radian of joint cancel + for (m = 0; m < 3; m++) { + double s = 0; + for (a = 0; a < 3; a++) { s += Jinv.el[i][3+a] * E[a][m]; } + jac[i][3+m] = s; + } + } + + // the unrotate coupling, in link order as the inverse applies it + for (link = 1; link < genser->link_num; link++) { + rtapi_s32 unrotate = hal_get_si32(haldata->unrotate[link]); + if (unrotate) { + for (a = 0; a < EMCMOT_MAX_AXIS; a++) { + jac[link][a] += unrotate * jac[link-1][a]; + } + } + } + + // uvw pass through as joints 6, 7, 8 + if (total_joints > 6) jac[6][6] = 1; + if (total_joints > 7) jac[7][7] = 1; + if (total_joints > 8) jac[8][8] = 1; + + return 0; +} // genserKinematicsJacobian() + /* main function called by emc2 for forward Kins */ int genserKinematicsForward(const double *joint, EmcPose * world, diff --git a/src/emc/kinematics/genserkins.c b/src/emc/kinematics/genserkins.c index fa8d30598dc..94a325cbcf6 100644 --- a/src/emc/kinematics/genserkins.c +++ b/src/emc/kinematics/genserkins.c @@ -74,11 +74,13 @@ int switchkinsSetup(kparms* kp, *kinv1 = genserKinematicsInverse; switchkinsDeclare(0, KINSTYPE_IDENTITY); switchkinsDeclare(1, KINSTYPE_PRIMARY); + switchkinsRegisterJacobian(1, genserKinematicsJacobian); } else { rtapi_print("\n!!! switchkins-type 0 is %s\n",kp->kinsname); *kset0 = genserKinematicsSetup; *kfwd0 = genserKinematicsForward; *kinv0 = genserKinematicsInverse; + switchkinsRegisterJacobian(0, genserKinematicsJacobian); *kset1 = identityKinematicsSetup; *kfwd1 = identityKinematicsForward; diff --git a/src/emc/kinematics/genserkins.h b/src/emc/kinematics/genserkins.h index 3aa0756fc5a..b74b826d2ec 100644 --- a/src/emc/kinematics/genserkins.h +++ b/src/emc/kinematics/genserkins.h @@ -142,6 +142,11 @@ extern int compute_jfwd(go_link * link_params, extern int compute_jinv(go_matrix * Jfwd, go_matrix * Jinv); +extern int genserKinematicsJacobian(const double *joint, + const EmcPose *world, + double jac[EMCMOT_MAX_JOINTS][EMCMOT_MAX_AXIS], + const KINEMATICS_INVERSE_FLAGS *iflags); + extern int genserKinematicsForward(const double *joint, EmcPose * world, const KINEMATICS_FORWARD_FLAGS * fflags, From ea2bbf56448154a74d5faa8713a470184d8ae117 Mon Sep 17 00:00:00 2001 From: Luca Toniolo <10792599+grandixximo@users.noreply.github.com> Date: Thu, 3 Sep 2026 18:21:25 +1000 Subject: [PATCH 16/60] tests: check the Jacobian of every module where it runs A realtime component loaded after the module under test, reaching it through the exported entry points; a failed check fails the load. Every module in the tree, every switchkins type, both direction 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, which catches a transposed matrix, a wrong sign, column or unit whichever way the module answered. Against the inverse: difference it here with a different step and compare entry by entry, the check for the gantry, whose forward is not one to one. Verified by mutation, one per module or shared routine, every one caught. The forward check is also a round trip of each module and found four whose forward and inverse disagreed, fixed in the commits before this one. maxkins, which disagrees away from c = 0 and u = 0, is checked against its inverse; the nutating heads read their angles from the inverse's joint argument, so they are checked against the forward. --- tests/kins-jacobian/checkresult | 4 + tests/kins-jacobian/jaccheck.c | 360 ++++++++++++++++++++++++++++++++ tests/kins-jacobian/skip | 4 + tests/kins-jacobian/test.sh | 173 +++++++++++++++ 4 files changed, 541 insertions(+) create mode 100755 tests/kins-jacobian/checkresult create mode 100644 tests/kins-jacobian/jaccheck.c create mode 100755 tests/kins-jacobian/skip create mode 100755 tests/kins-jacobian/test.sh diff --git a/tests/kins-jacobian/checkresult b/tests/kins-jacobian/checkresult new file mode 100755 index 00000000000..b49a90b17c6 --- /dev/null +++ b/tests/kins-jacobian/checkresult @@ -0,0 +1,4 @@ +#!/bin/sh +[ "$(grep -c 'jacobian agrees' "$1")" = "$(grep -c '^=== ' "$1")" ] \ + && [ "$(grep -c '^=== ' "$1")" -ge 20 ] \ + && ! grep -q "FAIL" "$1" diff --git a/tests/kins-jacobian/jaccheck.c b/tests/kins-jacobian/jaccheck.c new file mode 100644 index 00000000000..0e5501f6943 --- /dev/null +++ b/tests/kins-jacobian/jaccheck.c @@ -0,0 +1,360 @@ +/* Check a kinematics module's Jacobian where it runs in service. + * + * Loaded after the module under test, so kinematicsForward(), + * kinematicsInverse() and kinematicsJacobian() resolve to it. A + * failed check fails the load, and a failed load fails the test. + * + * Two checks, neither of which reuses the module's own answer. + * + * Against the forward: perturb one joint, difference the forward to + * get how the pose responds, and multiply by the reported Jacobian. + * The result has to be that joint's unit vector, since the Jacobian + * is the derivative of the inverse and the two are inverse maps. 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, whether the module answered in closed form or by + * differencing. + * + * Against the inverse: difference the inverse here, with a different + * step, and compare entry by entry. This is the check for a machine + * whose forward is not one to one, the gantry with two joints on one + * letter, where the product above is not the identity. + * + * Author: LinuxCNC + * License: GPL Version 2 + * System: Linux + * + * Copyright (c) 2026 All rights reserved. + */ + +#include +#include +#include +#include +#include +#include +#include +#include + +MODULE_LICENSE("GPL"); +MODULE_DESCRIPTION("kinematics Jacobian checker"); + +static int joints = 3; +RTAPI_MP_INT(joints, "joint count the module under test was loaded for"); + +static int types = -1; +RTAPI_MP_INT(types, "how many switchkins types to check, from 0; -1 for all the module has"); + +static int r1 = -1, r2 = -1, r3 = -1; +RTAPI_MP_INT(r1, "joint number of the first joint to sweep"); +RTAPI_MP_INT(r2, "joint number of the second joint to sweep, -1 for none"); +RTAPI_MP_INT(r3, "joint number of the third joint to sweep, -1 for none"); + +#define MAX_ANGLES 8 +#define NO_ANGLE 9999 +static int angles[MAX_ANGLES] = { NO_ANGLE, NO_ANGLE, NO_ANGLE, NO_ANGLE, + NO_ANGLE, NO_ANGLE, NO_ANGLE, NO_ANGLE }; +RTAPI_MP_ARRAY_INT(angles, MAX_ANGLES, "values each swept joint takes; default 0,30,-25,90,180"); + +static int base[EMCMOT_MAX_JOINTS] = { 10, 20, 30 }; +RTAPI_MP_ARRAY_INT(base, EMCMOT_MAX_JOINTS, "joint values before the sweep, from joint 0"); + +static int frompose = 0; +RTAPI_MP_INT(frompose, "1 to read base and the sweep as pose coordinates and take the joints from the inverse"); + +static char *check = "both"; +RTAPI_MP_STRING(check, "fwd, inv or both: which checks to run"); + +static int tolexp = 6; +RTAPI_MP_INT(tolexp, "tolerance for the checks is 10 to the minus this"); + +/* switchkins.h is not an exported header, and a module rejects a type + it does not have, so the loop only needs an upper bound */ +#define MAX_TYPES 9 + +#define FWD_STEP 1e-5 /* joint units, for differencing the forward */ +#define INV_STEP 2e-3 /* pose units, for differencing the inverse; not + the step kins_util.c uses, on purpose */ + +static int comp_id = -1; +static int failures; +static int poses; +static double tolerance = 1e-6; +static int do_fwd = 1, do_inv = 1; + +static void expect(int ok, const char *what, const double *j, int m, int n) +{ + char pose[160]; + int i, k = 0; + + if (ok) { return; } + for (i = 0; i < joints && k < (int)sizeof(pose) - 12; i++) { + k += rtapi_snprintf(pose + k, sizeof(pose) - k, "%s%.4g", + i ? "," : "", j[i]); + } + rtapi_print_msg(RTAPI_MSG_ERR, "jaccheck: FAIL %s [%d][%d] at [%s]\n", + what, m, n, pose); + failures++; +} + +static double pose_coord(const EmcPose *p, int a) +{ + switch (a) { + case 0: return p->tran.x; + case 1: return p->tran.y; + case 2: return p->tran.z; + case 3: return p->a; + case 4: return p->b; + case 5: return p->c; + case 6: return p->u; + case 7: return p->v; + default: return p->w; + } +} + +static void pose_add(EmcPose *p, int a, double d) +{ + switch (a) { + case 0: p->tran.x += d; break; + case 1: p->tran.y += d; break; + case 2: p->tran.z += d; break; + case 3: p->a += d; break; + case 4: p->b += d; break; + case 5: p->c += d; break; + case 6: p->u += d; break; + case 7: p->v += d; break; + default: p->w += d; break; + } +} + +/* how the pose responds to joint m: column m of the forward's derivative. + A forward that iterates starts from the pose it is handed, so both + calls start from the pose the joints are known to reach. */ +static int fwd_column(const double *j, int m, KINEMATICS_FORWARD_FLAGS ff, + const EmcPose *near, double *col) +{ + double t[EMCMOT_MAX_JOINTS]; + EmcPose lo = *near, hi = *near; + KINEMATICS_INVERSE_FLAGS inf = 0; + int a; + + memcpy(t, j, sizeof(t)); + + t[m] = j[m] - FWD_STEP; + if (kinematicsForward(t, &lo, &ff, &inf)) { return -1; } + t[m] = j[m] + FWD_STEP; + if (kinematicsForward(t, &hi, &ff, &inf)) { return -1; } + + for (a = 0; a < EMCMOT_MAX_AXIS; a++) { + col[a] = (pose_coord(&hi, a) - pose_coord(&lo, a)) / (2 * FWD_STEP); + } + return 0; +} + +/* near is where the pose is expected to be, for a forward that iterates + from the pose it is handed; zero where nothing better is known */ +static void check_pose(const double *j, const EmcPose *near) +{ + double jac[EMCMOT_MAX_JOINTS][EMCMOT_MAX_AXIS]; + double col[EMCMOT_MAX_AXIS]; + double qp[EMCMOT_MAX_JOINTS], qm[EMCMOT_MAX_JOINTS]; + EmcPose world = *near, p; + KINEMATICS_FORWARD_FLAGS ff = 0; + KINEMATICS_INVERSE_FLAGS inf = 0; + int m, n, a; + + m = kinematicsForward(j, &world, &ff, &inf); + if (m) { + rtapi_print_msg(RTAPI_MSG_ERR, + "jaccheck: forward started from [%.4g,%.4g,%.4g,%.4g,%.4g,%.4g]" + " and left [%.4g,%.4g,%.4g,%.4g,%.4g,%.4g]\n", + near->tran.x, near->tran.y, near->tran.z, near->a, near->b, near->c, + world.tran.x, world.tran.y, world.tran.z, world.a, world.b, world.c); + expect(0, "forward kinematics", j, m, -1); + return; + } + poses++; + + if (kinematicsJacobian(j, &world, jac, &inf)) { + /* say what the inverse makes of the same pose, since a module + that differences its inverse declines when that does not come + back to the joints it was given */ + memcpy(qp, j, sizeof(qp)); + if (kinematicsInverse(&world, qp, &inf, &ff)) { + rtapi_print_msg(RTAPI_MSG_ERR, "jaccheck: inverse fails at the pose\n"); + } else { + rtapi_print_msg(RTAPI_MSG_ERR, + "jaccheck: inverse gives [%.4g,%.4g,%.4g,%.4g,%.4g,%.4g] flags %lu\n", + qp[0], qp[1], qp[2], qp[3], qp[4], qp[5], inf); + } + expect(0, "jacobian declined", j, -1, -1); + return; + } + + /* rows the module has no joint for stay zero */ + for (m = joints; m < EMCMOT_MAX_JOINTS; m++) { + for (a = 0; a < EMCMOT_MAX_AXIS; a++) { + expect(jac[m][a] == 0, "row past the joint count", j, m, a); + } + } + + if (do_fwd) { + for (m = 0; m < joints; m++) { + if (fwd_column(j, m, ff, &world, col)) { + expect(0, "forward kinematics near the pose", j, m, -1); + return; + } + for (n = 0; n < joints; n++) { + double s = 0; + for (a = 0; a < EMCMOT_MAX_AXIS; a++) { s += jac[n][a] * col[a]; } + expect(fabs(s - (m == n ? 1.0 : 0.0)) < tolerance, + "jacobian times forward column", j, n, m); + } + } + } + + if (do_inv) { + for (a = 0; a < EMCMOT_MAX_AXIS; a++) { + p = world; + memcpy(qp, j, sizeof(qp)); + memcpy(qm, j, sizeof(qm)); + pose_add(&p, a, INV_STEP); + if (kinematicsInverse(&p, qp, &inf, &ff)) { + expect(0, "inverse kinematics near the pose", j, -1, a); + return; + } + pose_add(&p, a, -2 * INV_STEP); + if (kinematicsInverse(&p, qm, &inf, &ff)) { + expect(0, "inverse kinematics near the pose", j, -1, a); + return; + } + for (n = 0; n < joints; n++) { + double d = (qp[n] - qm[n]) / (2 * INV_STEP); + expect(fabs(d - jac[n][a]) < tolerance * (1 + fabs(d)), + "jacobian against the inverse", j, n, a); + } + } + } +} + +int rtapi_app_main(void) +{ + double j[EMCMOT_MAX_JOINTS]; + int angles_n; + int a, b, c, t, i; + int checked = 0; + + if (joints < 1 || joints > EMCMOT_MAX_JOINTS) { + rtapi_print_msg(RTAPI_MSG_ERR, "jaccheck: joints=%d\n", joints); + return -1; + } + /* the list given ends at the first untouched entry; none given means + the quarter and half turns where a sine changes sign or a cosine + vanishes, and the values in between */ + if (angles[0] == NO_ANGLE) { + static const int usual[] = { 0, 30, -25, 90, 180 }; + for (i = 0; i < (int)(sizeof(usual)/sizeof(usual[0])); i++) { angles[i] = usual[i]; } + } + for (angles_n = 0; angles_n < MAX_ANGLES; angles_n++) { + if (angles[angles_n] == NO_ANGLE) { break; } + } + for (tolerance = 1, i = 0; i < tolexp; i++) { tolerance *= 0.1; } + do_fwd = !strcmp(check, "fwd") || !strcmp(check, "both"); + do_inv = !strcmp(check, "inv") || !strcmp(check, "both"); + if (!do_fwd && !do_inv) { + rtapi_print_msg(RTAPI_MSG_ERR, "jaccheck: check=%s\n", check); + return -1; + } + + comp_id = hal_init("jaccheck"); + if (comp_id < 0) { return comp_id; } + + if (kinematicsType() == 0) { + rtapi_print_msg(RTAPI_MSG_ERR, "jaccheck: the module reports no type\n"); + hal_exit(comp_id); + return -1; + } + + for (i = 0; i < EMCMOT_MAX_JOINTS; i++) { j[i] = base[i]; } + + /* A switchable module's first forward after load restarts an + iterating forward from a stored pose that is still zero, which + for a hexapod is the singular pose it cannot leave; motion's first + cycle takes that failure and carries on. Take it here. */ + if (kinematicsSwitchable()) { + double q[EMCMOT_MAX_JOINTS]; + EmcPose seed; + KINEMATICS_FORWARD_FLAGS ff = 0; + KINEMATICS_INVERSE_FLAGS inf = 0; + ZERO_EMC_POSE(seed); + memcpy(q, j, sizeof(q)); + if (r1 >= 0) { q[r1] = angles[0]; } + if (r2 >= 0) { q[r2] = angles[0]; } + if (r3 >= 0) { q[r3] = angles[0]; } + if (frompose) { + for (i = 0; i < EMCMOT_MAX_AXIS; i++) { pose_add(&seed, i, q[i]); } + memset(q, 0, sizeof(q)); + kinematicsInverse(&seed, q, &inf, &ff); + } + kinematicsForward(q, &seed, &ff, &inf); + } + + /* every kinematics the module offers, since the answer is per type. + The module starts in type 0, and is not switched to it: a switch + restarts an iterating forward from a stored pose that is still + zero, which for a hexapod is the singular pose it cannot leave */ + for (t = 0; t < MAX_TYPES && (types < 0 || t < types); t++) { + if (kinematicsSwitchable() && t > 0 && kinematicsSwitch(t)) { break; } + checked++; + + for (a = 0; a < angles_n; a++) { + if (r1 >= 0) { j[r1] = angles[a]; } + for (b = 0; b < angles_n; b++) { + if (r2 >= 0) { j[r2] = angles[b]; } + for (c = 0; c < angles_n; c++) { + if (r3 >= 0) { j[r3] = angles[c]; } + if (frompose) { + /* base and sweep name a pose; the machine that + reaches it comes from the module's inverse */ + double q[EMCMOT_MAX_JOINTS]; + EmcPose want; + KINEMATICS_INVERSE_FLAGS inf = 0; + KINEMATICS_FORWARD_FLAGS ff = 0; + ZERO_EMC_POSE(want); + for (i = 0; i < EMCMOT_MAX_AXIS; i++) { pose_add(&want, i, j[i]); } + memset(q, 0, sizeof(q)); + if (kinematicsInverse(&want, q, &inf, &ff)) { + expect(0, "inverse kinematics at the base pose", j, -1, -1); + } else { + check_pose(q, &want); + } + } else { + EmcPose zero; + ZERO_EMC_POSE(zero); + check_pose(j, &zero); + } + if (r3 < 0) { break; } + } + if (r2 < 0) { break; } + } + if (r1 < 0) { break; } + } + + if (!kinematicsSwitchable()) { break; } + } + + if (failures) { + rtapi_print_msg(RTAPI_MSG_ERR, + "jaccheck: %d check(s) failed over %d pose(s)\n", + failures, poses); + hal_exit(comp_id); + return -1; + } + + rtapi_print("jaccheck: jacobian agrees for %d kinematics type(s), %d pose(s)\n", + checked, poses); + hal_ready(comp_id); + return 0; +} + +void rtapi_app_exit(void) { hal_exit(comp_id); } diff --git a/tests/kins-jacobian/skip b/tests/kins-jacobian/skip new file mode 100755 index 00000000000..a12f31a77c2 --- /dev/null +++ b/tests/kins-jacobian/skip @@ -0,0 +1,4 @@ +#!/bin/sh +# Builds a realtime component with halcompile, which needs the build +# tools present. Skip when testing installed packages. +[ -z "$SYSTEM_BUILD" ] diff --git a/tests/kins-jacobian/test.sh b/tests/kins-jacobian/test.sh new file mode 100755 index 00000000000..bf98c0eaeae --- /dev/null +++ b/tests/kins-jacobian/test.sh @@ -0,0 +1,173 @@ +#!/bin/bash +set -e + +${SUDO} halcompile --install jaccheck.c >/dev/null + +# One hal file per module: they all define the same entry points, so +# only one can be loaded at a time. A run that leaves the sweep at its +# default takes each rotary through the quarter and half turns where a +# sine changes sign or a cosine vanishes; the arms and the parallel +# machines name their own, away from the poses they cannot hold. +# ONLY= in the environment runs the entries for that module alone +run() { + local hal + case "$1" in "${ONLY:-}"*) ;; *) return 0 ;; esac + hal=$(mktemp --suffix=.hal) + { printf 'loadrt %s\n' "$1" + printf '%s\n' "$2" + printf 'loadrt jaccheck %s\n' "$3" + } > "$hal" + echo "=== $1" + halrun -f "$hal" + rm -f "$hal" +} + +# identity, including a gantry: two joints on one letter is the case where +# the forward is not one to one, so it is checked against the inverse +run "trivkins coordinates=XYZ" "" "joints=3" +run "trivkins coordinates=XYZY kinstype=BOTH" "" "joints=4 check=inv" +run "trivkins coordinates=XYZABCUVW" "" "joints=9 r1=3 r2=5" +run "userkins" "" "joints=3" +run "millturn" "" "joints=4" + +# linear maps and one rotation +run "corexykins" "" "joints=9" +run "rotatekins" "" "joints=9 r1=5" +run "matrixkins" \ + "setp matrixkins.C_xy 0.02 +setp matrixkins.C_xz -0.01 +setp matrixkins.C_yx 0.03 +setp matrixkins.C_yz 0.015 +setp matrixkins.C_zx -0.02 +setp matrixkins.C_zy 0.01 +setp matrixkins.C_zz 1.001" \ + "joints=9" + +# tables and heads; offsets set so no term drops out +run "maxkins" \ + "setp maxkins.pivot-length 100" \ + "joints=9 r1=4 r2=5 base=10,20,30,0,0,0,7,0,3" + +run "5axiskins coordinates=XYZBCW" "" "joints=6 r1=3 r2=4 base=10,20,30,0,0,5" +run "5axiskins coordinates=XYZBCW sparm=identityfirst" "" "joints=6 r1=3 r2=4 base=10,20,30,0,0,5" + +run "xyzac-trt-kins coordinates=XYZAC" \ + "setp xyzac-trt-kins.y-offset 3 +setp xyzac-trt-kins.z-offset 11 +setp xyzac-trt-kins.tool-offset 7 +setp xyzac-trt-kins.x-rot-point 1 +setp xyzac-trt-kins.y-rot-point 2 +setp xyzac-trt-kins.z-rot-point 5" \ + "joints=5 r1=3 r2=4" + +run "xyzbc-trt-kins coordinates=XYZBC" \ + "setp xyzbc-trt-kins.x-offset 3 +setp xyzbc-trt-kins.z-offset 11 +setp xyzbc-trt-kins.tool-offset 7 +setp xyzbc-trt-kins.x-rot-point 1 +setp xyzbc-trt-kins.y-rot-point 2 +setp xyzbc-trt-kins.z-rot-point 5" \ + "joints=5 r1=3 r2=4" + +# and both with the rotation sense the chapter asks for +run "xyzac-trt-kins coordinates=XYZAC" \ + "setp xyzac-trt-kins.conventional-directions 1 +setp xyzac-trt-kins.y-offset 3 +setp xyzac-trt-kins.z-offset 11 +setp xyzac-trt-kins.tool-offset 7 +setp xyzac-trt-kins.x-rot-point 1 +setp xyzac-trt-kins.y-rot-point 2 +setp xyzac-trt-kins.z-rot-point 5" \ + "joints=5 r1=3 r2=4" + +run "xyzbc-trt-kins coordinates=XYZBC" \ + "setp xyzbc-trt-kins.conventional-directions 1 +setp xyzbc-trt-kins.x-offset 3 +setp xyzbc-trt-kins.z-offset 11 +setp xyzbc-trt-kins.tool-offset 7 +setp xyzbc-trt-kins.x-rot-point 1 +setp xyzbc-trt-kins.y-rot-point 2 +setp xyzbc-trt-kins.z-rot-point 5" \ + "joints=5 r1=3 r2=4" + +run "xyzab_tdr_kins" \ + "setp xyzab_tdr_kins.x-offset 3 +setp xyzab_tdr_kins.z-offset 11 +setp xyzab_tdr_kins.tool-offset-z 7 +setp xyzab_tdr_kins.x-rot-point 1 +setp xyzab_tdr_kins.y-rot-point 2 +setp xyzab_tdr_kins.z-rot-point 5" \ + "joints=5 r1=3 r2=4" + +# The nutating heads read their rotary angles from the joint argument of +# the inverse rather than from the pose, so differencing the inverse +# about a pose cannot see the coupling; the forward is the check here. +run "xyzacb_trsrn" \ + "setp xyzacb_trsrn_kins.nut-angle 45 +setp xyzacb_trsrn_kins.y-pivot 100 +setp xyzacb_trsrn_kins.z-pivot 200 +setp xyzacb_trsrn_kins.x-offset 5 +setp xyzacb_trsrn_kins.y-offset 7 +setp xyzacb_trsrn_kins.y-rot-axis 300 +setp xyzacb_trsrn_kins.z-rot-axis 400 +setp xyzacb_trsrn_kins.tool-offset-z 50 +setp xyzacb_trsrn_kins.pre-rot 0.3 +setp xyzacb_trsrn_kins.primary-angle 20 +setp xyzacb_trsrn_kins.secondary-angle 35" \ + "joints=6 r1=3 r2=4 r3=5 check=fwd" + +run "xyzbca_trsrn" \ + "setp xyzbca_trsrn_kins.nut-angle 45 +setp xyzbca_trsrn_kins.x-pivot 100 +setp xyzbca_trsrn_kins.z-pivot 200 +setp xyzbca_trsrn_kins.x-offset 5 +setp xyzbca_trsrn_kins.y-offset 7 +setp xyzbca_trsrn_kins.x-rot-axis 300 +setp xyzbca_trsrn_kins.z-rot-axis 400 +setp xyzbca_trsrn_kins.tool-offset-z 50 +setp xyzbca_trsrn_kins.pre-rot 0.3 +setp xyzbca_trsrn_kins.primary-angle 20 +setp xyzbca_trsrn_kins.secondary-angle 35" \ + "joints=6 r1=3 r2=4 r3=5 check=fwd" + +# polar +run "rosekins" "" "joints=3 r1=2 base=10,5,0 angles=30,-25,90,120" + +# arms. Straight or folded they are singular, so the sweep keeps clear +# of 0 and 180 on the elbow. genserkins iterates its inverse to a +# tolerance the differences would not see through, so it is checked +# against its forward only; pumakins and three21kins answer by differencing +# their own inverse and the forward is what proves the answer. +run "scarakins" "" "joints=6 r1=1 r2=3 r3=0 base=0,0,20,0,0,0 angles=30,-25,90,120,-60" +# scorbot's inverse returns the elbow-up arm, shoulder above elbow, so the +# poses have to be ones it can return: j1 above j2, and j2 within a quarter +# turn of level +run "scorbot-kins" "" "joints=5 r1=1 base=0,70,-20,0,0 angles=40,55,70,85" +run "scorbot-kins" "" "joints=5 r1=2 base=0,80,0,0,0 angles=-60,-30,0,20" +run "pumakins" "setp pumakins.D6 50" "joints=6 r1=1 r2=2 r3=4 base=15,0,0,10,0,20 angles=20,45,-35,70" +run "three21kins" "" "joints=6 r1=1 r2=2 r3=4 base=15,0,0,10,0,20 angles=20,45,-35,70" +run "genserkins" "" "joints=9 r1=1 r2=2 r3=4 base=15,0,0,10,0,20 angles=20,45,-35,70 check=fwd" +# and with a joint counted relative to the one before it +run "genserkins" "setp genserkins.unrotate-3 1" "joints=9 r1=1 r2=2 r3=4 base=15,0,0,10,0,20 angles=20,45,-35,70 check=fwd" + +# parallel machines. The struts cannot tilt the platform far, and the +# forward of the hexapod and the pentapod iterates to a tolerance, so the +# product check on those two is held to what that tolerance allows. The +# hexapod module runs its own forward for its GUI pins in every type, with +# whatever joint values that type has, and identity joint values are not +# strut lengths it can converge from; its identity types are the shared +# ones trivkins covers, so only its own type is checked. +run "tripodkins" \ + "setp tripodkins.Bx 2 +setp tripodkins.Cx 1 +setp tripodkins.Cy 2" \ + "joints=3 frompose=1 base=1,1,2" +run "lineardeltakins" "" "joints=9 frompose=1 base=20,30,-200" +run "rotarydeltakins" "" "joints=9 r1=0 r2=1 frompose=1 base=0,0,-12 angles=0,2,-3" +run "genhexkins" \ + "setp genhexkins.screw-lead 0" \ + "joints=6 r1=3 r2=4 r3=5 frompose=1 base=2,3,20 angles=0,5,-7,10 tolexp=3 types=1" +run "genhexkins" \ + "setp genhexkins.screw-lead 5" \ + "joints=6 r1=3 r2=4 r3=5 frompose=1 base=2,3,20 angles=0,5,-7,10 tolexp=3 types=1" +run "pentakins" "" "joints=5 r1=3 r2=4 frompose=1 base=10,20,0 angles=0,5,-7,10 tolexp=3" From 35444907bca631baaa9888da7d7794f5d2dfb30f Mon Sep 17 00:00:00 2001 From: Luca Toniolo <10792599+grandixximo@users.noreply.github.com> Date: Fri, 11 Sep 2026 15:43:09 +0800 Subject: [PATCH 17/60] kinematics.h: C linkage guards, and the include guard closed at the end of the file The definitions are C and the header had no linkage guards, so the first C++ include of it, emc_nml.hh in many translation units, gave the declarations C++ linkage and references to the C definitions (toolFrameInWork, the TRT tables) no longer link. The header now wraps its declarations in extern "C" for a C++ includer, as a C header does. Its include guard also closed before the trt declarations at the end of the file, so a second include, which motion.h and emc_nml.hh together make, declared trtKinematicsSetup and the xyzac and xyzbc entry points twice. The guard now closes at the end of the file. --- src/emc/kinematics/kinematics.h | 10 +++++++++- 1 file changed, 9 insertions(+), 1 deletion(-) diff --git a/src/emc/kinematics/kinematics.h b/src/emc/kinematics/kinematics.h index 900fe5aa474..9376b7d9d6d 100644 --- a/src/emc/kinematics/kinematics.h +++ b/src/emc/kinematics/kinematics.h @@ -19,6 +19,10 @@ #include "emcmotcfg.h" /* EMCMOT_MAX_JOINTS, EMCMOT_MAX_AXIS */ #include "rtapi_bool.h" +#ifdef __cplusplus +extern "C" { +#endif + /* The type of kinematics used. @@ -474,7 +478,6 @@ extern int userkKinematicsInverse(const struct EmcPose * world, double *joint, const KINEMATICS_INVERSE_FLAGS * iflags, KINEMATICS_FORWARD_FLAGS * fflags); -#endif //********************************************************************* // xyzac,xyzbc; extern int trtKinematicsSetup(const int comp_id, @@ -529,3 +532,8 @@ extern int xyzbcKinematicsJacobian(const double *joints, const KINEMATICS_INVERSE_FLAGS *iflags); //********************************************************************* +#ifdef __cplusplus +} +#endif + +#endif // __LINUXCNC_KINEMATICS_H From a0402a83ae3bd3621c6732b40cd874c960cfd9fd Mon Sep 17 00:00:00 2001 From: Luca Toniolo <10792599+grandixximo@users.noreply.github.com> Date: Fri, 11 Sep 2026 19:55:55 +0800 Subject: [PATCH 18/60] interp: G43.4 and G49 as spellings over the kinematics switch G43.4 is G43 on the module's working kinematics: it switches to the kinstype declared KINSTYPE_PRIMARY, then applies the offset, as if the switch line had run and drained. G49 clears the offset and drops to the identity kinstype, but only when the offset in effect is still G43.4's: a G12.1 or G13.1 in between, or a plain G43, means the program took the kinematics over and G49 leaves it alone. A switchable module without a primary rejects G43.4 at read time; without an identity it keeps the plain cancel; with no kinematics attached (sai, preview) both are plain. Already on the target kinstype there is no switch and no drain. The modal group 8 label follows the kinstype motion reports, which is the only authority, and a flag value of -1, "no information", matches every flag. The switch inside G49 exposed a latent deadlock: the startup code runs at task init, before the main loop can service INTERP_EXECUTE_FINISH, so a switch requested there waited forever, as G12.1 in the startup code did. Both now queue the switch without the wait while the startup code runs; no motion exists yet and the switch lands before the first move. tests/kins-switch walks G43.4, G49, G43 and G12.1 through the cases; docs in g-code.adoc and switchkins.adoc. --- docs/src/gcode/g-code.adoc | 49 +++++++++++++ docs/src/gcode/overview.adoc | 2 +- docs/src/motion/switchkins.adoc | 39 +++++++---- src/emc/rs274ngc/interp_array.cc | 2 +- src/emc/rs274ngc/interp_base.hh | 4 ++ src/emc/rs274ngc/interp_check.cc | 4 +- src/emc/rs274ngc/interp_convert.cc | 104 ++++++++++++++++++++-------- src/emc/rs274ngc/interp_internal.hh | 3 + src/emc/rs274ngc/interp_setup.cc | 2 + src/emc/rs274ngc/interp_write.cc | 13 +++- src/emc/rs274ngc/rs274ngc_interp.hh | 1 + src/emc/rs274ngc/rs274ngc_pre.cc | 5 ++ src/emc/rs274ngc/rs274ngc_return.hh | 1 + src/emc/task/emctask.cc | 5 ++ tests/kins-switch/test-ui.py | 24 ++++++- tests/kins-switch/test.ngc | 29 ++++++++ tests/kins-switch/tool.tbl | 2 +- 17 files changed, 235 insertions(+), 54 deletions(-) diff --git a/docs/src/gcode/g-code.adoc b/docs/src/gcode/g-code.adoc index 12dbcdf599f..42dc7a37a46 100644 --- a/docs/src/gcode/g-code.adoc +++ b/docs/src/gcode/g-code.adoc @@ -84,6 +84,7 @@ as the 'L number', and so on for any other letter. |<> |Use Tool Length Offset from Tool Table |<> |Dynamic Tool Length Offset |<> |Apply additional Tool Length Offset +|<> |Tool Length Offset on Primary Kinematics |<> |Cancel Tool Length Offset |<> |Local Coordinate System Offset |<> |Move in Machine Coordinates @@ -1641,11 +1642,59 @@ It is an error if: NOTE: G43.2 does not write to the tool table. +[[gcode:g43.4]] +== G43.4 Tool Length Offset on Primary Kinematics(((G43.4 Tool Length Offset on Primary Kinematics))) + +[source,ngc] +---- +G43.4 +---- + +* 'H' - tool number (optional) + +'G43.4' is 'G43' together with a switch to the kinematics the module +declares its working transform, so the program runs with tool length +compensation in the module's working kinematics. The switch happens +first and the offset applies after it, as if the two had been written +on consecutive lines. 'G49' is the matching cancel: it clears the +offset and switches back to identity kinematics, as long as the offset +in effect is still 'G43.4''s and no 'G12.1' or 'G13.1' has selected a +kinematics since. + +The H word, the offset itself and the parameters it lands in are +'G43''s. Which kinematics is the working one is declared by the module +(KINSTYPE_PRIMARY, see the <> chapter), the number is not the answer; a module that +declares none rejects 'G43.4' at read time. + +The switch is a queue synchronisation point like +'<>': when 'G43.4' or 'G49' changes the +kinematics, the interpreter waits for queued motion to finish first, so +both stop any blending in progress. Already on the target kinematics +they do not switch and blend normally. A plain 'G43', 'G43.1' or +'G43.2' never switches, whatever kinematics is selected, and the 'G49' +that cancels one of them does not switch either. + +On a machine without switchable kinematics there is nothing to switch: +'G43.4' is a plain 'G43' and 'G49' a plain cancel. + +It is an error if: + +* the kinematics module is switchable but declares no primary + kinematics, or +* any of the 'G43' error conditions holds. + [[gcode:g49]] == G49 Cancel Tool Length Compensation(((G49 Cancel Tool Length Offset))) * 'G49' - cancels tool length compensation +'G49' also switches a switchable kinematics module back to its identity +kinematics when it cancels a 'G43.4', undoing the switch that made. It +leaves a kinematics selected by 'G12.1' or 'G13.1' alone, as it does +after a plain 'G43', and a module that declares no identity kinematics +gets the plain cancel. + It is OK to program using the same offset already in use. It is also OK to program using no tool length offset if none is currently being used. diff --git a/docs/src/gcode/overview.adoc b/docs/src/gcode/overview.adoc index 62c6fd87198..07fc326c613 100644 --- a/docs/src/gcode/overview.adoc +++ b/docs/src/gcode/overview.adoc @@ -973,7 +973,7 @@ The modal groups are shown in the following Table. |Feed Rate Mode (Group 5) | G93, G94, G95 |Units (Group 6) | G20, G21 |Cutter Diameter Compensation (Group 7) | G40, G41, G42, G41.1, G42.1 -|Tool Length Offset (Group 8) | G43, G43.1, G49 +|Tool Length Offset (Group 8) | G43, G43.1, G43.2, G43.4, G49 |Canned Cycles Return Mode (Group 10) | G98, G99 |Coordinate System (Group 12) | G54, G55, G56, G57, G58, G59, G59.1, G59.2, G59.3 |Control Mode (Group 13) | G61, G61.1, G64 diff --git a/docs/src/motion/switchkins.adoc b/docs/src/motion/switchkins.adoc index 8c1fea13e24..fe1fd05eed9 100644 --- a/docs/src/motion/switchkins.adoc +++ b/docs/src/motion/switchkins.adoc @@ -218,8 +218,12 @@ A module that declares no identity kinstype refuses 'G13.1' with an error and can still be driven by number with 'G12.1'; see Code Notes for how a module declares its types. -See the G-code documentation for 'G12.1' and 'G13.1' for the full -description. +For tool length work there are spellings that name the kinematics by +what it is rather than by number: 'G43.4' applies the tool length +offset and switches to the kinstype the module declares its working +transform, and the 'G49' that cancels it switches back to identity. +See the G-code documentation for 'G43.4' and 'G49', and for 'G12.1' +and 'G13.1', for the full description. === M-code commands @@ -491,26 +495,31 @@ which kinstype is at fault. Each kinstype gets its own 'kinstype.is-N' pin, so a module providing the usual three keeps the pin names it always had. -A module should also declare what each kinstype IS, again from within +A module also declares what each kinstype IS, with flags from +kinematics.h: + +. *KINSTYPE_IDENTITY* no transform: the joints are the world +. *KINSTYPE_PRIMARY* the module's working transform + +A kinstype registered with switchkinsRegisterOps() carries its flag in +the ops table itself, as the 'identity' or 'primary' field; a kinstype +registered the older way gets it from a call, again from within switchkinsSetup(): ---- int switchkinsDeclare(int ktype, int flags); ---- -with flags from kinematics.h: - -. *KINSTYPE_IDENTITY* no transform: the joints are the world -. *KINSTYPE_PRIMARY* the module's working transform - G-code reads these declarations: 'G13.1' cancels to the kinstype -declared KINSTYPE_IDENTITY, whatever its number, so a module whose -identity kinematics is not kinstype 0 still gets a working 'G13.1'. -At most one kinstype may be declared identity, and declaring a -kinstype the module does not provide fails the module load. A module -that declares nothing keeps working exactly as before for 'G12.1 P-', -but 'G13.1' is an error, since the number of the identity kinematics -is then a guess. +declared KINSTYPE_IDENTITY, and 'G43.4' switches to the kinstype +declared KINSTYPE_PRIMARY, whatever their numbers, so a module whose +kinematics are not in the conventional order still gets working +spellings. At most one kinstype may be declared identity and at most +one primary, and declaring a kinstype the module does not provide +fails the module load. A module that declares nothing keeps working +exactly as before for 'G12.1 P-' and 'G49', but 'G13.1' and 'G43.4' +are an error, since the numbers of the identity and primary kinematics +are then a guess. After calling switchkinsSetup(), rtapi_app_main() checks the supplied parameters, creates a HAL component, and then invokes diff --git a/src/emc/rs274ngc/interp_array.cc b/src/emc/rs274ngc/interp_array.cc index 63f6d20975a..28f8c11df37 100644 --- a/src/emc/rs274ngc/interp_array.cc +++ b/src/emc/rs274ngc/interp_array.cc @@ -91,7 +91,7 @@ const int Interp::gees[] = { /* 360 */ -1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1, /* 380 */ -1,-1, 1, 1, 1, 1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1, /* 400 */ 7,-1,-1,-1,-1,-1,-1,-1,-1,-1, 7, 7,-1,-1,-1,-1,-1,-1,-1,-1, -/* 420 */ 7, 7,-1,-1,-1,-1,-1,-1,-1,-1, 8, 8, 8,-1,-1,-1,-1,-1,-1,-1, +/* 420 */ 7, 7,-1,-1,-1,-1,-1,-1,-1,-1, 8, 8, 8,-1, 8,-1,-1,-1,-1,-1, /* 440 */ -1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1, /* 460 */ -1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1, /* 480 */ -1,-1,-1,-1,-1,-1,-1,-1,-1,-1, 8,-1,-1,-1,-1,-1,-1,-1,-1,-1, diff --git a/src/emc/rs274ngc/interp_base.hh b/src/emc/rs274ngc/interp_base.hh index 5ac25b3f446..7c930a1bf1e 100644 --- a/src/emc/rs274ngc/interp_base.hh +++ b/src/emc/rs274ngc/interp_base.hh @@ -63,6 +63,10 @@ public: virtual void print_state_tag(StateTag const &tag) = 0; virtual void set_loglevel(int level) = 0; virtual void set_loop_on_main_m99(bool state) = 0; + // true while the startup code runs at task init, when the motion + // queue cannot drain yet: a kinematics switch there queues without + // the drain-and-assert wait, which could never complete + virtual void set_in_startup_code(bool) {}; virtual FILE* get_stdout() { return stdout; }; }; diff --git a/src/emc/rs274ngc/interp_check.cc b/src/emc/rs274ngc/interp_check.cc index 8be61f9c609..0e027a74798 100644 --- a/src/emc/rs274ngc/interp_check.cc +++ b/src/emc/rs274ngc/interp_check.cc @@ -285,8 +285,8 @@ int Interp::check_other_codes(block_pointer block) //!< pointer to a block } if (block->h_flag) { - CHKS((block->g_modes[GM_TOOL_LENGTH_OFFSET] != G_43 && motion != G_76 && block->g_modes[GM_TOOL_LENGTH_OFFSET] != G_43_2), - _("H word with no G43 or G76 to use it")); + CHKS((block->g_modes[GM_TOOL_LENGTH_OFFSET] != G_43 && motion != G_76 && block->g_modes[GM_TOOL_LENGTH_OFFSET] != G_43_2 && block->g_modes[GM_TOOL_LENGTH_OFFSET] != G_43_4), + _("H word with no G43, G43.4 or G76 to use it")); } if (block->i_flag) { /* could still be useless if yz_plane arc */ diff --git a/src/emc/rs274ngc/interp_convert.cc b/src/emc/rs274ngc/interp_convert.cc index 4ed206d83e7..ec7e6205582 100644 --- a/src/emc/rs274ngc/interp_convert.cc +++ b/src/emc/rs274ngc/interp_convert.cc @@ -34,7 +34,7 @@ #include "interp_internal.hh" #include "interp_queue.hh" #include "interp_parameter_def.hh" -#include "kinematics.h" // KINSTYPE_IDENTITY, SWITCHKINS_MAX_TYPES +#include // KINSTYPE_IDENTITY, SWITCHKINS_MAX_TYPES #include "units.h" #define TOOL_INSIDE_ARC(side, turn) (((side)==CUTTER_COMP::LEFT&&(turn)>0)||((side)==CUTTER_COMP::RIGHT&&(turn)<0)) @@ -4049,12 +4049,13 @@ int Interp::convert_m(block_pointer block, //!< pointer to a block of RS27 if (FEATURE(RETAIN_G43)) { - if ((settings->active_g_codes[9] == G_43) && ONCE(STEP_RETAIN_G43)) { + if (((settings->active_g_codes[9] == G_43) || + (settings->active_g_codes[9] == G_43_4)) && ONCE(STEP_RETAIN_G43)) { if(settings->selected_pocket > 0) { struct block_struct g43; init_block(&g43); - block->g_modes[gees[G_43]] = G_43; - CHP(convert_tool_length_offset(G_43, &g43, settings)); + block->g_modes[gees[settings->active_g_codes[9]]] = settings->active_g_codes[9]; + CHP(convert_tool_length_offset(settings->active_g_codes[9], &g43, settings)); } else { struct block_struct g49; init_block(&g49); @@ -4362,8 +4363,10 @@ int Interp::convert_modal_0(int code, //!< G-code, must be from group 0 // will be queued: ask every time. The exception is an // ON_ABORT_COMMAND routine, run by one execute() call that cannot // service INTERP_EXECUTE_FINISH and would drop the rest of the - // routine; the abort has just flushed the queue anyway. - if (!settings->in_abort_command) { + // routine; the abort has just flushed the queue anyway. The startup + // code is the other exception: it runs before the main loop can + // service the wait, and no motion exists yet to protect. + if (!settings->in_abort_command && !settings->in_startup_code) { settings->kinsSwitch_flag = true; } CHP(convert_kins_switch(code, block, settings)); @@ -6389,6 +6392,45 @@ int Interp::convert_tool_change(setup_pointer settings) //!< pointer to machine /****************************************************************************/ +// the kinematics module declares what each type is (KINSTYPE_* flags); +// where the flags say nothing at all there is no kinematics attached +// (sai, preview) and the codes fall back to type 0, as before +static int kins_type_info_available() +{ + int k; + + for (k = 0; k < SWITCHKINS_MAX_TYPES; k++) { + if (GET_EXTERNAL_KINS_TYPE_FLAGS(k) >= 0) return 1; + } + return 0; +} + +// the type carrying a KINSTYPE_ flag, or -1 when the module declares none; +// -1 for a type is "no information", and it matches every flag, so it must +// be excluded before the bit test +static int flagged_kins_type(int flag) +{ + int k, f; + + for (k = 0; k < SWITCHKINS_MAX_TYPES; k++) { + f = GET_EXTERNAL_KINS_TYPE_FLAGS(k); + if (f >= 0 && (f & flag)) { return k; } + } + return -1; +} + +// a kinematics switch as G12.1/G13.1 do, with the drain wait and its two +// exceptions; already on the type there is nothing to do +static void switch_kins_type(int kins_type, setup_pointer settings) +{ + if (settings->kins_type == kins_type) { return; } + if (!settings->in_abort_command && !settings->in_startup_code) { + settings->kinsSwitch_flag = true; + } + SELECT_KINS_TYPE(kins_type); + settings->kins_type = kins_type; +} + /*! convert_tool_length_offset Returned Value: int @@ -6429,9 +6471,21 @@ int Interp::convert_tool_length_offset(int g_code, //!< g_code being execu CHKS((settings->cutter_comp_side != CUTTER_COMP::OFF), (_("Cannot change tool offset with cutter radius compensation on"))); + if (g_code == G_43_4) { + int primary = flagged_kins_type(KINSTYPE_PRIMARY); + // G43.4 is G43 on the module's working transform: switch first, then + // apply the offset, as if the switch line had run and drained. With + // no kinematics attached there is nothing to switch to. + CHKS(primary < 0 && kins_type_info_available(), NCE_NO_PRIMARY_KINEMATICS_TYPE); + if (primary >= 0) { switch_kins_type(primary, settings); } + settings->kins_by_g43_4 = true; + } else if (g_code != G_49) { + // the offset in effect is no longer G43.4's, so G49 has no switch to undo + settings->kins_by_g43_4 = false; + } if (g_code == G_49) { idx = 0; - } else if (g_code == G_43) { + } else if (g_code == G_43 || g_code == G_43_4) { logDebug("convert_tool_length_offset h_flag=%d h_number=%d toolchange_flag=%d current_pocket=%d\n", block->h_flag,block->h_number,settings->toolchange_flag,settings->current_pocket); if(block->h_flag) { @@ -6511,7 +6565,7 @@ int Interp::convert_tool_length_offset(int g_code, //!< g_code being execu if(block->w_flag) tool_offset.w += block->w_number; } } else { - ERS("BUG: Code not G43, G43.1, G43.2, or G49"); + ERS("BUG: Code not G43, G43.1, G43.2, G43.4, or G49"); } USE_TOOL_LENGTH_OFFSET(tool_offset); @@ -6552,6 +6606,16 @@ int Interp::convert_tool_length_offset(int g_code, //!< g_code being execu settings->parameters[5088] = PROGRAM_TO_USER_LEN(tool_offset.v); settings->parameters[5089] = PROGRAM_TO_USER_LEN(tool_offset.w); + if (g_code == G_49 && settings->kins_by_g43_4) { + // G49 undoes what G43.4 did: after the cancel it drops the machine + // to identity kinematics, as if G13.1 had run on the next line. A + // kinematics the program selected itself is left alone, and a + // module that declares no identity type keeps the plain cancel. + int identity = flagged_kins_type(KINSTYPE_IDENTITY); + if (identity >= 0) { switch_kins_type(identity, settings); } + settings->kins_by_g43_4 = false; + } + return INTERP_OK; } @@ -6609,19 +6673,6 @@ so no motion is ever planned across a change of kinematics. */ -// the kinematics module declares what each type is (KINSTYPE_* flags); -// where the flags say nothing at all there is no kinematics attached -// (sai, preview) and the codes fall back to type 0, as before -static int kins_type_info_available() -{ - int k; - - for (k = 0; k < SWITCHKINS_MAX_TYPES; k++) { - if (GET_EXTERNAL_KINS_TYPE_FLAGS(k) >= 0) return 1; - } - return 0; -} - int Interp::convert_kins_switch(int code, //!< G_12_1 or G_13_1 block_pointer block, //!< pointer to a block of RS274 instructions setup_pointer settings) //!< pointer to machine settings @@ -6629,16 +6680,9 @@ int Interp::convert_kins_switch(int code, //!< G_12_1 or G_13_1 int kins_type; if (code == G_13_1) { - int k; - // G13.1 cancels to identity kinematics; which type that is, the // module declares, the number is not the answer - for (k = 0, kins_type = -1; k < SWITCHKINS_MAX_TYPES; k++) { - if (GET_EXTERNAL_KINS_TYPE_FLAGS(k) & KINSTYPE_IDENTITY) { - kins_type = k; - break; - } - } + kins_type = flagged_kins_type(KINSTYPE_IDENTITY); if (kins_type < 0) { CHKS(kins_type_info_available(), NCE_NO_IDENTITY_KINEMATICS_TYPE); kins_type = 0; // no kinematics attached: standalone interpreter @@ -6655,6 +6699,8 @@ int Interp::convert_kins_switch(int code, //!< G_12_1 or G_13_1 SELECT_KINS_TYPE(kins_type); settings->kins_type = kins_type; + // the program has taken the kinematics over from G43.4 + settings->kins_by_g43_4 = false; return INTERP_OK; } diff --git a/src/emc/rs274ngc/interp_internal.hh b/src/emc/rs274ngc/interp_internal.hh index b58d40e525f..bb9118a88aa 100644 --- a/src/emc/rs274ngc/interp_internal.hh +++ b/src/emc/rs274ngc/interp_internal.hh @@ -247,6 +247,7 @@ enum GCodes G_43 = 430, G_43_1 = 431, G_43_2 = 432, + G_43_4 = 434, G_49 = 490, G_50 = 500, G_51 = 510, @@ -759,6 +760,7 @@ struct setup bool input_flag; // flag indicating waiting for input done bool kinsSwitch_flag; // flag indicating waiting for kinematics switch done int kins_type; // kinematics selected by G12.1/G13.1 + bool kins_by_g43_4; // G43.4 selected the kinematics, for G49 to undo bool toolchange_flag; // flag indicating we just had a tool change int input_index; // channel queried bool input_digital; // input queried was digital (false=analog) @@ -861,6 +863,7 @@ struct setup boost::python::object *pythis; // boost::cref to 'this' const char *on_abort_command; bool in_abort_command; // running the ON_ABORT_COMMAND routine + bool in_startup_code; // running the startup code at task init int_remap_map g_remapped,m_remapped; remap_map remaps; #define INIT_FUNC "__init__" diff --git a/src/emc/rs274ngc/interp_setup.cc b/src/emc/rs274ngc/interp_setup.cc index 3b5445795c3..05255499f21 100644 --- a/src/emc/rs274ngc/interp_setup.cc +++ b/src/emc/rs274ngc/interp_setup.cc @@ -118,6 +118,7 @@ setup::setup() : input_flag(0), kinsSwitch_flag(0), kins_type(0), + kins_by_g43_4(false), toolchange_flag(0), input_index(0), input_digital(0), @@ -201,6 +202,7 @@ setup::setup() : pythis(), on_abort_command(NULL), in_abort_command(false), + in_startup_code(false), init_once(CANON_STOPPED) { std::fill(parameters, parameters + interp_param_global::RS274NGC_MAX_PARAMETERS, 0); diff --git a/src/emc/rs274ngc/interp_write.cc b/src/emc/rs274ngc/interp_write.cc index b6b2e7d53cf..b61982ea4f8 100644 --- a/src/emc/rs274ngc/interp_write.cc +++ b/src/emc/rs274ngc/interp_write.cc @@ -22,6 +22,7 @@ #include "nml_intf/interp_return.hh" #include "interp_internal.hh" #include "rs274ngc_interp.hh" +#include // KINSTYPE_PRIMARY /****************************************************************************/ /*! write_g_codes @@ -71,8 +72,10 @@ group 16 - array[15] g7,g8 - lathe diameter mode */ int Interp::write_g_codes(block_pointer block, //!< pointer to a block of RS274/NGC instructions - setup_pointer settings) //!< pointer to machine settings + setup_pointer settings) //!< pointer to machine settings { + int kf; + settings->active_g_codes[0] = settings->sequence_number; settings->active_g_codes[1] = settings->motion_mode; settings->active_g_codes[2] = ((block == NULL) ? -1 : block->g_modes[GM_MODAL_0]); @@ -107,11 +110,17 @@ int Interp::write_g_codes(block_pointer block, //!< pointer to a block of RS27 (settings->origin_index < 7) ? (530 + (10 * settings->origin_index)) : (584 + settings->origin_index); + // the kins type, not the label, is the authority: a G43 given on the + // module's primary type shows as G43.4, and the label follows the type + // motion reports after a resync. -1 is "no information" and matches + // every flag, so it is excluded before the bit test. + kf = GET_EXTERNAL_KINS_TYPE_FLAGS(settings->kins_type); settings->active_g_codes[9] = (settings->g43_with_zero_offset || settings->tool_offset.tran.x || settings->tool_offset.tran.y || settings->tool_offset.tran.z || settings->tool_offset.a || settings->tool_offset.b || settings->tool_offset.c || - settings->tool_offset.u || settings->tool_offset.v || settings->tool_offset.w) ? G_43 : G_49; + settings->tool_offset.u || settings->tool_offset.v || settings->tool_offset.w) ? + ((kf >= 0 && (kf & KINSTYPE_PRIMARY)) ? G_43_4 : G_43) : G_49; settings->active_g_codes[10] = (settings->retract_mode == RETRACT_MODE::OLD_Z) ? G_98 : G_99; // Three modes: G_64, G_61, G_61_1 or CANON_CONTINUOUS/EXACT_PATH/EXACT_STOP settings->active_g_codes[11] = diff --git a/src/emc/rs274ngc/rs274ngc_interp.hh b/src/emc/rs274ngc/rs274ngc_interp.hh index 46491d6a627..0ad9bda006f 100644 --- a/src/emc/rs274ngc/rs274ngc_interp.hh +++ b/src/emc/rs274ngc/rs274ngc_interp.hh @@ -46,6 +46,7 @@ public: // get ready to run int init() override; void set_loop_on_main_m99(bool state) override; + void set_in_startup_code(bool state) override; // load a tool table int load_tool_table(); diff --git a/src/emc/rs274ngc/rs274ngc_pre.cc b/src/emc/rs274ngc/rs274ngc_pre.cc index 4163dc99a7c..4ebe7bccec8 100644 --- a/src/emc/rs274ngc/rs274ngc_pre.cc +++ b/src/emc/rs274ngc/rs274ngc_pre.cc @@ -1306,6 +1306,11 @@ void Interp::set_loop_on_main_m99(bool state) { _setup.loop_on_main_m99 = state; } +void Interp::set_in_startup_code(bool state) { + // the startup code runs before the motion queue can drain + _setup.in_startup_code = state; +} + /***********************************************************************/ diff --git a/src/emc/rs274ngc/rs274ngc_return.hh b/src/emc/rs274ngc/rs274ngc_return.hh index ca45ccedc07..f7f8dfcacfc 100644 --- a/src/emc/rs274ngc/rs274ngc_return.hh +++ b/src/emc/rs274ngc/rs274ngc_return.hh @@ -208,6 +208,7 @@ #define NCE_QUEUE_IS_NOT_EMPTY_AFTER_KINS_SWITCH _("Queue is not empty after Kinematics Switch") #define NCE_KINS_TYPE_NOT_PROVIDED _("G12.1 P word does not name a kinematics type this module provides") #define NCE_NO_IDENTITY_KINEMATICS_TYPE _("G13.1 needs the kinematics module to declare its identity type (see the switchkins documentation)") +#define NCE_NO_PRIMARY_KINEMATICS_TYPE _("G43.4 needs the kinematics module to declare its primary type (see the switchkins documentation)") #define NCE_ANALOG_INPUT_WITH_WAIT_NOT_IMMEDIATE _("Can't select analog input with wait type != immediate return") #define NCE_ZERO_TIMEOUT_WITH_WAIT_NOT_IMMEDIATE _("Zero timeout with wait type != immediate return") #define NCE_BOTH_DIGITAL_AND_ANALOG_INPUT_SELECTED _("Invalid to select both a digital and an analog input with M66") diff --git a/src/emc/task/emctask.cc b/src/emc/task/emctask.cc index 67465d507dd..1e93d3b3090 100644 --- a/src/emc/task/emctask.cc +++ b/src/emc/task/emctask.cc @@ -463,10 +463,15 @@ int emcTaskPlanInit() print_interp_error(retval); } else { if (0 != rs274ngc_startup_code[0]) { + // the startup code runs before the main loop can service a + // drain-and-assert wait, so a kinematics switch there must not + // ask for one + interp.set_in_startup_code(true); retval = interp.execute(rs274ngc_startup_code); while (retval == INTERP_EXECUTE_FINISH) { retval = interp.execute(NULL); } + interp.set_in_startup_code(false); if (retval > INTERP_MIN_ERROR) { print_interp_error(retval); } diff --git a/tests/kins-switch/test-ui.py b/tests/kins-switch/test-ui.py index 19a85744a15..1f0aeb0f592 100755 --- a/tests/kins-switch/test-ui.py +++ b/tests/kins-switch/test-ui.py @@ -114,10 +114,11 @@ def mdi(cmd): # the abort routine's G13.1 fires at estop reset, so the machine may stand # in identity (1) already when the program starts; either way the program -# itself walks 0, 1, 0, 1 +# itself walks 0, 1, 0, 1 through the selection codes, and the same +# again through G43.4, G49 and the codes between want = [0, 1, 0, 1] -if seen[-4:] != want: - error("motion.kins-type went %s, not ...%s" % (seen, want)) +if seen[-8:] != want + want: + error("motion.kins-type went %s, not ...%s" % (seen, want + want)) reported = [m[1].strip() for m in said if m[1].strip().startswith("KINSTYPE=")] want_reported = ["KINSTYPE=%d.000000" % k for k in want] @@ -126,6 +127,23 @@ def mdi(cmd): else: print("#<_kins_type> reported %s" % " ".join(reported)) +# G43.4 switches to the primary kinematics (0) and applies the offset, +# G49 cancels both, and a plain G43 touches the offset only; a G49 that +# cancels a plain G43, or comes after the program selected a kinematics +# itself, leaves the selection alone +g434 = [m[1].strip() for m in said if m[1].strip().startswith("G434")] +want_g434 = ["G434 KINSTYPE=0.000000 TLOZ=12.500000", + "G434 KINSTYPE=1.000000 TLOZ=0.000000", + "G434 KINSTYPE=1.000000 TLOZ=12.500000", + "G434 KINSTYPE=0.000000 TLOZ=0.000000", + "G434 KINSTYPE=0.000000 TLOZ=0.000000", + "G434 KINSTYPE=0.000000 TLOZ=12.500000", + "G434 KINSTYPE=1.000000 TLOZ=0.000000"] +if g434 != want_g434: + error("G43.4/G49 reported %s" % (g434,)) +else: + print("G43.4 switched to primary with the offset, G49 cancelled both") + # ---- a negative kinematics number is refused ----------------------------- c.mode(linuxcnc.MODE_MDI) diff --git a/tests/kins-switch/test.ngc b/tests/kins-switch/test.ngc index 96c03dfef17..eb512ea6b18 100644 --- a/tests/kins-switch/test.ngc +++ b/tests/kins-switch/test.ngc @@ -19,4 +19,33 @@ g12.1 p0 ; it is the flag that decides, not the number g13.1 (debug, KINSTYPE=#<_kins_type>) +; G43.4 is G43 on the primary kinematics: it switches from identity to +; the fiveaxis kinematics (0) and applies the tool offset +g43.4 h1 +(debug, G434 KINSTYPE=#<_kins_type> TLOZ=#5083) +; G49 cancels the offset and undoes the switch G43.4 made +g49 +(debug, G434 KINSTYPE=#<_kins_type> TLOZ=#5083) +; a plain G43 applies the offset but does not switch +g43 h1 +(debug, G434 KINSTYPE=#<_kins_type> TLOZ=#5083) +g49 +; on a kinematics the program selected itself, a plain G43 and its G49 +; leave the selection alone +g12.1 p0 +g43 h1 +g49 +(debug, G434 KINSTYPE=#<_kins_type> TLOZ=#5083) +; and G43.4 on it followed by G13.1: the program took over, the G49 +; has nothing to undo +g43.4 h1 +g13.1 +g12.1 p0 +g49 +(debug, G434 KINSTYPE=#<_kins_type> TLOZ=#5083) +; and back once more, ending in identity +g43.4 h1 +(debug, G434 KINSTYPE=#<_kins_type> TLOZ=#5083) +g49 +(debug, G434 KINSTYPE=#<_kins_type> TLOZ=#5083) m2 diff --git a/tests/kins-switch/tool.tbl b/tests/kins-switch/tool.tbl index a5809a7ac2f..d793e2d60ed 100644 --- a/tests/kins-switch/tool.tbl +++ b/tests/kins-switch/tool.tbl @@ -1 +1 @@ -T1 P1 D0.0 Z0.0 ; +T1 P1 D0.0 Z12.5 ; From 2652267ea9a345088ae6bf7c1359c2d1e23af107 Mon Sep 17 00:00:00 2001 From: Luca Toniolo <10792599+grandixximo@users.noreply.github.com> Date: Sat, 5 Sep 2026 00:06:27 +1000 Subject: [PATCH 19/60] interpreter: add the tilted work plane, G68.2, G68.4 and G69 A frame composed inside the offset chain, so the blocks between a definition and its cancel are programmed in the tilted plane while G54 itself is untouched: world = TLO + G5x + Rz(rotation_xy) * (G92 + O + R * program) O and R are the plane's origin and rotation in the coordinate system active when it was defined; rotary and UVW words do not pass through it. G68.2 takes the Fanuc forms: three angles about the axes Q names (P0 Euler, P1 fixed axes), three points over four blocks (P2), two vectors over two blocks (P3), each with an R about the plane's own Z. G68.4 composes any of those onto the active plane. G69 cancels; so do init, M2 and M30. An abort cancels it and tells canon even when the read ahead had already cancelled: status carries the plane the executed canon stream last set, and an abort throws the queued G69 away, so the two disagree until canon is told. An explicit G69 tells canon either way. Canon gains SET_G68_FRAME and the frame stage in rotate_and_offset_pos() and its inverse, so positions, probe results and arcs follow the plane. Task status carries g68_offset, g68_rotation and g68_active; the python canons, AXIS and halui apply it. While a plane is active G92, G52, G10 L2/L20/L10/L11 and a coordinate system change are refused, since each defines the system the plane sits on. --- docs/src/config/python-interface.adoc | 9 + docs/src/gcode/g-code.adoc | 107 +++++ docs/src/gcode/overview.adoc | 1 + lib/python/rs274/glcanon.py | 13 + lib/python/rs274/interpret.py | 14 + src/emc/nml_intf/canon.hh | 14 + src/emc/nml_intf/emc.cc | 17 + src/emc/nml_intf/emc.hh | 1 + src/emc/nml_intf/emc_nml.hh | 23 + src/emc/nml_intf/emcops.cc | 3 + src/emc/rs274ngc/Submakefile | 1 + src/emc/rs274ngc/gcodemodule.cc | 46 +- src/emc/rs274ngc/interp_array.cc | 2 +- src/emc/rs274ngc/interp_check.cc | 22 +- src/emc/rs274ngc/interp_convert.cc | 54 ++- src/emc/rs274ngc/interp_find.cc | 40 +- src/emc/rs274ngc/interp_internal.cc | 5 + src/emc/rs274ngc/interp_internal.hh | 18 +- src/emc/rs274ngc/interp_namedparams.cc | 21 +- src/emc/rs274ngc/interp_setup.cc | 8 + src/emc/rs274ngc/interp_workplane.cc | 437 ++++++++++++++++++ src/emc/rs274ngc/interp_write.cc | 2 +- src/emc/rs274ngc/rs274ngc_interp.hh | 14 + src/emc/rs274ngc/rs274ngc_pre.cc | 9 + src/emc/sai/saicanon.cc | 10 + src/emc/task/emccanon.cc | 76 ++- src/emc/task/emctaskmain.cc | 11 + src/emc/usr_intf/axis/extensions/emcmodule.cc | 17 + src/emc/usr_intf/axis/scripts/axis.py | 1 + src/emc/usr_intf/halui.cc | 35 +- tests/interp/g68-frame/expected | 55 +++ tests/interp/g68-frame/g68.ngc | 44 ++ tests/interp/g68-frame/test.sh | 3 + 33 files changed, 1051 insertions(+), 82 deletions(-) create mode 100644 src/emc/rs274ngc/interp_workplane.cc create mode 100644 tests/interp/g68-frame/expected create mode 100644 tests/interp/g68-frame/g68.ngc create mode 100755 tests/interp/g68-frame/test.sh diff --git a/docs/src/config/python-interface.adoc b/docs/src/config/python-interface.adoc index 25d5762445b..0fce1c760ac 100644 --- a/docs/src/config/python-interface.adoc +++ b/docs/src/config/python-interface.adoc @@ -179,6 +179,15 @@ see <> for an example. *g5x_offset*:: '(returns tuple of floats)' - offset of the currently active coordinate system. +*g68_active*:: '(returns integer)' - + a tilted work plane (G68.2) is in effect. + +*g68_offset*:: '(returns tuple of floats)' - + origin of the tilted work plane, in the coordinate system it was defined in. + +*g68_rotation*:: '(returns tuple of floats)' - + rotation of the tilted work plane, nine values row by row; its columns are the plane's X, Y and Z. + *g92_offset*:: '(returns tuple of floats)' - pose of the current g92 offset. diff --git a/docs/src/gcode/g-code.adoc b/docs/src/gcode/g-code.adoc index 42dc7a37a46..a83201d412e 100644 --- a/docs/src/gcode/g-code.adoc +++ b/docs/src/gcode/g-code.adoc @@ -92,6 +92,7 @@ as the 'L number', and so on for any other letter. |<> |Exact Path Mode |<> |Exact Stop Mode |<> |Path Control Mode with Optional Tolerance +|<> |Tilted Work Plane |<> |Lathe finishing cycle |<> |Lathe roughing cycle |<> |Drilling Cycle with Chip Breaking @@ -1914,6 +1915,112 @@ G64 P0.015 Q2 .G64 Heart image::images/G64_Heart_Q2.png["G64 Heart",align="center"] +[[gcode:g68.2]] +== G68.2, G68.4, G69 Tilted Work Plane(((G68.2 Tilted Work Plane))) + +[source,ngc] +---- +G68.2 X- Y- Z- I- J- K- (three angles, Euler) +G68.2 P1 X- Y- Z- I- J- K- (three angles about fixed axes) +G68.2 P2 Q0 X- Y- Z- (three points: the origin, then) +G68.2 P2 Q1 X- Y- Z- (a first point,) +G68.2 P2 Q2 X- Y- Z- (a second point on the plane's +X,) +G68.2 P2 Q3 X- Y- Z- (a third point on its +Y side) +G68.2 P3 Q1 X- Y- Z- I- J- K- (two vectors: the origin and +X, then) +G68.2 P3 Q2 I- J- K- (+Z, the normal) +G68.4 ... (any G68.2 form, on the active plane) +G69 (cancel) +---- + +A tilted work plane is a coordinate system composed on top of the active one: +the blocks between the definition and 'G69' are programmed in the plane, with +X and Y in it and Z along its normal, while the work offset underneath, 'G54' +say, is untouched. Positions on the display, probe results and +<> all take the plane into account. It is the same offset +chain as always with one more stage, applied first: + +---- +absolute = tool offset + G5x + XY rotation applied to (G92 + origin + rotation applied to program) +---- + +'X', 'Y' and 'Z' are the plane's origin and the plane's rotation is built +from the rest of the words, both in the coordinate system that is active when +the plane is defined: the work offset with 'G92' and the XY rotation in +place, which is what the position display shows at that moment. A rotary +word does not pass through the plane, since on a TCP kinematics the rotary +coordinates are the rotary joints and a plane does not change what a joint +is. Words left out are zero. + +'P' selects how the rotation is given: + +* 'P0', or no 'P': three angles 'I', 'J', 'K' applied one after another, + each about an axis of the plane as rotated so far (Euler angles). 'Q' + names the axes with three digits, 1 for X, 2 for Y and 3 for Z, no two + adjacent alike; the default is 'Q313', Z then X then Z. +* 'P1': three angles 'I', 'J', 'K', each about an axis of the coordinate + system the plane is defined in, in the order 'Q' gives; the default is + 'Q123', X then Y then Z. +* 'P2': three points, over up to four blocks with 'Q0' to 'Q3'. The + direction from the first point to the second is the plane's +X, the third + point lies on the +Y side. The 'Q0' block gives the origin and 'R'; without + it the origin is the first point. +* 'P3': two vectors, over two blocks. 'Q1' gives the origin and the +X + direction in 'I', 'J', 'K'; 'Q2' gives +Z, the normal, in 'I', 'J', 'K'. + The X direction need not be exactly at right angles to the normal; the + part of it along the normal is dropped. + +'R' turns the plane about its own Z after everything else, in degrees. + +The blocks of a 'P2' or 'P3' definition have to follow one another; any +other block in between is an error. A definition with a plane already active +replaces it, with the words in the coordinate system underneath, not in the +old plane. + +'G68.4' takes any 'G68.2' form and composes it onto the active plane: the +words are in the plane, and the result is a new plane relative to the old +one. It needs a plane to build on. + +'G69' cancels the plane. So does the end of the program, 'M2' or 'M30', and +an abort: the plane is not persistent and nothing about it is written to the +parameter file. + +Defining the plane does not move anything. + +While a plane is active the codes that define the coordinate system the +plane sits on are refused: 'G92', 'G92.1', 'G92.2', 'G92.3', 'G52', 'G10 L2', +'G10 L20', 'G10 L10', 'G10 L11', and a change of coordinate system +('G54' to 'G59.3'). Cancel the plane first. + +The active plane is reported in the modal G-code display as the code that +defined it. Status carries it as `g68_offset`, `g68_rotation` and +`g68_active`, next to the other offsets, for displays that want to show +plane coordinates or draw the plane. + +.G68.2 Example +[source,ngc] +---- +G54 +G68.2 X50 Y50 Z0 I30 J20 K0 (Euler: 30 about Z, 20 about the new X) +G0 X0 Y0 Z5 (5 above the plane's origin, along its normal) +G1 Z-3 F150 (a hole 3 deep, straight into the plane) +G0 Z5 +G68.4 X20 P1 I0 J0 K90 (a plane 20 along X in the old one, turned 90 about Z) +G0 X0 Y0 Z5 +G69 (back to G54 as it was) +---- + +It is an error if: + +* 'P' is not 0, 1, 2 or 3, or 'Q' with 'P0' or 'P1' is not three axis digits + with no two adjacent alike. +* A 'P2' or 'P3' definition is interrupted, or its 'Q' words come out of + order. +* The points of a 'P2' definition coincide or lie on one line, or a vector of + a 'P3' definition is zero or the X direction lies along the normal. +* 'G68.4' is used with no plane active. +* Cutter compensation is on. +* Polar coordinates or a motion code are used on the same line. + [[gcode:g70]] == G70 Lathe finishing cycle(((G70 Lathe finishing cycle))) diff --git a/docs/src/gcode/overview.adoc b/docs/src/gcode/overview.adoc index 07fc326c613..d66e25e09ff 100644 --- a/docs/src/gcode/overview.adoc +++ b/docs/src/gcode/overview.adoc @@ -974,6 +974,7 @@ The modal groups are shown in the following Table. |Units (Group 6) | G20, G21 |Cutter Diameter Compensation (Group 7) | G40, G41, G42, G41.1, G42.1 |Tool Length Offset (Group 8) | G43, G43.1, G43.2, G43.4, G49 +|Tilted Work Plane (Group 9) | G68.2, G68.4, G69 |Canned Cycles Return Mode (Group 10) | G98, G99 |Coordinate System (Group 12) | G54, G55, G56, G57, G58, G59, G59.1, G59.2, G59.3 |Control Mode (Group 13) | G61, G61.1, G64 diff --git a/lib/python/rs274/glcanon.py b/lib/python/rs274/glcanon.py index 0c32c87b3ae..4363f7de2fb 100644 --- a/lib/python/rs274/glcanon.py +++ b/lib/python/rs274/glcanon.py @@ -315,6 +315,10 @@ def set_xy_rotation(self, theta): self._flush_moves() Translated.set_xy_rotation(self, theta) + def set_g68_frame(self, *args): + self._flush_moves() + Translated.set_g68_frame(self, *args) + def set_g5x_offset(self, *args, **kw): self._flush_moves() Translated.set_g5x_offset(self, *args, **kw) @@ -1363,6 +1367,15 @@ def posstrs(self): positions[X] = _x * math.cos(t) - _y * math.sin(t) positions[Y] = _x * math.sin(t) + _y * math.cos(t) positions = [(i-j) for i, j in zip(positions, s.g92_offset)] + if s.g68_active: + # the tilted work plane sits inside G92 + r = s.g68_rotation + _x = positions[X] - s.g68_offset[X] + _y = positions[Y] - s.g68_offset[Y] + _z = positions[Z] - s.g68_offset[Z] + positions[X] = r[0]*_x + r[3]*_y + r[6]*_z + positions[Y] = r[1]*_x + r[4]*_y + r[7]*_z + positions[Z] = r[2]*_x + r[5]*_y + r[8]*_z else: positions = list(positions) diff --git a/lib/python/rs274/interpret.py b/lib/python/rs274/interpret.py index 8815f7f8696..3c83b5502a8 100644 --- a/lib/python/rs274/interpret.py +++ b/lib/python/rs274/interpret.py @@ -24,8 +24,18 @@ class Translated: g5x_offset_a = g5x_offset_b = g5x_offset_c = 0 g5x_offset_u = g5x_offset_v = g5x_offset_w = 0 rotation_xy = 0 + g68_active = 0 + g68_offset = (0.0, 0.0, 0.0) + g68_rotation = (1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0) def rotate_and_translate(self, x,y,z,a,b,c,u,v,w): + if self.g68_active: + r = self.g68_rotation + o = self.g68_offset + x, y, z = (r[0]*x + r[1]*y + r[2]*z + o[0], + r[3]*x + r[4]*y + r[5]*z + o[1], + r[6]*x + r[7]*y + r[8]*z + o[2]) + x += self.g92_offset_x y += self.g92_offset_y z += self.g92_offset_z @@ -83,6 +93,10 @@ def set_xy_rotation(self, theta): t = math.radians(theta) self.rotation_sin = math.sin(t) self.rotation_cos = math.cos(t) + def set_g68_frame(self, x, y, z, r0, r1, r2, r3, r4, r5, r6, r7, r8, active): + self.g68_active = active + self.g68_offset = (x, y, z) + self.g68_rotation = (r0, r1, r2, r3, r4, r5, r6, r7, r8) class ArcsToSegmentsMixin: plane = 1 diff --git a/src/emc/nml_intf/canon.hh b/src/emc/nml_intf/canon.hh index 51fce63005f..6e3d6c79a01 100644 --- a/src/emc/nml_intf/canon.hh +++ b/src/emc/nml_intf/canon.hh @@ -155,6 +155,9 @@ typedef struct CanonConfig_t { rotary_unlock_for_traverse(-1), g5xOffset{}, g92Offset{}, + g68Offset{}, + g68Rotation{1, 0, 0, 0, 1, 0, 0, 0, 1}, + g68Active(0), endPoint{}, lengthUnits(CANON_UNITS_INCHES), activePlane(CANON_PLANE::XY), @@ -178,6 +181,11 @@ typedef struct CanonConfig_t { CANON_POSITION g5xOffset; CANON_POSITION g92Offset; +/* The tilted work plane (G68.2): a frame inside the G92 stage of the chain, + in mm. Program X Y Z go through R * xyz + O before anything else. */ + double g68Offset[3]; + double g68Rotation[9]; // row major + int g68Active; /* canonEndPoint is the last programmed end point, stored in case it's needed for subsequent calculations. It's in absolute frame, mm units. @@ -243,6 +251,12 @@ extern void SET_G92_OFFSET(double x, double y, double z, extern void SET_XY_ROTATION(double t); +/* The tilted work plane. Origin in program units and a row major rotation + matrix, both in the coordinate system active when the plane was defined; + active 0 cancels it. */ +extern void SET_G68_FRAME(double x, double y, double z, + const double rotation[9], int active); + /* Offset the origin to the point with absolute coordinates x, y, z, a, b, c, u, v, and w. Values of x, y, z, a, b, c, u, v, and w are real numbers. The units are whatever length units are being used at the time diff --git a/src/emc/nml_intf/emc.cc b/src/emc/nml_intf/emc.cc index bf20de160e3..06b4fbe9688 100644 --- a/src/emc/nml_intf/emc.cc +++ b/src/emc/nml_intf/emc.cc @@ -308,6 +308,9 @@ int emcFormat(NMLTYPE type, void *buffer, CMS * cms) case EMC_TRAJ_SET_ROTATION_TYPE: ((EMC_TRAJ_SET_ROTATION *) buffer)->update(cms); break; + case EMC_TRAJ_SET_G68_TYPE: + ((EMC_TRAJ_SET_G68 *) buffer)->update(cms); + break; case EMC_TRAJ_SET_SCALE_TYPE: ((EMC_TRAJ_SET_SCALE *) buffer)->update(cms); break; @@ -531,6 +534,8 @@ const char *emc_symbol_lookup(uint32_t type) return "EMC_TRAJ_SET_G92"; case EMC_TRAJ_SET_ROTATION_TYPE: return "EMC_TRAJ_SET_ROTATION"; + case EMC_TRAJ_SET_G68_TYPE: + return "EMC_TRAJ_SET_G68"; case EMC_TRAJ_SET_SCALE_TYPE: return "EMC_TRAJ_SET_SCALE"; case EMC_TRAJ_SET_RAPID_SCALE_TYPE: @@ -1399,6 +1404,9 @@ void EMC_TASK_STAT::update(CMS * cms) cms->update(g5x_index); EmcPose_update(cms, &g92_offset); cms->update(rotation_xy); + EmcPose_update(cms, &g68_offset); + cms->update(g68_rotation, 9); + cms->update(g68_active); EmcPose_update(cms, &toolOffset); cms->update(activeGCodes, ACTIVE_G_CODES); cms->update(activeMCodes, ACTIVE_M_CODES); @@ -1682,6 +1690,15 @@ void EMC_TRAJ_SET_ROTATION::update(CMS * cms) cms->update(rotation); } +// cppcheck-suppress duplInheritedMember +void EMC_TRAJ_SET_G68::update(CMS * cms) +{ + EMC_TRAJ_CMD_MSG::update(cms); + EmcPose_update(cms, &origin); + cms->update(rotation, 9); + cms->update(active); +} + /* * NML/CMS Update function for EMC_SPINDLE_BRAKE_ENGAGE * Automatically generated by NML CodeGen Java Applet. diff --git a/src/emc/nml_intf/emc.hh b/src/emc/nml_intf/emc.hh index bf4777478e7..f5e45d54d6e 100644 --- a/src/emc/nml_intf/emc.hh +++ b/src/emc/nml_intf/emc.hh @@ -111,6 +111,7 @@ struct PM_CARTESIAN; #define EMC_TRAJ_SET_SO_ENABLE_TYPE ((NMLTYPE) 235) #define EMC_TRAJ_SET_FH_ENABLE_TYPE ((NMLTYPE) 236) #define EMC_TRAJ_RIGID_TAP_TYPE ((NMLTYPE) 237) +#define EMC_TRAJ_SET_G68_TYPE ((NMLTYPE) 239) #define EMC_TRAJ_SELECT_KINS_TYPE ((NMLTYPE) 289) #define EMC_TRAJ_STAT_TYPE ((NMLTYPE) 299) diff --git a/src/emc/nml_intf/emc_nml.hh b/src/emc/nml_intf/emc_nml.hh index 7690268f9ee..36ee2fed645 100644 --- a/src/emc/nml_intf/emc_nml.hh +++ b/src/emc/nml_intf/emc_nml.hh @@ -889,6 +889,26 @@ class EMC_TRAJ_SET_ROTATION:public EMC_TRAJ_CMD_MSG { double rotation; }; +// the tilted work plane frame (G68.2, G68.3, G68.4, G69): origin in user +// units and a rotation matrix, both in the coordinate system that was active +// when the plane was defined +class EMC_TRAJ_SET_G68:public EMC_TRAJ_CMD_MSG { + public: + EMC_TRAJ_SET_G68() + : EMC_TRAJ_CMD_MSG(EMC_TRAJ_SET_G68_TYPE, sizeof(EMC_TRAJ_SET_G68)), + origin{}, rotation{1, 0, 0, 0, 1, 0, 0, 0, 1}, active(0) + {}; + + // For internal NML/CMS use only. + // Sub-class update() calls base-class update() + // cppcheck-suppress duplInheritedMember + void update(CMS * cms); + + EmcPose origin; + double rotation[9]; // row major + int active; +}; + class EMC_TRAJ_CLEAR_PROBE_TRIPPED_FLAG:public EMC_TRAJ_CMD_MSG { public: EMC_TRAJ_CLEAR_PROBE_TRIPPED_FLAG() @@ -1493,6 +1513,9 @@ class EMC_TASK_STAT:public EMC_TASK_STAT_MSG { int g5x_index; // index of active g5x system EmcPose g92_offset; // in user units, currently active double rotation_xy; + EmcPose g68_offset; // tilted work plane origin, in user units + double g68_rotation[9]; // tilted work plane rotation, row major + int g68_active; // a tilted work plane is in effect EmcPose toolOffset; // tool offset, in general pose form int activeGCodes[ACTIVE_G_CODES]; int activeMCodes[ACTIVE_M_CODES]; diff --git a/src/emc/nml_intf/emcops.cc b/src/emc/nml_intf/emcops.cc index c6e77f40d9d..d38ebd8bced 100644 --- a/src/emc/nml_intf/emcops.cc +++ b/src/emc/nml_intf/emcops.cc @@ -141,6 +141,9 @@ EMC_TASK_STAT::EMC_TASK_STAT() g5x_index(0), g92_offset{}, rotation_xy(0.0), + g68_offset{}, + g68_rotation{1, 0, 0, 0, 1, 0, 0, 0, 1}, + g68_active(0), toolOffset{}, activeSettings{}, programUnits(CANON_UNITS_MM), diff --git a/src/emc/rs274ngc/Submakefile b/src/emc/rs274ngc/Submakefile index 28e974a36c5..1a6f76f58d5 100644 --- a/src/emc/rs274ngc/Submakefile +++ b/src/emc/rs274ngc/Submakefile @@ -15,6 +15,7 @@ LIBRS274SRCS := $(addprefix emc/rs274ngc/, \ interp_inverse.cc \ interp_read.cc \ interp_write.cc \ + interp_workplane.cc \ interp_o_word.cc \ interp_g7x.cc \ nurbs_additional_functions.cc \ diff --git a/src/emc/rs274ngc/gcodemodule.cc b/src/emc/rs274ngc/gcodemodule.cc index 58df5809b06..f861a2cbadc 100644 --- a/src/emc/rs274ngc/gcodemodule.cc +++ b/src/emc/rs274ngc/gcodemodule.cc @@ -694,6 +694,20 @@ void SET_XY_ROTATION(double t) { Py_XDECREF(result); }; +void SET_G68_FRAME(double x, double y, double z, + const double rotation[9], int active) { + maybe_new_line(); + if(interp_error) return; + PyObject *result = + callmethod(callback, "set_g68_frame", "ffffffffffffi", + x, y, z, + rotation[0], rotation[1], rotation[2], + rotation[3], rotation[4], rotation[5], + rotation[6], rotation[7], rotation[8], active); + if(result == NULL) interp_error ++; + Py_XDECREF(result); +}; + void USE_LENGTH_UNITS(CANON_UNITS u) { metric = u == CANON_UNITS_MM; } void SELECT_PLANE(CANON_PLANE pl) { @@ -1457,7 +1471,8 @@ static PyObject *rs274_arc_to_segments(PyObject * /*self*/, PyObject *args) { PyObject *canon; double x1, y1, cx, cy, z1, a, b, c, u, v, w; double o[9], n[9], g5xoffset[9], g92offset[9]; - int rot, plane; + double g68o[3], g68r[9]; + int rot, plane, g68active; int X, Y, Z; double rotation_cos, rotation_sin; int max_segments = 128; @@ -1488,6 +1503,32 @@ static PyObject *rs274_arc_to_segments(PyObject * /*self*/, PyObject *args) { if(!get_attr(canon, "g92_offset_u", &g92offset[6])) return NULL; if(!get_attr(canon, "g92_offset_v", &g92offset[7])) return NULL; if(!get_attr(canon, "g92_offset_w", &g92offset[8])) return NULL; + if(!get_attr(canon, "g68_active", &g68active)) return NULL; + if(g68active) { + if(!get_attr(canon, "g68_offset", "ddd:arcs_to_segments g68_offset", + &g68o[0], &g68o[1], &g68o[2])) + return NULL; + if(!get_attr(canon, "g68_rotation", "ddddddddd:arcs_to_segments g68_rotation", + &g68r[0], &g68r[1], &g68r[2], &g68r[3], &g68r[4], + &g68r[5], &g68r[6], &g68r[7], &g68r[8])) + return NULL; + } + // the tilted work plane sits inside G92: take it off the last point on + // the way in and put it back on every point on the way out + auto g68_remove = [&](double *p) { + if(!g68active) return; + double x = p[0] - g68o[0], y = p[1] - g68o[1], z = p[2] - g68o[2]; + p[0] = g68r[0]*x + g68r[3]*y + g68r[6]*z; + p[1] = g68r[1]*x + g68r[4]*y + g68r[7]*z; + p[2] = g68r[2]*x + g68r[5]*y + g68r[8]*z; + }; + auto g68_apply = [&](double *p) { + if(!g68active) return; + double x = p[0], y = p[1], z = p[2]; + p[0] = g68r[0]*x + g68r[1]*y + g68r[2]*z + g68o[0]; + p[1] = g68r[3]*x + g68r[4]*y + g68r[5]*z + g68o[1]; + p[2] = g68r[6]*x + g68r[7]*y + g68r[8]*z + g68o[2]; + }; if(plane == 1) { X=0; Y=1; Z=2; @@ -1508,6 +1549,7 @@ static PyObject *rs274_arc_to_segments(PyObject * /*self*/, PyObject *args) { for(int ax=0; ax<9; ax++) o[ax] -= g5xoffset[ax]; unrotate(o[0], o[1], rotation_cos, rotation_sin); for(int ax=0; ax<9; ax++) o[ax] -= g92offset[ax]; + g68_remove(o); double theta1 = atan2(o[Y]-cy, o[X]-cx); double theta2 = atan2(n[Y]-cy, n[X]-cx); @@ -1550,12 +1592,14 @@ static PyObject *rs274_arc_to_segments(PyObject * /*self*/, PyObject *args) { p[6] = o[6] + d[6] * f; p[7] = o[7] + d[7] * f; p[8] = o[8] + d[8] * f; + g68_apply(p); for(int ax=0; ax<9; ax++) p[ax] += g92offset[ax]; rotate(p[0], p[1], rotation_cos, rotation_sin); for(int ax=0; ax<9; ax++) p[ax] += g5xoffset[ax]; PyList_SET_ITEM(segs, i, Py_BuildValue("ddddddddd", p[0], p[1], p[2], p[3], p[4], p[5], p[6], p[7], p[8])); } + g68_apply(n); for(int ax=0; ax<9; ax++) n[ax] += g92offset[ax]; rotate(n[0], n[1], rotation_cos, rotation_sin); for(int ax=0; ax<9; ax++) n[ax] += g5xoffset[ax]; diff --git a/src/emc/rs274ngc/interp_array.cc b/src/emc/rs274ngc/interp_array.cc index 28f8c11df37..5c115cab1ad 100644 --- a/src/emc/rs274ngc/interp_array.cc +++ b/src/emc/rs274ngc/interp_array.cc @@ -104,7 +104,7 @@ const int Interp::gees[] = { /* 620 */ -1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1, /* 640 */ 13,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1, /* 660 */ -1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1, -/* 680 */ -1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1, +/* 680 */ -1,-1, 9,-1, 9,-1,-1,-1,-1,-1, 9,-1,-1,-1,-1,-1,-1,-1,-1,-1, /* 700 */ 1,-1,-1,-1,-1,-1,-1,-1,-1,-1, 1, 1, 1,-1,-1,-1,-1,-1,-1,-1, /* 720 */ 1, 1, 1,-1,-1,-1,-1,-1,-1,-1, 1,-1,-1,-1,-1,-1,-1,-1,-1,-1, /* 740 */ 1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1, diff --git a/src/emc/rs274ngc/interp_check.cc b/src/emc/rs274ngc/interp_check.cc index 0e027a74798..b0e398faf85 100644 --- a/src/emc/rs274ngc/interp_check.cc +++ b/src/emc/rs274ngc/interp_check.cc @@ -294,20 +294,23 @@ int Interp::check_other_codes(block_pointer block) //!< pointer to a block (motion != G_6) && (motion != G_6_1) && (motion != G_71) && (motion != G_71_1) && (motion != G_71_2) && (motion != G_72) && (motion != G_72_1) && (motion != G_72_2) && - (motion != G_76) && (motion != G_87) && (motion != G_33_1) && (block->g_modes[GM_MODAL_0] != G_10)), - _("I word with no G2, G3, G5, G5.1, G6, G6.1, G10, G33.1, G76, or G87 to use it")); + (motion != G_76) && (motion != G_87) && (motion != G_33_1) && (block->g_modes[GM_MODAL_0] != G_10) && + (block->g_modes[GM_WORK_PLANE] == -1)), + _("I word with no G2, G3, G5, G5.1, G6, G6.1, G10, G33.1, G68.2, G76, or G87 to use it")); } if (block->j_flag) { /* could still be useless if xz_plane arc */ CHKS(((motion != G_2) && (motion != G_3) && (motion != G_5) && (motion != G_5_1) && (motion != G_6) && (motion != G_6_1) && - (motion != G_76) && (motion != G_87) && (block->g_modes[GM_MODAL_0] != G_10)), - _("J word with no G2, G3, G5, G5.1, G6, G6.1, G10, G76 or G87 to use it")); + (motion != G_76) && (motion != G_87) && (block->g_modes[GM_MODAL_0] != G_10) && + (block->g_modes[GM_WORK_PLANE] == -1)), + _("J word with no G2, G3, G5, G5.1, G6, G6.1, G10, G68.2, G76 or G87 to use it")); } if (block->k_flag) { /* could still be useless if xy_plane arc */ - CHKS(((motion != G_2) && (motion != G_3) && (motion != G_6_2) && (motion != G_33) && (motion != G_33_1) && (motion != G_76) && (motion != G_87)), - _("K word with no G2, G3, G6.2, G33, G33.1, G76, or G87 to use it")); + CHKS(((motion != G_2) && (motion != G_3) && (motion != G_6_2) && (motion != G_33) && (motion != G_33_1) && (motion != G_76) && (motion != G_87) && + (block->g_modes[GM_WORK_PLANE] == -1)), + _("K word with no G2, G3, G6.2, G33, G33.1, G68.2, G76, or G87 to use it")); } if (block->l_number != -1) { @@ -325,6 +328,7 @@ int Interp::check_other_codes(block_pointer block) //!< pointer to a block if (block->p_flag) { CHKS(((block->g_modes[GM_MODAL_0] != G_10) && (block->g_modes[GM_MODAL_0] != G_4) && (block->g_modes[GM_CONTROL_MODE] != G_64 && (block->g_modes[GM_MODAL_0] != G_12_1)) && + (block->g_modes[GM_WORK_PLANE] == -1) && (motion != G_76) && (motion != G_82) && (motion != G_86) && (motion != G_88) && (motion != G_89) && (motion != G_5) && (motion != G_5_2) && (motion != G_70) && @@ -336,7 +340,7 @@ int Interp::check_other_codes(block_pointer block) //!< pointer to a block (block->m_modes[5] != 64) && (block->m_modes[5] != 65) && (block->m_modes[5] != 66) && (block->m_modes[7] != 19) && (block->user_m != 1) && (block->o_type != M_98)), - _("P word with no G2 G3 G4 G10 G12.1 G64 G5 G5.2 G6, G6.2, G76 G82 G86 G88 G89" + _("P word with no G2 G3 G4 G10 G12.1 G64 G68.2 G5 G5.2 G6, G6.2, G76 G82 G86 G88 G89" " or M50 M51 M52 M53 M62 M63 M64 M65 M66 M98 " "or user M code to use it")); int p_value = round_to_int(block->p_number); @@ -353,11 +357,12 @@ int Interp::check_other_codes(block_pointer block) //!< pointer to a block CHKS((motion != G_83) && (motion != G_73) && (motion != G_5) && (motion != G_6) && (motion != G_6_2) && (block->user_m != 1) && (motion != G_76) && (block->m_modes[5] != 66) && (block->m_modes[5] != 67) && (block->m_modes[5] != 68) && (block->g_modes[GM_MODAL_0] != G_10) && (block->m_modes[6] != 61) && (block->g_modes[GM_CONTROL_MODE] != G_64) && + (block->g_modes[GM_WORK_PLANE] == -1) && (motion != G_70) && (motion != G_71) && (motion != G_71_1) && (motion != G_71_2) && (motion != G_72) && (motion != G_72_1) && (motion != G_72_2) && (block->m_modes[7] != 19), - _("Q word with no G5, G6, G10, G64, G73, G76, G83, M19, M66, M67, M68 or user M code that uses it")); + _("Q word with no G5, G6, G10, G64, G68.2, G73, G76, G83, M19, M66, M67, M68 or user M code that uses it")); } if (block->r_flag) { @@ -368,6 +373,7 @@ int Interp::check_other_codes(block_pointer block) //!< pointer to a block (motion != G_74) && (block->g_modes[GM_CUTTER_COMP] != G_41_1) && (block->g_modes[GM_CUTTER_COMP] != G_42_1) && (block->g_modes[GM_MODAL_0] != G_10) && (block->m_modes[7] != 19) && + (block->g_modes[GM_WORK_PLANE] == -1) && (block->g_modes[GM_CONTROL_MODE] != G_64) ), /* G64_R_PLANNER: R selects planner on G64 */ NCE_R_WORD_WITH_NO_G_CODE_THAT_USES_IT); /* G64_R_PLANNER: a block has one shared R word; with G64 it is the planner diff --git a/src/emc/rs274ngc/interp_convert.cc b/src/emc/rs274ngc/interp_convert.cc index ec7e6205582..2412281c3c3 100644 --- a/src/emc/rs274ngc/interp_convert.cc +++ b/src/emc/rs274ngc/interp_convert.cc @@ -1641,6 +1641,8 @@ int Interp::convert_axis_offsets(int g_code, //!< g_code being executed (mus CHKS((settings->cutter_comp_side != CUTTER_COMP::OFF), /* not "== true" */ NCE_CANNOT_CHANGE_AXIS_OFFSETS_WITH_CUTTER_RADIUS_COMP); + CHKS((settings->g68_active), + _("Cannot change G92 offsets while a tilted work plane (G68.2) is active")); CHKS((block->a_flag && settings->a_axis_wrapped && (block->a_number <= -360.0 || block->a_number >= 360.0)), (_("Invalid absolute position %5.2f for wrapped rotary axis %c")), @@ -2364,6 +2366,12 @@ int Interp::convert_coordinate_system(int g_code, //!< g_code called (mus CHKS((settings->cutter_comp_side != CUTTER_COMP::OFF), (_("Cannot change coordinate systems with cutter radius compensation on"))); + { + // the plane sits on the active system; reselecting that one is harmless + int target = (g_code < G_59_1) ? (g_code - G_54) / 10 + 1 : g_code - G_59_1 + 7; + CHKS((settings->g68_active && target != settings->origin_index), + _("Cannot change coordinate systems while a tilted work plane (G68.2) is active")); + } parameters = settings->parameters; switch (g_code) { case G_54: @@ -2960,6 +2968,7 @@ int Interp::convert_g(block_pointer block, //!< pointer to a block of RS27 { int status; + CHP(work_plane_check_sequence(block, settings)); if ((block->g_modes[GM_MODAL_0] == G_4) && ONCE(STEP_DWELL)) { status = convert_dwell(settings, block->p_number); CHP(status); @@ -2988,6 +2997,10 @@ int Interp::convert_g(block_pointer block, //!< pointer to a block of RS27 status = convert_coordinate_system(block->g_modes[GM_COORD_SYSTEM], settings); CHP(status); } + if ((block->g_modes[GM_WORK_PLANE] != -1) && ONCE(STEP_WORK_PLANE)){ + status = convert_work_plane(block->g_modes[GM_WORK_PLANE], block, settings); + CHP(status); + } if ((block->g_modes[GM_CONTROL_MODE] != -1) && ONCE(STEP_CONTROL_MODE)) { status = convert_control_mode(block->g_modes[GM_CONTROL_MODE], block->p_number, block->q_number, @@ -3040,12 +3053,8 @@ offsetless machine coordinate. void Interp::get_abs_position(setup_pointer s, double abs_pos[9]) { - double x = s->current_x + s->axis_offset_x; - double y = s->current_y + s->axis_offset_y; - rotate(&x, &y, s->rotation_xy); - abs_pos[0] = x + s->origin_offset_x + s->tool_offset.tran.x; - abs_pos[1] = y + s->origin_offset_y + s->tool_offset.tran.y; - abs_pos[2] = s->current_z + s->axis_offset_z + s->origin_offset_z + s->tool_offset.tran.z; + program_to_world_xyz(s, s->current_x, s->current_y, s->current_z, + &abs_pos[0], &abs_pos[1], &abs_pos[2]); abs_pos[3] = s->AA_current + s->AA_axis_offset + s->AA_origin_offset + s->tool_offset.a; abs_pos[4] = s->BB_current + s->BB_axis_offset + s->BB_origin_offset + s->tool_offset.b; abs_pos[5] = s->CC_current + s->CC_axis_offset + s->CC_origin_offset + s->tool_offset.c; @@ -3077,12 +3086,11 @@ int Interp::convert_savehome(int code, block_pointer /*block*/, setup_pointer s) ERS(_("Cannot set reference point with cutter compensation in effect")); } - double x = s->current_x + s->axis_offset_x; - double y = s->current_y + s->axis_offset_y; - rotate(&x, &y, s->rotation_xy); - x = PROGRAM_TO_USER_LEN(x + s->tool_offset.tran.x + s->origin_offset_x); - y = PROGRAM_TO_USER_LEN(y + s->tool_offset.tran.y + s->origin_offset_y); - double z = PROGRAM_TO_USER_LEN(s->current_z + s->tool_offset.tran.z + s->origin_offset_z + s->axis_offset_z); + double x, y, z; + program_to_world_xyz(s, s->current_x, s->current_y, s->current_z, &x, &y, &z); + x = PROGRAM_TO_USER_LEN(x); + y = PROGRAM_TO_USER_LEN(y); + z = PROGRAM_TO_USER_LEN(z); double a = PROGRAM_TO_USER_ANG(s->AA_current + s->tool_offset.a + s->AA_origin_offset + s->AA_axis_offset); double b = PROGRAM_TO_USER_ANG(s->BB_current + s->tool_offset.b + s->BB_origin_offset + s->BB_axis_offset); double c = PROGRAM_TO_USER_ANG(s->CC_current + s->tool_offset.c + s->CC_origin_offset + s->CC_axis_offset); @@ -3368,6 +3376,9 @@ int Interp::convert_length_units(int g_code, //!< g_code being executed (mus settings->origin_offset_x = (settings->origin_offset_x * INCH_PER_MM); settings->origin_offset_y = (settings->origin_offset_y * INCH_PER_MM); settings->origin_offset_z = (settings->origin_offset_z * INCH_PER_MM); + settings->g68_offset[0] = (settings->g68_offset[0] * INCH_PER_MM); + settings->g68_offset[1] = (settings->g68_offset[1] * INCH_PER_MM); + settings->g68_offset[2] = (settings->g68_offset[2] * INCH_PER_MM); settings->u_current = (settings->u_current * INCH_PER_MM); settings->v_current = (settings->v_current * INCH_PER_MM); @@ -3411,6 +3422,9 @@ int Interp::convert_length_units(int g_code, //!< g_code being executed (mus settings->origin_offset_x = (settings->origin_offset_x * MM_PER_INCH); settings->origin_offset_y = (settings->origin_offset_y * MM_PER_INCH); settings->origin_offset_z = (settings->origin_offset_z * MM_PER_INCH); + settings->g68_offset[0] = (settings->g68_offset[0] * MM_PER_INCH); + settings->g68_offset[1] = (settings->g68_offset[1] * MM_PER_INCH); + settings->g68_offset[2] = (settings->g68_offset[2] * MM_PER_INCH); settings->u_current = (settings->u_current * MM_PER_INCH); settings->v_current = (settings->v_current * MM_PER_INCH); @@ -4609,6 +4623,8 @@ int Interp::convert_setup_tool(block_pointer block, setup_pointer settings) { double tx, ty, tz, ta, tb, tc, tu, tv, tw; int direct = block->l_number == 1; + CHKS((settings->g68_active && !direct), + _("Cannot use G10 L%d while a tilted work plane (G68.2) is active"), block->l_number); is_near_int(&toolno, block->p_number); CHP((find_tool_index(settings, toolno, &idx))); @@ -4848,6 +4864,9 @@ int Interp::convert_setup(block_pointer block, //!< pointer to a block of RS27 double c; double u, v, w; double r; + + CHKS((settings->g68_active), + _("Cannot use G10 L%d while a tilted work plane (G68.2) is active"), block->l_number); double *parameters; int p_int; @@ -5306,6 +5325,8 @@ int Interp::convert_stop(block_pointer block, //!< pointer to a block of RS27 ) { /* reset stuff here */ /*1*/ + // a tilted work plane does not survive the end of the program + CHP(work_plane_cancel(settings)); if (!settings->disable_auto_g54) { rotate(&settings->current_x, &settings->current_y, settings->rotation_xy); @@ -6569,16 +6590,21 @@ int Interp::convert_tool_length_offset(int g_code, //!< g_code being execu } USE_TOOL_LENGTH_OFFSET(tool_offset); - double dx, dy; + double dx, dy, dz; + // the tool does not move, so its program coordinates change by the + // offset difference seen from the program: the XY rotation and the + // tilted work plane taken off it dx = settings->tool_offset.tran.x - tool_offset.tran.x; dy = settings->tool_offset.tran.y - tool_offset.tran.y; + dz = settings->tool_offset.tran.z - tool_offset.tran.z; rotate(&dx, &dy, -settings->rotation_xy); + g68_unrotate(settings, &dx, &dy, &dz); settings->current_x += dx; settings->current_y += dy; - settings->current_z += settings->tool_offset.tran.z - tool_offset.tran.z; + settings->current_z += dz; settings->AA_current += settings->tool_offset.a - tool_offset.a; settings->BB_current += settings->tool_offset.b - tool_offset.b; settings->CC_current += settings->tool_offset.c - tool_offset.c; diff --git a/src/emc/rs274ngc/interp_find.cc b/src/emc/rs274ngc/interp_find.cc index 7a12d49d878..64b81bb989a 100644 --- a/src/emc/rs274ngc/interp_find.cc +++ b/src/emc/rs274ngc/interp_find.cc @@ -172,31 +172,14 @@ int Interp::find_ends(block_pointer block, //!< pointer to a block of RS27 #endif CHKS((block->radius_flag || block->theta_flag), _("Cannot use polar coordinates with G53")); - double cx = s->current_x + s->axis_offset_x; - double cy = s->current_y + s->axis_offset_y; - rotate(&cx, &cy, s->rotation_xy); - - if(block->x_flag) { - *px = block->x_number - s->origin_offset_x - s->tool_offset.tran.x; - } else { - *px = cx; - } - - if(block->y_flag) { - *py = block->y_number - s->origin_offset_y - s->tool_offset.tran.y; - } else { - *py = cy; - } - - rotate(px, py, -s->rotation_xy); - *px -= s->axis_offset_x; - *py -= s->axis_offset_y; - - if(block->z_flag) { - *pz = block->z_number - s->origin_offset_z - s->axis_offset_z - s->tool_offset.tran.z; - } else { - *pz = s->current_z; - } + // the words are absolute; the current point supplies the rest, + // taken to the absolute frame and back with them + double wx, wy, wz; + program_to_world_xyz(s, s->current_x, s->current_y, s->current_z, &wx, &wy, &wz); + if(block->x_flag) { wx = block->x_number; } + if(block->y_flag) { wy = block->y_number; } + if(block->z_flag) { wz = block->z_number; } + world_to_program_xyz(s, wx, wy, wz, px, py, pz); if(block->a_flag) { if(s->a_axis_wrapped) { @@ -424,12 +407,7 @@ int Interp::find_relative(double x1, //!< absolute x position double *w_2, setup_pointer settings) //!< pointer to machine settings { - *x2 = x1 - settings->origin_offset_x - settings->tool_offset.tran.x; - *y2 = y1 - settings->origin_offset_y - settings->tool_offset.tran.y; - rotate(x2, y2, -settings->rotation_xy); - *x2 -= settings->axis_offset_x; - *y2 -= settings->axis_offset_y; - *z2 = z1 - settings->origin_offset_z - settings->axis_offset_z - settings->tool_offset.tran.z; + world_to_program_xyz(settings, x1, y1, z1, x2, y2, z2); if(settings->a_axis_wrapped) { CHP(unwrap_rotary(AA_2, AA_1, diff --git a/src/emc/rs274ngc/interp_internal.cc b/src/emc/rs274ngc/interp_internal.cc index ff4ba82cb86..ad0f886ccf6 100644 --- a/src/emc/rs274ngc/interp_internal.cc +++ b/src/emc/rs274ngc/interp_internal.cc @@ -175,6 +175,11 @@ int Interp::enhance_block(block_pointer block, //!< pointer to a block to be c mode_zero_covets_axes = ((mode0 == G_10) || (mode0 == G_28) || (mode0 == G_30) || (mode0 == G_52) || (mode0 == G_92)); + // a tilted work plane definition takes the axis words the same way + if (block->g_modes[GM_WORK_PLANE] == G_68_2 || block->g_modes[GM_WORK_PLANE] == G_68_4) { + CHKS(polar_flag, _("Polar coordinates cannot define a tilted work plane")); + mode_zero_covets_axes = 1; + } if (mode1 != -1) { if (mode1 == G_80) { diff --git a/src/emc/rs274ngc/interp_internal.hh b/src/emc/rs274ngc/interp_internal.hh index bb9118a88aa..54b10e9b834 100644 --- a/src/emc/rs274ngc/interp_internal.hh +++ b/src/emc/rs274ngc/interp_internal.hh @@ -262,6 +262,9 @@ enum GCodes G_59_1 = 591, G_59_2 = 592, G_59_3 = 593, + G_68_2 = 682, + G_68_4 = 684, + G_69 = 690, G_61 = 610, G_61_1 = 611, G_64 = 640, @@ -365,6 +368,7 @@ enum phases { STEP_CUTTER_COMP, STEP_TOOL_LENGTH_OFFSET, STEP_COORD_SYSTEM, + STEP_WORK_PLANE, STEP_CONTROL_MODE, STEP_DISTANCE_MODE, STEP_IJK_DISTANCE_MODE, @@ -379,7 +383,7 @@ enum phases { // Modal groups // also indices into g_modes -// unused: 9,11 +// unused: 11 enum ModalGroups { GM_MODAL_0 = 0, @@ -391,7 +395,7 @@ enum ModalGroups GM_LENGTH_UNITS = 6, GM_CUTTER_COMP = 7, GM_TOOL_LENGTH_OFFSET = 8, - // 9 unused + GM_WORK_PLANE = 9, GM_RETRACT_MODE = 10, // 11 unused GM_COORD_SYSTEM = 12, @@ -747,6 +751,16 @@ struct setup double origin_offset_y; // g5x offset y double origin_offset_z; // g5x offset z double rotation_xy; // rotation of coordinate system around Z, in degrees + // the tilted work plane (G68.2): a frame inside G92, program units, + // in the coordinate system that was active when it was defined + bool g68_active; + int g68_code; // the code that defined it, for the modal display + double g68_offset[3]; + double g68_rotation[3][3]; // row major, columns are the plane's axes + int g68_seq_code; // a three-point or two-vector definition in progress + int g68_seq_p; + unsigned g68_seq_have; // bit per Q received + double g68_seq_word[4][7]; // per Q: x y z i j k r double parameters[interp_param_global::RS274NGC_MAX_PARAMETERS]; // system parameters int parameter_occurrence; // parameter buffer index int parameter_numbers[MAX_NAMED_PARAMETERS]; // parameter number buffer diff --git a/src/emc/rs274ngc/interp_namedparams.cc b/src/emc/rs274ngc/interp_namedparams.cc index fb12f1e8d26..9ab4bae38b5 100644 --- a/src/emc/rs274ngc/interp_namedparams.cc +++ b/src/emc/rs274ngc/interp_namedparams.cc @@ -759,26 +759,27 @@ int Interp::lookup_named_param(const char *nameBuf, case NP_ABS_X: // abs position { - double x = _setup.current_x + _setup.axis_offset_x; - double y = _setup.current_y + _setup.axis_offset_y; - rotate(&x, &y, _setup.rotation_xy); - *value = x + _setup.origin_offset_x + _setup.tool_offset.tran.x; + double abs_pos[9]; + get_abs_position(&_setup, abs_pos); + *value = abs_pos[0]; } break; case NP_ABS_Y: // abs position { - double x = _setup.current_x + _setup.axis_offset_x; - double y = _setup.current_y + _setup.axis_offset_y; - rotate(&x, &y, _setup.rotation_xy); - *value = y + _setup.origin_offset_y + _setup.tool_offset.tran.y; + double abs_pos[9]; + get_abs_position(&_setup, abs_pos); + *value = abs_pos[1]; } break; case NP_ABS_Z: // abs position - *value = _setup.current_z + _setup.axis_offset_z + - _setup.origin_offset_z + _setup.tool_offset.tran.z; + { + double abs_pos[9]; + get_abs_position(&_setup, abs_pos); + *value = abs_pos[2]; + } break; case NP_ABS_A: // abs position diff --git a/src/emc/rs274ngc/interp_setup.cc b/src/emc/rs274ngc/interp_setup.cc index 05255499f21..171f7e3a1c9 100644 --- a/src/emc/rs274ngc/interp_setup.cc +++ b/src/emc/rs274ngc/interp_setup.cc @@ -104,6 +104,14 @@ setup::setup() : origin_offset_y (0.0), origin_offset_z (0.0), rotation_xy (0.0), + g68_active(false), + g68_code(0), + g68_offset{0.0, 0.0, 0.0}, + g68_rotation{{1.0, 0.0, 0.0}, {0.0, 1.0, 0.0}, {0.0, 0.0, 1.0}}, + g68_seq_code(0), + g68_seq_p(0), + g68_seq_have(0), + g68_seq_word{}, parameters{0}, parameter_occurrence(0), diff --git a/src/emc/rs274ngc/interp_workplane.cc b/src/emc/rs274ngc/interp_workplane.cc new file mode 100644 index 00000000000..5f14ceea40e --- /dev/null +++ b/src/emc/rs274ngc/interp_workplane.cc @@ -0,0 +1,437 @@ +/******************************************************************** +* Description: interp_workplane.cc +* +* The tilted work plane: G68.2, G68.4 and G69, and the frame they put +* inside the offset chain. +* +* The chain, as canon applies it: +* +* world = TLO + G5x + Rz(rotation_xy) * (G92 + O + R * program) +* +* O and R are the plane's origin and rotation, expressed in the +* coordinate system that was active when the plane was defined: G5x +* with G92 and the XY rotation in place, which is what the operator +* sees on the display and what G68.2 X Y Z means on every control. +* Rotary and UVW words do not pass through the plane: on a TCP +* kinematics the rotary world coordinates are the rotary joints, and a +* plane does not change what a joint is. +* +* The interpreter keeps its current position in program coordinates +* and only needs the chain where it reasons about absolute coordinates +* itself (G53, G28/G30, #5021, G28.1, a G43 change). Those places +* call program_to_world_xyz() and world_to_program_xyz() from here +* rather than repeating the stages. +* +* The plane is not persistent: Interp::init(), M2/M30 and G69 clear +* it. Nothing is written to the var file. +* +* License: GPL Version 2 +* System: Linux +* +* Copyright (c) 2026 All rights reserved. +********************************************************************/ + +#include +#include +#include "rs274ngc.hh" +#include "rs274ngc_return.hh" +#include "interp_internal.hh" +#include "rs274ngc_interp.hh" + +//---------------------------------------------------------------------- +// small matrix helpers, row major double[3][3] +//---------------------------------------------------------------------- + +static void mat_identity(double m[3][3]) +{ + for (int i = 0; i < 3; i++) { + for (int j = 0; j < 3; j++) { m[i][j] = (i == j) ? 1.0 : 0.0; } + } +} + +// rotation about axis 1, 2 or 3 (X, Y, Z) by an angle in degrees +static void mat_rotation(int axis, double deg, double m[3][3]) +{ + double c = cos(deg * M_PI / 180.0), s = sin(deg * M_PI / 180.0); + mat_identity(m); + switch (axis) { + case 1: m[1][1] = c; m[1][2] = -s; m[2][1] = s; m[2][2] = c; break; + case 2: m[0][0] = c; m[0][2] = s; m[2][0] = -s; m[2][2] = c; break; + default: m[0][0] = c; m[0][1] = -s; m[1][0] = s; m[1][1] = c; break; + } +} + +static void mat_mul(const double a[3][3], const double b[3][3], double out[3][3]) +{ + double r[3][3]; + for (int i = 0; i < 3; i++) { + for (int j = 0; j < 3; j++) { + r[i][j] = a[i][0]*b[0][j] + a[i][1]*b[1][j] + a[i][2]*b[2][j]; + } + } + memcpy(out, r, sizeof(r)); +} + +static void mat_apply(const double m[3][3], double *x, double *y, double *z) +{ + double px = *x, py = *y, pz = *z; + *x = m[0][0]*px + m[0][1]*py + m[0][2]*pz; + *y = m[1][0]*px + m[1][1]*py + m[1][2]*pz; + *z = m[2][0]*px + m[2][1]*py + m[2][2]*pz; +} + +static void mat_apply_transposed(const double m[3][3], double *x, double *y, double *z) +{ + double px = *x, py = *y, pz = *z; + *x = m[0][0]*px + m[1][0]*py + m[2][0]*pz; + *y = m[0][1]*px + m[1][1]*py + m[2][1]*pz; + *z = m[0][2]*px + m[1][2]*py + m[2][2]*pz; +} + +static double vec_norm(const double v[3]) +{ + return sqrt(v[0]*v[0] + v[1]*v[1] + v[2]*v[2]); +} + +static void vec_cross(const double a[3], const double b[3], double out[3]) +{ + out[0] = a[1]*b[2] - a[2]*b[1]; + out[1] = a[2]*b[0] - a[0]*b[2]; + out[2] = a[0]*b[1] - a[1]*b[0]; +} + +// a rotation whose columns are the three axes +static void mat_from_axes(const double x[3], const double y[3], const double z[3], double m[3][3]) +{ + for (int i = 0; i < 3; i++) { m[i][0] = x[i]; m[i][1] = y[i]; m[i][2] = z[i]; } +} + +//---------------------------------------------------------------------- +// the chain +//---------------------------------------------------------------------- + +// the plane stage alone: program coordinates to the system the plane was +// defined in, and back +void Interp::g68_apply(setup_pointer s, double *x, double *y, double *z) +{ + if (!s->g68_active) { return; } + mat_apply(s->g68_rotation, x, y, z); + *x += s->g68_offset[0]; + *y += s->g68_offset[1]; + *z += s->g68_offset[2]; +} + +void Interp::g68_remove(setup_pointer s, double *x, double *y, double *z) +{ + if (!s->g68_active) { return; } + *x -= s->g68_offset[0]; + *y -= s->g68_offset[1]; + *z -= s->g68_offset[2]; + mat_apply_transposed(s->g68_rotation, x, y, z); +} + +// a displacement in the system the plane was defined in, seen from the +// program: the rotation without the origin +void Interp::g68_unrotate(setup_pointer s, double *x, double *y, double *z) +{ + if (!s->g68_active) { return; } + mat_apply_transposed(s->g68_rotation, x, y, z); +} + +// The whole chain for X Y Z, program coordinates to the absolute (G53) +// frame: the plane, G92, the XY rotation, G5x and the tool offset. +void Interp::program_to_world_xyz(setup_pointer s, + double px, double py, double pz, + double *wx, double *wy, double *wz) +{ + double x = px, y = py, z = pz; + + g68_apply(s, &x, &y, &z); + x += s->axis_offset_x; + y += s->axis_offset_y; + z += s->axis_offset_z; + rotate(&x, &y, s->rotation_xy); + *wx = x + s->origin_offset_x + s->tool_offset.tran.x; + *wy = y + s->origin_offset_y + s->tool_offset.tran.y; + *wz = z + s->origin_offset_z + s->tool_offset.tran.z; +} + +void Interp::world_to_program_xyz(setup_pointer s, + double wx, double wy, double wz, + double *px, double *py, double *pz) +{ + double x = wx - s->origin_offset_x - s->tool_offset.tran.x; + double y = wy - s->origin_offset_y - s->tool_offset.tran.y; + double z = wz - s->origin_offset_z - s->tool_offset.tran.z; + + rotate(&x, &y, -s->rotation_xy); + x -= s->axis_offset_x; + y -= s->axis_offset_y; + z -= s->axis_offset_z; + g68_remove(s, &x, &y, &z); + *px = x; + *py = y; + *pz = z; +} + +//---------------------------------------------------------------------- +// setting and clearing the plane +//---------------------------------------------------------------------- + +// Install a plane. The tool does not move, so its program coordinates +// change: take the current point through the old chain to the absolute +// frame and back through the new one. +int Interp::work_plane_set(setup_pointer s, int code, + const double origin[3], const double rotation[3][3]) +{ + double wx, wy, wz, flat[9]; + + program_to_world_xyz(s, s->current_x, s->current_y, s->current_z, &wx, &wy, &wz); + + for (int i = 0; i < 3; i++) { + s->g68_offset[i] = origin[i]; + for (int j = 0; j < 3; j++) { + s->g68_rotation[i][j] = rotation[i][j]; + flat[3*i + j] = rotation[i][j]; + } + } + s->g68_active = true; + s->g68_code = code; + + world_to_program_xyz(s, wx, wy, wz, &s->current_x, &s->current_y, &s->current_z); + + SET_G68_FRAME(origin[0], origin[1], origin[2], flat, 1); + return INTERP_OK; +} + +// Cancel the plane if one is in effect. Canon is told only when there was +// something to cancel, unless tell_canon_anyway: an abort throws away the +// queued cancel the read ahead sent, so status and the interpreter can +// disagree and only canon can settle it. +int Interp::work_plane_cancel(setup_pointer s, bool tell_canon_anyway) +{ + double wx, wy, wz; + static const double identity[9] = { 1, 0, 0, 0, 1, 0, 0, 0, 1 }; + + s->g68_seq_code = 0; + if (!s->g68_active) { + if (tell_canon_anyway) { SET_G68_FRAME(0.0, 0.0, 0.0, identity, 0); } + return INTERP_OK; + } + + program_to_world_xyz(s, s->current_x, s->current_y, s->current_z, &wx, &wy, &wz); + s->g68_active = false; + s->g68_code = 0; + for (int i = 0; i < 3; i++) { s->g68_offset[i] = 0.0; } + mat_identity(s->g68_rotation); + world_to_program_xyz(s, wx, wy, wz, &s->current_x, &s->current_y, &s->current_z); + + SET_G68_FRAME(0.0, 0.0, 0.0, identity, 0); + return INTERP_OK; +} + +// A block that is not part of a pending three-point or two-vector +// sequence: the sequence was left incomplete. +int Interp::work_plane_check_sequence(block_pointer block, setup_pointer s) +{ + if (s->g68_seq_code == 0) { return INTERP_OK; } + if (block->g_modes[GM_WORK_PLANE] == s->g68_seq_code) { return INTERP_OK; } + s->g68_seq_code = 0; + ERS(_("G68.2 P%d sequence is incomplete: the next block must carry the next Q"), s->g68_seq_p); +} + +//---------------------------------------------------------------------- +// the definitions +//---------------------------------------------------------------------- + +// Q names the axes of a three-angle definition, three digits from 1 to 3, +// no two adjacent alike: 313 is Z X Z, 123 is X Y Z. +static int parse_axis_order(double q, int order[3]) +{ + int n = (int)round(q); + if (fabs(q - n) > 1e-9 || n < 111 || n > 333) { return -1; } + order[0] = n / 100; + order[1] = (n / 10) % 10; + order[2] = n % 10; + for (int i = 0; i < 3; i++) { + if (order[i] < 1 || order[i] > 3) { return -1; } + } + if (order[0] == order[1] || order[1] == order[2]) { return -1; } + return 0; +} + +// The rotation of a G68.2 or G68.4 block, and whether the block completes +// a definition. The three-point and two-vector forms arrive over several +// blocks with Q; the words are kept in the setup until the last one. +int Interp::work_plane_build(block_pointer block, setup_pointer s, + double origin[3], double rotation[3][3], int *complete) +{ + int p = block->p_flag ? (int)round(block->p_number) : 0; + double r = block->r_flag ? block->r_number : 0.0; + double rz[3][3]; + + *complete = 0; + CHKS((block->p_flag && (fabs(block->p_number - p) > 1e-9 || p < 0 || p > 3)), + _("P word with G68.2 must be 0, 1, 2 or 3")); + + if (p == 0 || p == 1) { + // three angles. P0: each about an axis of the frame as rotated so + // far (Euler, ZXZ by default). P1: each about a fixed axis of the + // system the plane is defined in, in the order Q gives (XYZ by + // default). + int order[3]; + double angle[3], m[3][3]; + + CHKS((s->g68_seq_code != 0), _("G68.2 P%d cannot interrupt a P%d sequence"), p, s->g68_seq_p); + CHKS((parse_axis_order(block->q_flag ? block->q_number : (p == 0 ? 313.0 : 123.0), order) != 0), + _("Q word with G68.2 P%d must be three axis digits 1 to 3 with no two adjacent alike"), p); + angle[0] = block->i_flag ? block->i_number : 0.0; + angle[1] = block->j_flag ? block->j_number : 0.0; + angle[2] = block->k_flag ? block->k_number : 0.0; + + mat_identity(rotation); + for (int i = 0; i < 3; i++) { + mat_rotation(order[i], angle[i], m); + if (p == 0) { + mat_mul(rotation, m, rotation); + } else { + mat_mul(m, rotation, rotation); + } + } + origin[0] = block->x_flag ? block->x_number : 0.0; + origin[1] = block->y_flag ? block->y_number : 0.0; + origin[2] = block->z_flag ? block->z_number : 0.0; + mat_rotation(3, r, rz); + mat_mul(rotation, rz, rotation); + *complete = 1; + return INTERP_OK; + } + + // the sequences + { + int q = block->q_flag ? (int)round(block->q_number) : -1; + int code = block->g_modes[GM_WORK_PLANE]; + int first = (p == 2) ? 0 : 1, last = (p == 2) ? 3 : 2; + int expect; + + CHKS((q < 0 || fabs(block->q_number - q) > 1e-9), _("Q word missing with G68.2 P%d"), p); + if (s->g68_seq_code == 0) { + // the first block of a sequence; a three-point definition may + // leave out Q0 and take the first point as origin + CHKS((q != first && !(p == 2 && q == 1)), + _("G68.2 P%d sequence must start with Q%d"), p, first); + s->g68_seq_code = code; + s->g68_seq_p = p; + s->g68_seq_have = 0; + } else { + CHKS((s->g68_seq_code != code || s->g68_seq_p != p), + _("G68.2 P%d cannot interrupt a P%d sequence"), p, s->g68_seq_p); + } + expect = -1; + for (int i = first; i <= last; i++) { + if (!(s->g68_seq_have & (1 << i))) { expect = i; break; } + } + if (!(q == expect || (p == 2 && expect == 0 && q == 1))) { + s->g68_seq_code = 0; + ERS(_("G68.2 P%d expects Q%d here"), p, expect); + } + s->g68_seq_have |= 1 << q; + s->g68_seq_word[q][0] = block->x_flag ? block->x_number : 0.0; + s->g68_seq_word[q][1] = block->y_flag ? block->y_number : 0.0; + s->g68_seq_word[q][2] = block->z_flag ? block->z_number : 0.0; + s->g68_seq_word[q][3] = block->i_flag ? block->i_number : 0.0; + s->g68_seq_word[q][4] = block->j_flag ? block->j_number : 0.0; + s->g68_seq_word[q][5] = block->k_flag ? block->k_number : 0.0; + s->g68_seq_word[q][6] = r; + if (q != last) { return INTERP_OK; } + } + + // the sequence is complete + s->g68_seq_code = 0; + if (p == 2) { + // three points: the first to the second is +X, the third lies on + // the +Y side; Q0 gives the origin and R, else the origin is the + // first point + const double *p1 = s->g68_seq_word[1], *p2 = s->g68_seq_word[2], *p3 = s->g68_seq_word[3]; + double x[3], v[3], y[3], z[3], len; + + for (int i = 0; i < 3; i++) { x[i] = p2[i] - p1[i]; v[i] = p3[i] - p1[i]; } + len = vec_norm(x); + CHKS((len < 1e-9), _("G68.2 P2: the first two points coincide")); + for (int i = 0; i < 3; i++) { x[i] /= len; } + vec_cross(x, v, z); + len = vec_norm(z); + CHKS((len < 1e-9 * fmax(1.0, vec_norm(v))), _("G68.2 P2: the three points are on one line")); + for (int i = 0; i < 3; i++) { z[i] /= len; } + vec_cross(z, x, y); + mat_from_axes(x, y, z, rotation); + if (s->g68_seq_have & 1) { + for (int i = 0; i < 3; i++) { origin[i] = s->g68_seq_word[0][i]; } + r = s->g68_seq_word[0][6]; + } else { + for (int i = 0; i < 3; i++) { origin[i] = p1[i]; } + r = 0.0; + } + } else { + // two vectors: the origin and +X on the first block, +Z on the + // second; X is projected onto the plane so that a request a few + // digits off square still names a frame + const double *q1 = s->g68_seq_word[1], *q2 = s->g68_seq_word[2]; + double x[3], y[3], z[3], len, along; + + for (int i = 0; i < 3; i++) { z[i] = q2[3 + i]; x[i] = q1[3 + i]; } + len = vec_norm(z); + CHKS((len < 1e-12), _("G68.2 P3: the Z direction is a zero vector")); + for (int i = 0; i < 3; i++) { z[i] /= len; } + len = vec_norm(x); + CHKS((len < 1e-12), _("G68.2 P3: the X direction is a zero vector")); + along = x[0]*z[0] + x[1]*z[1] + x[2]*z[2]; + for (int i = 0; i < 3; i++) { x[i] -= along * z[i]; } + CHKS((vec_norm(x) < 1e-6 * len), _("G68.2 P3: the X direction lies along the Z direction")); + len = vec_norm(x); + for (int i = 0; i < 3; i++) { x[i] /= len; } + vec_cross(z, x, y); + mat_from_axes(x, y, z, rotation); + for (int i = 0; i < 3; i++) { origin[i] = q1[i]; } + r = q1[6]; + } + mat_rotation(3, r, rz); + mat_mul(rotation, rz, rotation); + *complete = 1; + return INTERP_OK; +} + +// G68.2, G68.4 and G69 from convert_g +int Interp::convert_work_plane(int g_code, block_pointer block, setup_pointer s) +{ + double origin[3], rotation[3][3]; + int complete; + + if (g_code == G_69) { + CHKS((s->cutter_comp_side != CUTTER_COMP::OFF), + _("Cannot cancel a tilted work plane with cutter radius compensation on")); + return work_plane_cancel(s, true); + } + + CHKS((g_code != G_68_2 && g_code != G_68_4), "BUG: code not G68.2, G68.4 or G69"); + CHKS((s->cutter_comp_side != CUTTER_COMP::OFF), + _("Cannot define a tilted work plane with cutter radius compensation on")); + CHKS((g_code == G_68_4 && !s->g68_active), + _("G68.4 needs an active tilted work plane to build on")); + + CHP(work_plane_build(block, s, origin, rotation, &complete)); + if (!complete) { return INTERP_OK; } + + if (g_code == G_68_4) { + // composed onto the active plane: the new origin is a point of the + // old plane and the new rotation follows the old one + double ox = origin[0], oy = origin[1], oz = origin[2]; + + g68_apply(s, &ox, &oy, &oz); + origin[0] = ox; + origin[1] = oy; + origin[2] = oz; + mat_mul(s->g68_rotation, rotation, rotation); + } + return work_plane_set(s, g_code, origin, rotation); +} diff --git a/src/emc/rs274ngc/interp_write.cc b/src/emc/rs274ngc/interp_write.cc index b61982ea4f8..54dfe128f98 100644 --- a/src/emc/rs274ngc/interp_write.cc +++ b/src/emc/rs274ngc/interp_write.cc @@ -126,7 +126,7 @@ int Interp::write_g_codes(block_pointer block, //!< pointer to a block of RS27 settings->active_g_codes[11] = (settings->control_mode == CANON_CONTINUOUS) ? G_64 : (settings->control_mode == CANON_EXACT_PATH) ? G_61 : G_61_1; - settings->active_g_codes[12] = -1; + settings->active_g_codes[12] = settings->g68_active ? settings->g68_code : -1; settings->active_g_codes[13] = //I don't even know how to display the mode of an arbitrary number of spindles (andypugh 17/6/16) (settings->spindle_mode[0] == SPINDLE_MODE::CONSTANT_RPM) ? G_97 : G_96; settings->active_g_codes[14] = (settings->ijk_distance_mode == DISTANCE_MODE::ABSOLUTE) ? G_90_1 : G_91_1; diff --git a/src/emc/rs274ngc/rs274ngc_interp.hh b/src/emc/rs274ngc/rs274ngc_interp.hh index 0ad9bda006f..0b480f4ab0f 100644 --- a/src/emc/rs274ngc/rs274ngc_interp.hh +++ b/src/emc/rs274ngc/rs274ngc_interp.hh @@ -362,6 +362,20 @@ public: setup_pointer settings); int convert_tool_select(block_pointer block, setup_pointer settings); int convert_kins_switch(int code, block_pointer block, setup_pointer settings); + int convert_work_plane(int g_code, block_pointer block, setup_pointer settings); + int work_plane_build(block_pointer block, setup_pointer settings, + double origin[3], double rotation[3][3], int *complete); + int work_plane_set(setup_pointer settings, int code, + const double origin[3], const double rotation[3][3]); + int work_plane_cancel(setup_pointer settings, bool tell_canon_anyway = false); + int work_plane_check_sequence(block_pointer block, setup_pointer settings); + void g68_apply(setup_pointer settings, double *x, double *y, double *z); + void g68_remove(setup_pointer settings, double *x, double *y, double *z); + void g68_unrotate(setup_pointer settings, double *x, double *y, double *z); + void program_to_world_xyz(setup_pointer settings, double px, double py, double pz, + double *wx, double *wy, double *wz); + void world_to_program_xyz(setup_pointer settings, double wx, double wy, double wz, + double *px, double *py, double *pz); int update_tag(StateTag &tag); int cycle_feed(block_pointer block, CANON_PLANE plane, double end1, double end2, double end3); diff --git a/src/emc/rs274ngc/rs274ngc_pre.cc b/src/emc/rs274ngc/rs274ngc_pre.cc index 4ebe7bccec8..ad04ae98bbe 100644 --- a/src/emc/rs274ngc/rs274ngc_pre.cc +++ b/src/emc/rs274ngc/rs274ngc_pre.cc @@ -1195,6 +1195,9 @@ int Interp::init() _setup.toolchange_flag = false; _setup.input_flag = false; _setup.kinsSwitch_flag = false; + // the tilted work plane does not survive an abort or a program start; + // canon hears about it only if there was one + work_plane_cancel(&_setup); _setup.input_index = -1; _setup.input_digital = false; _setup.program_x = 0.; /* for cutter comp */ @@ -2685,6 +2688,12 @@ int Interp::on_abort(int reason, const char *message) reset(); _setup.mdi_interrupt = false; + // the tilted work plane goes before the abort routine runs, so that + // routine can change coordinate systems as it likes. Canon is told + // even when the read ahead had already cancelled it, since the message + // that would have said so died with the queue. + work_plane_cancel(&_setup, true); + /* A thread's queued override restore is lost when abort clears the interpreter list, so re-assert the modal state here. */ if (_setup.speed_override[_setup.active_spindle]) { diff --git a/src/emc/sai/saicanon.cc b/src/emc/sai/saicanon.cc index 87c77b36fe7..fc7cde1c018 100644 --- a/src/emc/sai/saicanon.cc +++ b/src/emc/sai/saicanon.cc @@ -112,6 +112,16 @@ void SET_XY_ROTATION(double t) { ECHO_WITH_ARGS("%.4f", t); } +void SET_G68_FRAME(double x, double y, double z, + const double rotation[9], int active) { + ECHO_WITH_ARGS("%.4f, %.4f, %.4f, " + "[%.4f, %.4f, %.4f, %.4f, %.4f, %.4f, %.4f, %.4f, %.4f], %d", + x, y, z, + rotation[0], rotation[1], rotation[2], + rotation[3], rotation[4], rotation[5], + rotation[6], rotation[7], rotation[8], active); +} + void SET_G5X_OFFSET(int index, double x, double y, double z, double a, double b, double c, diff --git a/src/emc/task/emccanon.cc b/src/emc/task/emccanon.cc index 2c7e8ea5fa0..5ef10868ebb 100644 --- a/src/emc/task/emccanon.cc +++ b/src/emc/task/emccanon.cc @@ -181,13 +181,46 @@ static void rotate(double &x, double &y, double theta) { } +// The tilted work plane, the innermost stage of the chain: what a program +// calls X Y Z is R * xyz + O in the coordinate system that was active when +// the plane was defined. Rotary and UVW words do not pass through it. +static void g68_apply(double &x, double &y, double &z) { + if (!canon.g68Active) { return; } + const double *r = canon.g68Rotation; + double px = x, py = y, pz = z; + x = r[0]*px + r[1]*py + r[2]*pz + canon.g68Offset[0]; + y = r[3]*px + r[4]*py + r[5]*pz + canon.g68Offset[1]; + z = r[6]*px + r[7]*py + r[8]*pz + canon.g68Offset[2]; +} + +static void g68_remove(double &x, double &y, double &z) { + if (!canon.g68Active) { return; } + const double *r = canon.g68Rotation; + double px = x - canon.g68Offset[0]; + double py = y - canon.g68Offset[1]; + double pz = z - canon.g68Offset[2]; + x = r[0]*px + r[3]*py + r[6]*pz; + y = r[1]*px + r[4]*py + r[7]*pz; + z = r[2]*px + r[5]*py + r[8]*pz; +} + +// a direction: the rotation of the plane without its origin +static void g68_rotate(double &x, double &y, double &z) { + if (!canon.g68Active) { return; } + const double *r = canon.g68Rotation; + double px = x, py = y, pz = z; + x = r[0]*px + r[1]*py + r[2]*pz; + y = r[3]*px + r[4]*py + r[5]*pz; + z = r[6]*px + r[7]*py + r[8]*pz; +} + /** - * Implementation of planar rotation for a 3D vector. - * This is basically a shortcut for "rotate" when the values are stored in a - * cartesian vector. + * Rotation of a direction vector into the world frame: the tilted work + * plane first, then the planar rotation about Z. * The use of static "xy_rotation" is ugly here, but is at least consistent. */ static void to_rotated(PM_CARTESIAN &vec) { + g68_rotate(vec.x, vec.y, vec.z); rotate(vec.x,vec.y,canon.xy_rotation); } #if 0 @@ -197,6 +230,8 @@ static void from_rotated(PM_CARTESIAN &vec) { #endif static void rotate_and_offset(CANON_POSITION & pos) { + g68_apply(pos.x, pos.y, pos.z); + pos += canon.g92Offset; rotate(pos.x, pos.y, canon.xy_rotation); @@ -208,6 +243,8 @@ static void rotate_and_offset(CANON_POSITION & pos) { static void rotate_and_offset_xyz(PM_CARTESIAN & xyz) { + g68_apply(xyz.x, xyz.y, xyz.z); + xyz += canon.g92Offset.xyz(); rotate(xyz.x, xyz.y, canon.xy_rotation); @@ -232,10 +269,14 @@ static CANON_POSITION unoffset_and_unrotate_pos(const CANON_POSITION& pos) { res -= canon.g92Offset; + g68_remove(res.x, res.y, res.z); + return res; } static void rotate_and_offset_pos(double &x, double &y, double &z, double &a, double &b, double &c, double &u, double &v, double &w) { + g68_apply(x, y, z); + x += canon.g92Offset.x; y += canon.g92Offset.y; z += canon.g92Offset.z; @@ -478,6 +519,26 @@ void SET_XY_ROTATION(double t) { canon.xy_rotation = t; } +void SET_G68_FRAME(double x, double y, double z, + const double rotation[9], int active) +{ + flush_segments(); + + canon.g68Offset[0] = FROM_PROG_LEN(x); + canon.g68Offset[1] = FROM_PROG_LEN(y); + canon.g68Offset[2] = FROM_PROG_LEN(z); + for (int i = 0; i < 9; i++) { canon.g68Rotation[i] = rotation[i]; } + canon.g68Active = active; + + auto msg = std::make_unique(); + msg->origin.tran.x = TO_EXT_LEN(canon.g68Offset[0]); + msg->origin.tran.y = TO_EXT_LEN(canon.g68Offset[1]); + msg->origin.tran.z = TO_EXT_LEN(canon.g68Offset[2]); + for (int i = 0; i < 9; i++) { msg->rotation[i] = rotation[i]; } + msg->active = active; + interp_list.append(std::move(msg)); +} + void SET_G5X_OFFSET(int index, double x, double y, double z, double a, double b, double c, @@ -2599,7 +2660,9 @@ void ARC_FEED(int line_number, canon_debug("line = %d\n", line_number); canon_debug("first_end = %f, second_end = %f\n", first_end,second_end); - if( canon.activePlane == CANON_PLANE::XY && canon.motionMode == CANON_CONTINUOUS) { + // the naive cam detector works on the world XY projection of the arc, + // which a tilted work plane takes out of the XY plane + if( canon.activePlane == CANON_PLANE::XY && canon.motionMode == CANON_CONTINUOUS && !canon.g68Active) { double mx, my; double lx, ly, lz; double unused = 0; @@ -2838,7 +2901,7 @@ void ARC_FEED(int line_number, double j2 = FROM_EXT_LEN(emcAxisGetMaxJerk(axis2)); double j_min = MIN(j1, j2); - if(canon.xy_rotation && canon.activePlane != CANON_PLANE::XY) { + if((canon.xy_rotation && canon.activePlane != CANON_PLANE::XY) || canon.g68Active) { // also consider the third plane's constraint, which may get // involved since we're rotated. @@ -3618,6 +3681,9 @@ void INIT_CANON() // initialize locals to original values canon.xy_rotation = 0.0; + canon.g68Offset[0] = canon.g68Offset[1] = canon.g68Offset[2] = 0.0; + for (int i = 0; i < 9; i++) { canon.g68Rotation[i] = (i % 4 == 0) ? 1.0 : 0.0; } + canon.g68Active = 0; canon.rotary_unlock_for_traverse = -1; canon.feed_mode = 0; canon.g5xOffset.x = 0.0; diff --git a/src/emc/task/emctaskmain.cc b/src/emc/task/emctaskmain.cc index 5e41127a5a2..7a1a1583275 100644 --- a/src/emc/task/emctaskmain.cc +++ b/src/emc/task/emctaskmain.cc @@ -1538,6 +1538,7 @@ static EMC_TASK_EXEC emcTaskCheckPreconditions(NMLmsg * cmd) case EMC_TRAJ_SET_G5X_TYPE: case EMC_TRAJ_SET_G92_TYPE: case EMC_TRAJ_SET_ROTATION_TYPE: + case EMC_TRAJ_SET_G68_TYPE: // this applies the program origin after previous motions return EMC_TASK_EXEC::WAITING_FOR_MOTION; break; @@ -1907,6 +1908,15 @@ static int emcTaskIssueCommand(NMLmsg * cmd) retval = 0; break; + case EMC_TRAJ_SET_G68_TYPE: { + EMC_TRAJ_SET_G68 *g68 = reinterpret_cast(cmd); + emcStatus->task.g68_offset = g68->origin; + for (int i = 0; i < 9; i++) { emcStatus->task.g68_rotation[i] = g68->rotation[i]; } + emcStatus->task.g68_active = g68->active; + retval = 0; + break; + } + case EMC_TRAJ_SET_G5X_TYPE: // struct-copy program origin emcStatus->task.g5x_offset = (reinterpret_cast(cmd))->origin; @@ -2497,6 +2507,7 @@ static EMC_TASK_EXEC emcTaskCheckPostconditions(NMLmsg * cmd) case EMC_TRAJ_SET_G5X_TYPE: case EMC_TRAJ_SET_G92_TYPE: case EMC_TRAJ_SET_ROTATION_TYPE: + case EMC_TRAJ_SET_G68_TYPE: case EMC_TRAJ_PROBE_TYPE: case EMC_TRAJ_RIGID_TAP_TYPE: case EMC_TRAJ_CLEAR_PROBE_TRIPPED_FLAG_TYPE: diff --git a/src/emc/usr_intf/axis/extensions/emcmodule.cc b/src/emc/usr_intf/axis/extensions/emcmodule.cc index 0c5a5742986..0e24ef9df82 100644 --- a/src/emc/usr_intf/axis/extensions/emcmodule.cc +++ b/src/emc/usr_intf/axis/extensions/emcmodule.cc @@ -1151,6 +1151,7 @@ static PyMemberDef Stat_members[] = { { "task_paused", T_INT, O(task.task_paused), READONLY, NULL}, { "input_timeout", T_BOOL, O(task.input_timeout), READONLY, NULL}, { "rotation_xy", T_DOUBLE, O(task.rotation_xy), READONLY, NULL}, + { "g68_active", T_INT, O(task.g68_active), READONLY, "A tilted work plane (G68.2) is in effect."}, { "ini_filename", T_STRING_INPLACE, O(task.ini_filename), READONLY, NULL}, { "delay_left", T_DOUBLE, O(task.delayLeft), READONLY, NULL}, { "queued_mdi_commands", T_INT, O(task.queuedMDIcommands), READONLY, @@ -1275,6 +1276,18 @@ static PyObject *Stat_tool_offset(pyStatChannel *s, void *) { return pose(s->status.task.toolOffset); } +static PyObject *Stat_g68_offset(pyStatChannel *s, void *) { + return pose(s->status.task.g68_offset); +} + +static PyObject *Stat_g68_rotation(pyStatChannel *s, void *) { + PyObject *res = PyTuple_New(9); + for (int i = 0; i < 9; i++) { + PyTuple_SET_ITEM(res, i, PyFloat_FromDouble(s->status.task.g68_rotation[i])); + } + return res; +} + static PyObject *Stat_position(pyStatChannel *s, void *) { return pose(s->status.motion.traj.position); } @@ -1550,6 +1563,10 @@ static PyGetSetDef Stat_getsetlist[] = { {(char*)"g5x_offset", (getter)Stat_g5x_offset, NULL, NULL, NULL}, {(char*)"g5x_index", (getter)Stat_g5x_index, NULL, NULL, NULL}, {(char*)"g92_offset", (getter)Stat_g92_offset, NULL, NULL, NULL}, + {(char*)"g68_offset", (getter)Stat_g68_offset, NULL, + (char*)"Origin of the tilted work plane (G68.2), in the coordinate system it was defined in.", NULL}, + {(char*)"g68_rotation", (getter)Stat_g68_rotation, NULL, + (char*)"Rotation matrix of the tilted work plane (G68.2), nine values row by row.", NULL}, {(char*)"position", (getter)Stat_position, NULL, NULL, NULL}, {(char*)"dtg", (getter)Stat_dtg, NULL, NULL, NULL}, {(char*)"joint_position", (getter)Stat_joint_position, NULL, NULL, NULL}, diff --git a/src/emc/usr_intf/axis/scripts/axis.py b/src/emc/usr_intf/axis/scripts/axis.py index 2a1c90cb57b..b2c5e46744d 100755 --- a/src/emc/usr_intf/axis/scripts/axis.py +++ b/src/emc/usr_intf/axis/scripts/axis.py @@ -1603,6 +1603,7 @@ def next_line(*args): pass def set_g5x_offset(*args): pass def set_g92_offset(*args): pass def set_xy_rotation(*args): pass + def set_g68_frame(*args): pass def get_external_angular_units(self): return 1.0 def get_external_length_units(self): return 1.0 def set_plane(*args): pass diff --git a/src/emc/usr_intf/halui.cc b/src/emc/usr_intf/halui.cc index 6796e4cf48d..1d46b23a28c 100644 --- a/src/emc/usr_intf/halui.cc +++ b/src/emc/usr_intf/halui.cc @@ -2041,28 +2041,45 @@ static void modify_hal_pins() hal_set_bool(halui_data->joint_has_fault[joint], emcStatus->motion.joint[joint].fault); } + // the relative position: the offset chain taken off in reverse, the + // tool offset, G5x, the XY rotation, G92 and the tilted work plane + double rx = emcStatus->motion.traj.actualPosition.tran.x - emcStatus->task.g5x_offset.tran.x - emcStatus->task.toolOffset.tran.x; + double ry = emcStatus->motion.traj.actualPosition.tran.y - emcStatus->task.g5x_offset.tran.y - emcStatus->task.toolOffset.tran.y; + double rz = emcStatus->motion.traj.actualPosition.tran.z - emcStatus->task.g5x_offset.tran.z - emcStatus->task.toolOffset.tran.z; + { + double t = -emcStatus->task.rotation_xy * TO_RAD; + double x = rx * cos(t) - ry * sin(t); + double y = ry * cos(t) + rx * sin(t); + rx = x - emcStatus->task.g92_offset.tran.x; + ry = y - emcStatus->task.g92_offset.tran.y; + rz -= emcStatus->task.g92_offset.tran.z; + } + if (emcStatus->task.g68_active) { + const double *r = emcStatus->task.g68_rotation; + double x = rx - emcStatus->task.g68_offset.tran.x; + double y = ry - emcStatus->task.g68_offset.tran.y; + double z = rz - emcStatus->task.g68_offset.tran.z; + rx = r[0]*x + r[3]*y + r[6]*z; + ry = r[1]*x + r[4]*y + r[7]*z; + rz = r[2]*x + r[5]*y + r[8]*z; + } + if (axis_mask & 0x0001) { hal_set_real(halui_data->axis_pos_commanded[0], emcStatus->motion.traj.position.tran.x); hal_set_real(halui_data->axis_pos_feedback[0], emcStatus->motion.traj.actualPosition.tran.x); - double x = emcStatus->motion.traj.actualPosition.tran.x - emcStatus->task.g5x_offset.tran.x - emcStatus->task.toolOffset.tran.x; - double y = emcStatus->motion.traj.actualPosition.tran.y - emcStatus->task.g5x_offset.tran.y - emcStatus->task.toolOffset.tran.y; - x = x * cos(-emcStatus->task.rotation_xy * TO_RAD) - y * sin(-emcStatus->task.rotation_xy * TO_RAD); - hal_set_real(halui_data->axis_pos_relative[0], x - emcStatus->task.g92_offset.tran.x); + hal_set_real(halui_data->axis_pos_relative[0], rx); } if (axis_mask & 0x0002) { hal_set_real(halui_data->axis_pos_commanded[1], emcStatus->motion.traj.position.tran.y); hal_set_real(halui_data->axis_pos_feedback[1], emcStatus->motion.traj.actualPosition.tran.y); - double x = emcStatus->motion.traj.actualPosition.tran.x - emcStatus->task.g5x_offset.tran.x - emcStatus->task.toolOffset.tran.x; - double y = emcStatus->motion.traj.actualPosition.tran.y - emcStatus->task.g5x_offset.tran.y - emcStatus->task.toolOffset.tran.y; - y = y * cos(-emcStatus->task.rotation_xy * TO_RAD) + x * sin(-emcStatus->task.rotation_xy * TO_RAD); - hal_set_real(halui_data->axis_pos_relative[1], y - emcStatus->task.g92_offset.tran.y); + hal_set_real(halui_data->axis_pos_relative[1], ry); } if (axis_mask & 0x0004) { hal_set_real(halui_data->axis_pos_commanded[2], emcStatus->motion.traj.position.tran.z); hal_set_real(halui_data->axis_pos_feedback[2], emcStatus->motion.traj.actualPosition.tran.z); - hal_set_real(halui_data->axis_pos_relative[2], emcStatus->motion.traj.actualPosition.tran.z - emcStatus->task.g5x_offset.tran.z - emcStatus->task.g92_offset.tran.z - emcStatus->task.toolOffset.tran.z); + hal_set_real(halui_data->axis_pos_relative[2], rz); } if (axis_mask & 0x0008) { diff --git a/tests/interp/g68-frame/expected b/tests/interp/g68-frame/expected new file mode 100644 index 00000000000..98c0321fffe --- /dev/null +++ b/tests/interp/g68-frame/expected @@ -0,0 +1,55 @@ + 1 N..... USE_LENGTH_UNITS(CANON_UNITS_MM) + 2 N..... SET_G5X_OFFSET(1, 0.0000, 0.0000, 0.0000, 0.0000, 0.0000, 0.0000) + 3 N..... SET_G92_OFFSET(0.0000, 0.0000, 0.0000, 0.0000, 0.0000, 0.0000) + 4 N..... SET_XY_ROTATION(0.0000) + 5 N..... SET_FEED_REFERENCE(CANON_XYZ) + 6 N..... ON_RESET() + 7 N..... COMMENT("the tilted work plane through the stand alone canon") + 8 N..... USE_LENGTH_UNITS(CANON_UNITS_MM) + 9 N..... COMMENT("interpreter: setting coordinate system origin") + 10 N..... SET_G5X_OFFSET(2, 100.0000, 200.0000, 300.0000, 0.0000, 0.0000, 0.0000) + 11 N..... SET_G92_OFFSET(0.0000, 0.0000, 0.0000, 0.0000, 0.0000, 0.0000) + 12 N..... SET_XY_ROTATION(0.0000) + 13 N..... COMMENT("a plane rotated 90 about X: plane Y is world Z, plane Z is world -Y") + 14 N..... SET_G68_FRAME(10.0000, 20.0000, 30.0000, [1.0000, 0.0000, 0.0000, 0.0000, 0.0000, -1.0000, 0.0000, 1.0000, 0.0000], 1) + 15 N..... STRAIGHT_TRAVERSE(1.0000, 2.0000, 3.0000, 0.0000, 0.0000, 0.0000) + 16 N..... COMMENT("the same plane by fixed axis angles about X") + 17 N..... SET_G68_FRAME(10.0000, 20.0000, 30.0000, [1.0000, 0.0000, 0.0000, 0.0000, 0.0000, -1.0000, 0.0000, 1.0000, 0.0000], 1) + 18 N..... STRAIGHT_TRAVERSE(1.0000, 2.0000, 3.0000, 0.0000, 0.0000, 0.0000) + 19 N..... COMMENT("the same plane by three points") + 20 N..... SET_G68_FRAME(10.0000, 20.0000, 30.0000, [1.0000, 0.0000, 0.0000, 0.0000, 0.0000, -1.0000, 0.0000, 1.0000, 0.0000], 1) + 21 N..... STRAIGHT_TRAVERSE(1.0000, 2.0000, 3.0000, 0.0000, 0.0000, 0.0000) + 22 N..... COMMENT("the same plane by two vectors, X nudged off square") + 23 N..... SET_G68_FRAME(10.0000, 20.0000, 30.0000, [1.0000, 0.0000, 0.0000, 0.0000, 0.0000, -1.0000, 0.0000, 1.0000, 0.0000], 1) + 24 N..... STRAIGHT_TRAVERSE(1.0000, 2.0000, 3.0000, 0.0000, 0.0000, 0.0000) + 25 N..... COMMENT("R turns the plane about its own Z") + 26 N..... SET_G68_FRAME(10.0000, 20.0000, 30.0000, [0.0000, -1.0000, 0.0000, 0.0000, 0.0000, -1.0000, 1.0000, 0.0000, 0.0000], 1) + 27 N..... STRAIGHT_TRAVERSE(1.0000, 2.0000, 3.0000, 0.0000, 0.0000, 0.0000) + 28 N..... COMMENT("an arc in the plane") + 29 N..... SET_FEED_RATE(100.0000) + 30 N..... STRAIGHT_FEED(0.0000, 0.0000, 0.0000, 0.0000, 0.0000, 0.0000) + 31 N..... ARC_FEED(2.0000, 0.0000, 1.0000, 0.0000, -1, 0.0000, 0.0000, 0.0000, 0.0000) + 32 N..... COMMENT("G53 inside the plane goes to absolute coordinates") + 33 N..... STRAIGHT_TRAVERSE(-30.0000, 10.0000, 20.0000, 0.0000, 0.0000, 0.0000) + 34 N..... COMMENT("and #5021 reports them") + 35 N..... MESSAGE(" abs 100.000000 200.000000 300.000000 prog -30.000000 10.000000 20.000000") + 36 N..... COMMENT("a probe result comes back in plane coordinates: nothing to run here") + 37 N..... COMMENT("a tool length change moves the program coordinates along the plane axis that is world Z") + 38 N..... USE_TOOL_LENGTH_OFFSET(0.0000 0.0000 7.0000, 0.0000 0.0000 0.0000, 0.0000 0.0000 0.0000) + 39 N..... MESSAGE(" prog -37.000000 10.000000 20.000000") + 40 N..... USE_TOOL_LENGTH_OFFSET(0.0000 0.0000 0.0000, 0.0000 0.0000 0.0000, 0.0000 0.0000 0.0000) + 41 N..... COMMENT("G68.4 composes: a further 90 about the plane's X") + 42 N..... SET_G68_FRAME(10.0000, 20.0000, 30.0000, [0.0000, 0.0000, 1.0000, 0.0000, -1.0000, 0.0000, 1.0000, 0.0000, 0.0000], 1) + 43 N..... STRAIGHT_TRAVERSE(1.0000, 2.0000, 3.0000, 0.0000, 0.0000, 0.0000) + 44 N..... COMMENT("G69 cancels") + 45 N..... SET_G68_FRAME(0.0000, 0.0000, 0.0000, [1.0000, 0.0000, 0.0000, 0.0000, 1.0000, 0.0000, 0.0000, 0.0000, 1.0000], 0) + 46 N..... STRAIGHT_TRAVERSE(1.0000, 2.0000, 3.0000, 0.0000, 0.0000, 0.0000) + 47 N..... SET_G5X_OFFSET(1, 0.0000, 0.0000, 0.0000, 0.0000, 0.0000, 0.0000) + 48 N..... SET_XY_ROTATION(0.0000) + 49 N..... SET_FEED_MODE(0, 0) + 50 N..... SET_FEED_RATE(0.0000) + 51 N..... STOP_SPINDLE_TURNING(0) + 52 N..... SET_SPINDLE_MODE(0 0.0000) + 53 N..... PROGRAM_END() + 54 N..... ON_RESET() + 55 N..... ON_RESET() diff --git a/tests/interp/g68-frame/g68.ngc b/tests/interp/g68-frame/g68.ngc new file mode 100644 index 00000000000..656cce9b906 --- /dev/null +++ b/tests/interp/g68-frame/g68.ngc @@ -0,0 +1,44 @@ +% +(the tilted work plane through the stand alone canon) +g21 g90 +g10 l2 p2 x100 y200 z300 r0 +g55 +(a plane rotated 90 about X: plane Y is world Z, plane Z is world -Y) +g68.2 x10 y20 z30 i0 j90 k0 +g0 x1 y2 z3 +(the same plane by fixed axis angles about X) +g68.2 p1 q123 x10 y20 z30 i90 j0 k0 +g0 x1 y2 z3 +(the same plane by three points) +g68.2 p2 q0 x10 y20 z30 +g68.2 p2 q1 x0 y0 z0 +g68.2 p2 q2 x5 y0 z0 +g68.2 p2 q3 x0 y0 z5 +g0 x1 y2 z3 +(the same plane by two vectors, X nudged off square) +g68.2 p3 q1 x10 y20 z30 i1 j0 k0.000001 +g68.2 p3 q2 i0 j-1 k0 +g0 x1 y2 z3 +(R turns the plane about its own Z) +g68.2 p1 q123 x10 y20 z30 i90 j0 k0 r90 +g0 x1 y2 z3 +(an arc in the plane) +g1 f100 x0 y0 z0 +g2 x2 y0 i1 j0 +(G53 inside the plane goes to absolute coordinates) +g53 g0 x100 y200 z300 +(and #5021 reports them) +(debug, abs #5021 #5022 #5023 prog #5420 #5421 #5422) +(a probe result comes back in plane coordinates: nothing to run here) +(a tool length change moves the program coordinates along the plane axis that is world Z) +g43.1 z7 +(debug, prog #5420 #5421 #5422) +g49 +(G68.4 composes: a further 90 about the plane's X) +g68.4 p1 q123 i90 j0 k0 +g0 x1 y2 z3 +(G69 cancels) +g69 +g0 x1 y2 z3 +m2 +% diff --git a/tests/interp/g68-frame/test.sh b/tests/interp/g68-frame/test.sh new file mode 100755 index 00000000000..c11ecd785a9 --- /dev/null +++ b/tests/interp/g68-frame/test.sh @@ -0,0 +1,3 @@ +#!/bin/bash +rs274 -g g68.ngc | sed 's/-0\.0000/0.0000/g' +exit "${PIPESTATUS[0]}" From ac40f9394689d9e326a9c5068104c1a782f49737 Mon Sep 17 00:00:00 2001 From: Luca Toniolo <10792599+grandixximo@users.noreply.github.com> Date: Tue, 15 Sep 2026 16:21:36 +1000 Subject: [PATCH 20/60] glcanon: draw the tilted work planes a program defines Where a program's G68.2 planes sit is the hardest thing to check by reading it, so the preview draws each: a rectangle lying in the plane over the moves made under it, with a margin, the plane's X, Y and Z at the centre of the rectangle in the machine axis colours, and a cross at the plane's origin, which need not lie in the rectangle: a program drilling a sphere puts every origin at the centre and works out on the surface. The rectangle sits at the plane's Z where the program reaches it and otherwise at the nearest end of the Z range worked in, so a drilling cycle shows its holes; a plane nothing moved under gets a square a tenth of the program. A plane restated in a loop is one plane. The plane in effect on the machine, what status reports the last executed G68.2 set, is drawn over the others in its own colour, so the plane the program is in stands out; an MDI plane shows the same way. The canon already receives every plane through set_g68_frame(); it records each as a glcanon_scene.WorkPlane, origin and axes through rotate_and_translate() like a move endpoint, and every move under it extends its extents. WorkPlanePart draws them through the workpiece's machine-to-display transform, gated by show_workplane, colours 'workplane' and 'workplane_active'. Suggested by Sigma1912. tests/glcanon/test_workplane.py; docs in the G68.2 section. --- docs/src/gcode/g-code.adoc | 17 +++ lib/python/rs274/glcanon.py | 36 +++++ lib/python/rs274/glcanon_scene.py | 230 +++++++++++++++++++++++++++++- tests/glcanon/test.sh | 3 +- tests/glcanon/test_workplane.py | 183 ++++++++++++++++++++++++ 5 files changed, 467 insertions(+), 2 deletions(-) create mode 100644 tests/glcanon/test_workplane.py diff --git a/docs/src/gcode/g-code.adoc b/docs/src/gcode/g-code.adoc index a83201d412e..d0692eab77c 100644 --- a/docs/src/gcode/g-code.adoc +++ b/docs/src/gcode/g-code.adoc @@ -1980,6 +1980,23 @@ old plane. words are in the plane, and the result is a new plane relative to the old one. It needs a plane to build on. +The G-code preview draws every plane a program defines, so a program can +be checked for where its planes sit before it runs: a rectangle lying in +the plane, over the moves made under it with a margin around them, the +plane's own X, Y and Z at the centre of the rectangle in the colours of +the machine axes, and a cross at the plane's origin, which need not lie +in the rectangle: a program drilling a sphere puts every plane's origin +at the centre and works out on the surface. The rectangle is drawn at the plane's Z where the program comes +down to it, and otherwise at the end of the Z range the program worked +in that is nearest to it, so a drilling cycle that stays above the +origin shows its holes where they are cut; a plane nothing moved under +is drawn as a square sized from the program. A plane restated with the +same words is one plane, not one per restatement. The plane in effect +on the machine, the one the last executed 'G68.2' set, is drawn over +the others in its own colour, so as the program runs the plane it is +in stands out from the ones it has been in and will be in; a plane set +from MDI shows the same way, as a square with nothing under it. + 'G69' cancels the plane. So does the end of the program, 'M2' or 'M30', and an abort: the plane is not persistent and nothing about it is written to the parameter file. diff --git a/lib/python/rs274/glcanon.py b/lib/python/rs274/glcanon.py index 4363f7de2fb..8ad25736ef5 100644 --- a/lib/python/rs274/glcanon.py +++ b/lib/python/rs274/glcanon.py @@ -168,6 +168,10 @@ def __init__(self, colors, geometry, is_foam=0, foam_w=1.5, foam_z=0.0): # fixture may hold several pieces. A canon is built per file load, so # nothing else ever clears this. self.workpieces = [] + # The tilted work planes the program defined, in order, and the one + # in effect, which the moves made under it extend. + self.workplanes = [] + self._workplane = None def comment(self, arg): if arg.startswith("WORKPIECE,"): @@ -318,6 +322,21 @@ def set_xy_rotation(self, theta): def set_g68_frame(self, *args): self._flush_moves() Translated.set_g68_frame(self, *args) + self._workplane = None + if not self.g68_active: + return + # where the plane sits on the machine: its origin and axes through + # the same offsets a move endpoint gets + origin = self.rotate_and_translate(0, 0, 0, 0, 0, 0, 0, 0, 0)[:3] + axes = [] + for x, y, z in ((1, 0, 0), (0, 1, 0), (0, 0, 1)): + p = self.rotate_and_translate(x, y, z, 0, 0, 0, 0, 0, 0)[:3] + axes.append((p[0] - origin[0], p[1] - origin[1], p[2] - origin[2])) + if self.workplanes and self.workplanes[-1].same_as(origin, axes): + self._workplane = self.workplanes[-1] + return + self._workplane = glcanon_scene.WorkPlane(self.lineno, origin, axes) + self.workplanes.append(self._workplane) def set_g5x_offset(self, *args, **kw): self._flush_moves() @@ -439,6 +458,7 @@ def change_tool(self, arg): def straight_traverse(self, x,y,z, a,b,c, u,v,w): if self.suppress > 0: return l = self.rotate_and_translate(x,y,z,a,b,c,u,v,w) + if self._workplane is not None: self._workplane.extend(l) if not self.first_move: self._stage_move(self.lineno, self.lo, l, 0.0, (self.xo, self.yo, self.zo), CAT_TRAVERSE) @@ -450,6 +470,7 @@ def rigid_tap(self, x, y, z): l = self.rotate_and_translate(x,y,z,0,0,0,0,0,0)[:3] l += (self.lo[3], self.lo[4], self.lo[5], self.lo[6], self.lo[7], self.lo[8]) + if self._workplane is not None: self._workplane.extend(l) offset = (self.xo, self.yo, self.zo) # self.dwells.append((self.lineno, self.colors['dwell'], x + self.offset_x, y + self.offset_y, z + self.offset_z, 0)) self._reserve_staging(2) @@ -479,11 +500,14 @@ def straight_arcsegments(self, segs): self._stage_move(lineno, lo, l, feedrate, to, CAT_ARC) lo = l self.lo = lo + if self._workplane is not None and len(segs): + self._workplane.extend(lo) def straight_feed(self, x,y,z, a,b,c, u,v,w): if self.suppress > 0: return self.first_move = False l = self.rotate_and_translate(x,y,z,a,b,c,u,v,w) + if self._workplane is not None: self._workplane.extend(l) self._stage_move(self.lineno, self.lo, l, self.feedrate, (self.xo, self.yo, self.zo), CAT_FEED) self.lo = l @@ -492,6 +516,7 @@ def straight_probe(self, x,y,z, a,b,c, u,v,w): if self.suppress > 0: return self.first_move = False l = self.rotate_and_translate(x,y,z,a,b,c,u,v,w) + if self._workplane is not None: self._workplane.extend(l) self._stage_move(self.lineno, self.lo, l, self.feedrate, (self.xo, self.yo, self.zo), CAT_FEED) self.lo = l @@ -673,6 +698,10 @@ class GlCanonDraw: 'limits': (1.0, 0.0, 0.0), 'workpiece': glcanon_scene.WORKPIECE_COLOR, 'workpiece_alpha': glcanon_scene.WORKPIECE_ALPHA, + 'workplane': glcanon_scene.WORKPLANE_COLOR, + 'workplane_alpha': glcanon_scene.WORKPLANE_ALPHA, + 'workplane_active': glcanon_scene.WORKPLANE_ACTIVE_COLOR, + 'workplane_active_alpha': glcanon_scene.WORKPLANE_ACTIVE_ALPHA, } def __init__(self, s=None, lp=None, g=None): self.stat = s @@ -1144,6 +1173,12 @@ def get_show_workpiece(self): and any host can toggle it by setting self.show_workpiece.""" return getattr(self, 'show_workpiece', True) + def get_show_workplane(self): + """Whether the tilted work planes the program defined are drawn. + Defaulted like get_show_workpiece, and toggled the same way, by + setting self.show_workplane.""" + return getattr(self, 'show_workplane', True) + def get_workpieces(self): """The stock the loaded program declared, as rs274.glcanon_scene .Workpiece records - the declared params, the outline in machine @@ -1283,6 +1318,7 @@ def frame_context(self) -> glcanon_scene.FrameContext: show_metric=self.get_show_metric(), show_small_origin=self.show_small_origin, show_workpiece=self.get_show_workpiece(), + show_workplane=self.get_show_workplane(), program_alpha=self.get_program_alpha(), grid_size=self.get_grid_size(), highlight_line=self.get_highlight_line(), diff --git a/lib/python/rs274/glcanon_scene.py b/lib/python/rs274/glcanon_scene.py index 664c23dfc0c..88b761362da 100644 --- a/lib/python/rs274/glcanon_scene.py +++ b/lib/python/rs274/glcanon_scene.py @@ -98,6 +98,20 @@ #: default black background. WORKPIECE_ALPHA = 0.4 +#: Colour of a tilted work plane the program defined with G68.2, drawn as a +#: rectangle lying in the plane over what the program did there, with the +#: plane's own axes at its origin in the machine-axis colours. +WORKPLANE_COLOR = (0.35, 0.75, 1.00) + +#: The rectangle sits under the path it frames, like the stock outline. +WORKPLANE_ALPHA = 0.5 + +#: The plane in effect on the machine, the one the last executed G68.2 set, +#: drawn over the others so the one the program is in can be told from the +#: ones it has been in or will be in. +WORKPLANE_ACTIVE_COLOR = (1.00, 0.80, 0.20) +WORKPLANE_ACTIVE_ALPHA = 0.9 + def minmax(*args: float) -> tuple[float, float]: return min(*args), max(*args) @@ -211,7 +225,7 @@ class FrameContext: 'view', 'width', 'height', 'show_program', 'show_rapids', 'show_extents', 'show_offsets', 'show_limits', 'show_tool', 'show_live_plot', 'show_relative', 'show_metric', 'show_small_origin', - 'show_workpiece', + 'show_workpiece', 'show_workplane', 'program_alpha', 'grid_size', 'highlight_line', 'enable_dro', 'cone_basesize', 'disable_cone_scaling', 'view_tool_min_dia', # callables: overridable hooks and lazily-needed values @@ -289,6 +303,7 @@ class FrameContext: show_metric: bool show_small_origin: bool show_workpiece: bool + show_workplane: bool program_alpha: bool #: Ground-grid spacing in internal units; ``0`` means "no grid", and is the #: grid part's visibility gate. @@ -2438,6 +2453,216 @@ def _to_display(machine_points: Float64Points, program.ro) +class WorkPlane: + """One tilted work plane the program defined, with G68.2, G68.3 or + G68.4: where it sits on the machine, and what the program did in it. + + :attr:`origin` and :attr:`axes` are in absolute machine coordinates, in + the canon's units, with the g92 offset, the g5x XY rotation and the g5x + offset that were active at the definition applied the way a move + endpoint on the same line gets them. The extents are in the plane's own + coordinates, the ones the program writes under it, accumulated from + every move made while it was in effect, so a reader can tell a plane + the program only oriented to from one it cut in; they are empty + (``min > max``) while nothing moved. + + Read them off the widget's canon, as the workpieces:: + + for plane in gremlin_widget.canon.workplanes: + print(plane.lineno, plane.origin, plane.extents) + """ + + __slots__ = ('lineno', 'origin', 'axes', 'min_extents', 'max_extents') + + def __init__(self, lineno: int, origin: Sequence[float], + axes: Sequence[Sequence[float]]) -> None: + #: Source line of the definition, or ``-1`` without one. + self.lineno = lineno + #: The plane's origin, machine coordinates. + self.origin = tuple(origin) + #: The plane's X, Y and Z as unit vectors in machine coordinates. + self.axes = tuple(tuple(a) for a in axes) + self.min_extents = [9e99, 9e99, 9e99] + self.max_extents = [-9e99, -9e99, -9e99] + + def __repr__(self) -> str: + return "" % (self.lineno, self.origin) + + def same_as(self, origin: Sequence[float], + axes: Sequence[Sequence[float]], tol: float = 1e-9) -> bool: + """Whether a definition names this plane again, so a program that + restates its plane in a loop is one plane, not one per pass.""" + for a, b in zip(self.origin, origin): + if abs(a - b) > tol: + return False + for u, v in zip(self.axes, axes): + for a, b in zip(u, v): + if abs(a - b) > tol: + return False + return True + + def extend(self, point: Sequence[float]) -> None: + """Take in a machine point the program reached under the plane.""" + o = self.origin + d = (point[0] - o[0], point[1] - o[1], point[2] - o[2]) + for i, a in enumerate(self.axes): + v = d[0]*a[0] + d[1]*a[1] + d[2]*a[2] + if v < self.min_extents[i]: + self.min_extents[i] = v + if v > self.max_extents[i]: + self.max_extents[i] = v + + @property + def has_moves(self) -> bool: + return self.max_extents[X] >= self.min_extents[X] + + @property + def extents(self) -> tuple[tuple[float, ...], tuple[float, ...]]: + """``(min_xyz, max_xyz)`` of the moves made under the plane, in the + plane's coordinates.""" + return tuple(self.min_extents), tuple(self.max_extents) + + def machine_point(self, x: float, y: float, z: float) -> tuple[float, float, float]: + """A point of the plane in machine coordinates.""" + o, (ax, ay, az) = self.origin, self.axes + return (o[0] + x*ax[0] + y*ay[0] + z*az[0], + o[1] + x*ax[1] + y*ay[1] + z*az[1], + o[2] + x*ax[2] + y*ay[2] + z*az[2]) + + +def active_workplane(ctx: FrameContext) -> "WorkPlane | None": + """The plane in effect on the machine, as status reports it: the one the + last executed G68.2 set, whether from the program or from MDI. + + Status carries it as the interpreter gave it, the origin in the + coordinate system the plane was defined in and in machine units, so it + goes through the g92 offset, the XY rotation and the g5x offset the way + the canon takes a definition, in the canon's units. Where it is one the + loaded program defined, that record is returned, extents and all; a + plane from MDI, or from a program no longer loaded, comes back as a + record of its own with nothing under it. + """ + s = ctx.stat + if s is None or not getattr(s, 'g68_active', 0): + return None + try: + o = ctx.to_internal_units(s.g68_offset) + r = s.g68_rotation + g92 = ctx.to_internal_units(s.g92_offset) + g5x = ctx.to_internal_units(s.g5x_offset) + t = math.radians(s.rotation_xy) + except (AttributeError, TypeError): + return None + c, sn = math.cos(t), math.sin(t) + + def through(x: float, y: float, z: float) -> tuple[float, float, float]: + x, y, z = (r[0]*x + r[1]*y + r[2]*z + o[X] + g92[X], + r[3]*x + r[4]*y + r[5]*z + o[Y] + g92[Y], + r[6]*x + r[7]*y + r[8]*z + o[Z] + g92[Z]) + return (x*c - y*sn + g5x[X], x*sn + y*c + g5x[Y], z + g5x[Z]) + + origin = through(0, 0, 0) + axes = [] + for unit in ((1, 0, 0), (0, 1, 0), (0, 0, 1)): + p = through(*unit) + axes.append((p[0] - origin[0], p[1] - origin[1], p[2] - origin[2])) + for plane in getattr(ctx.canon, 'workplanes', ()): + # status has been through machine units and back, so not to the bit + if plane.same_as(origin, axes, tol=1e-6): + return plane + return WorkPlane(-1, origin, axes) + + +class WorkPlanePart(Part): + """The tilted work planes the program defined, one drawing each, and + the one in effect on the machine over them in its own colour. + + A plane is drawn where the program worked in it: a rectangle lying in + the plane, over the moves made under it with a margin around them, at + the plane's own Z where the program reached that and otherwise at the + end of the Z range it worked in nearest to it, so a drilling cycle that + never comes down to the plane's origin still shows its holes; a plane + nothing moved under gets a square sized from the program. The plane's + axes stand at the centre of the rectangle in the machine-axis colours, + so the direction the program's X and Y took can be read where the work + is, and the plane's origin is marked with a cross, since the two need + not coincide: a program that drills a sphere puts every plane's origin + at the centre and works out on the surface. + + The active plane is whatever status says the last executed G68.2 set, + so it walks through the program's planes as the program runs, and an + MDI plane shows too, as a square with nothing under it. + """ + + #: Inner lines each way, to read as a surface rather than an outline. + SUBDIVISIONS = 4 + + def draw(self, ctx: FrameContext) -> None: + canon = ctx.canon + planes = list(getattr(canon, 'workplanes', ())) + active = active_workplane(ctx) + if not planes and active is None: + return + # the size a plane with nothing, or only a point, under it is drawn + # at: a tenth of the program, as the extents part spaces its labels + size = max(canon.max_extents[X] - canon.min_extents[X], + canon.max_extents[Y] - canon.min_extents[Y], + canon.max_extents[Z] - canon.min_extents[Z], 2) * .1 + others = [p for p in planes if p is not active] + if others: + self.draw_planes(ctx, others, size, + ctx.colors.get('workplane', WORKPLANE_COLOR), + ctx.colors.get('workplane_alpha', WORKPLANE_ALPHA)) + if active is not None: + self.draw_planes(ctx, [active], size, + ctx.colors.get('workplane_active', WORKPLANE_ACTIVE_COLOR), + ctx.colors.get('workplane_active_alpha', WORKPLANE_ACTIVE_ALPHA)) + + def draw_planes(self, ctx: FrameContext, planes: Sequence[WorkPlane], + size: float, color: Color, alpha: float) -> None: + canon = ctx.canon + outline, inner, cross = [], [], [] + axes = {'axis_x': [], 'axis_y': [], 'axis_z': []} + n = self.SUBDIVISIONS + for plane in planes: + if plane.has_moves: + lo, hi = plane.min_extents, plane.max_extents + cx, cy = (lo[X] + hi[X]) / 2, (lo[Y] + hi[Y]) / 2 + hw = max((hi[X] - lo[X]) * .6, size / 2) + hh = max((hi[Y] - lo[Y]) * .6, size / 2) + z = min(max(lo[Z], 0.0), hi[Z]) + else: + cx = cy = z = 0.0 + hw = hh = size / 2 + x0, x1, y0, y1 = cx - hw, cx + hw, cy - hh, cy + hh + corner = [plane.machine_point(x0, y0, z), plane.machine_point(x1, y0, z), + plane.machine_point(x1, y1, z), plane.machine_point(x0, y1, z)] + for i in range(4): + outline += [corner[i], corner[(i + 1) % 4]] + for i in range(1, n): + f = i / n + inner += [plane.machine_point(x0 + (x1 - x0) * f, y0, z), + plane.machine_point(x0 + (x1 - x0) * f, y1, z), + plane.machine_point(x0, y0 + (y1 - y0) * f, z), + plane.machine_point(x1, y0 + (y1 - y0) * f, z)] + length = max(hw, hh) * .5 + centre = plane.machine_point(cx, cy, z) + axes['axis_x'] += [centre, plane.machine_point(cx + length, cy, z)] + axes['axis_y'] += [centre, plane.machine_point(cx, cy + length, z)] + axes['axis_z'] += [centre, plane.machine_point(cx, cy, z + length)] + arm = length * .4 + cross += [plane.machine_point(-arm, 0, 0), plane.machine_point(arm, 0, 0), + plane.machine_point(0, -arm, 0), plane.machine_point(0, arm, 0), + plane.machine_point(0, 0, -arm), plane.machine_point(0, 0, arm)] + to_display = Workpiece._to_display + ctx.prim.draw_lines(ctx, to_display(np.array(outline), canon), color, alpha) + ctx.prim.draw_lines(ctx, to_display(np.array(inner), canon), color, alpha * .4) + ctx.prim.draw_lines(ctx, to_display(np.array(cross), canon), color, alpha) + for name, verts in axes.items(): + ctx.prim.draw_lines(ctx, to_display(np.array(verts), canon), + ctx.colors[name], alpha) + + class WorkpiecePart(Part): """Wireframe stock outlines declared by ``(WORKPIECE,...)`` comments. @@ -2504,6 +2729,7 @@ def __init__(self) -> None: self.limits_box = LimitsBoxPart() self.backplot = BackplotPart() self.workpiece = WorkpiecePart() + self.workplane = WorkPlanePart() self.tool = ToolPart() self.overlay = OverlayPart() super().__init__([ @@ -2520,6 +2746,8 @@ def __init__(self) -> None: (self.backplot, lambda ctx: ctx.show_live_plot), (self.workpiece, lambda ctx: ctx.show_workpiece and ctx.canon is not None), + (self.workplane, lambda ctx: ctx.show_workplane + and ctx.canon is not None), (self.tool, lambda ctx: ctx.show_tool), (self.overlay, lambda ctx: ctx.enable_dro), ]) diff --git a/tests/glcanon/test.sh b/tests/glcanon/test.sh index 2a33658aa7b..f7999bf093e 100755 --- a/tests/glcanon/test.sh +++ b/tests/glcanon/test.sh @@ -15,8 +15,9 @@ python3 test_camera_matrices.py >&2 if python3 -c 'import OpenGL' 2>/dev/null; then python3 test_workpiece.py >&2 + python3 test_workplane.py >&2 else - echo "skip: test_workpiece.py needs PyOpenGL (headless build)" >&2 + echo "skip: test_workpiece.py and test_workplane.py need PyOpenGL (headless build)" >&2 fi echo ok diff --git a/tests/glcanon/test_workplane.py b/tests/glcanon/test_workplane.py new file mode 100644 index 00000000000..db338fde130 --- /dev/null +++ b/tests/glcanon/test_workplane.py @@ -0,0 +1,183 @@ +#!/usr/bin/env python3 +"""The tilted work planes the preview draws: what the canon records from +G68.2 and the moves under it, and what the part draws from that. + +Needs the RIP environment (rs274 pulls the compiled gcode extension) but no +display and no GL context: nothing below calls into OpenGL. + + . scripts/rip-environment && runtests tests/glcanon +""" +import math +import unittest + +import rs274.glcanon as glcanon +from rs274 import glcanon_scene + + +def make_canon(): + return glcanon.GLCanon(colors={}, geometry="XYZ") + + +def frame(canon, origin, tilt_about_x=0.0, active=1): + """G68.2 as canon sees it: the origin and the rotation matrix, row by row.""" + c, s = math.cos(math.radians(tilt_about_x)), math.sin(math.radians(tilt_about_x)) + canon.set_g68_frame(origin[0], origin[1], origin[2], + 1, 0, 0, + 0, c, -s, + 0, s, c, active) + + +def move(canon, x, y, z): + canon.straight_feed(x, y, z, 0, 0, 0, 0, 0, 0) + + +class WorkPlaneRecordTest(unittest.TestCase): + def test_where_the_plane_sits(self): + canon = make_canon() + canon.set_g5x_offset(1, 100, 0, 0, 0, 0, 0, 0, 0, 0) + frame(canon, (0, 40, 20), tilt_about_x=30) + plane, = canon.workplanes + # the origin goes through the g5x offset like a move endpoint + self.assertAlmostEqual(plane.origin[0], 100) + self.assertAlmostEqual(plane.origin[1], 40) + self.assertAlmostEqual(plane.origin[2], 20) + # X is untouched by a tilt about X; Z leans back towards -Y + x, y, z = plane.axes + self.assertAlmostEqual(x[0], 1) + self.assertAlmostEqual(z[1], -math.sin(math.radians(30))) + self.assertAlmostEqual(z[2], math.cos(math.radians(30))) + self.assertFalse(plane.has_moves) + + def test_moves_extend_the_plane_in_its_own_coordinates(self): + canon = make_canon() + frame(canon, (0, 40, 20), tilt_about_x=30) + move(canon, -30, -20, 5) + move(canon, 30, 20, -4) + plane, = canon.workplanes + self.assertTrue(plane.has_moves) + lo, hi = plane.extents + for got, want in zip(lo + hi, (-30, -20, -4, 30, 20, 5)): + self.assertAlmostEqual(got, want) + # and a point of the plane comes back where the move went + p = plane.machine_point(30, 20, -4) + for a, b in zip(p, canon.lo[:3]): + self.assertAlmostEqual(a, b) + + def test_cancel_stops_the_recording(self): + canon = make_canon() + frame(canon, (0, 0, 0)) + move(canon, 1, 1, 0) + frame(canon, (0, 0, 0), active=0) + move(canon, 50, 50, 50) + plane, = canon.workplanes + self.assertAlmostEqual(plane.max_extents[0], 1) + + def test_restating_the_plane_is_one_plane(self): + canon = make_canon() + for i in range(3): + frame(canon, (0, 0, 0), tilt_about_x=30) + move(canon, i, 0, 0) + frame(canon, (0, 0, 0), tilt_about_x=45) + self.assertEqual(len(canon.workplanes), 2) + self.assertAlmostEqual(canon.workplanes[0].max_extents[0], 2) + + +class StatStub: + """The plane in effect as status reports it, in machine units (mm).""" + + def __init__(self, active=0, origin=(0, 0, 0), tilt_about_x=0.0): + c, s = math.cos(math.radians(tilt_about_x)), math.sin(math.radians(tilt_about_x)) + self.g68_active = active + self.g68_offset = tuple(origin) + (0,) * 6 + self.g68_rotation = (1, 0, 0, 0, c, -s, 0, s, c) + self.g92_offset = (0,) * 9 + self.g5x_offset = (0,) * 9 + self.rotation_xy = 0.0 + + +class CtxStub: + """What WorkPlanePart is allowed to read, and a record of what it drew.""" + + class Prim: + def __init__(self): + self.calls = [] + + def draw_lines(self, ctx, points, color, alpha=1.0): + self.calls.append((len(points), tuple(color), alpha)) + + def __init__(self, canon, stat=None): + self.canon = canon + self.stat = stat + self.colors = glcanon.GlCanonDraw.colors + self.prim = self.Prim() + + @staticmethod + def to_internal_units(pos): + return [v / 25.4 for v in pos[:3]] + list(pos[3:]) + + +class WorkPlanePartTest(unittest.TestCase): + def test_draws_the_outline_the_grid_and_the_axes(self): + canon = make_canon() + frame(canon, (0, 40, 20), tilt_about_x=30) + move(canon, -30, -20, 5) + move(canon, 30, 20, -4) + canon.calc_extents() + ctx = CtxStub(canon) + glcanon_scene.WorkPlanePart().draw(ctx) + n = glcanon_scene.WorkPlanePart.SUBDIVISIONS + # four edges, the inner lines each way, the origin cross, then one + # line per axis + self.assertEqual([c[0] for c in ctx.prim.calls], [8, 4 * (n - 1), 6, 2, 2, 2]) + self.assertEqual(ctx.prim.calls[0][1], tuple(glcanon_scene.WORKPLANE_COLOR)) + self.assertEqual([c[1] for c in ctx.prim.calls[3:]], + [tuple(glcanon.GlCanonDraw.colors[k]) for k in ('axis_x', 'axis_y', 'axis_z')]) + + def test_draws_nothing_without_planes(self): + for canon in (None, object(), make_canon()): + ctx = CtxStub(canon) + glcanon_scene.WorkPlanePart().draw(ctx) + self.assertEqual(ctx.prim.calls, []) + + def test_the_active_plane_is_the_program_record(self): + canon = make_canon() + frame(canon, (0, 40, 20), tilt_about_x=30) + move(canon, -30, -20, 5) + frame(canon, (0, 0, 0)) + move(canon, 1, 1, 1) + canon.calc_extents() + # the canon counts in inches, status in this machine's mm + stat = StatStub(active=1, origin=(0, 40 * 25.4, 20 * 25.4), tilt_about_x=30) + ctx = CtxStub(canon, stat) + self.assertIs(glcanon_scene.active_workplane(ctx), canon.workplanes[0]) + glcanon_scene.WorkPlanePart().draw(ctx) + # the other plane in the plane colour, then the active one in its own + colors = [c[1] for c in ctx.prim.calls] + self.assertEqual(colors[0], tuple(glcanon_scene.WORKPLANE_COLOR)) + self.assertEqual(colors[6], tuple(glcanon_scene.WORKPLANE_ACTIVE_COLOR)) + self.assertEqual(len(ctx.prim.calls), 12) + + def test_an_mdi_plane_is_drawn_on_its_own(self): + canon = make_canon() + canon.calc_extents() + ctx = CtxStub(canon, StatStub(active=1, origin=(10, 0, 0))) + plane = glcanon_scene.active_workplane(ctx) + self.assertEqual(plane.lineno, -1) + self.assertAlmostEqual(plane.origin[0], 10 / 25.4) + self.assertFalse(plane.has_moves) + glcanon_scene.WorkPlanePart().draw(ctx) + self.assertEqual(len(ctx.prim.calls), 6) + self.assertEqual(ctx.prim.calls[0][1], tuple(glcanon_scene.WORKPLANE_ACTIVE_COLOR)) + + def test_no_plane_in_effect(self): + canon = make_canon() + frame(canon, (0, 0, 0)) + canon.calc_extents() + ctx = CtxStub(canon, StatStub(active=0)) + self.assertIsNone(glcanon_scene.active_workplane(ctx)) + glcanon_scene.WorkPlanePart().draw(ctx) + self.assertEqual(len(ctx.prim.calls), 6) + + +if __name__ == '__main__': + unittest.main() From 52f554655249f24a6628d7a1ba6717963f770998 Mon Sep 17 00:00:00 2001 From: Luca Toniolo <10792599+grandixximo@users.noreply.github.com> Date: Tue, 11 Aug 2026 22:59:37 +1000 Subject: [PATCH 21/60] switchkins: separate the dispatch from rtapi_app_main() switchkins.c owned rtapi_app_main(), so a module could only use it by having no main of its own, which ruled out halcompile components: the switchable kinematics in hal/components each carry a private copy of the dispatch. Move rtapi_app_main(), rtapi_app_exit() and the coordinates= and sparm= parameters to switchkins_main.c and give switchkins.c one entry point, switchkinsInit(comp_id, kp, coordinates), which counts and validates the registered types, creates the pins and starts on type 0. The caller owns the component. The types switchkinsSetup() supplies now go through switchkinsRegister() like any others, so one registration path checks every type and a double registration is refused. The eight existing modules add switchkins_main.o to their objects and are otherwise untouched. --- src/Makefile | 8 +++ src/emc/kinematics/switchkins.c | 61 ++++++------------ src/emc/kinematics/switchkins.h | 11 +++- src/emc/kinematics/switchkins_main.c | 94 ++++++++++++++++++++++++++++ 4 files changed, 130 insertions(+), 44 deletions(-) create mode 100644 src/emc/kinematics/switchkins_main.c diff --git a/src/Makefile b/src/Makefile index 64d7d75c34a..1cdadd2ffd7 100644 --- a/src/Makefile +++ b/src/Makefile @@ -1181,6 +1181,7 @@ genhexkins-objs += libposemath/_posemath.o genhexkins-objs += $(MATHSTUB) genhexkins-objs += emc/kinematics/kins_util.o genhexkins-objs += emc/kinematics/switchkins.o +genhexkins-objs += emc/kinematics/switchkins_main.o genhexkins-objs += $(USERKFUNCS) obj-m += genserkins.o @@ -1190,6 +1191,7 @@ genserkins-objs += libposemath/gomath.o genserkins-objs += $(MATHSTUB) genserkins-objs += emc/kinematics/kins_util.o genserkins-objs += emc/kinematics/switchkins.o +genserkins-objs += emc/kinematics/switchkins_main.o genserkins-objs += $(USERKFUNCS) obj-m += xyzac-trt-kins.o @@ -1197,6 +1199,7 @@ xyzac-trt-kins-objs := emc/kinematics/xyzac-trt-kins.o xyzac-trt-kins-objs += emc/kinematics/trtfuncs.o xyzac-trt-kins-objs += emc/kinematics/kins_util.o xyzac-trt-kins-objs += emc/kinematics/switchkins.o +xyzac-trt-kins-objs += emc/kinematics/switchkins_main.o xyzac-trt-kins-objs += $(USERKFUNCS) obj-m += xyzbc-trt-kins.o @@ -1204,6 +1207,7 @@ xyzbc-trt-kins-objs := emc/kinematics/xyzbc-trt-kins.o xyzbc-trt-kins-objs += emc/kinematics/trtfuncs.o xyzbc-trt-kins-objs += emc/kinematics/kins_util.o xyzbc-trt-kins-objs += emc/kinematics/switchkins.o +xyzbc-trt-kins-objs += emc/kinematics/switchkins_main.o xyzbc-trt-kins-objs += $(USERKFUNCS) obj-m += scarakins.o @@ -1212,6 +1216,7 @@ scarakins-objs += libposemath/_posemath.o scarakins-objs += $(MATHSTUB) scarakins-objs += emc/kinematics/kins_util.o scarakins-objs += emc/kinematics/switchkins.o +scarakins-objs += emc/kinematics/switchkins_main.o scarakins-objs += $(USERKFUNCS) obj-m += pumakins.o @@ -1220,6 +1225,7 @@ pumakins-objs += libposemath/_posemath.o pumakins-objs += $(MATHSTUB) pumakins-objs += emc/kinematics/kins_util.o pumakins-objs += emc/kinematics/switchkins.o +pumakins-objs += emc/kinematics/switchkins_main.o pumakins-objs += $(USERKFUNCS) obj-m += three21kins.o @@ -1228,6 +1234,7 @@ three21kins-objs += libposemath/_posemath.o three21kins-objs += $(MATHSTUB) three21kins-objs += emc/kinematics/kins_util.o three21kins-objs += emc/kinematics/switchkins.o +three21kins-objs += emc/kinematics/switchkins_main.o three21kins-objs += $(USERKFUNCS) obj-m += 5axiskins.o @@ -1236,6 +1243,7 @@ obj-m += 5axiskins.o 5axiskins-objs += $(MATHSTUB) 5axiskins-objs += emc/kinematics/kins_util.o 5axiskins-objs += emc/kinematics/switchkins.o +5axiskins-objs += emc/kinematics/switchkins_main.o 5axiskins-objs += $(USERKFUNCS) #---------------------------------------------------------------- diff --git a/src/emc/kinematics/switchkins.c b/src/emc/kinematics/switchkins.c index f832246a926..a9fa9027cd5 100644 --- a/src/emc/kinematics/switchkins.c +++ b/src/emc/kinematics/switchkins.c @@ -27,7 +27,6 @@ * Using modules must supply function: switchkinsSetup() */ #include -#include #include #include #include @@ -422,12 +421,6 @@ int kinematicsTypeFlags(int ktype) return ktype_flags[ktype]; } // kinematicsTypeFlags() -//********************************************************************* -static char *coordinates; -RTAPI_MP_STRING(coordinates, "Axes-to-joints-ordering"); -static char *sparm; -RTAPI_MP_STRING(sparm, "switchkins module-specific parameter"); - EXPORT_SYMBOL(kinematicsSwitchable); EXPORT_SYMBOL(kinematicsSwitch); EXPORT_SYMBOL(kinematicsType); @@ -443,33 +436,24 @@ EXPORT_SYMBOL(switchkinsRegisterToolFrameInverse); EXPORT_SYMBOL(switchkinsDeclare); EXPORT_SYMBOL(kinematicsTypeFlags); EXPORT_SYMBOL(switchkinsRegisterJacobian); -MODULE_LICENSE("GPL"); +EXPORT_SYMBOL(switchkinsInit); -static int comp_id; //********************************************************************* -int rtapi_app_main(void) +// The caller owns the hal component: it does hal_init() before this and +// hal_ready() after it. Every switchkins-type must be registered by +// now. +int switchkinsInit(const int comp_id, + kparms* ksetup_parms, + const char* coordinates) { - int i,res,identities; - char* emsg="other"; - - // defaults prior to switchkinsSetup() call - kp.kinsname = NULL; - kp.halprefix = NULL; - kp.required_coordinates = ""; - kp.max_joints = 0; // Setup must supply - kp.allow_duplicates = 0; - kp.fwd_iterates_mask = 0; - kp.gui_kinstype = -1; // negative means: not used - - kp.sparm = sparm; // module parm passed to kins - - // may also call switchkinsRegister() - res = switchkinsSetup(&kp, - &ksetups[0], &ksetups[1], &ksetups[2], - &kfwds[0], &kfwds[1], &kfwds[2], - &kinvs[0], &kinvs[1], &kinvs[2]); - if (res) {emsg="switchkinsSetp FAIL"; goto error;} - if (register_error) {emsg="switchkinsRegister FAIL"; goto error;} + int i; + int identities; + int res = 0; + char* emsg = "other"; + + kp = *ksetup_parms; // kinematics parms are needed after this returns + + if (register_error) {emsg = "switchkinsRegister FAIL"; goto error;} // an identity type answers the tool frame the same way whichever module // asked for it, so supply it here rather than in every switchkinsSetup() @@ -485,7 +469,7 @@ int rtapi_app_main(void) } } - // the highest type provided by either route sets the count + // the highest type registered sets the count for (i=0; i < SWITCHKINS_MAX_TYPES; i++) { if (ksetups[i] || kfwds[i] || kinvs[i]) { kins_count = i + 1; } } @@ -544,11 +528,8 @@ int rtapi_app_main(void) emsg = "incomplete switchkins-type"; goto error; } - comp_id = hal_init(kp.kinsname); - if(comp_id < 0) goto error; - swdata = hal_malloc(sizeof(struct swdata)); - if (!swdata) goto error; + if (!swdata) {emsg = "hal_malloc fail"; goto error;} for (i=0; i < kins_count; i++) { res += hal_pin_new_bool(comp_id, HAL_OUT, &(swdata->kinstype_is[i]), @@ -562,8 +543,8 @@ int rtapi_app_main(void) res += hal_pin_new_real(comp_id, HAL_IN, &swdata->gui_a, 0.0, "skgui.a"); res += hal_pin_new_real(comp_id, HAL_IN, &swdata->gui_b, 0.0, "skgui.b"); res += hal_pin_new_real(comp_id, HAL_IN, &swdata->gui_c, 0.0, "skgui.c"); - if (res) {emsg = "hal pin create fail";goto error;} } + if (res) {emsg = "hal pin create fail"; goto error;} switchkins_type = 0; // startup with default type kinematicsSwitch(switchkins_type); @@ -574,14 +555,10 @@ int rtapi_app_main(void) ksetups[i](comp_id,coordinates,&kp); } - hal_ready(comp_id); return 0; error: rtapi_print_msg(RTAPI_MSG_ERR, "\nSwitchkins FAIL %s:<%s>\n",kp.kinsname,emsg); - hal_exit(comp_id); return -1; -} // rtapi_app_main() - -void rtapi_app_exit(void) { hal_exit(comp_id); } +} // switchkinsInit() diff --git a/src/emc/kinematics/switchkins.h b/src/emc/kinematics/switchkins.h index 77caca90629..f6b8aa06e4f 100644 --- a/src/emc/kinematics/switchkins.h +++ b/src/emc/kinematics/switchkins.h @@ -34,14 +34,14 @@ typedef int (*KS)(const int comp_id, // halpins ); //********************************************************************* -// supplied by the using module, provides types 0,1,2 +// supplied by a module using switchkins_main.c, provides types 0,1,2 extern int switchkinsSetup(kparms* ksetup_parms, KS* kset0, KS* kset1, KS* kset2, KF* kfwd0, KF* kfwd1, KF* kfwd2, KI* kinv0, KI* kinv1, KI* kinv2 ); -// called from switchkinsSetup(), once per type it does not provide itself +// provide one switchkins-type, before switchkinsInit() extern int switchkinsRegister(int ktype, KS kset, KF kfwd, KI kinv); // called from switchkinsSetup() for each type that reports its frames; a type @@ -84,4 +84,11 @@ typedef int (*KJ)(const double *joint, // that does not gets the exact answer if it is an identity type, and // otherwise the generic differences of its own inverse. extern int switchkinsRegisterJacobian(int ktype, KJ kjac); + +// create the hal pins and start on type 0; the caller owns the hal +// component and does hal_init() before and hal_ready() after +extern int switchkinsInit(const int comp_id, + kparms* ksetup_parms, + const char* coordinates + ); #endif // } diff --git a/src/emc/kinematics/switchkins_main.c b/src/emc/kinematics/switchkins_main.c new file mode 100644 index 00000000000..4a4cc05153c --- /dev/null +++ b/src/emc/kinematics/switchkins_main.c @@ -0,0 +1,94 @@ +/* + Copyright 2019 Dewey Garrett + + This program is free software; you can redistribute it and/or modify + it under the terms of the GNU General Public License as published by + the Free Software Foundation; either version 2 of the License, or + (at your option) any later version. + + This program is distributed in the hope that it will be useful, + but WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + GNU General Public License for more details. + + You should have received a copy of the GNU General Public License + along with this program; if not, write to the Free Software + Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA. +*/ + +/* switchkins_main.c provides rtapi_app_main() for kinematics modules +* built around switchkins.c. A module that gets its rtapi_app_main() +* from somewhere else (a halcompile component, for instance) links +* switchkins.c alone and calls switchkinsInit() itself. +* +* Using modules must supply function: switchkinsSetup() +*/ +#include +#include +#include + +#include "switchkins.h" + +static char *coordinates; +RTAPI_MP_STRING(coordinates, "Axes-to-joints-ordering"); +static char *sparm; +RTAPI_MP_STRING(sparm, "switchkins module-specific parameter"); + +MODULE_LICENSE("GPL"); + +static int comp_id = -1; + +int rtapi_app_main(void) +{ + kparms kp; + KS ksetup[3] = {NULL}; + KF kfwd[3] = {NULL}; + KI kinv[3] = {NULL}; + int i; + + // defaults prior to switchkinsSetup() call + kp.kinsname = NULL; + kp.halprefix = NULL; + kp.required_coordinates = ""; + kp.max_joints = 0; // Setup must supply + kp.allow_duplicates = 0; + kp.fwd_iterates_mask = 0; + kp.gui_kinstype = -1; // negative means: not used + + kp.sparm = sparm; // module parm passed to kins + + // switchkinsSetup() provides types 0,1,2 and may also call + // switchkinsRegister() for any others + if (switchkinsSetup(&kp, + &ksetup[0], &ksetup[1], &ksetup[2], + &kfwd[0], &kfwd[1], &kfwd[2], + &kinv[0], &kinv[1], &kinv[2])) { + rtapi_print_msg(RTAPI_MSG_ERR,"\nSwitchkins FAIL:\n"); + return -1; + } + + // the types switchkinsSetup() supplied go in by the same route as + // any other, so that providing one twice is caught + for (i=0; i < 3; i++) { + if (!ksetup[i] && !kfwd[i] && !kinv[i]) { continue; } + if (switchkinsRegister(i, ksetup[i], kfwd[i], kinv[i])) { return -1; } + } + + if (!kp.kinsname) { + rtapi_print_msg(RTAPI_MSG_ERR,"\nSwitchkins FAIL:\n"); + return -1; + } + + comp_id = hal_init(kp.kinsname); + if (comp_id < 0) return comp_id; + + if (switchkinsInit(comp_id, &kp, coordinates)) { + hal_exit(comp_id); + return -1; + } + + hal_ready(comp_id); + return 0; +} // rtapi_app_main() + +void rtapi_app_exit(void) { hal_exit(comp_id); } From 2f431a462d31ef3ed51450e657c81c7c5a4087b5 Mon Sep 17 00:00:00 2001 From: Luca Toniolo <10792599+grandixximo@users.noreply.github.com> Date: Wed, 12 Aug 2026 00:17:36 +1000 Subject: [PATCH 22/60] switchkins: let halcompile components use the switchkins core millturn, xyzab_tdr_kins, xyzacb_trsrn and xyzbca_trsrn each carried a copy of the switchkins dispatch, a private switchkins_type, a hand-written kinematicsSwitch() and a setup that had to hal_set_unready() the component again. Now that the dispatch is separate from the main program a component links it and calls switchkinsInit() from EXTRA_SETUP(). Two build changes: the generated per-comp .mak takes a -extra-objs list, and switchkins.h is copied to ../include and installed so resolves from a generated source. Each of the four registers its types and calls switchkinsInit(); their identity type comes from kins_util.c, which gets them the coordinates= parameter, and a bad motion.switchkins-type is refused instead of stranding the module. Pin names are unchanged except millturn's unused in/out template pins. The four sim configs give the same positions through the same MDI sequence as before, in every kinematics type. --- docs/src/motion/switchkins.adoc | 78 +++- src/Makefile | 1 + src/emc/kinematics/switchkins.h | 8 +- src/hal/components/Submakefile | 13 +- src/hal/components/millturn.comp | 269 ++++------- src/hal/components/xyzab_tdr_kins.comp | 384 ++++++--------- src/hal/components/xyzacb_trsrn.comp | 621 +++++++++++------------- src/hal/components/xyzbca_trsrn.comp | 623 +++++++++++-------------- 8 files changed, 869 insertions(+), 1128 deletions(-) diff --git a/docs/src/motion/switchkins.adoc b/docs/src/motion/switchkins.adoc index fe1fd05eed9..cfa230f5f43 100644 --- a/docs/src/motion/switchkins.adoc +++ b/docs/src/motion/switchkins.adoc @@ -46,6 +46,10 @@ The following kinematics modules support switchable kinematics: . *three21kins* (type0:three21kins type1:identity) . *scarakins* (type0:scarakins type1:identity) . *5axiskins* (type0:5axiskins type1:identity) (bridgemill) +. *millturn* (type0:identity type1:turn) +. *xyzab_tdr_kins* (type0:identity type1:tcp) +. *xyzacb_trsrn* (type0:identity type1:tcp type2:tool) +. *xyzbca_trsrn* (type0:identity type1:tcp type2:tool) Every module listed above uses its own kinematics for type0 and identity kinematics for type1. Each accepts the module string @@ -419,6 +423,10 @@ configs/sim/axis/vismach/ . . puma/puma560.ini (genserkins) . puma/puma.ini (pumakins) . hexapod-sim/hexapod.ini (genhexkins) +. millturn/millturn.ini (millturn) +. 5axis/table-dual-rotary/xyzab-tdr.ini (xyzab_tdr_kins) +. 5axis/table-rotary_spindle-rotary-nutating/xyzacb-trsrn_twp/xyzacb-trsrn.ini (xyzacb_trsrn) +. 5axis/table-rotary_spindle-rotary-nutating/xyzbca-trsrn_twp/xyzbca-trsrn.ini (xyzbca_trsrn) == User kinematics provisions @@ -466,19 +474,14 @@ protocols. == Code Notes Kinematic modules providing switchkins functionality are linked to -the switchkins.o object (switchkins.c) that provides the module -'main' program (rtapi_app_main()) and related functions. This -'main' program reads (optional) module command-line parameters -(coordinates, sparm) and passes them to the module-provided -function switchkinsSetup(). - -The switchkinsSetup() function identifies kinstype-specific setup -routines and the functions for forward an inverse calculation for -each kinstype (0,1,2) and sets a number of configuration -settings. - -A module can provide further kinstypes by calling -switchkinsRegister() from within switchkinsSetup(), once per +the switchkins.o object (switchkins.c). It provides +kinematicsForward(), kinematicsInverse(), kinematicsSwitch() and +the rest of the kinematics interface, dispatching each call to the +kinstype currently selected, and it creates the HAL pins common to +all switchkins modules. It does not provide the module 'main' +program, so a module can get that from wherever suits it. + +A kinstype is supplied by calling switchkinsRegister(), once per kinstype: ---- @@ -521,16 +524,53 @@ exactly as before for 'G12.1 P-' and 'G49', but 'G13.1' and 'G43.4' are an error, since the numbers of the identity and primary kinematics are then a guess. -After calling switchkinsSetup(), rtapi_app_main() checks the -supplied parameters, creates a HAL component, and then invokes -the setup routine identified for each kinstype. +When every kinstype is registered, the module calls: + +---- +int switchkinsInit(const int comp_id, kparms* kp, const char* coordinates); +---- + +which checks the supplied parameters, creates the HAL pins, selects +kinstype 0, and then invokes the setup routine registered for each +kinstype. The caller owns the HAL component: it does hal_init() +before switchkinsInit() and hal_ready() after it. Each kinstype setup routine can (optionally) create HAL pins and set them to default values. A setup routine is called once per kinstype it is registered for, so a routine used for two -kinstypes must not create the same pin twice. When all setup -routines finish, rtapi_app_main() issues hal_ready() for the -component to complete creation of the module. +kinstypes must not create the same pin twice. + +=== Module main program + +A module written as a plain C file links switchkins_main.o +(switchkins_main.c) for its rtapi_app_main(). That 'main' program +reads the (optional) module command-line parameters (coordinates, +sparm) and passes them to the module-provided function +switchkinsSetup(): + +---- +int switchkinsSetup(kparms* kp, + KS* kset0, KS* kset1, KS* kset2, + KF* kfwd0, KF* kfwd1, KF* kfwd2, + KI* kinv0, KI* kinv1, KI* kinv2); +---- + +which identifies the setup, forward and inverse routines for +kinstypes 0,1,2 and sets a number of configuration settings. Those +three are registered for the module, so it can supply further +kinstypes by calling switchkinsRegister() itself, and registering +one that switchkinsSetup() has already filled in is the same error +as any other duplicate. + +A module written as a halcompile component gets rtapi_app_main() +from halcompile instead. It registers its kinstypes and calls +switchkinsInit() from its EXTRA_SETUP() routine, which halcompile +runs after hal_init() and before hal_ready(). The component names +the objects it needs in hal/components/Submakefile: + +---- +millturn-extra-objs := emc/kinematics/switchkins.o emc/kinematics/kins_util.o +---- === Outline diff --git a/src/Makefile b/src/Makefile index 1cdadd2ffd7..b7563d22623 100644 --- a/src/Makefile +++ b/src/Makefile @@ -402,6 +402,7 @@ SRCHEADERS := \ hal/drivers/mesa-hostmot2/hostmot2-serial.h \ emc/linuxcnc.h \ emc/kinematics/kinematics.h \ + emc/kinematics/switchkins.h \ emc/nml_intf/emcmotcfg.h \ emc/ini/inifile.hh \ emc/ini/inifile.h \ diff --git a/src/emc/kinematics/switchkins.h b/src/emc/kinematics/switchkins.h index f6b8aa06e4f..c7114262403 100644 --- a/src/emc/kinematics/switchkins.h +++ b/src/emc/kinematics/switchkins.h @@ -1,10 +1,10 @@ /* ** License GPL Version 2 */ -#ifndef SWITCHKINS_H // { -#define SWITCHKINS_H +#ifndef __LINUXCNC_SWITCHKINS_H +#define __LINUXCNC_SWITCHKINS_H -#include +#include "kinematics.h" //SWITCHKINS_MAX_TYPES (max number of types a module may provide) //is in kinematics.h: motion and the NML status channel need it too @@ -91,4 +91,4 @@ extern int switchkinsInit(const int comp_id, kparms* ksetup_parms, const char* coordinates ); -#endif // } +#endif diff --git a/src/hal/components/Submakefile b/src/hal/components/Submakefile index 62c9940cfbb..d97a0baf2f1 100644 --- a/src/hal/components/Submakefile +++ b/src/hal/components/Submakefile @@ -94,11 +94,20 @@ endif obj-m += $(patsubst hal/drivers/%.comp, %.o, $(patsubst hal/components/%.comp, %.o, $(COMPS) $(COMP_DRIVERS))) +# A component that links objects besides its own names them here as +# -extra-objs. The list is expanded when the .mak is written, +# so it has to be defined in this file (which the .mak depends on). +SWITCHKINS_OBJS := emc/kinematics/switchkins.o emc/kinematics/kins_util.o +millturn-extra-objs := $(SWITCHKINS_OBJS) +xyzab_tdr_kins-extra-objs := $(SWITCHKINS_OBJS) +xyzacb_trsrn-extra-objs := $(SWITCHKINS_OBJS) +xyzbca_trsrn-extra-objs := $(SWITCHKINS_OBJS) + objects/%.mak: %.comp hal/components/Submakefile $(ECHO) "Creating $(notdir $@)" @mkdir -p $(dir $@) - $(Q)echo $(notdir $*)-objs := objects/$*.o > $@.tmp - $(Q)echo ../rtlib/$(notdir $*)$(MODULE_EXT): objects/rtobjects/$*.o >> $@.tmp + $(Q)echo $(notdir $*)-objs := objects/$*.o $($(notdir $*)-extra-objs) > $@.tmp + $(Q)echo ../rtlib/$(notdir $*)$(MODULE_EXT): objects/rtobjects/$*.o $(addprefix objects/rt,$($(notdir $*)-extra-objs)) >> $@.tmp $(Q)mv -f $@.tmp $@ objects/%.c: %.comp ../bin/halcompile diff --git a/src/hal/components/millturn.comp b/src/hal/components/millturn.comp index abb217f7a3a..161e8abebba 100644 --- a/src/hal/components/millturn.comp +++ b/src/hal/components/millturn.comp @@ -10,16 +10,15 @@ rotary axis. type1 is a turn (Z-YX) configuration with A configured to be a spindle. +The kinematics-type switching, the *kinstype.is-N* pins and the +joints-to-coordinates mapping are provided by switchkins.c, so the +*coordinates=* module parameter and the kinematics switching described in +the switchkins document chapter apply here too. + For an example configuration, run the sim config: 'configs/sim/axis/vismach/millturn/millturn.ini'. Further explanations can be found in the README in 'configs/sim/axis/vismach/millturn'. -millturn.comp was constructed by modifying the template file: -userkins.comp. - -For more information on how to modify userkins.comp run: $ man -userkins. Also, see additional information inside: 'userkins.comp'. - For information on kinematics in general see the kinematics document chapter (docs/src/motion/kinematics.txt) and for switchable kinematics in particular see the switchkins document @@ -27,7 +26,7 @@ chapter (docs/src/motion/switchkins.txt) """; // The fpin pin is not accessible in kinematics functions. -// Use EXTRA_SETUP() for pins and params used by kinematics. +// Use the *_setup() function for pins and params used by kinematics. pin out si32 fpin=0"pin to demonstrate use of a conventional (non-kinematics) function fdemo"; option period no; option extra_setup; @@ -36,20 +35,10 @@ license "GPL"; author "David Mueller"; ;; -#include +#include -static struct haldata { - // Example pin pointers: - hal_uint_t in; - hal_uint_t out; - // Example parameters: - //hal_real_t param_rw; - //hal_real_t param_ro; - - //Declare hal pin pointers used for switchable kinematics - hal_bool_t kinstype_is_0; - hal_bool_t kinstype_is_1; -} *haldata; +static char *coordinates; +RTAPI_MP_STRING(coordinates, "Axes-to-joints-ordering"); FUNCTION(fdemo) { // This function can be added to a thread (addf) for @@ -60,121 +49,30 @@ FUNCTION(fdemo) { fpin_set(fpin + 1); } -EXTRA_SETUP() { - (void)__comp_inst; - (void)prefix; - (void)extra_arg; -#define HAL_PREFIX "millturn" - int res=0; - - // inherit comp_id from rtapi_main() - if (comp_id < 0) goto error; - - haldata = hal_malloc(sizeof(*haldata)); - if (!haldata) goto error; - - // hal pin examples: - res += hal_pin_new_ui32(comp_id, HAL_IN, &haldata->in, 0, "%s.in", HAL_PREFIX); - res += hal_pin_new_ui32(comp_id, HAL_OUT, &haldata->out, 0, "%s.out", HAL_PREFIX); - // hal parameter examples: - //res += hal_param_new_real(comp_id, HAL_RW, &haldata->param_rw, 0.0, "%s.param-rw", HAL_PREFIX); - //res += hal_param_new_real(comp_id, HAL_RO, &haldata->param_ro, 0.0, "%s.param-ro", HAL_PREFIX); - - // hal pins required for switchable kinematics: - //default at startup -> mill configuration - //-> turn configuration - res += hal_pin_new_bool(comp_id, HAL_OUT, &haldata->kinstype_is_0, 1, "kinstype.is-0"); - res += hal_pin_new_bool(comp_id, HAL_OUT, &haldata->kinstype_is_1, 0, "kinstype.is-1"); - - if (res) goto error; - rtapi_print("*** %s setup ok\n",__FILE__); - return 0; -error: - rtapi_print("\n!!! %s setup failed res=%d\n\n",__FILE__,res); - return -1; -#undef HAL_PREFIX -} - -EXPORT_SYMBOL(kinematicsType); -EXPORT_SYMBOL(kinematicsSwitchable); -EXPORT_SYMBOL(kinematicsTypeFlags); -EXPORT_SYMBOL(kinematicsSwitch); -EXPORT_SYMBOL(kinematicsInverse); -EXPORT_SYMBOL(kinematicsJacobian); -EXPORT_SYMBOL(kinematicsForward); - -static rtapi_u32 switchkins_type; - -int kinematicsSwitchable() {return 1;} - -int kinematicsTypeFlags(int ktype) -{ - switch (ktype) { - case 0: return KINSTYPE_IDENTITY; - case 1: return 0; // the turn mapping, no flag to declare - default: return -1; - } -} - -int kinematicsSwitch(int new_switchkins_type) +// the turn kinematics need no hal pins of their own +static int turnKinematicsSetup(const int comp_id, + const char* coords, + kparms* kp) { - switchkins_type = new_switchkins_type; - rtapi_print("kinematicsSwitch(): type=%d\n",switchkins_type); - // create case structure for switchable kinematics - switch (switchkins_type) { - case 0: rtapi_print_msg(RTAPI_MSG_INFO, - "kinematicsSwitch:TYPE0\n"); - hal_set_bool(haldata->kinstype_is_0, 1); - hal_set_bool(haldata->kinstype_is_1, 0); - break; - case 1: rtapi_print_msg(RTAPI_MSG_INFO, - "kinematicsSwitch:TYPE1\n"); - hal_set_bool(haldata->kinstype_is_0, 0); - hal_set_bool(haldata->kinstype_is_1, 1); - break; - default: rtapi_print_msg(RTAPI_MSG_ERR, - "kinematicsSwitch:BAD VALUE <%d>\n", - switchkins_type); - hal_set_bool(haldata->kinstype_is_1, 0); - hal_set_bool(haldata->kinstype_is_0, 0); - return -1; // FAIL - } - return 0; // ok -} - -KINEMATICS_TYPE kinematicsType() -{ - return KINEMATICS_BOTH; // set as required - // Note: If kinematics are identity, using KINEMATICS_BOTH - // may be used in order to allow a gui to display - // joint values in preview prior to homing -} // kinematicsType() + (void)comp_id; + (void)coords; + (void)kp; + return 0; +} // turnKinematicsSetup() -static bool is_ready=0; -int kinematicsForward(const double *j, - EmcPose * pos, - const KINEMATICS_FORWARD_FLAGS * fflags, - KINEMATICS_INVERSE_FLAGS * iflags) +static int turnKinematicsForward(const double *j, + EmcPose * pos, + const KINEMATICS_FORWARD_FLAGS * fflags, + KINEMATICS_INVERSE_FLAGS * iflags) { (void)fflags; (void)iflags; - static bool gave_msg; - // define forward kinematic models using case structure for - // for switchable kinematics - switch (switchkins_type) { - case 0: - pos->tran.x = j[0]; - pos->tran.y = j[1]; - pos->tran.z = j[2]; - pos->a = j[3]; - break; - case 1: - pos->tran.x = j[2]; - pos->tran.y = -j[1]; - pos->tran.z = j[0]; - pos->a = j[3]; - break; - } + + pos->tran.x = j[2]; + pos->tran.y = -j[1]; + pos->tran.z = j[0]; + pos->a = j[3]; + // unused coordinates: pos->b = 0; pos->c = 0; @@ -182,77 +80,70 @@ int kinematicsForward(const double *j, pos->v = 0; pos->w = 0; - if (hal_get_ui32(haldata->in) && !is_ready && !gave_msg) { - rtapi_print_msg(RTAPI_MSG_ERR, - "%s the 'in' pin not echoed until Inverse called\n", - __FILE__); - gave_msg=1; - } return 0; -} // kinematicsForward() +} // turnKinematicsForward() -int kinematicsInverse(const EmcPose * pos, - double *j, - const KINEMATICS_INVERSE_FLAGS * iflags, - KINEMATICS_FORWARD_FLAGS * fflags) +static int turnKinematicsInverse(const EmcPose * pos, + double *j, + const KINEMATICS_INVERSE_FLAGS * iflags, + KINEMATICS_FORWARD_FLAGS * fflags) { (void)iflags; (void)fflags; - is_ready = 1; // Inverse is not called until homed for KINEMATICS_BOTH - - // Update the kinematic joints specified by the - // [KINS]JOINTS setting (4 required for this template). - // define forward kinematic models using case structure for - // for switchable kinematics - switch (switchkins_type) { - case 0: - j[0] = pos->tran.x; - j[1] = pos->tran.y; - j[2] = pos->tran.z; - j[3] = pos->a; - break; - case 1: - j[2] = pos->tran.x; - j[1] = -pos->tran.y; - j[0] = pos->tran.z; - j[3] = pos->a; - break; - } - //example hal pin update (homing reqd before kinematicsInverse) - hal_set_ui32(haldata->out, hal_get_ui32(haldata->in)); //dereference - //read from param example: *haldata->out = hal_get_real(haldata->param_rw); + j[0] = pos->tran.z; + j[1] = -pos->tran.y; + j[2] = pos->tran.x; + j[3] = pos->a; return 0; -} // kinematicsInverse() +} // turnKinematicsInverse() -int kinematicsJacobian(const double *j, - const EmcPose * pos, - double jac[EMCMOT_MAX_JOINTS][EMCMOT_MAX_AXIS], - const KINEMATICS_INVERSE_FLAGS * iflags) +static int turnKinematicsJacobian(const double *j, + const EmcPose * pos, + double jac[EMCMOT_MAX_JOINTS][EMCMOT_MAX_AXIS], + const KINEMATICS_INVERSE_FLAGS * iflags) { - int r, c; + int R, C; (void)j; (void)pos; (void)iflags; - for (r = 0; r < EMCMOT_MAX_JOINTS; r++) { - for (c = 0; c < EMCMOT_MAX_AXIS; c++) { jac[r][c] = 0; } - } - // the derivative of kinematicsInverse() for each type: which joint - // follows which pose coordinate, and in which sense - switch (switchkins_type) { - case 0: - jac[0][0] = 1; - jac[1][1] = 1; - jac[2][2] = 1; - jac[3][3] = 1; - break; - case 1: - jac[2][0] = 1; - jac[1][1] = -1; - jac[0][2] = 1; - jac[3][3] = 1; - break; + for (R = 0; R < EMCMOT_MAX_JOINTS; R++) { + for (C = 0; C < EMCMOT_MAX_AXIS; C++) { jac[R][C] = 0; } } + // the derivative of turnKinematicsInverse(): which joint follows which + // pose coordinate, and in which sense + jac[2][0] = 1; + jac[1][1] = -1; + jac[0][2] = 1; + jac[3][3] = 1; return 0; -} // kinematicsJacobian() +} // turnKinematicsJacobian() + +// halcompile has done hal_init() and does hal_ready() after this returns, +// which is what switchkinsInit() expects +EXTRA_SETUP() { + kparms kp; + (void)__comp_inst; (void)prefix; (void)extra_arg; + + kp.kinsname = "millturn"; + kp.halprefix = "millturn"; + kp.required_coordinates = "xyza"; + kp.allow_duplicates = 0; + kp.fwd_iterates_mask = 0; + kp.gui_kinstype = -1; + kp.sparm = NULL; + kp.max_joints = strlen(kp.required_coordinates); + + if (switchkinsRegister(0, identityKinematicsSetup, + identityKinematicsForward, + identityKinematicsInverse)) { return -1; } + if (switchkinsRegister(1, turnKinematicsSetup, + turnKinematicsForward, + turnKinematicsInverse)) { return -1; } + if (switchkinsRegisterJacobian(1, turnKinematicsJacobian)) { return -1; } + + if (switchkinsDeclare(0, KINSTYPE_IDENTITY)) { return -1; } + + return switchkinsInit(comp_id, &kp, coordinates); +} // EXTRA_SETUP() diff --git a/src/hal/components/xyzab_tdr_kins.comp b/src/hal/components/xyzab_tdr_kins.comp index 2ee61e6a9b8..0387d73c85a 100644 --- a/src/hal/components/xyzab_tdr_kins.comp +++ b/src/hal/components/xyzab_tdr_kins.comp @@ -13,16 +13,15 @@ axes XYZAB respectively. type1 is a XYZAB configuration with tool center point (TCP) compensation. +The kinematics-type switching, the *kinstype.is-N* pins and the +joints-to-coordinates mapping are provided by switchkins.c, so the +*coordinates=* module parameter and the kinematics switching described in +the switchkins document chapter apply here too. + For an example configuration, run the sim config: '/configs/sim/axis/vismach/5axis/table-dual-rotary/xyzab-tdr.ini'. Further explanations can be found in the README in '/configs/sim/axis/vismach/5axis/table-dual-rotary/'. -xyzab_tdr_kins.comp was constructed by modifying the template file: -userkins.comp. - -For more information on how to modify userkins.comp run: $ man -userkins. Also, see additional information inside: 'userkins.comp'. - For information on kinematics in general see the kinematics document chapter (docs/src/motion/kinematics.txt) and for switchable kinematics in particular see the switchkins document @@ -31,6 +30,7 @@ chapter (docs/src/motion/switchkins.txt) """; pin out si32 dummy=0"one pin needed to satisfy halcompile requirement"; + option extra_setup; license "GPL"; @@ -38,127 +38,61 @@ author "David Mueller"; ;; #include -#include -static struct haldata { +#include - // Declare hal pin pointers used for xyzab_tdr kinematics: +static char *coordinates; +RTAPI_MP_STRING(coordinates, "Axes-to-joints-ordering"); + +static struct haldata { hal_real_t tool_offset_z; hal_real_t x_offset; hal_real_t z_offset; hal_real_t x_rot_point; hal_real_t y_rot_point; hal_real_t z_rot_point; +} *tdrdata; - //Declare hal pin pointers used for switchable kinematics - hal_bool_t kinstype_is_0; - hal_bool_t kinstype_is_1; -} *haldata; - -EXTRA_SETUP() { - (void)__comp_inst; - (void)prefix; - (void)extra_arg; -#define HAL_PREFIX "xyzab_tdr_kins" - int res=0; - // inherit comp_id from rtapi_main() - if (comp_id < 0) goto error; - - haldata = hal_malloc(sizeof(*haldata)); - if (!haldata) goto error; - - // hal pins required for xyzab_tdr kinematics: - res += hal_pin_new_real(comp_id, HAL_IN, &haldata->tool_offset_z, 0.0, "%s.tool-offset-z", HAL_PREFIX); - res += hal_pin_new_real(comp_id, HAL_IN, &haldata->x_offset, 0.0, "%s.x-offset", HAL_PREFIX); - res += hal_pin_new_real(comp_id, HAL_IN, &haldata->z_offset, 0.0, "%s.z-offset", HAL_PREFIX); - res += hal_pin_new_real(comp_id, HAL_IN, &haldata->x_rot_point, 0.0, "%s.x-rot-point", HAL_PREFIX); - res += hal_pin_new_real(comp_id, HAL_IN, &haldata->y_rot_point, 0.0, "%s.y-rot-point", HAL_PREFIX); - res += hal_pin_new_real(comp_id, HAL_IN, &haldata->z_rot_point, 0.0, "%s.z-rot-point", HAL_PREFIX); - - // hal pins required for switchable kinematics: - //default at startup -> identity kinematics - //-> XYZAB TCP - res += hal_pin_new_bool(comp_id, HAL_OUT, &haldata->kinstype_is_0, 1, "kinstype.is-0"); - res += hal_pin_new_bool(comp_id, HAL_OUT, &haldata->kinstype_is_1, 0, "kinstype.is-1"); - - if (res) goto error; - rtapi_print("*** %s setup ok\n",__FILE__); - return 0; -error: - rtapi_print("\n!!! %s setup failed res=%d\n\n",__FILE__,res); - return -1; -#undef HAL_PREFIX -} - -EXPORT_SYMBOL(kinematicsType); -EXPORT_SYMBOL(kinematicsSwitchable); -EXPORT_SYMBOL(kinematicsSwitch); -EXPORT_SYMBOL(kinematicsTypeFlags); -EXPORT_SYMBOL(kinematicsInverse); -EXPORT_SYMBOL(kinematicsJacobian); -EXPORT_SYMBOL(kinematicsForward); - -static rtapi_u32 switchkins_type; - -int kinematicsSwitchable() {return 1;} - -int kinematicsTypeFlags(int ktype) -{ - switch (ktype) { - case 0: return KINSTYPE_IDENTITY; - case 1: return KINSTYPE_PRIMARY; - default: return -1; - } -} - -int kinematicsSwitch(int new_switchkins_type) -{ - switchkins_type = new_switchkins_type; - rtapi_print("kinematicsSwitch(): type=%d\n",switchkins_type); - // create case structure for switchable kinematics - switch (switchkins_type) { - case 0: rtapi_print_msg(RTAPI_MSG_INFO, - "kinematicsSwitch:TYPE0\n"); - hal_set_bool(haldata->kinstype_is_0, 1); - hal_set_bool(haldata->kinstype_is_1, 0); - break; - case 1: rtapi_print_msg(RTAPI_MSG_INFO, - "kinematicsSwitch:TYPE1\n"); - hal_set_bool(haldata->kinstype_is_0, 0); - hal_set_bool(haldata->kinstype_is_1, 1); - break; - default: rtapi_print_msg(RTAPI_MSG_ERR, - "kinematicsSwitch:BAD VALUE <%d>\n", - switchkins_type); - hal_set_bool(haldata->kinstype_is_1, 0); - hal_set_bool(haldata->kinstype_is_0, 0); - return -1; // FAIL - } - return 0; // ok -} - -KINEMATICS_TYPE kinematicsType() +static int tdrKinematicsSetup(const int comp_id, + const char* coords, + kparms* kp) { - return KINEMATICS_BOTH; // set as required - // Note: If kinematics are identity, using KINEMATICS_BOTH - // may be used in order to allow a gui to display - // joint values in preview prior to homing -} // kinematicsType() + int res = 0; + (void)coords; + + tdrdata = hal_malloc(sizeof(*tdrdata)); + if (!tdrdata) return -1; + + res += hal_pin_new_real(comp_id, HAL_IN, &tdrdata->tool_offset_z, 0.0, + "%s.tool-offset-z", kp->halprefix); + res += hal_pin_new_real(comp_id, HAL_IN, &tdrdata->x_offset, 0.0, + "%s.x-offset", kp->halprefix); + res += hal_pin_new_real(comp_id, HAL_IN, &tdrdata->z_offset, 0.0, + "%s.z-offset", kp->halprefix); + res += hal_pin_new_real(comp_id, HAL_IN, &tdrdata->x_rot_point, 0.0, + "%s.x-rot-point", kp->halprefix); + res += hal_pin_new_real(comp_id, HAL_IN, &tdrdata->y_rot_point, 0.0, + "%s.y-rot-point", kp->halprefix); + res += hal_pin_new_real(comp_id, HAL_IN, &tdrdata->z_rot_point, 0.0, + "%s.z-rot-point", kp->halprefix); + if (res) return -1; -int kinematicsForward(const double *j, - EmcPose * pos, - const KINEMATICS_FORWARD_FLAGS * fflags, - KINEMATICS_INVERSE_FLAGS * iflags) + return 0; +} // tdrKinematicsSetup() +static int tdrKinematicsForward(const double *j, + EmcPose * pos, + const KINEMATICS_FORWARD_FLAGS * fflags, + KINEMATICS_INVERSE_FLAGS * iflags) { (void)fflags; (void)iflags; - double x_rot_point = hal_get_real(haldata->x_rot_point); - double y_rot_point = hal_get_real(haldata->y_rot_point); - double z_rot_point = hal_get_real(haldata->z_rot_point); + double x_rot_point = hal_get_real(tdrdata->x_rot_point); + double y_rot_point = hal_get_real(tdrdata->y_rot_point); + double z_rot_point = hal_get_real(tdrdata->z_rot_point); - double dz = hal_get_real(haldata->z_offset); - double dt = hal_get_real(haldata->tool_offset_z); + double dz = hal_get_real(tdrdata->z_offset); + double dt = hal_get_real(tdrdata->tool_offset_z); // substitutions as used in mathematical documentation // including degree -> radians angle conversion @@ -168,39 +102,22 @@ int kinematicsForward(const double *j, double cb = cos(j[4]*TO_RAD); // used to be consistent with math in the documentation - double px = 0; - double py = 0; - double pz = 0; - - // define forward kinematic models using case structure for - // for switchable kinematics - switch (switchkins_type) { - case 0: // ====================== IDENTITY kinematics FORWARD ==================== - pos->tran.x = j[0]; - pos->tran.y = j[1]; - pos->tran.z = j[2]; - pos->a = j[3]; - pos->b = j[4]; - break; - case 1: // ========================= TCP kinematics FORWARD ====================== - px = j[0] - x_rot_point; - py = j[1] - y_rot_point; - pz = j[2] - z_rot_point - dt; - - pos->tran.x = cb*px + sb*pz - + x_rot_point; - - pos->tran.y = sa*sb*px + ca*py - cb*sa*pz + sa*dz - + y_rot_point; - - pos->tran.z = - ca*sb*px + sa*py + ca*cb*pz - ca*dz - + z_rot_point + dz + dt; - - pos->a = j[3]; - pos->b = j[4]; - pos->c = j[5]; - break; - } + double px = j[0] - x_rot_point; + double py = j[1] - y_rot_point; + double pz = j[2] - z_rot_point - dt; + + pos->tran.x = cb*px + sb*pz + + x_rot_point; + + pos->tran.y = sa*sb*px + ca*py - cb*sa*pz + sa*dz + + y_rot_point; + + pos->tran.z = - ca*sb*px + sa*py + ca*cb*pz - ca*dz + + z_rot_point + dz + dt; + + pos->a = j[3]; + pos->b = j[4]; + // unused coordinates: pos->c = 0; pos->u = 0; @@ -208,22 +125,22 @@ int kinematicsForward(const double *j, pos->w = 0; return 0; -} // kinematicsForward() +} // tdrKinematicsForward() -int kinematicsInverse(const EmcPose * pos, - double *j, - const KINEMATICS_INVERSE_FLAGS * iflags, - KINEMATICS_FORWARD_FLAGS * fflags) +static int tdrKinematicsInverse(const EmcPose * pos, + double *j, + const KINEMATICS_INVERSE_FLAGS * iflags, + KINEMATICS_FORWARD_FLAGS * fflags) { (void)iflags; (void)fflags; - double x_rot_point = hal_get_real(haldata->x_rot_point); - double y_rot_point = hal_get_real(haldata->y_rot_point); - double z_rot_point = hal_get_real(haldata->z_rot_point); + double x_rot_point = hal_get_real(tdrdata->x_rot_point); + double y_rot_point = hal_get_real(tdrdata->y_rot_point); + double z_rot_point = hal_get_real(tdrdata->z_rot_point); - double dx = hal_get_real(haldata->x_offset); - double dz = hal_get_real(haldata->z_offset); - double dt = hal_get_real(haldata->tool_offset_z); + double dx = hal_get_real(tdrdata->x_offset); + double dz = hal_get_real(tdrdata->z_offset); + double dt = hal_get_real(tdrdata->tool_offset_z); // substitutions as used in mathematical documentation // including degree -> radians angle conversion @@ -233,53 +150,38 @@ int kinematicsInverse(const EmcPose * pos, double cb = cos(pos->b*TO_RAD); // used to be consistent with math in the documentation - double qx = 0; - double qy = 0; - double qz = 0; - - switch (switchkins_type) { - case 0:// ====================== IDENTITY kinematics INVERSE ===================== - j[0] = pos->tran.x; - j[1] = pos->tran.y; - j[2] = pos->tran.z; - j[3] = pos->a; - j[4] = pos->b; - break; - case 1: // ========================= TCP kinematics INVERSE ====================== - qx = pos->tran.x - x_rot_point - dx; - qy = pos->tran.y - y_rot_point; - qz = pos->tran.z - z_rot_point - dz - dt; - - j[0] = cb*qx + sa*sb*qy - ca*sb*qz + cb*dx - sb*dz - + x_rot_point; - - j[1] = ca*qy + sa*qz - + y_rot_point; - - j[2] = sb*qx - sa*cb*qy + ca*cb*qz + sb*dx + cb*dz - + z_rot_point + dt; - - j[3] = pos->a; - j[4] = pos->b; - break; - } + double qx = pos->tran.x - x_rot_point - dx; + double qy = pos->tran.y - y_rot_point; + double qz = pos->tran.z - z_rot_point - dz - dt; + + j[0] = cb*qx + sa*sb*qy - ca*sb*qz + cb*dx - sb*dz + + x_rot_point; + + j[1] = ca*qy + sa*qz + + y_rot_point; + + j[2] = sb*qx - sa*cb*qy + ca*cb*qz + sb*dx + cb*dz + + z_rot_point + dt; + + j[3] = pos->a; + j[4] = pos->b; return 0; -} // kinematicsInverse() +} // tdrKinematicsInverse() -int kinematicsJacobian(const double *j, - const EmcPose * pos, - double jac[EMCMOT_MAX_JOINTS][EMCMOT_MAX_AXIS], - const KINEMATICS_INVERSE_FLAGS * iflags) +static int tdrKinematicsJacobian(const double *j, + const EmcPose * pos, + double jac[EMCMOT_MAX_JOINTS][EMCMOT_MAX_AXIS], + const KINEMATICS_INVERSE_FLAGS * iflags) { (void)j; (void)iflags; - double x_rot_point = hal_get_real(haldata->x_rot_point); - double y_rot_point = hal_get_real(haldata->y_rot_point); - double z_rot_point = hal_get_real(haldata->z_rot_point); - double dx = hal_get_real(haldata->x_offset); - double dz = hal_get_real(haldata->z_offset); - double dt = hal_get_real(haldata->tool_offset_z); + double x_rot_point = hal_get_real(tdrdata->x_rot_point); + double y_rot_point = hal_get_real(tdrdata->y_rot_point); + double z_rot_point = hal_get_real(tdrdata->z_rot_point); + double dx = hal_get_real(tdrdata->x_offset); + double dz = hal_get_real(tdrdata->z_offset); + double dt = hal_get_real(tdrdata->tool_offset_z); double sa = sin(pos->a*TO_RAD); double ca = cos(pos->a*TO_RAD); double sb = sin(pos->b*TO_RAD); @@ -287,43 +189,61 @@ int kinematicsJacobian(const double *j, double qx = pos->tran.x - x_rot_point - dx; double qy = pos->tran.y - y_rot_point; double qz = pos->tran.z - z_rot_point - dz - dt; - int r, c; + int R, C; - for (r = 0; r < EMCMOT_MAX_JOINTS; r++) { - for (c = 0; c < EMCMOT_MAX_AXIS; c++) { jac[r][c] = 0; } + for (R = 0; R < EMCMOT_MAX_JOINTS; R++) { + for (C = 0; C < EMCMOT_MAX_AXIS; C++) { jac[R][C] = 0; } } - switch (switchkins_type) { - case 0: // ====================== IDENTITY kinematics JACOBIAN ==================== - jac[0][0] = 1; - jac[1][1] = 1; - jac[2][2] = 1; - jac[3][3] = 1; - jac[4][4] = 1; - break; - case 1: // ========================= TCP kinematics JACOBIAN ====================== - // the TCP inverse above differentiated: its coefficients of - // qx, qy and qz for the linear columns, and the same terms - // with a or b advanced a quarter turn for the rotary columns - jac[0][0] = cb; - jac[0][1] = sa*sb; - jac[0][2] = -ca*sb; - jac[0][3] = ( ca*sb*qy + sa*sb*qz) * TO_RAD; - jac[0][4] = (-sb*qx + sa*cb*qy - ca*cb*qz - sb*dx - cb*dz) * TO_RAD; - - jac[1][1] = ca; - jac[1][2] = sa; - jac[1][3] = (-sa*qy + ca*qz) * TO_RAD; - - jac[2][0] = sb; - jac[2][1] = -sa*cb; - jac[2][2] = ca*cb; - jac[2][3] = (-ca*cb*qy - sa*cb*qz) * TO_RAD; - jac[2][4] = ( cb*qx + sa*sb*qy - ca*sb*qz + cb*dx - sb*dz) * TO_RAD; - - jac[3][3] = 1; - jac[4][4] = 1; - break; - } + // tdrKinematicsInverse() differentiated: its coefficients of qx, qy + // and qz for the linear columns, and the same terms with a or b + // advanced a quarter turn for the rotary columns + jac[0][0] = cb; + jac[0][1] = sa*sb; + jac[0][2] = -ca*sb; + jac[0][3] = ( ca*sb*qy + sa*sb*qz) * TO_RAD; + jac[0][4] = (-sb*qx + sa*cb*qy - ca*cb*qz - sb*dx - cb*dz) * TO_RAD; + + jac[1][1] = ca; + jac[1][2] = sa; + jac[1][3] = (-sa*qy + ca*qz) * TO_RAD; + + jac[2][0] = sb; + jac[2][1] = -sa*cb; + jac[2][2] = ca*cb; + jac[2][3] = (-ca*cb*qy - sa*cb*qz) * TO_RAD; + jac[2][4] = ( cb*qx + sa*sb*qy - ca*sb*qz + cb*dx - sb*dz) * TO_RAD; + + jac[3][3] = 1; + jac[4][4] = 1; return 0; -} // kinematicsJacobian() +} // tdrKinematicsJacobian() + +// halcompile has done hal_init() and does hal_ready() after this returns, +// which is what switchkinsInit() expects +EXTRA_SETUP() { + kparms kp; + (void)__comp_inst; (void)prefix; (void)extra_arg; + + kp.kinsname = "xyzab_tdr_kins"; + kp.halprefix = "xyzab_tdr_kins"; + kp.required_coordinates = "xyzab"; + kp.allow_duplicates = 0; + kp.fwd_iterates_mask = 0; + kp.gui_kinstype = -1; + kp.sparm = NULL; + kp.max_joints = strlen(kp.required_coordinates); + + if (switchkinsRegister(0, identityKinematicsSetup, + identityKinematicsForward, + identityKinematicsInverse)) { return -1; } + if (switchkinsRegister(1, tdrKinematicsSetup, + tdrKinematicsForward, + tdrKinematicsInverse)) { return -1; } + if (switchkinsRegisterJacobian(1, tdrKinematicsJacobian)) { return -1; } + + if (switchkinsDeclare(0, KINSTYPE_IDENTITY)) { return -1; } + if (switchkinsDeclare(1, KINSTYPE_PRIMARY)) { return -1; } + + return switchkinsInit(comp_id, &kp, coordinates); +} // EXTRA_SETUP() diff --git a/src/hal/components/xyzacb_trsrn.comp b/src/hal/components/xyzacb_trsrn.comp index 67fd97715a8..45a8a9af4f9 100644 --- a/src/hal/components/xyzacb_trsrn.comp +++ b/src/hal/components/xyzacb_trsrn.comp @@ -4,6 +4,11 @@ description """ FIXME +The kinematics-type switching, the *kinstype.is-N* pins and the +joints-to-coordinates mapping are provided by switchkins.c, so the +*coordinates=* module parameter and the kinematics switching described in +the switchkins document chapter apply here too. + """; pin out si32 dummy=0 "dummy pin to satisfy halcompile"; option period no; @@ -14,8 +19,11 @@ author "David Mueller"; ;; #include -#include +#include + +static char *coordinates; +RTAPI_MP_STRING(coordinates, "Axes-to-joints-ordering"); static struct haldata { // these should be parameters really but we want to be able to @@ -36,133 +44,50 @@ static struct haldata { // Declare hal pin pointers used for xyzacb_trsrn kinematics: hal_real_t tool_offset_z; - - //Declare hal pin pointers used for switchable kinematics - hal_bool_t kinstype_is_0; - hal_bool_t kinstype_is_1; - hal_bool_t kinstype_is_2; } *haldata; - -EXTRA_SETUP() { - (void)__comp_inst; - (void)prefix; - (void)extra_arg; -#define HAL_PREFIX "xyzacb_trsrn_kins" - int res=0; - // inherit comp_id from rtapi_main() - if (comp_id < 0) goto error; - - haldata = hal_malloc(sizeof(struct haldata)); - if (!haldata) goto error; - - // hal pins required for xyzacb_trsrn kinematics: - res += hal_pin_new_real(comp_id, HAL_IN, &haldata->tool_offset_z, 0.0, "%s.tool-offset-z" ,HAL_PREFIX); - res += hal_pin_new_real(comp_id, HAL_IN, &haldata->y_pivot, 0.0, "%s.y-pivot" ,HAL_PREFIX); - res += hal_pin_new_real(comp_id, HAL_IN, &haldata->z_pivot, 0.0, "%s.z-pivot" ,HAL_PREFIX); - res += hal_pin_new_real(comp_id, HAL_IN, &haldata->x_offset, 0.0, "%s.x-offset" ,HAL_PREFIX); - res += hal_pin_new_real(comp_id, HAL_IN, &haldata->y_offset, 0.0, "%s.y-offset" ,HAL_PREFIX); - res += hal_pin_new_real(comp_id, HAL_IN, &haldata->y_rot_axis, 0.0, "%s.y-rot-axis" ,HAL_PREFIX); - res += hal_pin_new_real(comp_id, HAL_IN, &haldata->z_rot_axis, 0.0, "%s.z-rot-axis" ,HAL_PREFIX); - res += hal_pin_new_real(comp_id, HAL_IN, &haldata->pre_rot, 0.0, "%s.pre-rot" ,HAL_PREFIX); - res += hal_pin_new_real(comp_id, HAL_IN, &haldata->nut_angle, 0.0, "%s.nut-angle" ,HAL_PREFIX); - res += hal_pin_new_real(comp_id, HAL_IN, &haldata->prim_angle, 0.0, "%s.primary-angle" ,HAL_PREFIX); - res += hal_pin_new_real(comp_id, HAL_IN, &haldata->sec_angle, 0.0, "%s.secondary-angle" ,HAL_PREFIX); - - // hal pins required for switchable kinematics: - //default at startup -> identity kinematics - //-> xyzabc TCP - //-> xyzabc TOOL - res += hal_pin_new_bool(comp_id, HAL_OUT, &haldata->kinstype_is_0, 1, "kinstype.is-0"); - res += hal_pin_new_bool(comp_id, HAL_OUT, &haldata->kinstype_is_1, 0, "kinstype.is-1"); - res += hal_pin_new_bool(comp_id, HAL_OUT, &haldata->kinstype_is_2, 0, "kinstype.is-2"); - - if (res) goto error; - rtapi_print("*** %s setup ok\n",__FILE__); - return 0; -error: - rtapi_print("\n!!! %s setup failed res=%d\n\n",__FILE__,res); - return -1; -#undef HAL_PREFIX -} - -EXPORT_SYMBOL(kinematicsType); -EXPORT_SYMBOL(kinematicsSwitchable); -EXPORT_SYMBOL(kinematicsSwitch); -EXPORT_SYMBOL(kinematicsTypeFlags); -EXPORT_SYMBOL(kinematicsInverse); -EXPORT_SYMBOL(kinematicsJacobian); -EXPORT_SYMBOL(kinematicsForward); -EXPORT_SYMBOL(kinematicsToolFrame); -EXPORT_SYMBOL(kinematicsWorkFrame); - -static rtapi_u32 switchkins_type; - -int kinematicsSwitchable() {return 1;} - -int kinematicsTypeFlags(int ktype) +// the pins are shared by the TCP and TOOL kinematics; the TOOL type has +// no setup routine of its own +static int trsrnKinematicsSetup(const int comp_id, + const char* coords, + kparms* kp) { - switch (ktype) { - case 0: return KINSTYPE_IDENTITY; - case 1: return KINSTYPE_PRIMARY; - default: return -1; - } -} - + int res = 0; + (void)coords; + haldata = hal_malloc(sizeof(struct haldata)); + if (!haldata) return -1; + + res += hal_pin_new_real(comp_id, HAL_IN, &haldata->tool_offset_z, 0.0, "%s.tool-offset-z" ,kp->halprefix); + res += hal_pin_new_real(comp_id, HAL_IN, &haldata->y_pivot, 0.0, "%s.y-pivot" ,kp->halprefix); + res += hal_pin_new_real(comp_id, HAL_IN, &haldata->z_pivot, 0.0, "%s.z-pivot" ,kp->halprefix); + res += hal_pin_new_real(comp_id, HAL_IN, &haldata->x_offset, 0.0, "%s.x-offset" ,kp->halprefix); + res += hal_pin_new_real(comp_id, HAL_IN, &haldata->y_offset, 0.0, "%s.y-offset" ,kp->halprefix); + res += hal_pin_new_real(comp_id, HAL_IN, &haldata->y_rot_axis, 0.0, "%s.y-rot-axis" ,kp->halprefix); + res += hal_pin_new_real(comp_id, HAL_IN, &haldata->z_rot_axis, 0.0, "%s.z-rot-axis" ,kp->halprefix); + res += hal_pin_new_real(comp_id, HAL_IN, &haldata->pre_rot, 0.0, "%s.pre-rot" ,kp->halprefix); + res += hal_pin_new_real(comp_id, HAL_IN, &haldata->nut_angle, 0.0, "%s.nut-angle" ,kp->halprefix); + res += hal_pin_new_real(comp_id, HAL_IN, &haldata->prim_angle, 0.0, "%s.primary-angle" ,kp->halprefix); + res += hal_pin_new_real(comp_id, HAL_IN, &haldata->sec_angle, 0.0, "%s.secondary-angle" ,kp->halprefix); + if (res) return -1; -int kinematicsSwitch(int new_switchkins_type) -{ - switchkins_type = new_switchkins_type; - rtapi_print("kinematicsSwitch(): type=%d\n",switchkins_type); - // create case structure for switchable kinematics - switch (switchkins_type) { - case 0: rtapi_print_msg(RTAPI_MSG_INFO, - "kinematicsSwitch:TYPE0\n"); - hal_set_bool(haldata->kinstype_is_0, 1); - hal_set_bool(haldata->kinstype_is_1, 0); - hal_set_bool(haldata->kinstype_is_2, 0); - break; - case 1: rtapi_print_msg(RTAPI_MSG_INFO, - "kinematicsSwitch:TYPE1\n"); - hal_set_bool(haldata->kinstype_is_0, 0); - hal_set_bool(haldata->kinstype_is_1, 1); - hal_set_bool(haldata->kinstype_is_2, 0); - break; - case 2: rtapi_print_msg(RTAPI_MSG_INFO, - "kinematicsSwitch:TYPE1\n"); - hal_set_bool(haldata->kinstype_is_0, 0); - hal_set_bool(haldata->kinstype_is_1, 0); - hal_set_bool(haldata->kinstype_is_2, 1); - break; - default: rtapi_print_msg(RTAPI_MSG_ERR, - "kinematicsSwitch:BAD VALUE <%d>\n", - switchkins_type); - hal_set_bool(haldata->kinstype_is_1, 0); - hal_set_bool(haldata->kinstype_is_0, 0); - hal_set_bool(haldata->kinstype_is_2, 0); - return -1; // FAIL - } - return 0; // ok -} + return 0; +} // trsrnKinematicsSetup() -KINEMATICS_TYPE kinematicsType() +static int toolKinematicsSetup(const int comp_id, + const char* coords, + kparms* kp) { - return KINEMATICS_BOTH; // set as required - // Note: If kinematics are identity, using KINEMATICS_BOTH - // may be used in order to allow a gui to display - // joint values in preview prior to homing -} // kinematicsType() - - -int kinematicsForward(const double *j, - EmcPose * pos, - const KINEMATICS_FORWARD_FLAGS * fflags, - KINEMATICS_INVERSE_FLAGS * iflags) + (void)comp_id; + (void)coords; + (void)kp; + return 0; // pins created by trsrnKinematicsSetup() +} // toolKinematicsSetup() + +// tool_kins==0: TCP kinematics, using the current spindle joint positions +// tool_kins==1: TOOL kinematics, using the angles calculated in remap.py +static int trsrnForward(const double *j, EmcPose * pos, int tool_kins) { - (void)fflags; - (void)iflags; - // START of custom variable declaration for Forward kinematics // geometric offsets of the universal spindle head as defined in the ini file @@ -207,20 +132,7 @@ int kinematicsForward(const double *j, // END of custom variable declaration for Forward kinematics - // define forward kinematic models using case structure for - // for switchable kinematics - switch (switchkins_type) { - case 0: // ========================= IDENTITY kinematics FORWARD ====================== - pos->tran.x = j[0]; - pos->tran.y = j[1]; - pos->tran.z = j[2]; - pos->a = j[3]; - pos->b = j[4]; - pos->c = j[5]; - - break; - - case 1: // ========================= TCP kinematics FORWARD + if (!tool_kins) { // ========================= TCP kinematics FORWARD // in TCP we use the current positions of the spindle joints Ss = sin(j[4]*TO_RAD); Cs = cos(j[4]*TO_RAD); @@ -263,9 +175,7 @@ int kinematicsForward(const double *j, pos->b = j[4]; pos->c = j[5]; - break; - - case 2: // ========================= TOOL kinematics FORWARD + } else { // ========================= TOOL kinematics FORWARD // in TOOL kinematics we use the articulated joint positions from the TWP Ss = sin(theta_2*TO_RAD); Cs = cos(theta_2*TO_RAD); @@ -303,10 +213,6 @@ int kinematicsForward(const double *j, pos->a = j[3]; pos->b = j[4]; pos->c = j[5]; - - break; - - } // unused coordinates: pos->u = 0; @@ -314,98 +220,30 @@ int kinematicsForward(const double *j, pos->w = 0; return 0; -} // kinematicsForward() +} // trsrnForward() -// These modules do not link kins_util.c, so they cannot reach the shared -// TOOL_FRAME_SPINDLE: a kernel module has to resolve its own symbols. -static void frame_square_with_machine(PmRotationMatrix *rot) -{ - rot->x.x = 1; rot->y.x = 0; rot->z.x = 0; - rot->x.y = 0; rot->y.y = 1; rot->z.y = 0; - rot->x.z = 0; rot->y.z = 0; rot->z.z = 1; -} - -int kinematicsToolFrame(const double *j, - PmRotationMatrix *rot, - const KINEMATICS_FORWARD_FLAGS *fflags) +static int tcpKinematicsForward(const double *j, + EmcPose * pos, + const KINEMATICS_FORWARD_FLAGS * fflags, + KINEMATICS_INVERSE_FLAGS * iflags) { (void)fflags; - double nu = hal_get_real(haldata->nut_angle); // degrees - double Sv = sin(nu*TO_RAD); - double Cv = cos(nu*TO_RAD); - double Ss = sin(j[4]*TO_RAD); - double Cs = cos(j[4]*TO_RAD); - double Sp = sin(j[5]*TO_RAD); - double Cp = cos(j[5]*TO_RAD); - double r = Cs + Sv*Sv*(1-Cs); - double s = Cs + Cv*Cv*(1-Cs); - double t = Sv*Cv*(1-Cs); - int a, b, k; - - // identity kinematics, and tool kinematics where the world axes are the - // tool axes by construction, both leave the tool square with the machine - if (switchkins_type != 1) { - frame_square_with_machine(rot); - return 0; - } - - // the primary joint turns the head about z - const double Rp[3][3] = {{Cp, -Sp, 0}, {Sp, Cp, 0}, {0, 0, 1}}; - - // the nutating secondary joint - const double Rs[3][3] = {{Cs, -Cv*Ss, Sv*Ss}, - {Cv*Ss, r, t}, - {-Sv*Ss, t, s}}; - - double M[3][3]; - for (a = 0; a < 3; a++) { - for (b = 0; b < 3; b++) { - M[a][b] = 0; - for (k = 0; k < 3; k++) { M[a][b] += Rp[a][k] * Rs[k][b]; } - } - } - - rot->x.x = M[0][0]; rot->y.x = M[0][1]; rot->z.x = M[0][2]; - rot->x.y = M[1][0]; rot->y.y = M[1][1]; rot->z.y = M[1][2]; - rot->x.z = M[2][0]; rot->y.z = M[2][1]; rot->z.z = M[2][2]; - - return 0; -} // kinematicsToolFrame() + (void)iflags; + return trsrnForward(j, pos, 0); +} // tcpKinematicsForward() -int kinematicsWorkFrame(const double *j, - PmRotationMatrix *rot, - const KINEMATICS_FORWARD_FLAGS *fflags) +static int toolKinematicsForward(const double *j, + EmcPose * pos, + const KINEMATICS_FORWARD_FLAGS * fflags, + KINEMATICS_INVERSE_FLAGS * iflags) { (void)fflags; - double Sw = sin(j[3]*TO_RAD); - double Cw = cos(j[3]*TO_RAD); - - // in tool kinematics the world axes are the tool axes, so the work is not - // being reported against the machine and there is nothing to turn - if (switchkins_type != 1) { - frame_square_with_machine(rot); - return 0; - } - - // the A joint carries the work: its frame in machine coordinates - // is a rotation about x by the joint value - const double W[3][3] = {{1, 0, 0}, {0, Cw, Sw}, {0, -Sw, Cw}}; - - rot->x.x = W[0][0]; rot->y.x = W[0][1]; rot->z.x = W[0][2]; - rot->x.y = W[1][0]; rot->y.y = W[1][1]; rot->z.y = W[1][2]; - rot->x.z = W[2][0]; rot->y.z = W[2][1]; rot->z.z = W[2][2]; - - return 0; -} // kinematicsWorkFrame() - -int kinematicsInverse(const EmcPose * pos, - double *j, - const KINEMATICS_INVERSE_FLAGS * iflags, - KINEMATICS_FORWARD_FLAGS * fflags) -{ (void)iflags; - (void)fflags; + return trsrnForward(j, pos, 1); +} // toolKinematicsForward() +static int trsrnInverse(const EmcPose * pos, double *j, int tool_kins) +{ // START of custom variable declaration for Forward kinematics // geometric offsets of the universal spindle head as defined in the ini file @@ -453,23 +291,7 @@ int kinematicsInverse(const EmcPose * pos, // END of custom variable declaration for Forward kinematics - // Update the kinematic joints specified by the - // [KINS]JOINTS setting (4 required for this template). - // define forward kinematic models using case structure for - // for switchable kinematics - switch (switchkins_type) { - - case 0: // ========================= IDENTITY kinematics INVERSE ====================== - j[0] = pos->tran.x; - j[1] = pos->tran.y; - j[2] = pos->tran.z; - j[3] = pos->a; - j[4] = pos->b; - j[5] = pos->c; - - break; - - case 1: // ========================= TCP kinematics INVERSE + if (!tool_kins) { // ========================= TCP kinematics INVERSE // in TCP we use the current positions of the spindle joints Ss = sin(j[4]*TO_RAD); Cs = cos(j[4]*TO_RAD); @@ -506,9 +328,7 @@ int kinematicsInverse(const EmcPose * pos, j[4] = pos->b; j[5] = pos->c; - break; - - case 2: // ========================= TOOL kinematics INVERSE + } else { // ========================= TOOL kinematics INVERSE // in TOOL kinematics we use the articulated joint positions from the TWP Ss = sin(theta_2*TO_RAD); Cs = cos(theta_2*TO_RAD); @@ -550,38 +370,112 @@ int kinematicsInverse(const EmcPose * pos, j[3] = pos->a; j[4] = pos->b; j[5] = pos->c; + } + + return 0; +} // trsrnInverse() - break; +static int tcpKinematicsInverse(const EmcPose * pos, + double *j, + const KINEMATICS_INVERSE_FLAGS * iflags, + KINEMATICS_FORWARD_FLAGS * fflags) +{ + (void)iflags; + (void)fflags; + return trsrnInverse(pos, j, 0); +} // tcpKinematicsInverse() + +static int toolKinematicsInverse(const EmcPose * pos, + double *j, + const KINEMATICS_INVERSE_FLAGS * iflags, + KINEMATICS_FORWARD_FLAGS * fflags) +{ + (void)iflags; + (void)fflags; + return trsrnInverse(pos, j, 1); +} // toolKinematicsInverse() + +// The head answers in the convention already, so the native rotation +// registered with these frames is TOOL_FRAME_SPINDLE. +static int tcpKinematicsToolFrame(const double *j, + PmRotationMatrix *rot, + const KINEMATICS_FORWARD_FLAGS *fflags) +{ + (void)fflags; + double nu = hal_get_real(haldata->nut_angle); // degrees + double Sv = sin(nu*TO_RAD); + double Cv = cos(nu*TO_RAD); + double Ss = sin(j[4]*TO_RAD); + double Cs = cos(j[4]*TO_RAD); + double Sp = sin(j[5]*TO_RAD); + double Cp = cos(j[5]*TO_RAD); + double r = Cs + Sv*Sv*(1-Cs); + double s = Cs + Cv*Cv*(1-Cs); + double t = Sv*Cv*(1-Cs); + int a, b, k; + + // the primary joint turns the head about z + const double Rp[3][3] = {{Cp, -Sp, 0}, {Sp, Cp, 0}, {0, 0, 1}}; + + // the nutating secondary joint + const double Rs[3][3] = {{Cs, -Cv*Ss, Sv*Ss}, + {Cv*Ss, r, t}, + {-Sv*Ss, t, s}}; + + double M[3][3]; + for (a = 0; a < 3; a++) { + for (b = 0; b < 3; b++) { + M[a][b] = 0; + for (k = 0; k < 3; k++) { M[a][b] += Rp[a][k] * Rs[k][b]; } + } } + rot->x.x = M[0][0]; rot->y.x = M[0][1]; rot->z.x = M[0][2]; + rot->x.y = M[1][0]; rot->y.y = M[1][1]; rot->z.y = M[1][2]; + rot->x.z = M[2][0]; rot->y.z = M[2][1]; rot->z.z = M[2][2]; + return 0; -} // kinematicsInverse() +} // tcpKinematicsToolFrame() -int kinematicsJacobian(const double *j, - const EmcPose * pos, - double jac[EMCMOT_MAX_JOINTS][EMCMOT_MAX_AXIS], - const KINEMATICS_INVERSE_FLAGS * iflags) +static int tcpKinematicsWorkFrame(const double *j, + PmRotationMatrix *rot, + const KINEMATICS_FORWARD_FLAGS *fflags) +{ + (void)fflags; + double Sw = sin(j[3]*TO_RAD); + double Cw = cos(j[3]*TO_RAD); + + // the A joint carries the work: its frame in machine coordinates + // is a rotation about x by the joint value + const double W[3][3] = {{1, 0, 0}, {0, Cw, Sw}, {0, -Sw, Cw}}; + + rot->x.x = W[0][0]; rot->y.x = W[0][1]; rot->z.x = W[0][2]; + rot->x.y = W[1][0]; rot->y.y = W[1][1]; rot->z.y = W[1][2]; + rot->x.z = W[2][0]; rot->y.z = W[2][1]; rot->z.z = W[2][2]; + + return 0; +} // tcpKinematicsWorkFrame() + +static int tcpKinematicsJacobian(const double *j, + const EmcPose * pos, + double jac[EMCMOT_MAX_JOINTS][EMCMOT_MAX_AXIS], + const KINEMATICS_INVERSE_FLAGS * iflags) { (void)j; (void)iflags; - // the same geometry as kinematicsInverse(), read the same way + // the same geometry as trsrnInverse(), read the same way double Ly = hal_get_real(haldata->y_pivot); double Lz = hal_get_real(haldata->z_pivot); double Dx = hal_get_real(haldata->x_offset); double Dy = hal_get_real(haldata->y_offset); double Dray = hal_get_real(haldata->y_rot_axis) - (Dy + Ly); double Draz = hal_get_real(haldata->z_rot_axis) - Lz; - double tc = hal_get_real(haldata->pre_rot); double nu = hal_get_real(haldata->nut_angle); // degrees - double theta_1 = hal_get_real(haldata->prim_angle); // degrees - double theta_2 = hal_get_real(haldata->sec_angle); // degrees double Dt = hal_get_real(haldata->tool_offset_z); double Sv = sin(nu*TO_RAD); double Cv = cos(nu*TO_RAD); - double Stc = sin(tc); - double Ctc = cos(tc); // The TCP inverse reads the rotary angles from its joint argument, // where the machine is, and its own pose words for the same angles @@ -589,105 +483,152 @@ int kinematicsJacobian(const double *j, // against the pose, which is what a consumer multiplies by. double Sw = sin(pos->a*TO_RAD); double Cw = cos(pos->a*TO_RAD); - double Ss = 0, Cs = 0, Sp = 0, Cp = 0; - double CvSs = 0, SvSs = 0, r = 0, s = 0, t = 0; + double Ss = sin(pos->b*TO_RAD); + double Cs = cos(pos->b*TO_RAD); + double Sp = sin(pos->c*TO_RAD); + double Cp = cos(pos->c*TO_RAD); + double CvSs = Cv*Ss; + double SvSs = Sv*Ss; + double r = Cs + Sv*Sv*(1-Cs); + double t = Sv*Cv*(1-Cs); // derivatives of the above over the secondary angle (Ss, r, s, t, CvSs, // SvSs) and the primary angle (Sp, Cp), per degree - double dSs = 0, dr = 0, ds = 0, dt_ = 0, dCvSs = 0, dSvSs = 0; - double dSp = 0, dCp = 0; + double dSs = Cs*TO_RAD; + double dr = -Ss*Cv*Cv*TO_RAD; + double ds = -Ss*Sv*Sv*TO_RAD; + double dt_ = Sv*Cv*Ss*TO_RAD; + double dCvSs = Cv*dSs; + double dSvSs = Sv*dSs; + double dSp = Cp*TO_RAD; + double dCp = -Sp*TO_RAD; double Qy = pos->tran.y; double Qz = pos->tran.z; - double Ay, Az; // the two lever arms the table turns about + // the two lever arms the table turns about + double Ay = Dray + Dy + Ly - Qy; + double Az = Draz + Dt + Lz - Qz; int R, C; for (R = 0; R < EMCMOT_MAX_JOINTS; R++) { for (C = 0; C < EMCMOT_MAX_AXIS; C++) { jac[R][C] = 0; } } - switch (switchkins_type) { + // j[0]: Qx plus terms in the head angles only + jac[0][0] = 1; + jac[0][4] = (Cp*dSvSs - Sp*dt_)*(Dt + Lz) - (Cp*dCvSs + Sp*dr)*Ly; + jac[0][5] = (dCp*SvSs - dSp*t)*(Dt + Lz) + dCp*Dx + - (dCp*CvSs + dSp*r)*Ly - Dy*dSp; + + // j[1]: -Cw*Ay - Az*Sw plus head terms + jac[1][1] = Cw; + jac[1][2] = Sw; + jac[1][3] = ( Sw*Ay - Az*Cw)*TO_RAD; + jac[1][4] = (Sp*dSvSs + Cp*dt_)*(Dt + Lz) - (dCvSs*Sp - Cp*dr)*Ly; + jac[1][5] = dCp*Dy + Dx*dSp + (dSp*SvSs + dCp*t)*(Dt + Lz) + - (CvSs*dSp - dCp*r)*Ly; + + // j[2]: -Cw*Az + Ay*Sw plus head terms + jac[2][1] = -Sw; + jac[2][2] = Cw; + jac[2][3] = ( Sw*Az + Ay*Cw)*TO_RAD; + jac[2][4] = (Dt + Lz)*ds + Ly*dt_; + + jac[3][3] = 1; + jac[4][4] = 1; + jac[5][5] = 1; + return 0; +} // tcpKinematicsJacobian() - case 0: // ========================= IDENTITY kinematics JACOBIAN ==================== - for (R = 0; R < 6; R++) { jac[R][R] = 1; } - break; +static int toolKinematicsJacobian(const double *j, + const EmcPose * pos, + double jac[EMCMOT_MAX_JOINTS][EMCMOT_MAX_AXIS], + const KINEMATICS_INVERSE_FLAGS * iflags) +{ + (void)j; + (void)pos; + (void)iflags; - case 1: // ========================= TCP kinematics JACOBIAN - Ss = sin(pos->b*TO_RAD); - Cs = cos(pos->b*TO_RAD); - Sp = sin(pos->c*TO_RAD); - Cp = cos(pos->c*TO_RAD); - CvSs = Cv*Ss; - SvSs = Sv*Ss; - r = Cs + Sv*Sv*(1-Cs); - s = Cs + Cv*Cv*(1-Cs); - t = Sv*Cv*(1-Cs); + // the head angles come from pins, so the inverse is linear in the pose + // and the rows are its coefficients + double tc = hal_get_real(haldata->pre_rot); + double nu = hal_get_real(haldata->nut_angle); // degrees + double theta_1 = hal_get_real(haldata->prim_angle); // degrees + double theta_2 = hal_get_real(haldata->sec_angle); // degrees - dSs = Cs*TO_RAD; - dr = -Ss*Cv*Cv*TO_RAD; - ds = -Ss*Sv*Sv*TO_RAD; - dt_ = Sv*Cv*Ss*TO_RAD; - dCvSs = Cv*dSs; - dSvSs = Sv*dSs; - dSp = Cp*TO_RAD; - dCp = -Sp*TO_RAD; - - Ay = Dray + Dy + Ly - Qy; - Az = Draz + Dt + Lz - Qz; - - // j[0]: Qx plus terms in the head angles only - jac[0][0] = 1; - jac[0][4] = (Cp*dSvSs - Sp*dt_)*(Dt + Lz) - (Cp*dCvSs + Sp*dr)*Ly; - jac[0][5] = (dCp*SvSs - dSp*t)*(Dt + Lz) + dCp*Dx - - (dCp*CvSs + dSp*r)*Ly - Dy*dSp; - - // j[1]: -Cw*Ay - Az*Sw plus head terms - jac[1][1] = Cw; - jac[1][2] = Sw; - jac[1][3] = ( Sw*Ay - Az*Cw)*TO_RAD; - jac[1][4] = (Sp*dSvSs + Cp*dt_)*(Dt + Lz) - (dCvSs*Sp - Cp*dr)*Ly; - jac[1][5] = dCp*Dy + Dx*dSp + (dSp*SvSs + dCp*t)*(Dt + Lz) - - (CvSs*dSp - dCp*r)*Ly; - - // j[2]: -Cw*Az + Ay*Sw plus head terms - jac[2][1] = -Sw; - jac[2][2] = Cw; - jac[2][3] = ( Sw*Az + Ay*Cw)*TO_RAD; - jac[2][4] = (Dt + Lz)*ds + Ly*dt_; - - jac[3][3] = 1; - jac[4][4] = 1; - jac[5][5] = 1; - break; - - case 2: // ========================= TOOL kinematics JACOBIAN - // the head angles come from pins, so the inverse is linear in - // the pose and the rows are its coefficients - Ss = sin(theta_2*TO_RAD); - Cs = cos(theta_2*TO_RAD); - Sp = sin(theta_1*TO_RAD); - Cp = cos(theta_1*TO_RAD); - CvSs = Cv*Ss; - SvSs = Sv*Ss; - r = Cs + Sv*Sv*(1-Cs); - s = Cs + Cv*Cv*(1-Cs); - t = Sv*Cv*(1-Cs); + double Sv = sin(nu*TO_RAD); + double Cv = cos(nu*TO_RAD); + double Stc = sin(tc); + double Ctc = cos(tc); + double Ss = sin(theta_2*TO_RAD); + double Cs = cos(theta_2*TO_RAD); + double Sp = sin(theta_1*TO_RAD); + double Cp = cos(theta_1*TO_RAD); + double CvSs = Cv*Ss; + double SvSs = Sv*Ss; + double r = Cs + Sv*Sv*(1-Cs); + double s = Cs + Cv*Cv*(1-Cs); + double t = Sv*Cv*(1-Cs); + int R, C; - jac[0][0] = ((Cp*Cs - CvSs*Sp)*Ctc - (Cp*CvSs + Sp*r)*Stc); - jac[0][1] = -((Cp*CvSs + Sp*r)*Ctc + (Cp*Cs - CvSs*Sp)*Stc); - jac[0][2] = (Cp*SvSs - Sp*t); + for (R = 0; R < EMCMOT_MAX_JOINTS; R++) { + for (C = 0; C < EMCMOT_MAX_AXIS; C++) { jac[R][C] = 0; } + } - jac[1][0] = ((Cp*CvSs + Cs*Sp)*Ctc - (CvSs*Sp - Cp*r)*Stc); - jac[1][1] = -((CvSs*Sp - Cp*r)*Ctc + (Cp*CvSs + Cs*Sp)*Stc); - jac[1][2] = (Sp*SvSs + Cp*t); + jac[0][0] = ((Cp*Cs - CvSs*Sp)*Ctc - (Cp*CvSs + Sp*r)*Stc); + jac[0][1] = -((Cp*CvSs + Sp*r)*Ctc + (Cp*Cs - CvSs*Sp)*Stc); + jac[0][2] = (Cp*SvSs - Sp*t); - jac[2][0] = -(Ctc*SvSs - Stc*t); - jac[2][1] = (Stc*SvSs + Ctc*t); - jac[2][2] = s; + jac[1][0] = ((Cp*CvSs + Cs*Sp)*Ctc - (CvSs*Sp - Cp*r)*Stc); + jac[1][1] = -((CvSs*Sp - Cp*r)*Ctc + (Cp*CvSs + Cs*Sp)*Stc); + jac[1][2] = (Sp*SvSs + Cp*t); - jac[3][3] = 1; - jac[4][4] = 1; - jac[5][5] = 1; - break; - } + jac[2][0] = -(Ctc*SvSs - Stc*t); + jac[2][1] = (Stc*SvSs + Ctc*t); + jac[2][2] = s; + + jac[3][3] = 1; + jac[4][4] = 1; + jac[5][5] = 1; return 0; -} // kinematicsJacobian() +} // toolKinematicsJacobian() + +// halcompile has done hal_init() and does hal_ready() after this returns, +// which is what switchkinsInit() expects +EXTRA_SETUP() { + kparms kp; + (void)__comp_inst; (void)prefix; (void)extra_arg; + + kp.kinsname = "xyzacb_trsrn"; + kp.halprefix = "xyzacb_trsrn_kins"; + kp.required_coordinates = "xyzabc"; + kp.allow_duplicates = 0; + kp.fwd_iterates_mask = 0; + kp.gui_kinstype = -1; + kp.sparm = NULL; + kp.max_joints = strlen(kp.required_coordinates); + + if (switchkinsRegister(0, identityKinematicsSetup, + identityKinematicsForward, + identityKinematicsInverse)) { return -1; } + if (switchkinsRegister(1, trsrnKinematicsSetup, + tcpKinematicsForward, + tcpKinematicsInverse)) { return -1; } + if (switchkinsRegister(2, toolKinematicsSetup, + toolKinematicsForward, + toolKinematicsInverse)) { return -1; } + if (switchkinsRegisterFrames(1, tcpKinematicsWorkFrame, + tcpKinematicsToolFrame, + &TOOL_FRAME_SPINDLE)) { return -1; } + if (switchkinsRegisterJacobian(1, tcpKinematicsJacobian)) { return -1; } + // the tool kinematics report in tool axes, so the tool is square with + // the world by construction and nothing turns the work against it + if (switchkinsRegisterFrames(2, identityKinematicsWorkFrame, + identityKinematicsToolFrame, + &TOOL_FRAME_SPINDLE)) { return -1; } + if (switchkinsRegisterJacobian(2, toolKinematicsJacobian)) { return -1; } + + if (switchkinsDeclare(0, KINSTYPE_IDENTITY)) { return -1; } + if (switchkinsDeclare(1, KINSTYPE_PRIMARY)) { return -1; } + + return switchkinsInit(comp_id, &kp, coordinates); +} // EXTRA_SETUP() diff --git a/src/hal/components/xyzbca_trsrn.comp b/src/hal/components/xyzbca_trsrn.comp index 10126165eb1..75e6f7ea0f4 100644 --- a/src/hal/components/xyzbca_trsrn.comp +++ b/src/hal/components/xyzbca_trsrn.comp @@ -4,6 +4,11 @@ description """ FIXME +The kinematics-type switching, the *kinstype.is-N* pins and the +joints-to-coordinates mapping are provided by switchkins.c, so the +*coordinates=* module parameter and the kinematics switching described in +the switchkins document chapter apply here too. + """; pin out si32 dummy=0 "dummy pin to satisfy halcompile"; option period no; @@ -14,8 +19,11 @@ author "David Mueller"; ;; #include -#include +#include + +static char *coordinates; +RTAPI_MP_STRING(coordinates, "Axes-to-joints-ordering"); static struct haldata { // these should be parameters really but we want to be able to @@ -36,135 +44,50 @@ static struct haldata { // Declare hal pin pointers used for xyzbca_trsrn kinematics: hal_real_t tool_offset_z; - - //Declare hal pin pointers used for switchable kinematics - hal_bool_t kinstype_is_0; - hal_bool_t kinstype_is_1; - hal_bool_t kinstype_is_2; } *haldata; - -EXTRA_SETUP() { - (void)__comp_inst; - (void)prefix; - (void)extra_arg; -#define HAL_PREFIX "xyzbca_trsrn_kins" - int res=0; - // inbherit comp_id from rtapi_main() - if (comp_id < 0) goto error; - // set unready to allow creation of pins - if (hal_set_unready(comp_id)) goto error; - - haldata = hal_malloc(sizeof(struct haldata)); - if (!haldata) goto error; - - // hal pins required for xyzbca_trsrn kinematics: - res += hal_pin_new_real(comp_id, HAL_IN, &haldata->tool_offset_z, 0.0, "%s.tool-offset-z", HAL_PREFIX); - res += hal_pin_new_real(comp_id, HAL_IN, &haldata->x_pivot, 0.0, "%s.x-pivot", HAL_PREFIX); - res += hal_pin_new_real(comp_id, HAL_IN, &haldata->z_pivot, 0.0, "%s.z-pivot", HAL_PREFIX); - res += hal_pin_new_real(comp_id, HAL_IN, &haldata->x_offset, 0.0, "%s.x-offset", HAL_PREFIX); - res += hal_pin_new_real(comp_id, HAL_IN, &haldata->y_offset, 0.0, "%s.y-offset", HAL_PREFIX); - res += hal_pin_new_real(comp_id, HAL_IN, &haldata->x_rot_axis, 0.0, "%s.x-rot-axis", HAL_PREFIX); - res += hal_pin_new_real(comp_id, HAL_IN, &haldata->z_rot_axis, 0.0, "%s.z-rot-axis", HAL_PREFIX); - res += hal_pin_new_real(comp_id, HAL_IN, &haldata->pre_rot, 0.0, "%s.pre-rot", HAL_PREFIX); - res += hal_pin_new_real(comp_id, HAL_IN, &haldata->nut_angle, 0.0, "%s.nut-angle", HAL_PREFIX); - res += hal_pin_new_real(comp_id, HAL_IN, &haldata->prim_angle, 0.0, "%s.primary-angle", HAL_PREFIX); - res += hal_pin_new_real(comp_id, HAL_IN, &haldata->sec_angle, 0.0, "%s.secondary-angle", HAL_PREFIX); - - // hal pins required for switchable kinematics: - //default at startup -> identity kinematics - //-> xyzabc TCP - //-> xyzabc TOOL - res += hal_pin_new_bool(comp_id, HAL_OUT, &haldata->kinstype_is_0, 1, "kinstype.is-0"); - res += hal_pin_new_bool(comp_id, HAL_OUT, &haldata->kinstype_is_1, 0, "kinstype.is-1"); - res += hal_pin_new_bool(comp_id, HAL_OUT, &haldata->kinstype_is_2, 0, "kinstype.is-2"); - - if (res) goto error; - rtapi_print("*** %s setup ok\n",__FILE__); - return 0; -error: - rtapi_print("\n!!! %s setup failed res=%d\n\n",__FILE__,res); - return -1; -#undef HAL_PREFIX -} - -EXPORT_SYMBOL(kinematicsType); -EXPORT_SYMBOL(kinematicsSwitchable); -EXPORT_SYMBOL(kinematicsSwitch); -EXPORT_SYMBOL(kinematicsTypeFlags); -EXPORT_SYMBOL(kinematicsInverse); -EXPORT_SYMBOL(kinematicsJacobian); -EXPORT_SYMBOL(kinematicsForward); -EXPORT_SYMBOL(kinematicsToolFrame); -EXPORT_SYMBOL(kinematicsWorkFrame); - -static rtapi_u32 switchkins_type; - -int kinematicsSwitchable() {return 1;} - -int kinematicsTypeFlags(int ktype) +// the pins are shared by the TCP and TOOL kinematics; the TOOL type has +// no setup routine of its own +static int trsrnKinematicsSetup(const int comp_id, + const char* coords, + kparms* kp) { - switch (ktype) { - case 0: return KINSTYPE_IDENTITY; - case 1: return KINSTYPE_PRIMARY; - default: return -1; - } -} - + int res = 0; + (void)coords; + haldata = hal_malloc(sizeof(struct haldata)); + if (!haldata) return -1; + + res += hal_pin_new_real(comp_id, HAL_IN, &haldata->tool_offset_z, 0.0, "%s.tool-offset-z", kp->halprefix); + res += hal_pin_new_real(comp_id, HAL_IN, &haldata->x_pivot, 0.0, "%s.x-pivot", kp->halprefix); + res += hal_pin_new_real(comp_id, HAL_IN, &haldata->z_pivot, 0.0, "%s.z-pivot", kp->halprefix); + res += hal_pin_new_real(comp_id, HAL_IN, &haldata->x_offset, 0.0, "%s.x-offset", kp->halprefix); + res += hal_pin_new_real(comp_id, HAL_IN, &haldata->y_offset, 0.0, "%s.y-offset", kp->halprefix); + res += hal_pin_new_real(comp_id, HAL_IN, &haldata->x_rot_axis, 0.0, "%s.x-rot-axis", kp->halprefix); + res += hal_pin_new_real(comp_id, HAL_IN, &haldata->z_rot_axis, 0.0, "%s.z-rot-axis", kp->halprefix); + res += hal_pin_new_real(comp_id, HAL_IN, &haldata->pre_rot, 0.0, "%s.pre-rot", kp->halprefix); + res += hal_pin_new_real(comp_id, HAL_IN, &haldata->nut_angle, 0.0, "%s.nut-angle", kp->halprefix); + res += hal_pin_new_real(comp_id, HAL_IN, &haldata->prim_angle, 0.0, "%s.primary-angle", kp->halprefix); + res += hal_pin_new_real(comp_id, HAL_IN, &haldata->sec_angle, 0.0, "%s.secondary-angle", kp->halprefix); + if (res) return -1; -int kinematicsSwitch(int new_switchkins_type) -{ - switchkins_type = new_switchkins_type; - rtapi_print("kinematicsSwitch(): type=%d\n",switchkins_type); - // create case structure for switchable kinematics - switch (switchkins_type) { - case 0: rtapi_print_msg(RTAPI_MSG_INFO, - "kinematicsSwitch:TYPE0\n"); - hal_set_bool(haldata->kinstype_is_0, 1); - hal_set_bool(haldata->kinstype_is_1, 0); - hal_set_bool(haldata->kinstype_is_2, 0); - break; - case 1: rtapi_print_msg(RTAPI_MSG_INFO, - "kinematicsSwitch:TYPE1\n"); - hal_set_bool(haldata->kinstype_is_0, 0); - hal_set_bool(haldata->kinstype_is_1, 1); - hal_set_bool(haldata->kinstype_is_2, 0); - break; - case 2: rtapi_print_msg(RTAPI_MSG_INFO, - "kinematicsSwitch:TYPE1\n"); - hal_set_bool(haldata->kinstype_is_0, 0); - hal_set_bool(haldata->kinstype_is_1, 0); - hal_set_bool(haldata->kinstype_is_2, 1); - break; - default: rtapi_print_msg(RTAPI_MSG_ERR, - "kinematicsSwitch:BAD VALUE <%d>\n", - switchkins_type); - hal_set_bool(haldata->kinstype_is_1, 0); - hal_set_bool(haldata->kinstype_is_0, 0); - hal_set_bool(haldata->kinstype_is_2, 0); - return -1; // FAIL - } - return 0; // ok -} + return 0; +} // trsrnKinematicsSetup() -KINEMATICS_TYPE kinematicsType() +static int toolKinematicsSetup(const int comp_id, + const char* coords, + kparms* kp) { - return KINEMATICS_BOTH; // set as required - // Note: If kinematics are identity, using KINEMATICS_BOTH - // may be used in order to allow a gui to display - // joint values in preview prior to homing -} // kinematicsType() - - -int kinematicsForward(const double *j, - EmcPose * pos, - const KINEMATICS_FORWARD_FLAGS * fflags, - KINEMATICS_INVERSE_FLAGS * iflags) + (void)comp_id; + (void)coords; + (void)kp; + return 0; // pins created by trsrnKinematicsSetup() +} // toolKinematicsSetup() + +// tool_kins==0: TCP kinematics, using the current spindle joint positions +// tool_kins==1: TOOL kinematics, using the angles calculated in remap.py +static int trsrnForward(const double *j, EmcPose * pos, int tool_kins) { - (void)fflags; - (void)iflags; - // START of custom variable declaration for Forward kinematics // geometric offsets of the universal spindle head as defined in the ini file @@ -210,20 +133,7 @@ int kinematicsForward(const double *j, // END of custom variable declaration for Forward kinematics - // define forward kinematic models using case structure for - // for switchable kinematics - switch (switchkins_type) { - case 0: // ========================= IDENTITY kinematics FORWARD ====================== - pos->tran.x = j[0]; - pos->tran.y = j[1]; - pos->tran.z = j[2]; - pos->a = j[3]; - pos->b = j[4]; - pos->c = j[5]; - - break; - - case 1: // ========================= TCP kinematics FORWARD + if (!tool_kins) { // ========================= TCP kinematics FORWARD // in TCP we use the current positions of the spindle joints Ss = sin(j[3]*TO_RAD); Cs = cos(j[3]*TO_RAD); @@ -270,9 +180,7 @@ int kinematicsForward(const double *j, pos->b = j[4]; pos->c = j[5]; - break; - - case 2: // ========================= TOOL kinematics FORWARD + } else { // ========================= TOOL kinematics FORWARD // in TOOL kinematics we use the articulated joint positions from the TWP Ss = sin(theta_2*TO_RAD); Cs = cos(theta_2*TO_RAD); @@ -310,10 +218,6 @@ int kinematicsForward(const double *j, pos->a = j[3]; pos->b = j[4]; pos->c = j[5]; - - break; - - } // unused coordinates: pos->u = 0; @@ -321,98 +225,30 @@ int kinematicsForward(const double *j, pos->w = 0; return 0; -} // kinematicsForward() +} // trsrnForward() -// These modules do not link kins_util.c, so they cannot reach the shared -// TOOL_FRAME_SPINDLE: a kernel module has to resolve its own symbols. -static void frame_square_with_machine(PmRotationMatrix *rot) -{ - rot->x.x = 1; rot->y.x = 0; rot->z.x = 0; - rot->x.y = 0; rot->y.y = 1; rot->z.y = 0; - rot->x.z = 0; rot->y.z = 0; rot->z.z = 1; -} - -int kinematicsToolFrame(const double *j, - PmRotationMatrix *rot, - const KINEMATICS_FORWARD_FLAGS *fflags) +static int tcpKinematicsForward(const double *j, + EmcPose * pos, + const KINEMATICS_FORWARD_FLAGS * fflags, + KINEMATICS_INVERSE_FLAGS * iflags) { (void)fflags; - double nu = hal_get_real(haldata->nut_angle); // degrees - double Sv = sin(nu*TO_RAD); - double Cv = cos(nu*TO_RAD); - double Ss = sin(j[3]*TO_RAD); - double Cs = cos(j[3]*TO_RAD); - double Sp = sin(j[5]*TO_RAD); - double Cp = cos(j[5]*TO_RAD); - double r = Cs + Sv*Sv*(1-Cs); - double s = Cs + Cv*Cv*(1-Cs); - double t = Sv*Cv*(1-Cs); - int a, b, k; - - // identity kinematics, and tool kinematics where the world axes are the - // tool axes by construction, both leave the tool square with the machine - if (switchkins_type != 1) { - frame_square_with_machine(rot); - return 0; - } - - // the primary joint turns the head about z - const double Rp[3][3] = {{Cp, -Sp, 0}, {Sp, Cp, 0}, {0, 0, 1}}; - - // the nutating secondary joint - const double Rs[3][3] = {{r, -Cv*Ss, t}, - {Cv*Ss, Cs, -Sv*Ss}, - {t, Sv*Ss, s}}; - - double M[3][3]; - for (a = 0; a < 3; a++) { - for (b = 0; b < 3; b++) { - M[a][b] = 0; - for (k = 0; k < 3; k++) { M[a][b] += Rp[a][k] * Rs[k][b]; } - } - } - - rot->x.x = M[0][0]; rot->y.x = M[0][1]; rot->z.x = M[0][2]; - rot->x.y = M[1][0]; rot->y.y = M[1][1]; rot->z.y = M[1][2]; - rot->x.z = M[2][0]; rot->y.z = M[2][1]; rot->z.z = M[2][2]; - - return 0; -} // kinematicsToolFrame() + (void)iflags; + return trsrnForward(j, pos, 0); +} // tcpKinematicsForward() -int kinematicsWorkFrame(const double *j, - PmRotationMatrix *rot, - const KINEMATICS_FORWARD_FLAGS *fflags) +static int toolKinematicsForward(const double *j, + EmcPose * pos, + const KINEMATICS_FORWARD_FLAGS * fflags, + KINEMATICS_INVERSE_FLAGS * iflags) { (void)fflags; - double Sw = sin(j[4]*TO_RAD); - double Cw = cos(j[4]*TO_RAD); - - // in tool kinematics the world axes are the tool axes, so the work is not - // being reported against the machine and there is nothing to turn - if (switchkins_type != 1) { - frame_square_with_machine(rot); - return 0; - } - - // the B joint carries the work: its frame in machine coordinates - // is a rotation about y by the joint value - const double W[3][3] = {{Cw, 0, -Sw}, {0, 1, 0}, {Sw, 0, Cw}}; - - rot->x.x = W[0][0]; rot->y.x = W[0][1]; rot->z.x = W[0][2]; - rot->x.y = W[1][0]; rot->y.y = W[1][1]; rot->z.y = W[1][2]; - rot->x.z = W[2][0]; rot->y.z = W[2][1]; rot->z.z = W[2][2]; - - return 0; -} // kinematicsWorkFrame() - -int kinematicsInverse(const EmcPose * pos, - double *j, - const KINEMATICS_INVERSE_FLAGS * iflags, - KINEMATICS_FORWARD_FLAGS * fflags) -{ (void)iflags; - (void)fflags; + return trsrnForward(j, pos, 1); +} // toolKinematicsForward() +static int trsrnInverse(const EmcPose * pos, double *j, int tool_kins) +{ // START of custom variable declaration for Forward kinematics // geometric offsets of the universal spindle head as defined in the ini file @@ -458,23 +294,7 @@ int kinematicsInverse(const EmcPose * pos, // END of custom variable declaration for Forward kinematics - // Update the kinematic joints specified by the - // [KINS]JOINTS setting (4 required for this template). - // define forward kinematic models using case structure for - // for switchable kinematics - switch (switchkins_type) { - - case 0: // ========================= IDENTITY kinematics INVERSE ====================== - j[0] = pos->tran.x; - j[1] = pos->tran.y; - j[2] = pos->tran.z; - j[3] = pos->a; - j[4] = pos->b; - j[5] = pos->c; - - break; - - case 1: // ========================= TCP kinematics INVERSE + if (!tool_kins) { // ========================= TCP kinematics INVERSE // in TCP we use the current positions of the spindle joints Ss = sin(j[3]*TO_RAD); Cs = cos(j[3]*TO_RAD); @@ -511,9 +331,7 @@ int kinematicsInverse(const EmcPose * pos, j[4] = pos->b; j[5] = pos->c; - break; - - case 2: // ========================= TOOL kinematics INVERSE + } else { // ========================= TOOL kinematics INVERSE // in TOOL kinematics we use the articulated joint positions from the TWP Ss = sin(theta_2*TO_RAD); Cs = cos(theta_2*TO_RAD); @@ -555,38 +373,112 @@ int kinematicsInverse(const EmcPose * pos, j[3] = pos->a; j[4] = pos->b; j[5] = pos->c; + } + + return 0; +} // trsrnInverse() - break; +static int tcpKinematicsInverse(const EmcPose * pos, + double *j, + const KINEMATICS_INVERSE_FLAGS * iflags, + KINEMATICS_FORWARD_FLAGS * fflags) +{ + (void)iflags; + (void)fflags; + return trsrnInverse(pos, j, 0); +} // tcpKinematicsInverse() + +static int toolKinematicsInverse(const EmcPose * pos, + double *j, + const KINEMATICS_INVERSE_FLAGS * iflags, + KINEMATICS_FORWARD_FLAGS * fflags) +{ + (void)iflags; + (void)fflags; + return trsrnInverse(pos, j, 1); +} // toolKinematicsInverse() + +// The head answers in the convention already, so the native rotation +// registered with these frames is TOOL_FRAME_SPINDLE. +static int tcpKinematicsToolFrame(const double *j, + PmRotationMatrix *rot, + const KINEMATICS_FORWARD_FLAGS *fflags) +{ + (void)fflags; + double nu = hal_get_real(haldata->nut_angle); // degrees + double Sv = sin(nu*TO_RAD); + double Cv = cos(nu*TO_RAD); + double Ss = sin(j[3]*TO_RAD); + double Cs = cos(j[3]*TO_RAD); + double Sp = sin(j[5]*TO_RAD); + double Cp = cos(j[5]*TO_RAD); + double r = Cs + Sv*Sv*(1-Cs); + double s = Cs + Cv*Cv*(1-Cs); + double t = Sv*Cv*(1-Cs); + int a, b, k; + + // the primary joint turns the head about z + const double Rp[3][3] = {{Cp, -Sp, 0}, {Sp, Cp, 0}, {0, 0, 1}}; + + // the nutating secondary joint + const double Rs[3][3] = {{r, -Cv*Ss, t}, + {Cv*Ss, Cs, -Sv*Ss}, + {t, Sv*Ss, s}}; + + double M[3][3]; + for (a = 0; a < 3; a++) { + for (b = 0; b < 3; b++) { + M[a][b] = 0; + for (k = 0; k < 3; k++) { M[a][b] += Rp[a][k] * Rs[k][b]; } + } } + rot->x.x = M[0][0]; rot->y.x = M[0][1]; rot->z.x = M[0][2]; + rot->x.y = M[1][0]; rot->y.y = M[1][1]; rot->z.y = M[1][2]; + rot->x.z = M[2][0]; rot->y.z = M[2][1]; rot->z.z = M[2][2]; + return 0; -} // kinematicsInverse() +} // tcpKinematicsToolFrame() -int kinematicsJacobian(const double *j, - const EmcPose * pos, - double jac[EMCMOT_MAX_JOINTS][EMCMOT_MAX_AXIS], - const KINEMATICS_INVERSE_FLAGS * iflags) +static int tcpKinematicsWorkFrame(const double *j, + PmRotationMatrix *rot, + const KINEMATICS_FORWARD_FLAGS *fflags) +{ + (void)fflags; + double Sw = sin(j[4]*TO_RAD); + double Cw = cos(j[4]*TO_RAD); + + // the B joint carries the work: its frame in machine coordinates + // is a rotation about y by the joint value + const double W[3][3] = {{Cw, 0, -Sw}, {0, 1, 0}, {Sw, 0, Cw}}; + + rot->x.x = W[0][0]; rot->y.x = W[0][1]; rot->z.x = W[0][2]; + rot->x.y = W[1][0]; rot->y.y = W[1][1]; rot->z.y = W[1][2]; + rot->x.z = W[2][0]; rot->y.z = W[2][1]; rot->z.z = W[2][2]; + + return 0; +} // tcpKinematicsWorkFrame() + +static int tcpKinematicsJacobian(const double *j, + const EmcPose * pos, + double jac[EMCMOT_MAX_JOINTS][EMCMOT_MAX_AXIS], + const KINEMATICS_INVERSE_FLAGS * iflags) { (void)j; (void)iflags; - // the same geometry as kinematicsInverse(), read the same way + // the same geometry as trsrnInverse(), read the same way double Lx = hal_get_real(haldata->x_pivot); double Lz = hal_get_real(haldata->z_pivot); double Dx = hal_get_real(haldata->x_offset); double Dy = hal_get_real(haldata->y_offset); double Drax = hal_get_real(haldata->x_rot_axis) - Lx - Dx; double Draz = hal_get_real(haldata->z_rot_axis) - Lz; - double tc = hal_get_real(haldata->pre_rot); double nu = hal_get_real(haldata->nut_angle); // degrees - double theta_1 = hal_get_real(haldata->prim_angle); // degrees - double theta_2 = hal_get_real(haldata->sec_angle); // degrees double Dt = hal_get_real(haldata->tool_offset_z); double Sv = sin(nu*TO_RAD); double Cv = cos(nu*TO_RAD); - double Stc = sin(tc); - double Ctc = cos(tc); // The TCP inverse reads the rotary angles from its joint argument, // where the machine is, and its own pose words for the same angles @@ -594,105 +486,152 @@ int kinematicsJacobian(const double *j, // against the pose, which is what a consumer multiplies by. double Sw = sin(pos->b*TO_RAD); double Cw = cos(pos->b*TO_RAD); - double Ss = 0, Cs = 0, Sp = 0, Cp = 0; - double CvSs = 0, SvSs = 0, r = 0, s = 0, t = 0; + double Ss = sin(pos->a*TO_RAD); + double Cs = cos(pos->a*TO_RAD); + double Sp = sin(pos->c*TO_RAD); + double Cp = cos(pos->c*TO_RAD); + double CvSs = Cv*Ss; + double SvSs = Sv*Ss; + double r = Cs + Sv*Sv*(1-Cs); + double t = Sv*Cv*(1-Cs); // derivatives of the above over the secondary angle (Ss, r, s, t, CvSs, // SvSs) and the primary angle (Sp, Cp), per degree - double dSs = 0, dr = 0, ds = 0, dt_ = 0, dCvSs = 0, dSvSs = 0; - double dSp = 0, dCp = 0; + double dSs = Cs*TO_RAD; + double dr = -Ss*Cv*Cv*TO_RAD; + double ds = -Ss*Sv*Sv*TO_RAD; + double dt_ = Sv*Cv*Ss*TO_RAD; + double dCvSs = Cv*dSs; + double dSvSs = Sv*dSs; + double dSp = Cp*TO_RAD; + double dCp = -Sp*TO_RAD; double Qx = pos->tran.x; double Qz = pos->tran.z; - double Ax, Az; // the two lever arms the table turns about + // the two lever arms the table turns about + double Ax = Drax + Dx + Lx - Qx; + double Az = Draz + Dt + Lz - Qz; int R, C; for (R = 0; R < EMCMOT_MAX_JOINTS; R++) { for (C = 0; C < EMCMOT_MAX_AXIS; C++) { jac[R][C] = 0; } } - switch (switchkins_type) { + // j[0]: -Cw*Ax + Az*Sw plus head terms + jac[0][0] = Cw; + jac[0][2] = -Sw; + jac[0][3] = (Sp*dSvSs + Cp*dt_)*(Dt + Lz) - (dCvSs*Sp - Cp*dr)*Lx; + jac[0][4] = ( Sw*Ax + Az*Cw)*TO_RAD; + jac[0][5] = dCp*Dx - Dy*dSp + (dSp*SvSs + dCp*t)*(Dt + Lz) + - (CvSs*dSp - dCp*r)*Lx; + + // j[1]: Qy plus head terms + jac[1][1] = 1; + jac[1][3] = -(Cp*dSvSs - Sp*dt_)*(Dt + Lz) + (Cp*dCvSs + Sp*dr)*Lx; + jac[1][5] = -(dCp*SvSs - dSp*t)*(Dt + Lz) + dCp*Dy + + (dCp*CvSs + dSp*r)*Lx + Dx*dSp; + + // j[2]: -Cw*Az - Ax*Sw plus head terms + jac[2][0] = Sw; + jac[2][2] = Cw; + jac[2][3] = (Dt + Lz)*ds + Lx*dt_; + jac[2][4] = ( Sw*Az - Ax*Cw)*TO_RAD; + + jac[3][3] = 1; + jac[4][4] = 1; + jac[5][5] = 1; + return 0; +} // tcpKinematicsJacobian() - case 0: // ========================= IDENTITY kinematics JACOBIAN ==================== - for (R = 0; R < 6; R++) { jac[R][R] = 1; } - break; +static int toolKinematicsJacobian(const double *j, + const EmcPose * pos, + double jac[EMCMOT_MAX_JOINTS][EMCMOT_MAX_AXIS], + const KINEMATICS_INVERSE_FLAGS * iflags) +{ + (void)j; + (void)pos; + (void)iflags; - case 1: // ========================= TCP kinematics JACOBIAN - Ss = sin(pos->a*TO_RAD); - Cs = cos(pos->a*TO_RAD); - Sp = sin(pos->c*TO_RAD); - Cp = cos(pos->c*TO_RAD); - CvSs = Cv*Ss; - SvSs = Sv*Ss; - r = Cs + Sv*Sv*(1-Cs); - s = Cs + Cv*Cv*(1-Cs); - t = Sv*Cv*(1-Cs); + // the head angles come from pins, so the inverse is linear in the pose + // and the rows are its coefficients + double tc = hal_get_real(haldata->pre_rot); + double nu = hal_get_real(haldata->nut_angle); // degrees + double theta_1 = hal_get_real(haldata->prim_angle); // degrees + double theta_2 = hal_get_real(haldata->sec_angle); // degrees - dSs = Cs*TO_RAD; - dr = -Ss*Cv*Cv*TO_RAD; - ds = -Ss*Sv*Sv*TO_RAD; - dt_ = Sv*Cv*Ss*TO_RAD; - dCvSs = Cv*dSs; - dSvSs = Sv*dSs; - dSp = Cp*TO_RAD; - dCp = -Sp*TO_RAD; - - Ax = Drax + Dx + Lx - Qx; - Az = Draz + Dt + Lz - Qz; - - // j[0]: -Cw*Ax + Az*Sw plus head terms - jac[0][0] = Cw; - jac[0][2] = -Sw; - jac[0][3] = (Sp*dSvSs + Cp*dt_)*(Dt + Lz) - (dCvSs*Sp - Cp*dr)*Lx; - jac[0][4] = ( Sw*Ax + Az*Cw)*TO_RAD; - jac[0][5] = dCp*Dx - Dy*dSp + (dSp*SvSs + dCp*t)*(Dt + Lz) - - (CvSs*dSp - dCp*r)*Lx; - - // j[1]: Qy plus head terms - jac[1][1] = 1; - jac[1][3] = -(Cp*dSvSs - Sp*dt_)*(Dt + Lz) + (Cp*dCvSs + Sp*dr)*Lx; - jac[1][5] = -(dCp*SvSs - dSp*t)*(Dt + Lz) + dCp*Dy - + (dCp*CvSs + dSp*r)*Lx + Dx*dSp; - - // j[2]: -Cw*Az - Ax*Sw plus head terms - jac[2][0] = Sw; - jac[2][2] = Cw; - jac[2][3] = (Dt + Lz)*ds + Lx*dt_; - jac[2][4] = ( Sw*Az - Ax*Cw)*TO_RAD; - - jac[3][3] = 1; - jac[4][4] = 1; - jac[5][5] = 1; - break; - - case 2: // ========================= TOOL kinematics JACOBIAN - // the head angles come from pins, so the inverse is linear in - // the pose and the rows are its coefficients - Ss = sin(theta_2*TO_RAD); - Cs = cos(theta_2*TO_RAD); - Sp = sin(theta_1*TO_RAD); - Cp = cos(theta_1*TO_RAD); - CvSs = Cv*Ss; - SvSs = Sv*Ss; - r = Cs + Sv*Sv*(1-Cs); - s = Cs + Cv*Cv*(1-Cs); - t = Sv*Cv*(1-Cs); + double Sv = sin(nu*TO_RAD); + double Cv = cos(nu*TO_RAD); + double Stc = sin(tc); + double Ctc = cos(tc); + double Ss = sin(theta_2*TO_RAD); + double Cs = cos(theta_2*TO_RAD); + double Sp = sin(theta_1*TO_RAD); + double Cp = cos(theta_1*TO_RAD); + double CvSs = Cv*Ss; + double SvSs = Sv*Ss; + double r = Cs + Sv*Sv*(1-Cs); + double s = Cs + Cv*Cv*(1-Cs); + double t = Sv*Cv*(1-Cs); + int R, C; - jac[0][0] = -((CvSs*Sp - Cp*r)*Ctc + (Cp*CvSs + Cs*Sp)*Stc); - jac[0][1] = -((Cp*CvSs + Cs*Sp)*Ctc - (CvSs*Sp - Cp*r)*Stc); - jac[0][2] = (Sp*SvSs + Cp*t); + for (R = 0; R < EMCMOT_MAX_JOINTS; R++) { + for (C = 0; C < EMCMOT_MAX_AXIS; C++) { jac[R][C] = 0; } + } - jac[1][0] = ((Cp*CvSs + Sp*r)*Ctc + (Cp*Cs - CvSs*Sp)*Stc); - jac[1][1] = ((Cp*Cs - CvSs*Sp)*Ctc - (Cp*CvSs + Sp*r)*Stc); - jac[1][2] = -(Cp*SvSs - Sp*t); + jac[0][0] = -((CvSs*Sp - Cp*r)*Ctc + (Cp*CvSs + Cs*Sp)*Stc); + jac[0][1] = -((Cp*CvSs + Cs*Sp)*Ctc - (CvSs*Sp - Cp*r)*Stc); + jac[0][2] = (Sp*SvSs + Cp*t); - jac[2][0] = (Stc*SvSs + Ctc*t); - jac[2][1] = (Ctc*SvSs - Stc*t); - jac[2][2] = s; + jac[1][0] = ((Cp*CvSs + Sp*r)*Ctc + (Cp*Cs - CvSs*Sp)*Stc); + jac[1][1] = ((Cp*Cs - CvSs*Sp)*Ctc - (Cp*CvSs + Sp*r)*Stc); + jac[1][2] = -(Cp*SvSs - Sp*t); - jac[3][3] = 1; - jac[4][4] = 1; - jac[5][5] = 1; - break; - } + jac[2][0] = (Stc*SvSs + Ctc*t); + jac[2][1] = (Ctc*SvSs - Stc*t); + jac[2][2] = s; + + jac[3][3] = 1; + jac[4][4] = 1; + jac[5][5] = 1; return 0; -} // kinematicsJacobian() +} // toolKinematicsJacobian() + +// halcompile has done hal_init() and does hal_ready() after this returns, +// which is what switchkinsInit() expects +EXTRA_SETUP() { + kparms kp; + (void)__comp_inst; (void)prefix; (void)extra_arg; + + kp.kinsname = "xyzbca_trsrn"; + kp.halprefix = "xyzbca_trsrn_kins"; + kp.required_coordinates = "xyzabc"; + kp.allow_duplicates = 0; + kp.fwd_iterates_mask = 0; + kp.gui_kinstype = -1; + kp.sparm = NULL; + kp.max_joints = strlen(kp.required_coordinates); + + if (switchkinsRegister(0, identityKinematicsSetup, + identityKinematicsForward, + identityKinematicsInverse)) { return -1; } + if (switchkinsRegister(1, trsrnKinematicsSetup, + tcpKinematicsForward, + tcpKinematicsInverse)) { return -1; } + if (switchkinsRegister(2, toolKinematicsSetup, + toolKinematicsForward, + toolKinematicsInverse)) { return -1; } + if (switchkinsRegisterFrames(1, tcpKinematicsWorkFrame, + tcpKinematicsToolFrame, + &TOOL_FRAME_SPINDLE)) { return -1; } + if (switchkinsRegisterJacobian(1, tcpKinematicsJacobian)) { return -1; } + // the tool kinematics report in tool axes, so the tool is square with + // the world by construction and nothing turns the work against it + if (switchkinsRegisterFrames(2, identityKinematicsWorkFrame, + identityKinematicsToolFrame, + &TOOL_FRAME_SPINDLE)) { return -1; } + if (switchkinsRegisterJacobian(2, toolKinematicsJacobian)) { return -1; } + + if (switchkinsDeclare(0, KINSTYPE_IDENTITY)) { return -1; } + if (switchkinsDeclare(1, KINSTYPE_PRIMARY)) { return -1; } + + return switchkinsInit(comp_id, &kp, coordinates); +} // EXTRA_SETUP() From 6d4b7be63087fc43cf530fd6044c642bd241b916 Mon Sep 17 00:00:00 2001 From: Luca Toniolo <10792599+grandixximo@users.noreply.github.com> Date: Wed, 12 Aug 2026 11:05:02 +1000 Subject: [PATCH 23/60] switchkins: add an out-of-tree module template An out-of-tree module could not reach the switchkins implementation, so it reimplemented kinematicsSwitch() and the kinstype.is-N pins or did without. switchkinscomp.comp is the template: it sets TOPDIR to a source tree and includes switchkins.c and kins_util.c, as tpcomp.comp and homecomp.comp reach their sources, then registers its kinstypes and calls switchkinsInit() from EXTRA_SETUP(). The sources compile into the module, so no ABI is involved. Like tpcomp it is not built in tree, since it has no kinematics until TOPDIR is set. Renamed and loaded as [KINS]KINEMATICS it homes, switches to its example type and back, and rejects a type it does not have. --- docs/src/hal/components.adoc | 1 + docs/src/motion/switchkins.adoc | 48 +++++++ src/hal/components/Submakefile | 8 +- src/hal/components/switchkinscomp.comp | 167 +++++++++++++++++++++++++ 4 files changed, 220 insertions(+), 4 deletions(-) create mode 100644 src/hal/components/switchkinscomp.comp diff --git a/docs/src/hal/components.adoc b/docs/src/hal/components.adoc index d4987f82b26..6fab8fb2038 100644 --- a/docs/src/hal/components.adoc +++ b/docs/src/hal/components.adoc @@ -336,6 +336,7 @@ Limit its slew rate to less than maxv per second. Limit its second derivative to | link:../man/man9/rosekins.9.html[rosekins] |Kinematics for a rose engine || | link:../man/man9/rotatekins.9.html[rotatekins] |The X and Y axes are rotated 45 degrees compared to the joints 0 and 1. || | link:../man/man9/scarakins.9.html[scarakins] |Kinematics for SCARA-type robots. || +| link:../man/man9/switchkinscomp.9.html[switchkinscomp] |Switchable kinematics module template || | link:../man/man9/kins.9.html[three21kins] |Analytical kinematics solver for 6-DOF arm + wrist robots. || | link:../man/man9/tripodkins.9.html[tripodkins] |The joints represent the distance of the controlled point from three predefined locations (the motors), giving three degrees of freedom in position (XYZ). || | link:../man/man9/userkins.9.html[userkins] |Template for user-built kinematics || diff --git a/docs/src/motion/switchkins.adoc b/docs/src/motion/switchkins.adoc index cfa230f5f43..44b67ef6e88 100644 --- a/docs/src/motion/switchkins.adoc +++ b/docs/src/motion/switchkins.adoc @@ -430,6 +430,12 @@ configs/sim/axis/vismach/ . == User kinematics provisions +There are two ways to supply custom kinematics. Adding a kinstype to +a module that is already in the tree is the smaller job; building a +module of your own gives you every kinstype it provides. + +=== Adding a kinstype to an in-tree module + Custom kinematics can be coded and tested on Run-In-Place ('RIP') builds. A template file src/emc/kinematics/userkfuncs.c is provided in the distribution. This file can be copied/renamed to a user @@ -447,6 +453,47 @@ Preempt-rt make example: $ userkfuncs=/home/myname/kins/mykins.c make && sudo make setuid ---- +=== Building a switchkins module of your own + +A complete kinematics module can be built out-of-tree with halcompile +using the same switchkins implementation the in-tree modules use, so +it gets the kinematics switching, the 'kinstype.is-N' pins, the +'coordinates=' identity mapping and the G-code and HAL controls +without reimplementing any of them. + +The template is src/hal/components/switchkinscomp.comp. Copy and +rename it (both the file and the component name), point its TOPDIR +at a LinuxCNC source tree, and replace the example kinstype with the +real kinematics: + +[source,c] +---- +#define TOPDIR /home/myname/linuxcnc-dev +// ... +#include USE_TOPDIR(src/emc/kinematics/switchkins.c) +#include USE_TOPDIR(src/emc/kinematics/kins_util.c) +---- + +The module registers each of its kinstypes and calls switchkinsInit() +from EXTRA_SETUP(), which halcompile runs after hal_init() and before +hal_ready(). See <> for both +calls. + +---- +$ halcompile --install user_switchkins.comp +---- + +[source,ini] +---- +[KINS] +KINEMATICS = user_switchkins +JOINTS = 3 +---- + +[NOTE] +The switchkins sources are compiled into the module, so it is built +against one source tree and has to be rebuilt when that tree changes. + == Warnings Unexpected behavior can result if a G-code program is inadvertently @@ -471,6 +518,7 @@ The management of coordinate offsets, tool compensation, and INI file limits may require complicated and non-standard operating protocols. +[[sec:switchkins-code-notes]] == Code Notes Kinematic modules providing switchkins functionality are linked to diff --git a/src/hal/components/Submakefile b/src/hal/components/Submakefile index d97a0baf2f1..8ad4ee1740e 100644 --- a/src/hal/components/Submakefile +++ b/src/hal/components/Submakefile @@ -1,5 +1,5 @@ ifneq ($(KERNELRELEASE),) -COMPS := $(filter-out %/tpcomp.comp, $(patsubst $(BASEPWD)/%,%,$(wildcard $(BASEPWD)/hal/components/*.comp $(BASEPWD)/hal/drivers/*.comp))) +COMPS := $(filter-out %/tpcomp.comp %/switchkinscomp.comp, $(patsubst $(BASEPWD)/%,%,$(wildcard $(BASEPWD)/hal/components/*.comp $(BASEPWD)/hal/drivers/*.comp))) include $(patsubst %.comp, $(BASEPWD)/objects/%.mak, $(COMPS)) else CONVERTERS := \ @@ -32,8 +32,8 @@ CONVERTERS := \ conv_u64_s32.comp \ conv_u64_u32.comp \ conv_u64_s64.comp -COMPS := $(filter-out hal/components/tpcomp.comp, $(sort $(wildcard hal/components/*.comp) $(addprefix hal/components/, $(CONVERTERS)))) -COMP_MANPAGES := $(patsubst hal/components/%.comp, ../docs/build/man/man9/%.9, $(COMPS)) ../docs/build/man/man9/tpcomp.9 +COMPS := $(filter-out hal/components/tpcomp.comp hal/components/switchkinscomp.comp, $(sort $(wildcard hal/components/*.comp) $(addprefix hal/components/, $(CONVERTERS)))) +COMP_MANPAGES := $(patsubst hal/components/%.comp, ../docs/build/man/man9/%.9, $(COMPS)) ../docs/build/man/man9/tpcomp.9 ../docs/build/man/man9/switchkinscomp.9 ifeq ($(BUILD_SYS),uspace) COMP_DRIVERS += hal/drivers/serport.comp COMP_DRIVERS += hal/drivers/mesa_7i65.comp @@ -58,7 +58,7 @@ endif # wildcard that mixes hal/components and hal/drivers, so deriving the adoc # targets from it there yields hal/drivers/*.comp entries that fail the # hal/components/%.comp static pattern rule. -COMP_MANPAGE_ADOCS := $(patsubst hal/components/%.comp, objects/man/man9/%.9.adoc, $(COMPS)) objects/man/man9/tpcomp.9.adoc +COMP_MANPAGE_ADOCS := $(patsubst hal/components/%.comp, objects/man/man9/%.9.adoc, $(COMPS)) objects/man/man9/tpcomp.9.adoc objects/man/man9/switchkinscomp.9.adoc COMP_DRIVER_MANPAGE_ADOCS := $(patsubst hal/drivers/%.comp, objects/man/man9/%.9.adoc, $(COMP_DRIVERS)) # Extract adoc from .comp via halcompile --adoc. Only needs Python + diff --git a/src/hal/components/switchkinscomp.comp b/src/hal/components/switchkinscomp.comp new file mode 100644 index 00000000000..1d9fbdbe6d7 --- /dev/null +++ b/src/hal/components/switchkinscomp.comp @@ -0,0 +1,167 @@ +component switchkinscomp "switchable kinematics module template"; +// NOTE: component name must agree with filename + +description """ +Example of a switchable kinematics module buildable with halcompile. + +The switchkinscomp.comp file (src/hal/components/switchkinscomp.comp) +illustrates a method to use halcompile to build a kinematics module +on top of the switchkins implementation used by the in-tree kinematics +modules, so an out-of-tree module gets the same kinematics switching, +the same 'kinstype.is-N' pins, the same 'coordinates=' identity +mapping, and the same G-code and HAL controls, without reimplementing +any of it. + +The example switchkinscomp.comp is not usable until modified for the +user environment. To create a runnable switchkinscomp module, the +file must be edited to supply a valid '#define TOPDIR' pointing at a +LinuxCNC source tree. + +To avoid updates that overwrite switchkinscomp.comp, best practice is +to rename the file and its component name (example: +*user_switchkins.comp* creates module: *user_switchkins*). + +The (renamed) component can be built and installed with halcompile +and then used as the kinematics module by inifile setting: + +[source,ini] +---- +[KINS] +KINEMATICS = user_switchkins +JOINTS = 3 +---- + +*Note:* If using a deb install: + +1. halcompile is provided by the deb package linuxcnc-dev +2. This source file for BRANCHNAME (master, 2.9, etc) is downloadable from github: + +https://github.com/LinuxCNC/linuxcnc/blob/BRANCHNAME/src/hal/components/switchkinscomp.comp + +For information on switchable kinematics see the switchkins document +chapter (docs/src/motion/switchkins.txt). +"""; + +pin out bit is_module=1; //one pin is required to use halcompile + +license "GPL"; +option extra_setup; +;; + +//===================================================================== +/* To use the switchkins implementation from a local git src tree: +** set TOPDIR to the git tree top directory +** (Edit 'myname' as required) +*/ + +//#define TOPDIR /home/myname/linuxcnc-dev + +#ifdef TOPDIR // { + +#define STR(s) #s +#define XSTR(s) STR(s) +#define USE_TOPDIR(b) XSTR(TOPDIR/b) + +// switchkins.c provides kinematicsForward(), kinematicsInverse(), +// kinematicsSwitch() and the rest of the kinematics interface, and +// dispatches each call to the currently selected switchkins-type. +// kins_util.c provides the identity kinematics and the coordinates +// letters-to-joints mapping they use. +#include USE_TOPDIR(src/emc/kinematics/switchkins.c) +#include USE_TOPDIR(src/emc/kinematics/kins_util.c) + +#else +#error No TOPDIR defined, skeleton component provides no kinematics functions. +#endif // } +//===================================================================== + +// module parameter naming the joint order for the identity type +static char *coordinates; +RTAPI_MP_STRING(coordinates, "Axes-to-joints-ordering"); + +//--------------------------------------------------------------------- +// Example switchkins-type. A setup routine creating whatever hal pins +// the kinematics need, plus a forward and an inverse routine. Replace +// the arithmetic with the real kinematics. + +static struct { + hal_real_t x_offset; +} *mydata; + +static int myKinematicsSetup(const int comp_id, + const char* coords, + kparms* kp) +{ + (void)coords; // this type does not use the coordinates mapping + + mydata = hal_malloc(sizeof(*mydata)); + if (!mydata) return -1; + + return hal_pin_new_real(comp_id, HAL_IN, &mydata->x_offset, 0.0, + "%s.x-offset", kp->halprefix); +} // myKinematicsSetup() + +static int myKinematicsForward(const double *j, + EmcPose * pos, + const KINEMATICS_FORWARD_FLAGS * fflags, + KINEMATICS_INVERSE_FLAGS * iflags) +{ + (void)fflags; + (void)iflags; + + pos->tran.x = j[0] + hal_get_real(mydata->x_offset); + pos->tran.y = j[1]; + pos->tran.z = j[2]; + + // unused coordinates: + pos->a = pos->b = pos->c = 0; + pos->u = pos->v = pos->w = 0; + + return 0; +} // myKinematicsForward() + +static int myKinematicsInverse(const EmcPose * pos, + double *j, + const KINEMATICS_INVERSE_FLAGS * iflags, + KINEMATICS_FORWARD_FLAGS * fflags) +{ + (void)iflags; + (void)fflags; + + j[0] = pos->tran.x - hal_get_real(mydata->x_offset); + j[1] = pos->tran.y; + j[2] = pos->tran.z; + + return 0; +} // myKinematicsInverse() + +//--------------------------------------------------------------------- +// rtapi_app_main() is supplied by halcompile, which calls hal_init() +// before EXTRA_SETUP() and hal_ready() after it. That is what +// switchkinsInit() expects, so the switchkins-types are registered and +// the implementation started from here. + +EXTRA_SETUP() { + kparms kp; + (void)__comp_inst; (void)prefix; (void)extra_arg; + + kp.kinsname = "switchkinscomp"; // must agree with the module name + kp.halprefix = "switchkinscomp"; // hal pin names + kp.required_coordinates = "xyz"; + kp.allow_duplicates = 0; + kp.fwd_iterates_mask = 0; // set bit N if type N iterates + kp.gui_kinstype = -1; // negative means: not used + kp.sparm = NULL; + kp.max_joints = strlen(kp.required_coordinates); + + // switchkins-type 0 is the startup default. Types run from 0 to + // SWITCHKINS_MAX_TYPES-1 with no gaps. + if (switchkinsRegister(0, identityKinematicsSetup, + identityKinematicsForward, + identityKinematicsInverse)) { return -1; } + if (switchkinsRegister(1, myKinematicsSetup, + myKinematicsForward, + myKinematicsInverse)) { return -1; } + + return switchkinsInit(comp_id, &kp, coordinates); +} // EXTRA_SETUP() From ce628acd56e849725cbea1b54308738218cf8631 Mon Sep 17 00:00:00 2001 From: Luca Toniolo <10792599+grandixximo@users.noreply.github.com> Date: Wed, 12 Aug 2026 21:02:46 +1000 Subject: [PATCH 24/60] kins: include switchkins.h as an exported header 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. --- src/emc/kinematics/5axiskins.c | 3 +-- src/emc/kinematics/genhexkins.c | 3 +-- src/emc/kinematics/genserkins.c | 2 +- src/emc/kinematics/pumakins.c | 3 +-- src/emc/kinematics/scarakins.c | 3 +-- src/emc/kinematics/switchkins.c | 1 - src/emc/kinematics/three21kins.c | 3 +-- src/emc/kinematics/xyzac-trt-kins.c | 2 +- src/emc/kinematics/xyzbc-trt-kins.c | 2 +- 9 files changed, 8 insertions(+), 14 deletions(-) diff --git a/src/emc/kinematics/5axiskins.c b/src/emc/kinematics/5axiskins.c index 966a79479ee..63b8b59aca9 100644 --- a/src/emc/kinematics/5axiskins.c +++ b/src/emc/kinematics/5axiskins.c @@ -59,9 +59,8 @@ #include #include #include -#include -#include "switchkins.h" +#include static struct haldata { hal_real_t pivot_length; diff --git a/src/emc/kinematics/genhexkins.c b/src/emc/kinematics/genhexkins.c index 3493d58d5f3..b4750e5c540 100644 --- a/src/emc/kinematics/genhexkins.c +++ b/src/emc/kinematics/genhexkins.c @@ -110,10 +110,9 @@ #include #include #include -#include /* these decls, KINEMATICS_FORWARD_FLAGS */ #include "genhexkins.h" -#include "switchkins.h" +#include static struct haldata { hal_real_t basex[NUM_STRUTS]; diff --git a/src/emc/kinematics/genserkins.c b/src/emc/kinematics/genserkins.c index 94a325cbcf6..be71f33ffc5 100644 --- a/src/emc/kinematics/genserkins.c +++ b/src/emc/kinematics/genserkins.c @@ -42,7 +42,7 @@ frame-larger-than: #include #include "genserkins.h" -#include "switchkins.h" +#include //-7 is system defined -3 ok, -4 ok, -5 ok,-6 ok (mm system) #undef GO_REAL_EPSILON diff --git a/src/emc/kinematics/pumakins.c b/src/emc/kinematics/pumakins.c index 049d384b424..53aca15c6f7 100644 --- a/src/emc/kinematics/pumakins.c +++ b/src/emc/kinematics/pumakins.c @@ -20,10 +20,9 @@ #include #include #include -#include #include "pumakins.h" -#include "switchkins.h" +#include struct haldata { hal_real_t a2, a3, d3, d4, d6; diff --git a/src/emc/kinematics/scarakins.c b/src/emc/kinematics/scarakins.c index fa237e62b31..338f14a668d 100644 --- a/src/emc/kinematics/scarakins.c +++ b/src/emc/kinematics/scarakins.c @@ -19,9 +19,8 @@ #include #include #include -#include -#include "switchkins.h" +#include static struct scara_data { hal_real_t d1, d2, d3, d4, d5, d6; diff --git a/src/emc/kinematics/switchkins.c b/src/emc/kinematics/switchkins.c index a9fa9027cd5..472394cefdd 100644 --- a/src/emc/kinematics/switchkins.c +++ b/src/emc/kinematics/switchkins.c @@ -29,7 +29,6 @@ #include #include #include -#include #include "switchkins.h" diff --git a/src/emc/kinematics/three21kins.c b/src/emc/kinematics/three21kins.c index 2a3dfe08ee5..039001a8624 100644 --- a/src/emc/kinematics/three21kins.c +++ b/src/emc/kinematics/three21kins.c @@ -2,9 +2,8 @@ #include #include #include -#include -#include "switchkins.h" +#include /* default values for ar2 robot */ #define DEFAULT_THREE21_A1 64.2 diff --git a/src/emc/kinematics/xyzac-trt-kins.c b/src/emc/kinematics/xyzac-trt-kins.c index b8bb47bbc1f..b6b35538f25 100644 --- a/src/emc/kinematics/xyzac-trt-kins.c +++ b/src/emc/kinematics/xyzac-trt-kins.c @@ -15,7 +15,7 @@ #include #include -#include "switchkins.h" +#include int switchkinsSetup(kparms* kp, KS* kset0, KS* kset1, KS* kset2, diff --git a/src/emc/kinematics/xyzbc-trt-kins.c b/src/emc/kinematics/xyzbc-trt-kins.c index 7b61a69e301..401311e4398 100644 --- a/src/emc/kinematics/xyzbc-trt-kins.c +++ b/src/emc/kinematics/xyzbc-trt-kins.c @@ -15,7 +15,7 @@ #include #include -#include "switchkins.h" +#include int switchkinsSetup(kparms* kp, KS* kset0, KS* kset1, KS* kset2, From 430048849ca960a93170dfd79a08acdc09e6599e Mon Sep 17 00:00:00 2001 From: Luca Toniolo <10792599+grandixximo@users.noreply.github.com> Date: Wed, 12 Aug 2026 23:51:35 +1000 Subject: [PATCH 25/60] switchkins: install the implementation as source for out-of-tree modules 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 #include and builds as it stands. --- .gitignore | 2 ++ debian/linuxcnc-uspace-dev.install | 2 ++ docs/src/motion/switchkins.adoc | 17 +++++++------ src/Makefile | 1 + src/Makefile.modinc.in | 4 +-- src/emc/kinematics/Submakefile | 13 ++++++++++ src/hal/components/switchkinscomp.comp | 34 ++++++++------------------ 7 files changed, 40 insertions(+), 33 deletions(-) diff --git a/.gitignore b/.gitignore index 4e3ccbcb4c8..18eb2868130 100644 --- a/.gitignore +++ b/.gitignore @@ -21,6 +21,8 @@ share/desktop-directories/linuxcnc-cnc.directory share/desktop-directories/linuxcnc-ref.directory share/desktop-directories/linuxcnc-doc.directory share/linuxcnc/mesa_modbus.c.tmpl +share/linuxcnc/switchkins.c +share/linuxcnc/kins_util.c src/modules.order /configs/*/emc.nml !/configs/common/emc.nml diff --git a/debian/linuxcnc-uspace-dev.install b/debian/linuxcnc-uspace-dev.install index 199dae9fcc0..39c124d3532 100644 --- a/debian/linuxcnc-uspace-dev.install +++ b/debian/linuxcnc-uspace-dev.install @@ -5,3 +5,5 @@ usr/lib/liblinuxcnc.a usr/lib/*.so usr/share/linuxcnc/Makefile.modinc usr/share/linuxcnc/mesa_modbus.c.tmpl +usr/share/linuxcnc/switchkins.c +usr/share/linuxcnc/kins_util.c diff --git a/docs/src/motion/switchkins.adoc b/docs/src/motion/switchkins.adoc index 44b67ef6e88..14fb1b7aabb 100644 --- a/docs/src/motion/switchkins.adoc +++ b/docs/src/motion/switchkins.adoc @@ -462,18 +462,21 @@ it gets the kinematics switching, the 'kinstype.is-N' pins, the without reimplementing any of them. The template is src/hal/components/switchkinscomp.comp. Copy and -rename it (both the file and the component name), point its TOPDIR -at a LinuxCNC source tree, and replace the example kinstype with the -real kinematics: +rename it (both the file and the component name) and replace the +example kinstype with the real kinematics. The implementation itself +is included: [source,c] ---- -#define TOPDIR /home/myname/linuxcnc-dev -// ... -#include USE_TOPDIR(src/emc/kinematics/switchkins.c) -#include USE_TOPDIR(src/emc/kinematics/kins_util.c) +#include +#include ---- +A realtime module cannot link a library, so the implementation arrives +as source: switchkins.c and kins_util.c are installed beside the +headers, in share/linuxcnc, and halcompile already looks there. With +a deb install they come from the linuxcnc-dev package. + The module registers each of its kinstypes and calls switchkinsInit() from EXTRA_SETUP(), which halcompile runs after hal_init() and before hal_ready(). See <> for both diff --git a/src/Makefile b/src/Makefile index b7563d22623..98a48d3b933 100644 --- a/src/Makefile +++ b/src/Makefile @@ -785,6 +785,7 @@ ifeq ($(BUILD_GUI),yes) endif $(FILE) ../src/hal/drivers/mesa-hostmot2/modbus/*.tmpl $(DESTDIR)$(prefix)/share/linuxcnc/ + $(FILE) ../src/emc/kinematics/switchkins.c ../src/emc/kinematics/kins_util.c $(DESTDIR)$(prefix)/share/linuxcnc/ install-kernel-indep: install-python install-python: install-dirs diff --git a/src/Makefile.modinc.in b/src/Makefile.modinc.in index ed9d75d98c2..cfcf1bc0b7d 100644 --- a/src/Makefile.modinc.in +++ b/src/Makefile.modinc.in @@ -76,12 +76,12 @@ EXTRA_CFLAGS += -fno-builtin-sin -fno-builtin-cos -fno-builtin-sincos EMC2_HOME=@EMC2_HOME@ RUN_IN_PLACE=@RUN_IN_PLACE@ ifeq ($(RUN_IN_PLACE),yes) -EXTRA_CFLAGS := $(RTFLAGS) -D__MODULE__ -I$(EMC2_HOME)/include +EXTRA_CFLAGS := $(RTFLAGS) -D__MODULE__ -I$(EMC2_HOME)/include -I$(EMC2_HOME)/share/linuxcnc RTLIBDIR := @EMC2_HOME@/rtlib LIBDIR := @EMC2_HOME@/lib else prefix := @prefix@ -EXTRA_CFLAGS := $(RTFLAGS) -D__MODULE__ -I@includedir@/linuxcnc +EXTRA_CFLAGS := $(RTFLAGS) -D__MODULE__ -I@includedir@/linuxcnc -I${prefix}/share/linuxcnc RTLIBDIR := @EMC2_RTLIB_DIR@ LIBDIR := @libdir@ endif diff --git a/src/emc/kinematics/Submakefile b/src/emc/kinematics/Submakefile index 77085c21c2e..7e2f2d84b4b 100644 --- a/src/emc/kinematics/Submakefile +++ b/src/emc/kinematics/Submakefile @@ -33,3 +33,16 @@ $(RDELTAMODULE): $(call TOOBJS, $(RDELTAMODULESRCS)) $(ECHO) Linking python module $(notdir $@) $(CXX) $(LDFLAGS) -shared -o $@ $^ $(BOOST_PYTHON_LIB) PYTARGETS += $(RDELTAMODULE) + +# The switchkins implementation is shipped as source, since a realtime module +# cannot link a library, so a module built out of tree includes it the way the +# in-tree ones link it. +EMCKINEMATICSSRCS = \ + ../share/linuxcnc/switchkins.c \ + ../share/linuxcnc/kins_util.c + +$(EMCKINEMATICSSRCS): ../share/linuxcnc/%.c: ./emc/kinematics/%.c + $(ECHO) Copying switchkins source $(notdir $@) + $(Q)cp -f $< $@ + +TARGETS += $(EMCKINEMATICSSRCS) diff --git a/src/hal/components/switchkinscomp.comp b/src/hal/components/switchkinscomp.comp index 1d9fbdbe6d7..7e90edc380f 100644 --- a/src/hal/components/switchkinscomp.comp +++ b/src/hal/components/switchkinscomp.comp @@ -12,10 +12,10 @@ the same 'kinstype.is-N' pins, the same 'coordinates=' identity mapping, and the same G-code and HAL controls, without reimplementing any of it. -The example switchkinscomp.comp is not usable until modified for the -user environment. To create a runnable switchkinscomp module, the -file must be edited to supply a valid '#define TOPDIR' pointing at a -LinuxCNC source tree. +The example builds as it stands, its type 1 being an X offset to +replace with the kinematics wanted. The switchkins implementation is +installed as source alongside the headers, so nothing needs a path to +a LinuxCNC source tree. To avoid updates that overwrite switchkinscomp.comp, best practice is to rename the file and its component name (example: @@ -33,7 +33,8 @@ JOINTS = 3 *Note:* If using a deb install: -1. halcompile is provided by the deb package linuxcnc-dev +1. halcompile and the switchkins source are provided by the deb + package linuxcnc-dev 2. This source file for BRANCHNAME (master, 2.9, etc) is downloadable from github: https://github.com/LinuxCNC/linuxcnc/blob/BRANCHNAME/src/hal/components/switchkinscomp.comp @@ -49,30 +50,15 @@ option extra_setup; ;; //===================================================================== -/* To use the switchkins implementation from a local git src tree: -** set TOPDIR to the git tree top directory -** (Edit 'myname' as required) -*/ - -//#define TOPDIR /home/myname/linuxcnc-dev - -#ifdef TOPDIR // { - -#define STR(s) #s -#define XSTR(s) STR(s) -#define USE_TOPDIR(b) XSTR(TOPDIR/b) - // switchkins.c provides kinematicsForward(), kinematicsInverse(), // kinematicsSwitch() and the rest of the kinematics interface, and // dispatches each call to the currently selected switchkins-type. // kins_util.c provides the identity kinematics and the coordinates -// letters-to-joints mapping they use. -#include USE_TOPDIR(src/emc/kinematics/switchkins.c) -#include USE_TOPDIR(src/emc/kinematics/kins_util.c) +// letters-to-joints mapping they use. Both are installed with the +// headers, so halcompile finds them with no path of your own. -#else -#error No TOPDIR defined, skeleton component provides no kinematics functions. -#endif // } +#include +#include //===================================================================== // module parameter naming the joint order for the identity type From a6775c9aff93ff1d9ce59c9db4c005227e795877 Mon Sep 17 00:00:00 2001 From: Luca Toniolo <10792599+grandixximo@users.noreply.github.com> Date: Tue, 25 Aug 2026 01:03:33 +1000 Subject: [PATCH 26/60] canon: stop printing on every kinematics switch 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. --- src/emc/rs274ngc/gcodemodule.cc | 8 +------- src/emc/sai/saicanon.cc | 5 +---- 2 files changed, 2 insertions(+), 11 deletions(-) diff --git a/src/emc/rs274ngc/gcodemodule.cc b/src/emc/rs274ngc/gcodemodule.cc index f861a2cbadc..fdd6b4873a3 100644 --- a/src/emc/rs274ngc/gcodemodule.cc +++ b/src/emc/rs274ngc/gcodemodule.cc @@ -904,13 +904,7 @@ void ON_RESET() {} void PALLET_SHUTTLE() {} void SELECT_TOOL(int tool) {selected_tool = tool;} void UPDATE_TAG(const StateTag& /*tag*/) {} -void SELECT_KINS_TYPE(int switchkins_type) -{ - (void)switchkins_type; - printf("gcodemodule: SELECT_KINS_TYPE\n"); - - return; -} +void SELECT_KINS_TYPE(int /*switchkins_type*/) {} void OPTIONAL_PROGRAM_STOP() {} int GET_EXTERNAL_TC_FAULT() {return 0;} int GET_EXTERNAL_TC_REASON() {return 0;} diff --git a/src/emc/sai/saicanon.cc b/src/emc/sai/saicanon.cc index fc7cde1c018..5c0812dfc54 100644 --- a/src/emc/sai/saicanon.cc +++ b/src/emc/sai/saicanon.cc @@ -1229,8 +1229,5 @@ void UPDATE_TAG(const StateTag& /*tag*/){ void SELECT_KINS_TYPE(int switchkins_type) { - (void)switchkins_type; - printf("saicanon: SELECT_KINS_TYPE\n"); - - return; + ECHO_WITH_ARGS("%d", switchkins_type); } From b891bb5aa7fee8c5d9cfaaf382dc7c86844eceb7 Mon Sep 17 00:00:00 2001 From: Luca Toniolo <10792599+grandixximo@users.noreply.github.com> Date: Mon, 10 Aug 2026 18:05:21 +1000 Subject: [PATCH 27/60] kinematics: evaluate a module outside RT by binding to its live pins A module's haldata is a struct of pin handles, and a handle is a pointer to the value cell hal_get_real() reads, so a second copy of the module outside realtime can point its haldata at cells carrying the values the RT instance reads and its forward and inverse work unmodified, on live values, at any pose. Per module: export nonrt_attach(), which asks a caller-supplied resolver for each input pin by name, runs the same coordinate parse setup runs and returns the forward and inverse; the maths is untouched and no header grows per module. The cells bound are pins of the caller's own component, connected to the signal the RT pin reads or to one made for the purpose, since a reference into another component's pin has that component's lifetime. Name lookup lives in the loader, userspace code linked against liblinuxcnchal; walking the HAL name space from an RT object is what the HAL isolation work removes. Input pins only, or the two copies write each other's state. Verified against the shared-memory snapshot design this replaces: identical caps from kinslimits, and a pin set by setp is seen without a motion thread running. --- debian/linuxcnc.install.in | 1 + src/Makefile | 3 + src/emc/kinematics/5axiskins.c | 63 ++- src/emc/kinematics/nonrt_kins.h | 95 +++++ src/emc/kinematics/trivkins.c | 15 + src/emc/kinematics_userspace/Submakefile | 1 + .../kinematics_userspace/kinematics_user.c | 381 ++++++++++++++++++ .../kinematics_userspace/kinematics_user.h | 197 +++++++++ src/emc/motion_planning/Submakefile | 46 +++ src/emc/motion_planning/jacobian.cc | 197 +++++++++ src/emc/motion_planning/jacobian.hh | 99 +++++ src/emc/motion_planning/joint_limits.cc | 358 ++++++++++++++++ src/emc/motion_planning/joint_limits.hh | 234 +++++++++++ src/emc/motion_planning/kinslimits.cc | 269 +++++++++++++ 14 files changed, 1950 insertions(+), 9 deletions(-) create mode 100644 src/emc/kinematics/nonrt_kins.h create mode 100644 src/emc/kinematics_userspace/Submakefile create mode 100644 src/emc/kinematics_userspace/kinematics_user.c create mode 100644 src/emc/kinematics_userspace/kinematics_user.h create mode 100644 src/emc/motion_planning/Submakefile create mode 100644 src/emc/motion_planning/jacobian.cc create mode 100644 src/emc/motion_planning/jacobian.hh create mode 100644 src/emc/motion_planning/joint_limits.cc create mode 100644 src/emc/motion_planning/joint_limits.hh create mode 100644 src/emc/motion_planning/kinslimits.cc diff --git a/debian/linuxcnc.install.in b/debian/linuxcnc.install.in index d66045b43a5..a71302665a0 100644 --- a/debian/linuxcnc.install.in +++ b/debian/linuxcnc.install.in @@ -36,6 +36,7 @@ usr/bin/hy_vfd usr/bin/image-to-gcode usr/bin/inivalue usr/bin/inivar +usr/bin/kinslimits usr/bin/latency-histogram usr/bin/latency-plot usr/bin/latency-test diff --git a/src/Makefile b/src/Makefile index 98a48d3b933..2999e03ca9e 100644 --- a/src/Makefile +++ b/src/Makefile @@ -193,6 +193,7 @@ SUBDIRS := \ \ $(GUI_SUBDIRS) \ emc/usr_intf/axis emc/usr_intf emc/nml_intf emc/task emc/kinematics emc/canterp \ + emc/motion_planning emc/kinematics_userspace \ emc/ini emc/rs274ngc emc/sai emc/pythonplugin \ emc/motion-logger \ emc/tooldata \ @@ -403,6 +404,8 @@ SRCHEADERS := \ emc/linuxcnc.h \ emc/kinematics/kinematics.h \ emc/kinematics/switchkins.h \ + emc/kinematics/nonrt_kins.h \ + emc/kinematics_userspace/kinematics_user.h \ emc/nml_intf/emcmotcfg.h \ emc/ini/inifile.hh \ emc/ini/inifile.h \ diff --git a/src/emc/kinematics/5axiskins.c b/src/emc/kinematics/5axiskins.c index 63b8b59aca9..5d8a5c7a8ec 100644 --- a/src/emc/kinematics/5axiskins.c +++ b/src/emc/kinematics/5axiskins.c @@ -61,6 +61,7 @@ #include #include +#include static struct haldata { hal_real_t pivot_length; @@ -203,11 +204,20 @@ static int fiveaxis_KinematicsJacobian(const double *joints, jac); } // fiveaxis_KinematicsJacobian() -int fiveaxis_KinematicsSetup(const int comp_id, - const char* coordinates, - kparms* kp) +// module constants, shared by switchkinsSetup() and nonrt_attach() +static void fiveaxis_kparms(kparms* kp) +{ + kp->kinsname = "5axiskins"; // !!! must agree with filename + kp->halprefix = "5axiskins"; // hal pin names + kp->required_coordinates = REQUIRED_COORDINATES; + kp->allow_duplicates = 1; + kp->max_joints = EMCMOT_MAX_JOINTS; +} + +// assign principal joint numbers from the coordinates string. +// No HAL involvement, so the non-RT path can use it too. +static int fiveaxis_map_joints(const char* coordinates, kparms* kp) { - int result=0; int i,jno; int axis_idx_for_jno[EMCMOT_MAX_JOINTS]; int minjoints = strlen(kp->required_coordinates); @@ -256,6 +266,20 @@ int fiveaxis_KinematicsSetup(const int comp_id, if (axis_idx_for_jno[jno] == 8) {if (JW == -1) JW=jno;} } + return 0; + +error: + return -1; +} // fiveaxis_map_joints() + +int fiveaxis_KinematicsSetup(const int comp_id, + const char* coordinates, + kparms* kp) +{ + int result=0; + + if (fiveaxis_map_joints(coordinates, kp)) goto error; + haldata = hal_malloc(sizeof(*haldata)); if(!haldata) goto error; @@ -288,11 +312,7 @@ int switchkinsSetup(kparms* kp, KI* kinv0, KI* kinv1, KI* kinv2 ) { - kp->kinsname = "5axiskins"; // !!! must agree with filename - kp->halprefix = "5axiskins"; // hal pin names - kp->required_coordinates = REQUIRED_COORDINATES; - kp->allow_duplicates = 1; - kp->max_joints = EMCMOT_MAX_JOINTS; + fiveaxis_kparms(kp); if (kp->sparm && strstr(kp->sparm,"identityfirst")) { rtapi_print("\n!!! switchkins-type 0 is IDENTITY\n"); @@ -325,3 +345,28 @@ int switchkinsSetup(kparms* kp, return 0; } // switchkinsSetup() + +// Non-RT entry point: bind this copy of the module to the pins the +// running RT instance owns, then hand back the unmodified kinematics. +int nonrt_attach(const char* coordinates, nonrt_ops_t* ops, + nonrt_resolve_fn resolve, void* arg) +{ + static struct haldata nonrt_haldata; // private to this copy of the module + kparms kp = {0}; + + fiveaxis_kparms(&kp); + + haldata = &nonrt_haldata; + + if (nonrt_resolve_real(resolve, arg, &haldata->pivot_length, + "%s.pivot-length", kp.halprefix)) return -1; + + if (fiveaxis_map_joints(coordinates, &kp)) return -1; + + ops->forward = fiveaxis_KinematicsForward; + ops->inverse = fiveaxis_KinematicsInverse; + ops->is_identity = 0; + return 0; +} // nonrt_attach() + +EXPORT_SYMBOL(nonrt_attach); diff --git a/src/emc/kinematics/nonrt_kins.h b/src/emc/kinematics/nonrt_kins.h new file mode 100644 index 00000000000..ed564f21b6f --- /dev/null +++ b/src/emc/kinematics/nonrt_kins.h @@ -0,0 +1,95 @@ +/******************************************************************** + * Description: nonrt_kins.h + * Interface a kinematics module exports so that a non-RT caller can + * evaluate it. + * + * A trajectory planner needs forward and inverse kinematics at poses + * the machine has not reached yet, which means calling them outside + * the servo thread. A module opts in by exporting nonrt_attach(). + * + * The caller dlopens the module and calls nonrt_attach() once with + * the coordinates string and a resolver callback. The module names + * each of the pins it reads, keeps the references the resolver + * returns in its own haldata, and hands back its existing forward + * and inverse. The kinematics code itself does not change. + * + * A reference does not point into the RT instance's pin. The + * resolver creates an input pin on the caller's own component and + * connects it to the signal the RT pin reads, so the reference + * belongs to the caller and rewiring cannot strand it. + * + * Name lookup belongs to the caller, userspace code linked against + * liblinuxcnchal. This file is compiled into an RT module, which + * has no business walking the HAL name space and would risk binding + * against rtlib's copy of the same symbols. + * + * Resolve input pins only. Output pins and scratch storage stay + * private to the non-RT copy, or the two copies write to each + * other's state. + * + * Author: LinuxCNC + * License: GPL Version 2 + * System: Linux + * + * Copyright (c) 2024 All rights reserved. + ********************************************************************/ + +#ifndef NONRT_KINS_H +#define NONRT_KINS_H + +#include + +#include +#include +#include +#include + +/* Supplied by the caller. Finds 'pin_name' in HAL, checks that it has + type 'type', and writes to 'out' a reference carrying that pin's + value. The reference is to storage the caller owns, not to the named + pin itself. Returns 0 on success. */ +typedef int (*nonrt_resolve_fn)(const char *pin_name, + hal_type_t type, + hal_refs_u *out, + void *arg); + +/* Filled in by nonrt_attach(). A module that reports is_identity has + joints equal to axes and the caller needs no module code at all, so + forward and inverse may be left NULL. */ +typedef struct { + int (*forward)(const double *joints, EmcPose *pos, + const KINEMATICS_FORWARD_FLAGS *fflags, + KINEMATICS_INVERSE_FLAGS *iflags); + int (*inverse)(const EmcPose *pos, double *joints, + const KINEMATICS_INVERSE_FLAGS *iflags, + KINEMATICS_FORWARD_FLAGS *fflags); + int is_identity; +} nonrt_ops_t; + +/* Exported by a participating module: + int nonrt_attach(const char *coordinates, nonrt_ops_t *ops, + nonrt_resolve_fn resolve, void *arg); + Returns 0 on success. */ + +/* Convenience for the common case: resolve one float pin, by printf + style name, into a haldata field. */ +static inline int nonrt_resolve_real(nonrt_resolve_fn resolve, void *arg, + hal_real_t *dst, const char *fmt, ...) +{ + char name[HAL_NAME_LEN + 1]; + hal_refs_u ref; + va_list ap; + + if (!resolve || !dst) return -1; + + va_start(ap, fmt); + rtapi_vsnprintf(name, sizeof(name), fmt, ap); + va_end(ap); + + if (resolve(name, HAL_FLOAT, &ref, arg) != 0) return -1; + + *dst = ref.r; + return 0; +} + +#endif /* NONRT_KINS_H */ diff --git a/src/emc/kinematics/trivkins.c b/src/emc/kinematics/trivkins.c index f04d9642622..0690aa9ee39 100644 --- a/src/emc/kinematics/trivkins.c +++ b/src/emc/kinematics/trivkins.c @@ -18,6 +18,7 @@ #include #include #include +#include "nonrt_kins.h" #define SET(f) pos->f = joints[i] @@ -110,3 +111,17 @@ int rtapi_app_main(void) { } void rtapi_app_exit(void) { hal_exit(comp_id); } + +// Non-RT entry point: joints are axes, so a non-RT caller needs no +// module code at all and reads nothing from HAL. +int nonrt_attach(const char* coordinates, nonrt_ops_t* ops, + nonrt_resolve_fn resolve, void* arg) +{ + (void)coordinates; (void)resolve; (void)arg; + ops->forward = NULL; + ops->inverse = NULL; + ops->is_identity = 1; + return 0; +} + +EXPORT_SYMBOL(nonrt_attach); diff --git a/src/emc/kinematics_userspace/Submakefile b/src/emc/kinematics_userspace/Submakefile new file mode 100644 index 00000000000..f92b17d356e --- /dev/null +++ b/src/emc/kinematics_userspace/Submakefile @@ -0,0 +1 @@ +INCLUDES += emc/kinematics_userspace diff --git a/src/emc/kinematics_userspace/kinematics_user.c b/src/emc/kinematics_userspace/kinematics_user.c new file mode 100644 index 00000000000..69abd527dac --- /dev/null +++ b/src/emc/kinematics_userspace/kinematics_user.c @@ -0,0 +1,381 @@ +/******************************************************************** + * Description: kinematics_user.c + * Non-RT loader for kinematics modules + * + * Loads a kinematics .so with dlopen and calls the nonrt_attach() it + * exports, so this process evaluates the kinematics the machine is + * running, at whatever poses it likes. See nonrt_kins.h. + * + * Identity kinematics needs no module code: the module says so through + * nonrt_ops_t and this file maps joints to axes directly. A module + * exporting no nonrt_attach() is not an error either; the context comes + * back flagged rt_only. + * + * Author: LinuxCNC + * License: GPL Version 2 + * System: Linux + * + * Copyright (c) 2024 All rights reserved. + ********************************************************************/ + +#include "kinematics_user.h" +#include +#include +#include +#include +#include +#include + +#include "config.h" /* EMC2_HOME */ + +typedef int (*nonrt_attach_fn)(const char *coordinates, nonrt_ops_t *ops, + nonrt_resolve_fn resolve, void *arg); + +/* One per value a kinematics module reads is a generous bound. */ +#define MAX_MADE_SIGNALS 16 +#define MAX_BOUND_PINS 16 + +struct KinematicsUserContext { + int initialized; + int rt_only; /* 1 if the module exports no nonrt_attach() */ + int is_identity; /* 1 for identity kinematics: no module code needed */ + KINEMATICS_TYPE kins_type; + void *rt_handle; /* dlopen handle */ + nonrt_ops_t ops; + int num_joints; + int joint_to_axis[KINEMATICS_USER_MAX_JOINTS]; /* identity path only */ + char module_name[64]; + int comp_id; /* the caller's component, owns the pins made here */ + const char *prefix; /* its name, which those pin names start with */ + char made_signal[MAX_MADE_SIGNALS][HAL_NAME_LEN + 1]; + int num_made_signals; + hal_refs_u *cell; /* HAL storage those pins are made against */ + int num_cells; +}; + +/* ======================================================================== + * Pin binding + * ======================================================================== */ + +/* + * Give a kinematics module a reference to a value it asked for. + * + * The reference is to a pin of ours rather than into the RT instance's, + * so that its lifetime is ours: see nonrt_kins.h. Ours is connected to + * the signal the RT pin reads, or, when the RT pin has no signal, to one + * made here and removed again in kinematicsUserFree(). + * + * The reference has to 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 and the module gets what a cell holds once + * the connection is in place. + */ +static int make_signal(KinematicsUserContext *ctx, const char *pin_name, + hal_type_t type, char *out, size_t outlen) +{ + if (ctx->num_made_signals >= MAX_MADE_SIGNALS) { + fprintf(stderr, "kinematicsUserInit: too many signals to create\n"); + return -1; + } + if ((size_t)snprintf(out, outlen, "%s-nonrt", pin_name) >= outlen) { + fprintf(stderr, "kinematicsUserInit: signal name for '%s' too long\n", + pin_name); + return -1; + } + if (hal_signal_new(out, type) != 0) return -1; + if (hal_link(pin_name, out) != 0) { + hal_signal_delete(out); + return -1; + } + snprintf(ctx->made_signal[ctx->num_made_signals++], + sizeof(ctx->made_signal[0]), "%s", out); + return 0; +} + +static int new_pin(int comp_id, hal_type_t type, hal_refs_u *out, + const char *name) +{ + switch (type) { + case HAL_BIT: return hal_pin_new_bool(comp_id, HAL_IN, &out->b, 0, "%s", name); + case HAL_FLOAT: return hal_pin_new_real(comp_id, HAL_IN, &out->r, 0.0, "%s", name); + case HAL_S32: return hal_pin_new_si32(comp_id, HAL_IN, &out->s, 0, "%s", name); + case HAL_U32: return hal_pin_new_ui32(comp_id, HAL_IN, &out->u, 0, "%s", name); + case HAL_S64: return hal_pin_new_sint(comp_id, HAL_IN, &out->s, 0, "%s", name); + case HAL_U64: return hal_pin_new_uint(comp_id, HAL_IN, &out->u, 0, "%s", name); + default: break; + } + return -1; +} + +static int bind_pin(const char *pin_name, hal_type_t type, + hal_refs_u *out, void *arg) +{ + KinematicsUserContext *ctx = (KinematicsUserContext *)arg; + char signal[HAL_NAME_LEN + 1]; + char mine[HAL_NAME_LEN + 1]; + hal_refs_u *cell; + hal_query_t q; + + if (!ctx || !pin_name || !out) return -1; + + memset(&q, 0, sizeof(q)); + q.name = pin_name; + q.qtype = HAL_QTYPE_PIN; + + if (hal_getref_p(&q) != 0) { + fprintf(stderr, "kinematicsUserInit: no such pin '%s'\n", pin_name); + return -1; + } + if (q.pp.type != type) { + fprintf(stderr, "kinematicsUserInit: pin '%s' has the wrong type\n", + pin_name); + return -1; + } + + if (q.pp.signal) { + snprintf(signal, sizeof(signal), "%s", q.pp.signal); + } else if (make_signal(ctx, pin_name, type, signal, sizeof(signal))) { + fprintf(stderr, "kinematicsUserInit: cannot reach '%s'\n", pin_name); + return -1; + } + + if ((size_t)snprintf(mine, sizeof(mine), "%s.%s", ctx->prefix, pin_name) + >= sizeof(mine)) { + fprintf(stderr, "kinematicsUserInit: pin name for '%s' too long\n", + pin_name); + return -1; + } + if (ctx->num_cells >= MAX_BOUND_PINS) { + fprintf(stderr, "kinematicsUserInit: too many pins to bind\n"); + return -1; + } + cell = &ctx->cell[ctx->num_cells++]; + + if (new_pin(ctx->comp_id, type, cell, mine) != 0) { + fprintf(stderr, "kinematicsUserInit: cannot create pin '%s'\n", mine); + return -1; + } + if (hal_link(mine, signal) != 0) { + fprintf(stderr, "kinematicsUserInit: cannot link '%s' to '%s'\n", + mine, signal); + return -1; + } + + *out = *cell; + return 0; +} + +/* ======================================================================== + * Identity joint mapping + * ======================================================================== */ + +static void fill_identity_joint_map(KinematicsUserContext *ctx, const char *coords) +{ + int i, j = 0; + for (i = 0; i < KINEMATICS_USER_MAX_JOINTS; i++) ctx->joint_to_axis[i] = -1; + if (!coords) return; + for (; *coords && j < ctx->num_joints; coords++) { + int axis; + switch (tolower((unsigned char)*coords)) { + case 'x': axis = 0; break; case 'y': axis = 1; break; + case 'z': axis = 2; break; case 'a': axis = 3; break; + case 'b': axis = 4; break; case 'c': axis = 5; break; + case 'u': axis = 6; break; case 'v': axis = 7; break; + case 'w': axis = 8; break; default: continue; + } + ctx->joint_to_axis[j++] = axis; + } +} + +/* ======================================================================== + * Module loading + * ======================================================================== */ + +static int load_module(KinematicsUserContext *ctx, + const char *module_name, + const char *coordinates) +{ + char module_path[512]; + void *handle; + nonrt_attach_fn attach; + + snprintf(module_path, sizeof(module_path), + "%s/rtlib/%s.so", EMC2_HOME, module_name); + + handle = dlopen(module_path, RTLD_NOW | RTLD_LOCAL); + if (!handle) { + fprintf(stderr, "kinematicsUserInit: dlopen '%s': %s\n", + module_path, dlerror()); + return -1; + } + ctx->rt_handle = handle; + + attach = (nonrt_attach_fn)dlsym(handle, "nonrt_attach"); + if (!attach) { + fprintf(stderr, "kinematicsUserInit: '%s' exports no nonrt_attach\n", + module_name); + dlclose(handle); + ctx->rt_handle = NULL; + ctx->rt_only = 1; + return -1; + } + + if (attach(coordinates, &ctx->ops, bind_pin, ctx) != 0) { + fprintf(stderr, "kinematicsUserInit: nonrt_attach failed for '%s'\n", + module_name); + dlclose(handle); + ctx->rt_handle = NULL; + ctx->rt_only = 1; + return -1; + } + + if (ctx->ops.is_identity) { + ctx->is_identity = 1; + ctx->kins_type = KINEMATICS_IDENTITY; + return 0; + } + + if (!ctx->ops.forward || !ctx->ops.inverse) { + fprintf(stderr, "kinematicsUserInit: '%s' set no fwd/inv\n", module_name); + dlclose(handle); + ctx->rt_handle = NULL; + ctx->rt_only = 1; + return -1; + } + + ctx->kins_type = KINEMATICS_BOTH; + return 0; +} + +/* ======================================================================== + * Public API + * ======================================================================== */ + +KinematicsUserContext* kinematicsUserInit(const char* kins_type, + int num_joints, + const char* coordinates, + int comp_id, + const char* prefix) +{ + KinematicsUserContext *ctx; + + if (!kins_type || num_joints < 1 || num_joints > KINEMATICS_USER_MAX_JOINTS + || comp_id < 0 || !prefix) { + fprintf(stderr, "kinematicsUserInit: invalid arguments\n"); + return NULL; + } + + ctx = (KinematicsUserContext *)calloc(1, sizeof(KinematicsUserContext)); + if (!ctx) return NULL; + + ctx->num_joints = num_joints; + ctx->comp_id = comp_id; + ctx->prefix = prefix; + + ctx->cell = (hal_refs_u *)hal_malloc(MAX_BOUND_PINS * sizeof(hal_refs_u)); + if (!ctx->cell) { + fprintf(stderr, "kinematicsUserInit: out of HAL memory\n"); + free(ctx); + return NULL; + } + strncpy(ctx->module_name, kins_type, sizeof(ctx->module_name) - 1); + + load_module(ctx, kins_type, coordinates); + + if (ctx->is_identity) { + fill_identity_joint_map(ctx, coordinates); + } + + ctx->initialized = 1; + return ctx; +} + +int kinematicsUserInverse(KinematicsUserContext* ctx, + const EmcPose* world, + double* joints) +{ + if (!ctx || !ctx->initialized || !world || !joints) return -1; + + if (ctx->is_identity) { + int i; + for (i = 0; i < ctx->num_joints; i++) { + int ax = ctx->joint_to_axis[i]; + joints[i] = (ax >= 0) ? emcPoseGetAxis(world, ax) : 0.0; + } + return 0; + } + + if (ctx->rt_only) return -1; + return ctx->ops.inverse(world, joints, NULL, NULL); +} + +int kinematicsUserForward(KinematicsUserContext* ctx, + const double* joints, + EmcPose* world) +{ + if (!ctx || !ctx->initialized || !joints || !world) return -1; + + if (ctx->is_identity) { + int i; + memset(world, 0, sizeof(*world)); + for (i = 0; i < ctx->num_joints; i++) { + int ax = ctx->joint_to_axis[i]; + if (ax >= 0) emcPoseSetAxis(world, ax, joints[i]); + } + return 0; + } + + if (ctx->rt_only) return -1; + return ctx->ops.forward(joints, world, NULL, NULL); +} + +int kinematicsUserIsIdentity(KinematicsUserContext* ctx) +{ + if (!ctx || !ctx->initialized) return 0; + return ctx->is_identity; +} + +int kinematicsUserGetNumJoints(KinematicsUserContext* ctx) +{ + if (!ctx || !ctx->initialized) return 0; + return ctx->num_joints; +} + +KINEMATICS_TYPE kinematicsUserGetType(KinematicsUserContext* ctx) +{ + if (!ctx || !ctx->initialized) return KINEMATICS_IDENTITY; + return ctx->kins_type; +} + +const char* kinematicsUserGetModuleName(KinematicsUserContext* ctx) +{ + if (!ctx || !ctx->initialized) return "unknown"; + return ctx->module_name; +} + +int kinematicsUserRefreshParams(KinematicsUserContext* ctx) +{ + (void)ctx; + return 0; /* nothing to refresh: the bound pins are the live values */ +} + +int kinematicsUserIsRtOnly(KinematicsUserContext* ctx) +{ + if (!ctx || !ctx->initialized) return 1; + return ctx->rt_only; +} + +void kinematicsUserFree(KinematicsUserContext* ctx) +{ + int i; + + if (!ctx) return; + + /* Removing one hands its value back to the RT pin, leaving the + machine as it was found. */ + for (i = 0; i < ctx->num_made_signals; i++) { + hal_signal_delete(ctx->made_signal[i]); + } + if (ctx->rt_handle) dlclose(ctx->rt_handle); + free(ctx); +} diff --git a/src/emc/kinematics_userspace/kinematics_user.h b/src/emc/kinematics_userspace/kinematics_user.h new file mode 100644 index 00000000000..d01d8a8d277 --- /dev/null +++ b/src/emc/kinematics_userspace/kinematics_user.h @@ -0,0 +1,197 @@ +/******************************************************************** + * Description: kinematics_user.h + * Userspace kinematics interface for trajectory planning + * + * This provides a userspace-compatible kinematics interface that mirrors + * the RT kinematics interface. Used by the 9D planner to compute joint + * positions from world coordinates without requiring RT kernel calls. + * + * The kinematics module is loaded into this process and given input pins + * belonging to the caller's HAL component, connected to the same signals + * the running RT instance reads. Its own forward and inverse then work on + * live values, unmodified. + * + * Author: LinuxCNC + * License: GPL Version 2 + * System: Linux + * + * Copyright (c) 2024 All rights reserved. + ********************************************************************/ +#ifndef KINEMATICS_USER_H +#define KINEMATICS_USER_H + +#include /* EmcPose */ +#include /* KINEMATICS_TYPE, flags */ +#include /* hal_type_t, HAL_NAME_LEN */ + +#ifdef __cplusplus +extern "C" { +#endif + +/* Maximum number of joints supported */ +#define KINEMATICS_USER_MAX_JOINTS 9 + +/* Axis coordinate indices for EmcPose */ +typedef enum { + AXIS_X = 0, AXIS_Y = 1, AXIS_Z = 2, + AXIS_A = 3, AXIS_B = 4, AXIS_C = 5, + AXIS_U = 6, AXIS_V = 7, AXIS_W = 8, + AXIS_COUNT = 9 +} AxisIndex; + +/* Opaque context for userspace kinematics */ +typedef struct KinematicsUserContext KinematicsUserContext; + +/** + * Initialize userspace kinematics context + * + * The pins this creates belong to the caller's component, so call this + * after hal_init() and before hal_ready(): HAL refuses new pins once a + * component is ready. + * + * @param kins_type Kinematics module name (e.g., "trivkins", "5axiskins", "maxkins") + * @param num_joints Number of joints in the machine + * @param coordinates Coordinate string (e.g., "XYZABC", "XYZBCW") + * @param comp_id Caller's HAL component, from hal_init() + * @param prefix Its name, which the created pin names start with + * @return Allocated context, or NULL if kinematics type not supported + */ +KinematicsUserContext* kinematicsUserInit(const char* kins_type, + int num_joints, + const char* coordinates, + int comp_id, + const char* prefix); + +/** + * Perform inverse kinematics (world coords -> joint positions) + * + * @param ctx Kinematics context from kinematicsUserInit + * @param world World coordinates (X, Y, Z, A, B, C, U, V, W) + * @param joints Output array of joint positions [KINEMATICS_USER_MAX_JOINTS] + * @return 0 on success, -1 on failure + */ +int kinematicsUserInverse(KinematicsUserContext* ctx, + const EmcPose* world, + double* joints); + +/** + * Perform forward kinematics (joint positions -> world coords) + * + * @param ctx Kinematics context from kinematicsUserInit + * @param joints Array of joint positions [KINEMATICS_USER_MAX_JOINTS] + * @param world Output world coordinates + * @return 0 on success, -1 on failure + */ +int kinematicsUserForward(KinematicsUserContext* ctx, + const double* joints, + EmcPose* world); + +/** + * Check if kinematics type is identity (world coords = joint coords) + * + * @param ctx Kinematics context + * @return 1 if identity, 0 if not + */ +int kinematicsUserIsIdentity(KinematicsUserContext* ctx); + +/** + * Get number of joints + * + * @param ctx Kinematics context + * @return Number of joints + */ +int kinematicsUserGetNumJoints(KinematicsUserContext* ctx); + +/** + * Get KINEMATICS_TYPE (IDENTITY, BOTH, FORWARD_ONLY, INVERSE_ONLY) + * + * @param ctx Kinematics context + * @return KINEMATICS_TYPE enum value + */ +KINEMATICS_TYPE kinematicsUserGetType(KinematicsUserContext* ctx); + +/** + * Get kinematics module name + * + * @param ctx Kinematics context + * @return Module name string (e.g., "5axiskins") + */ +const char* kinematicsUserGetModuleName(KinematicsUserContext* ctx); + +/** + * Refresh kinematics parameters (no-op) + * + * The bound pins read the live values, so there is nothing to fetch. + * This function is kept for API compatibility but does nothing. + * + * @param ctx Kinematics context + * @return 0 always + */ +int kinematicsUserRefreshParams(KinematicsUserContext* ctx); + +/** + * Check if this context is RT-only + * + * An RT-only module exports no nonrt_attach() and so cannot be evaluated + * outside RT. Planner 2 is unavailable for such modules. + * + * @param ctx Kinematics context + * @return 1 if RT-only (planner 2 unavailable), 0 if the module is bound + */ +int kinematicsUserIsRtOnly(KinematicsUserContext* ctx); + +/** + * Free kinematics context + * + * @param ctx Context to free + */ +void kinematicsUserFree(KinematicsUserContext* ctx); + +/** + * Get axis value from EmcPose by index + * + * @param pose Pointer to EmcPose + * @param axis Axis index (AXIS_X through AXIS_W) + * @return Axis value + */ +static inline double emcPoseGetAxis(const EmcPose* pose, int axis) { + switch (axis) { + case AXIS_X: return pose->tran.x; + case AXIS_Y: return pose->tran.y; + case AXIS_Z: return pose->tran.z; + case AXIS_A: return pose->a; + case AXIS_B: return pose->b; + case AXIS_C: return pose->c; + case AXIS_U: return pose->u; + case AXIS_V: return pose->v; + case AXIS_W: return pose->w; + default: return 0.0; + } +} + +/** + * Set axis value in EmcPose by index + * + * @param pose Pointer to EmcPose + * @param axis Axis index (AXIS_X through AXIS_W) + * @param value Value to set + */ +static inline void emcPoseSetAxis(EmcPose* pose, int axis, double value) { + switch (axis) { + case AXIS_X: pose->tran.x = value; break; + case AXIS_Y: pose->tran.y = value; break; + case AXIS_Z: pose->tran.z = value; break; + case AXIS_A: pose->a = value; break; + case AXIS_B: pose->b = value; break; + case AXIS_C: pose->c = value; break; + case AXIS_U: pose->u = value; break; + case AXIS_V: pose->v = value; break; + case AXIS_W: pose->w = value; break; + } +} + +#ifdef __cplusplus +} +#endif + +#endif /* KINEMATICS_USER_H */ diff --git a/src/emc/motion_planning/Submakefile b/src/emc/motion_planning/Submakefile new file mode 100644 index 00000000000..553849e7ba5 --- /dev/null +++ b/src/emc/motion_planning/Submakefile @@ -0,0 +1,46 @@ +INCLUDES += emc/motion_planning +INCLUDES += emc/kinematics_userspace + +# Jacobian-based world-space limit calculation, plus the non-RT kinematics +# loader it sits on top of. +LIBKINSLIMITS_CXXSRCS := $(addprefix emc/motion_planning/, \ + jacobian.cc \ + joint_limits.cc \ + ) + +LIBKINSLIMITS_CSRCS := $(addprefix emc/kinematics_userspace/, \ + kinematics_user.c \ + ) + +USERSRCS += $(LIBKINSLIMITS_CXXSRCS) $(LIBKINSLIMITS_CSRCS) + +$(call TOOBJSDEPS, $(LIBKINSLIMITS_CXXSRCS)): EXTRAFLAGS = -fPIC +$(call TOOBJSDEPS, $(LIBKINSLIMITS_CSRCS)): EXTRAFLAGS = -fPIC -D_GNU_SOURCE + +../lib/libkinslimits.so.0: $(call TOOBJS, $(LIBKINSLIMITS_CXXSRCS) $(LIBKINSLIMITS_CSRCS)) \ + ../lib/libposemath.so.0 ../lib/liblinuxcnchal.so.0 + $(ECHO) Linking $(notdir $@) + @mkdir -p ../lib + $(Q)$(CXX) $(LDFLAGS) -Wl,-soname,$(notdir $@) -shared -o $@ $^ -ldl + +../lib/libkinslimits.so: ../lib/libkinslimits.so.0 + ln -sf $(notdir $<) $@ + +TARGETS += ../lib/libkinslimits.so ../lib/libkinslimits.so.0 + +# Diagnostic: print the Jacobian and the caps it implies for one move. +KINSLIMITS_SRCS := emc/motion_planning/kinslimits.cc +USERSRCS += $(KINSLIMITS_SRCS) + +../bin/kinslimits: $(call TOOBJS, $(KINSLIMITS_SRCS)) \ + ../lib/libkinslimits.so.0 ../lib/liblinuxcnchal.so.0 ../lib/libposemath.so.0 + $(ECHO) Linking $(notdir $@) + @mkdir -p ../bin + $(Q)$(CXX) $(LDFLAGS) -o $@ $^ + +TARGETS += ../bin/kinslimits + +MOTION_PLANNING_HH := emc/motion_planning/jacobian.hh emc/motion_planning/joint_limits.hh +$(patsubst emc/motion_planning/%,../include/%,$(MOTION_PLANNING_HH)): ../include/%.hh: emc/motion_planning/%.hh + cp $^ $@ +HEADERS += $(patsubst emc/motion_planning/%,../include/%,$(MOTION_PLANNING_HH)) diff --git a/src/emc/motion_planning/jacobian.cc b/src/emc/motion_planning/jacobian.cc new file mode 100644 index 00000000000..a7d5a7661e7 --- /dev/null +++ b/src/emc/motion_planning/jacobian.cc @@ -0,0 +1,197 @@ +/******************************************************************** + * Description: jacobian.cc + * Jacobian calculation implementation for userspace kinematics trajectory planning + * + * Author: LinuxCNC + * License: GPL Version 2 + * System: Linux + * + * Copyright (c) 2024 All rights reserved. + ********************************************************************/ + +#include "jacobian.hh" +#include +#include +#include + +namespace motion_planning { + +JacobianCalculator::JacobianCalculator() + : kins_ctx_(nullptr), + is_identity_(false), + num_joints_(0) { +} + +JacobianCalculator::~JacobianCalculator() { + // kins_ctx_ is owned externally +} + +bool JacobianCalculator::init(KinematicsUserContext* kins_ctx) { + if (!kins_ctx) { + return false; + } + + kins_ctx_ = kins_ctx; + is_identity_ = (kinematicsUserIsIdentity(kins_ctx) != 0); + num_joints_ = kinematicsUserGetNumJoints(kins_ctx); + + return true; +} + +void JacobianCalculator::computeTrivkins(double J[9][9]) { + // Zero the matrix + std::memset(J, 0, sizeof(double) * 9 * 9); + + // For trivkins, the Jacobian is identity (with axis mapping) + // Since trivkins maps: joint[i] = world_axis[mapped_axis[i]] + // The Jacobian is: J[joint][axis] = 1 if axis == mapped_axis[joint], else 0 + + // For a simple XYZ trivkins: + // J[0][AXIS_X] = 1 (joint 0 = X) + // J[1][AXIS_Y] = 1 (joint 1 = Y) + // J[2][AXIS_Z] = 1 (joint 2 = Z) + // etc. + + // We need to query the kinematics context for the mapping. + // Since the context is opaque, we use inverse kinematics to determine + // the mapping. + + // Test each axis: perturb it and see which joint changes + EmcPose zero_pose; + ZERO_EMC_POSE(zero_pose); + double zero_joints[9]; + kinematicsUserInverse(kins_ctx_, &zero_pose, zero_joints); + + for (int axis = 0; axis < AXIS_COUNT; axis++) { + EmcPose test_pose = zero_pose; + emcPoseSetAxis(&test_pose, axis, 1.0); + + double test_joints[9]; + kinematicsUserInverse(kins_ctx_, &test_pose, test_joints); + + for (int joint = 0; joint < num_joints_; joint++) { + double delta = test_joints[joint] - zero_joints[joint]; + if (std::fabs(delta) > 0.5) { + // This axis maps to this joint + J[joint][axis] = 1.0; + } + } + } +} + +bool JacobianCalculator::computeNumerical(const EmcPose& pose, double J[9][9]) { + // Zero the matrix + std::memset(J, 0, sizeof(double) * 9 * 9); + + // Compute joints at nominal pose + double joints_center[9]; + if (kinematicsUserInverse(kins_ctx_, &pose, joints_center) != 0) { + return false; + } + + // Perturb each axis and compute derivatives + for (int axis = 0; axis < AXIS_COUNT; axis++) { + // Choose perturbation size based on axis type + double delta = (axis < 3 || axis >= 6) ? DELTA_LINEAR : DELTA_ROTARY; + + // Positive perturbation + EmcPose pose_plus = pose; + double val_plus = emcPoseGetAxis(&pose_plus, axis) + delta; + emcPoseSetAxis(&pose_plus, axis, val_plus); + + double joints_plus[9]; + if (kinematicsUserInverse(kins_ctx_, &pose_plus, joints_plus) != 0) { + // Kinematics failed - use one-sided difference + for (int joint = 0; joint < num_joints_; joint++) { + J[joint][axis] = (joints_plus[joint] - joints_center[joint]) / delta; + } + continue; + } + + // Negative perturbation + EmcPose pose_minus = pose; + double val_minus = emcPoseGetAxis(&pose_minus, axis) - delta; + emcPoseSetAxis(&pose_minus, axis, val_minus); + + double joints_minus[9]; + if (kinematicsUserInverse(kins_ctx_, &pose_minus, joints_minus) != 0) { + // Use forward difference + for (int joint = 0; joint < num_joints_; joint++) { + J[joint][axis] = (joints_plus[joint] - joints_center[joint]) / delta; + } + continue; + } + + // Central difference (most accurate) + for (int joint = 0; joint < num_joints_; joint++) { + J[joint][axis] = (joints_plus[joint] - joints_minus[joint]) / (2.0 * delta); + } + } + + // Check for NaN/Inf values and replace with safe defaults + bool had_nan = false; + for (int joint = 0; joint < num_joints_; joint++) { + for (int axis = 0; axis < AXIS_COUNT; axis++) { + if (!std::isfinite(J[joint][axis])) { + // Replace NaN/Inf with 0 (assume no coupling) + J[joint][axis] = 0.0; + had_nan = true; + } + } + } + + // If we had NaN values, the Jacobian may be unreliable + // Return true anyway but the condition number check will catch issues + (void)had_nan; // Could log this in debug mode + + return true; +} + +bool JacobianCalculator::compute(const EmcPose& pose, double J[9][9]) { + if (!kins_ctx_) { + return false; + } + + if (is_identity_) { + // For trivkins, use the fast identity computation + computeTrivkins(J); + return true; + } else { + // For non-trivial kinematics, use numerical differentiation + return computeNumerical(pose, J); + } +} + +double JacobianCalculator::conditionNumber(const double J[9][9]) { + if (is_identity_) { + // Identity matrix has condition number 1 + return 1.0; + } + + // We use a simplified condition number estimate: + // Find the ratio of largest to smallest row norms + // This is not the true 2-norm condition number, but gives a rough indication + + double max_row_norm = 0.0; + double min_row_norm = 1e18; + + for (int joint = 0; joint < num_joints_; joint++) { + double row_norm = 0.0; + for (int axis = 0; axis < AXIS_COUNT; axis++) { + row_norm += J[joint][axis] * J[joint][axis]; + } + row_norm = std::sqrt(row_norm); + + if (row_norm > max_row_norm) max_row_norm = row_norm; + if (row_norm > 1e-15 && row_norm < min_row_norm) min_row_norm = row_norm; + } + + if (min_row_norm < 1e-15) { + // Near-singular: a row is almost zero + return 1e18; + } + + return max_row_norm / min_row_norm; +} + +} // namespace motion_planning diff --git a/src/emc/motion_planning/jacobian.hh b/src/emc/motion_planning/jacobian.hh new file mode 100644 index 00000000000..8713e89f180 --- /dev/null +++ b/src/emc/motion_planning/jacobian.hh @@ -0,0 +1,99 @@ +/******************************************************************** + * Description: jacobian.hh + * Jacobian calculation for userspace kinematics trajectory planning + * + * Computes the Jacobian matrix relating world velocities to joint + * velocities. For trivkins this is the identity matrix. + * + * Author: LinuxCNC + * License: GPL Version 2 + * System: Linux + * + * Copyright (c) 2024 All rights reserved. + ********************************************************************/ +#ifndef JACOBIAN_HH +#define JACOBIAN_HH + +#include +#include + +namespace motion_planning { + +/** + * Jacobian calculator class + * + * Computes the Jacobian matrix J where: + * joint_velocities = J × world_velocities + * + * For trivkins, J is the identity matrix (with appropriate axis mapping). + * For non-trivial kinematics, J is computed via numerical differentiation. + */ +class JacobianCalculator { +public: + JacobianCalculator(); + ~JacobianCalculator(); + + /** + * Initialize with kinematics context + * + * @param kins_ctx Userspace kinematics context + * @return true on success + */ + bool init(KinematicsUserContext* kins_ctx); + + /** + * Compute Jacobian at a given pose + * + * The Jacobian J[joint][axis] relates: + * d(joint[j])/dt = sum over axis a of J[j][a] * d(axis[a])/dt + * + * @param pose World pose at which to compute Jacobian + * @param J Output 9×9 Jacobian matrix [joint][axis] + * @return true on success, false on failure + */ + bool compute(const EmcPose& pose, double J[9][9]); + + /** + * Compute condition number of Jacobian + * + * The condition number indicates how close to a singularity the pose is. + * High condition number = near singularity. + * + * For trivkins, always returns 1.0 (no singularities). + * + * @param J Jacobian matrix + * @return Condition number (≥ 1.0), or -1.0 on error + */ + double conditionNumber(const double J[9][9]); + + /** + * Check if current kinematics is identity (trivkins) + */ + bool isIdentity() const { return is_identity_; } + +private: + /** + * Compute Jacobian for trivkins (identity with axis mapping) + */ + void computeTrivkins(double J[9][9]); + + /** + * Compute Jacobian via numerical differentiation + * Uses central differences: J[j][a] = (f(x+h) - f(x-h)) / (2h) + */ + bool computeNumerical(const EmcPose& pose, double J[9][9]); + + KinematicsUserContext* kins_ctx_; + bool is_identity_; + int num_joints_; + + // Perturbation size for numerical differentiation (mm or degrees) + // Must be large enough for kinematics to produce stable results + // but small enough for accurate derivatives + static constexpr double DELTA_LINEAR = 0.1; // 0.1 mm + static constexpr double DELTA_ROTARY = 0.1; // 0.1 degrees +}; + +} // namespace motion_planning + +#endif // JACOBIAN_HH diff --git a/src/emc/motion_planning/joint_limits.cc b/src/emc/motion_planning/joint_limits.cc new file mode 100644 index 00000000000..ee4ee34d06f --- /dev/null +++ b/src/emc/motion_planning/joint_limits.cc @@ -0,0 +1,358 @@ +/******************************************************************** + * Description: joint_limits.cc + * Joint limit calculation implementation for userspace kinematics trajectory planning + * + * Author: LinuxCNC + * License: GPL Version 2 + * System: Linux + * + * Copyright (c) 2024 All rights reserved. + ********************************************************************/ + +#include "joint_limits.hh" +#include +#include +#include + +namespace motion_planning { + +JointLimitCalculator::JointLimitCalculator() + : num_joints_(0), + initialized_(false) { +} + +JointLimitCalculator::~JointLimitCalculator() { +} + +bool JointLimitCalculator::init(int num_joints) { + if (num_joints < 1 || num_joints > KINEMATICS_USER_MAX_JOINTS) { + return false; + } + + num_joints_ = num_joints; + + // Initialize with default (very permissive) limits + for (int i = 0; i < KINEMATICS_USER_MAX_JOINTS; i++) { + limits_[i] = JointLimitConfig(); + } + + initialized_ = true; + return true; +} + +bool JointLimitCalculator::setJointLimits(int joint, const JointLimitConfig& limits) { + if (joint < 0 || joint >= num_joints_) { + return false; + } + limits_[joint] = limits; + return true; +} + +const JointLimitConfig& JointLimitCalculator::getJointLimits(int joint) const { + static JointLimitConfig default_limits; + if (joint < 0 || joint >= num_joints_) { + return default_limits; + } + return limits_[joint]; +} + +double JointLimitCalculator::getJointVelLimit(int joint) const { + if (joint < 0 || joint >= num_joints_) return 1e9; + return limits_[joint].vel_limit; +} + +double JointLimitCalculator::getJointAccLimit(int joint) const { + if (joint < 0 || joint >= num_joints_) return 1e9; + return limits_[joint].acc_limit; +} + +double JointLimitCalculator::getJointJerkLimit(int joint) const { + if (joint < 0 || joint >= num_joints_) return 1e9; + return limits_[joint].jerk_limit; +} + +bool JointLimitCalculator::updateAllLimits(const double* vel_limits, + const double* acc_limits, + const double* min_pos, + const double* max_pos, + const double* jerk_limits) { + if (!initialized_) { + return false; + } + + // Update limits from arrays + // This is used to refresh limits from shared memory (motion status), + // which reflects any runtime changes via HAL pins (ini.N.max_limit, etc.) + for (int j = 0; j < num_joints_; j++) { + if (vel_limits) limits_[j].vel_limit = vel_limits[j]; + if (acc_limits) limits_[j].acc_limit = acc_limits[j]; + if (min_pos) limits_[j].min_pos_limit = min_pos[j]; + if (max_pos) limits_[j].max_pos_limit = max_pos[j]; + if (jerk_limits) limits_[j].jerk_limit = jerk_limits[j]; + } + + return true; +} + +bool JointLimitCalculator::checkPositionLimits(const double joint_pos[9]) { + for (int j = 0; j < num_joints_; j++) { + if (joint_pos[j] > limits_[j].max_pos_limit || + joint_pos[j] < limits_[j].min_pos_limit) { + return false; + } + } + return true; +} + +double JointLimitCalculator::computeMaxVelocity(const double J[9][9], int& limiting_joint) { + // Conservative estimate: assume worst-case direction + // For each joint j, find the maximum Jacobian element magnitude + // max_world_vel = min over j of: vel_limit[j] / max(|J[j][:]|) + + double max_world_vel = 1e18; + limiting_joint = -1; + + for (int j = 0; j < num_joints_; j++) { + // Find maximum absolute value in this row of J + double max_abs_J = 0.0; + for (int a = 0; a < AXIS_COUNT; a++) { + double abs_J = std::fabs(J[j][a]); + if (abs_J > max_abs_J) { + max_abs_J = abs_J; + } + } + + if (max_abs_J > 1e-15) { + // This joint contributes to motion + double vel_limit_world = limits_[j].vel_limit / max_abs_J; + if (vel_limit_world < max_world_vel) { + max_world_vel = vel_limit_world; + limiting_joint = j; + } + } + } + + // Apply sanity bounds + if (max_world_vel > 1e9) max_world_vel = 1e9; + if (max_world_vel < 1e-9) max_world_vel = 1e-9; + + return max_world_vel; +} + +double JointLimitCalculator::computeMaxAcceleration(const double J[9][9], int& limiting_joint) { + // Same approach as velocity + double max_world_acc = 1e18; + limiting_joint = -1; + + for (int j = 0; j < num_joints_; j++) { + double max_abs_J = 0.0; + for (int a = 0; a < AXIS_COUNT; a++) { + double abs_J = std::fabs(J[j][a]); + if (abs_J > max_abs_J) { + max_abs_J = abs_J; + } + } + + if (max_abs_J > 1e-15) { + double acc_limit_world = limits_[j].acc_limit / max_abs_J; + if (acc_limit_world < max_world_acc) { + max_world_acc = acc_limit_world; + limiting_joint = j; + } + } + } + + if (max_world_acc > 1e9) max_world_acc = 1e9; + if (max_world_acc < 1e-9) max_world_acc = 1e-9; + + return max_world_acc; +} + +double JointLimitCalculator::computeMaxJerk(const double J[9][9], int& limiting_joint) { + // Same approach as velocity and acceleration + double max_world_jerk = 1e18; + limiting_joint = -1; + + for (int j = 0; j < num_joints_; j++) { + double max_abs_J = 0.0; + for (int a = 0; a < AXIS_COUNT; a++) { + double abs_J = std::fabs(J[j][a]); + if (abs_J > max_abs_J) { + max_abs_J = abs_J; + } + } + + if (max_abs_J > 1e-15) { + double jerk_limit_world = limits_[j].jerk_limit / max_abs_J; + if (jerk_limit_world < max_world_jerk) { + max_world_jerk = jerk_limit_world; + limiting_joint = j; + } + } + } + + if (max_world_jerk > 1e9) max_world_jerk = 1e9; + if (max_world_jerk < 1e-9) max_world_jerk = 1e-9; + + return max_world_jerk; +} + +double JointLimitCalculator::computeMaxVelocityForTangent(const double J[9][9], const double tangent[9], int& limiting_joint) { + double max_world_vel = 1e18; + limiting_joint = -1; + + for (int j = 0; j < num_joints_; j++) { + // Compute sum(|J[j][a]| * |tangent[a]|) — the actual amplification + // for this joint along the given path direction + double amplification = 0.0; + for (int a = 0; a < AXIS_COUNT; a++) { + amplification += std::fabs(J[j][a]) * std::fabs(tangent[a]); + } + + if (amplification > 1e-15) { + double vel_limit_world = limits_[j].vel_limit / amplification; + if (vel_limit_world < max_world_vel) { + max_world_vel = vel_limit_world; + limiting_joint = j; + } + } + } + + if (max_world_vel > 1e9) max_world_vel = 1e9; + if (max_world_vel < 1e-9) max_world_vel = 1e-9; + return max_world_vel; +} + +double JointLimitCalculator::computeMaxAccelerationForTangent(const double J[9][9], const double tangent[9], int& limiting_joint) { + double max_world_acc = 1e18; + limiting_joint = -1; + + for (int j = 0; j < num_joints_; j++) { + double amplification = 0.0; + for (int a = 0; a < AXIS_COUNT; a++) { + amplification += std::fabs(J[j][a]) * std::fabs(tangent[a]); + } + + if (amplification > 1e-15) { + double acc_limit_world = limits_[j].acc_limit / amplification; + if (acc_limit_world < max_world_acc) { + max_world_acc = acc_limit_world; + limiting_joint = j; + } + } + } + + if (max_world_acc > 1e9) max_world_acc = 1e9; + if (max_world_acc < 1e-9) max_world_acc = 1e-9; + return max_world_acc; +} + +double JointLimitCalculator::computeMaxJerkForTangent(const double J[9][9], const double tangent[9], int& limiting_joint) { + double max_world_jerk = 1e18; + limiting_joint = -1; + + for (int j = 0; j < num_joints_; j++) { + double amplification = 0.0; + for (int a = 0; a < AXIS_COUNT; a++) { + amplification += std::fabs(J[j][a]) * std::fabs(tangent[a]); + } + + if (amplification > 1e-15) { + double jerk_limit_world = limits_[j].jerk_limit / amplification; + if (jerk_limit_world < max_world_jerk) { + max_world_jerk = jerk_limit_world; + limiting_joint = j; + } + } + } + + if (max_world_jerk > 1e9) max_world_jerk = 1e9; + if (max_world_jerk < 1e-9) max_world_jerk = 1e-9; + return max_world_jerk; +} + +bool JointLimitCalculator::computeForTangent(const double J[9][9], + const double joint_pos[9], + const double tangent[9], + JointLimitResult& result, + double singularity_threshold) { + if (!initialized_) { + return false; + } + + result.position_ok = checkPositionLimits(joint_pos); + result.condition_number = computeConditionNumber(J); + + result.max_world_vel = computeMaxVelocityForTangent(J, tangent, result.limiting_joint_vel); + result.max_world_acc = computeMaxAccelerationForTangent(J, tangent, result.limiting_joint_acc); + result.max_world_jerk = computeMaxJerkForTangent(J, tangent, result.limiting_joint_jerk); + + if (result.condition_number > singularity_threshold) { + double slowdown_factor = singularity_threshold / result.condition_number; + result.max_world_vel *= slowdown_factor; + result.max_world_acc *= slowdown_factor; + result.max_world_jerk *= slowdown_factor; + } + + return true; +} + +double JointLimitCalculator::computeConditionNumber(const double J[9][9]) { + // Simplified condition number: ratio of max to min row norms + double max_row_norm = 0.0; + double min_row_norm = 1e18; + + for (int j = 0; j < num_joints_; j++) { + double row_norm = 0.0; + for (int a = 0; a < AXIS_COUNT; a++) { + row_norm += J[j][a] * J[j][a]; + } + row_norm = std::sqrt(row_norm); + + if (row_norm > max_row_norm) max_row_norm = row_norm; + if (row_norm > 1e-15 && row_norm < min_row_norm) min_row_norm = row_norm; + } + + if (min_row_norm < 1e-15) { + return 1e18; // Near-singular + } + + return max_row_norm / min_row_norm; +} + +bool JointLimitCalculator::compute(const double J[9][9], + const double joint_pos[9], + JointLimitResult& result, + double singularity_threshold) { + if (!initialized_) { + return false; + } + + // Check position limits + result.position_ok = checkPositionLimits(joint_pos); + + // Compute condition number + result.condition_number = computeConditionNumber(J); + + // Compute max velocity + result.max_world_vel = computeMaxVelocity(J, result.limiting_joint_vel); + + // Compute max acceleration + result.max_world_acc = computeMaxAcceleration(J, result.limiting_joint_acc); + + // Compute max jerk + result.max_world_jerk = computeMaxJerk(J, result.limiting_joint_jerk); + + // Apply singularity slowdown + // If condition number exceeds threshold, reduce limits proportionally + if (result.condition_number > singularity_threshold) { + double slowdown_factor = singularity_threshold / result.condition_number; + result.max_world_vel *= slowdown_factor; + result.max_world_acc *= slowdown_factor; + result.max_world_jerk *= slowdown_factor; + } + + return true; +} + +} // namespace motion_planning diff --git a/src/emc/motion_planning/joint_limits.hh b/src/emc/motion_planning/joint_limits.hh new file mode 100644 index 00000000000..d8bcbd82b57 --- /dev/null +++ b/src/emc/motion_planning/joint_limits.hh @@ -0,0 +1,234 @@ +/******************************************************************** + * Description: joint_limits.hh + * Joint limit calculation for userspace kinematics trajectory planning + * + * Uses the Jacobian to compute maximum world-space velocity and + * acceleration that respects all joint limits. + * + * Author: LinuxCNC + * License: GPL Version 2 + * System: Linux + * + * Copyright (c) 2024 All rights reserved. + ********************************************************************/ +#ifndef JOINT_LIMITS_HH +#define JOINT_LIMITS_HH + +#include +#include + +namespace motion_planning { + +/** + * Joint limit configuration + * Mirrors emcmot_joint_t limits from motion.h + */ +struct JointLimitConfig { + double max_pos_limit; // Upper soft limit on joint position + double min_pos_limit; // Lower soft limit on joint position + double vel_limit; // Maximum joint velocity + double acc_limit; // Maximum joint acceleration + double jerk_limit; // Maximum joint jerk (for S-curve planning) + + JointLimitConfig() : + max_pos_limit(1e9), + min_pos_limit(-1e9), + vel_limit(1e9), + acc_limit(1e9), + jerk_limit(1e9) {} +}; + +/** + * Result of joint limit calculation + */ +struct JointLimitResult { + double max_world_vel; // Max world velocity respecting joint vel limits + double max_world_acc; // Max world accel respecting joint acc limits + double max_world_jerk; // Max world jerk (for S-curve planning) + bool position_ok; // True if joint positions are within soft limits + int limiting_joint_vel; // Joint index that limits velocity (-1 if none) + int limiting_joint_acc; // Joint index that limits acceleration + int limiting_joint_jerk; // Joint index that limits jerk + double condition_number; // Jacobian condition number (singularity indicator) + + JointLimitResult() : + max_world_vel(1e9), + max_world_acc(1e9), + max_world_jerk(1e9), + position_ok(true), + limiting_joint_vel(-1), + limiting_joint_acc(-1), + limiting_joint_jerk(-1), + condition_number(1.0) {} +}; + +/** + * Joint limit calculator class + * + * Computes maximum world-space velocity/acceleration that respects + * all joint limits, given the Jacobian at a pose. + * + * The relationship is: + * joint_vel = J × world_vel + * |joint_vel[j]| ≤ joint_limit[j].vel_limit for all j + * + * To find max world velocity, we solve: + * max_world_vel = min over all joints j of: + * joint_limit[j].vel_limit / |J[j] · direction| + * + * For a general direction, we use a conservative estimate: + * max_world_vel = min over all joints j of: + * joint_limit[j].vel_limit / max(|J[j][:]|) + */ +class JointLimitCalculator { +public: + JointLimitCalculator(); + ~JointLimitCalculator(); + + /** + * Initialize with number of joints + * + * @param num_joints Number of joints + * @return true on success + */ + bool init(int num_joints); + + /** + * Set limits for a joint + * + * @param joint Joint index (0 to num_joints-1) + * @param limits Limit configuration for this joint + * @return true on success + */ + bool setJointLimits(int joint, const JointLimitConfig& limits); + + /** + * Update limits for all joints at once + * + * This is used to refresh limits from shared memory (motion status structure), + * which reflects any runtime changes via HAL pins (ini.N.max_limit, etc.) + * + * @param vel_limits Array of velocity limits [num_joints] + * @param acc_limits Array of acceleration limits [num_joints] + * @param min_pos Array of min position limits [num_joints] + * @param max_pos Array of max position limits [num_joints] + * @param jerk_limits Array of jerk limits [num_joints] (can be NULL) + * @return true on success + */ + bool updateAllLimits(const double* vel_limits, + const double* acc_limits, + const double* min_pos, + const double* max_pos, + const double* jerk_limits = nullptr); + + /** + * Get limits for a joint + */ + const JointLimitConfig& getJointLimits(int joint) const; + + /** + * Get velocity limit for a specific joint + */ + double getJointVelLimit(int joint) const; + + /** + * Get acceleration limit for a specific joint + */ + double getJointAccLimit(int joint) const; + + /** + * Get jerk limit for a specific joint + */ + double getJointJerkLimit(int joint) const; + + /** + * Compute world-space limits at a pose given the Jacobian + * + * Uses conservative direction-independent bound (max |J[j][:]|). + * + * @param J Jacobian matrix [joint][axis] + * @param joint_pos Current joint positions (for position limit check) + * @param result Output limit result + * @param singularity_threshold Condition number threshold for singularity + * @return true on success + */ + bool compute(const double J[9][9], + const double joint_pos[9], + JointLimitResult& result, + double singularity_threshold = 100.0); + + /** + * Compute world-space limits for a specific path tangent direction + * + * Uses the actual path tangent to compute tight bounds. The tangent + * is in world-axis units per unit of the Ruckig path parameter (which + * may be XYZ arc length). Rotary components can be >> 1.0 when + * rotary axes move much more than linear axes per unit path. + * + * The bound for each joint is: + * limit[j] / sum(|J[j][a]| * |tangent[a]|) + * + * @param J Jacobian matrix [joint][axis] + * @param joint_pos Current joint positions (for position limit check) + * @param tangent Path tangent: d(world_axis)/d(path_param) [9] + * @param result Output limit result + * @param singularity_threshold Condition number threshold for singularity + * @return true on success + */ + bool computeForTangent(const double J[9][9], + const double joint_pos[9], + const double tangent[9], + JointLimitResult& result, + double singularity_threshold = 100.0); + + /** + * Check if joint positions are within soft limits + * + * @param joint_pos Array of joint positions + * @return true if all joints within limits + */ + bool checkPositionLimits(const double joint_pos[9]); + + /** + * Get the number of joints + */ + int getNumJoints() const { return num_joints_; } + +private: + /** + * Compute maximum world velocity from joint velocity limits and Jacobian + * + * Uses conservative estimate: max over all directions + */ + double computeMaxVelocity(const double J[9][9], int& limiting_joint); + + /** + * Compute maximum world acceleration from joint accel limits and Jacobian + */ + double computeMaxAcceleration(const double J[9][9], int& limiting_joint); + + /** + * Compute maximum world jerk from joint jerk limits and Jacobian + */ + double computeMaxJerk(const double J[9][9], int& limiting_joint); + + /** + * Tangent-aware versions: use sum(|J[j][a]| * |tangent[a]|) instead of max(|J[j][a]|) + */ + double computeMaxVelocityForTangent(const double J[9][9], const double tangent[9], int& limiting_joint); + double computeMaxAccelerationForTangent(const double J[9][9], const double tangent[9], int& limiting_joint); + double computeMaxJerkForTangent(const double J[9][9], const double tangent[9], int& limiting_joint); + + /** + * Compute Jacobian condition number (simplified) + */ + double computeConditionNumber(const double J[9][9]); + + int num_joints_; + JointLimitConfig limits_[KINEMATICS_USER_MAX_JOINTS]; + bool initialized_; +}; + +} // namespace motion_planning + +#endif // JOINT_LIMITS_HH diff --git a/src/emc/motion_planning/kinslimits.cc b/src/emc/motion_planning/kinslimits.cc new file mode 100644 index 00000000000..1b750be6776 --- /dev/null +++ b/src/emc/motion_planning/kinslimits.cc @@ -0,0 +1,269 @@ +/******************************************************************** + * Description: kinslimits.cc + * Diagnostic tool: print the Jacobian and the world-space velocity, + * acceleration and jerk caps that a given kinematics module imposes + * on a straight move between two poses. + * + * The tool attaches to a running HAL instance, loads the kinematics + * module through the non-RT interface, samples the move, and reports + * the most restrictive cap found along it. The sampling loop here is + * the same one the trajectory planner uses to cap a segment. + * + * Example (in a terminal with a running config, or under halrun): + * + * halrun -I + * halcmd: loadrt 5axiskins coordinates=XYZBCW + * halcmd: setp 5axiskins.pivot-length 100 + * halcmd: loadusr -w kinslimits --module 5axiskins --joints 6 \ + * --coords XYZBCW --start 0,0,0,0,0,0,0,0,0 \ + * --end 100,0,0,0,90,0,0,0,0 \ + * --vel 100,100,100,30,30,30 --acc 500,500,500,200,200,200 + * + * Author: LinuxCNC + * License: GPL Version 2 + * System: Linux + * + * Copyright (c) 2024 All rights reserved. + ********************************************************************/ + +#include +#include +#include +#include +#include +#include + +#include +#include "jacobian.hh" +#include "joint_limits.hh" + +using namespace motion_planning; + +static const char *AXIS_NAME[9] = {"X","Y","Z","A","B","C","U","V","W"}; + +static std::vector parse_list(const char *s) +{ + std::vector out; + const char *p = s; + while (*p) { + char *endp = nullptr; + double v = strtod(p, &endp); + if (endp == p) break; + out.push_back(v); + p = endp; + while (*p == ',' || *p == ' ') p++; + } + return out; +} + +static void list_to_pose(const std::vector& v, EmcPose *p) +{ + double a[9] = {0,0,0,0,0,0,0,0,0}; + for (size_t i = 0; i < v.size() && i < 9; i++) a[i] = v[i]; + p->tran.x = a[0]; p->tran.y = a[1]; p->tran.z = a[2]; + p->a = a[3]; p->b = a[4]; p->c = a[5]; + p->u = a[6]; p->v = a[7]; p->w = a[8]; +} + +static double pose_axis(const EmcPose& p, int ax) +{ + switch (ax) { + case 0: return p.tran.x; case 1: return p.tran.y; case 2: return p.tran.z; + case 3: return p.a; case 4: return p.b; case 5: return p.c; + case 6: return p.u; case 7: return p.v; default: return p.w; + } +} + +static void set_pose_axis(EmcPose *p, int ax, double val) +{ + switch (ax) { + case 0: p->tran.x = val; break; case 1: p->tran.y = val; break; + case 2: p->tran.z = val; break; case 3: p->a = val; break; + case 4: p->b = val; break; case 5: p->c = val; break; + case 6: p->u = val; break; case 7: p->v = val; break; + default: p->w = val; break; + } +} + +static void usage(const char *argv0) +{ + fprintf(stderr, + "usage: %s --module NAME --joints N --coords LETTERS\n" + " --start x,y,z,a,b,c,u,v,w --end x,y,z,a,b,c,u,v,w\n" + " --vel v0,v1,... --acc a0,a1,... [--jerk j0,j1,...]\n" + " [--samples N] [--singularity COND]\n" + "\n" + "Prints the Jacobian and the world-space caps the joint limits imply\n" + "for a straight move from --start to --end. Requires a running HAL\n" + "instance with the kinematics module loaded.\n", argv0); +} + +int main(int argc, char **argv) +{ + const char *module = nullptr; + const char *coords = nullptr; + int num_joints = 0; + int samples = 11; + double singularity = 100.0; + std::vector start_v, end_v, vel_v, acc_v, jerk_v; + + for (int i = 1; i < argc; i++) { + const char *a = argv[i]; + const char *next = (i + 1 < argc) ? argv[i + 1] : nullptr; + if (!strcmp(a, "--module") && next) { module = next; i++; } + else if (!strcmp(a, "--coords") && next) { coords = next; i++; } + else if (!strcmp(a, "--joints") && next) { num_joints = atoi(next); i++; } + else if (!strcmp(a, "--samples") && next) { samples = atoi(next); i++; } + else if (!strcmp(a, "--singularity") && next){ singularity = atof(next); i++; } + else if (!strcmp(a, "--start") && next) { start_v = parse_list(next); i++; } + else if (!strcmp(a, "--end") && next) { end_v = parse_list(next); i++; } + else if (!strcmp(a, "--vel") && next) { vel_v = parse_list(next); i++; } + else if (!strcmp(a, "--acc") && next) { acc_v = parse_list(next); i++; } + else if (!strcmp(a, "--jerk") && next) { jerk_v = parse_list(next); i++; } + else { usage(argv[0]); return 1; } + } + + if (!module || !coords || num_joints < 1 || + start_v.empty() || end_v.empty() || vel_v.empty() || acc_v.empty()) { + usage(argv[0]); + return 1; + } + if ((int)vel_v.size() < num_joints || (int)acc_v.size() < num_joints) { + fprintf(stderr, "kinslimits: --vel and --acc need %d entries\n", num_joints); + return 1; + } + if (samples < 2) samples = 2; + + int comp_id = hal_init("kinslimits"); + if (comp_id < 0) { + fprintf(stderr, "kinslimits: hal_init failed (is HAL running?)\n"); + return 1; + } + + KinematicsUserContext *ctx = kinematicsUserInit(module, num_joints, coords, + comp_id, "kinslimits"); + if (!ctx) { + fprintf(stderr, "kinslimits: kinematicsUserInit failed for '%s'\n", module); + hal_exit(comp_id); + return 1; + } + if (kinematicsUserIsRtOnly(ctx)) { + fprintf(stderr, "kinslimits: '%s' is RT-only, no non-RT interface\n", module); + kinematicsUserFree(ctx); + hal_exit(comp_id); + return 1; + } + + JacobianCalculator jac; + JointLimitCalculator lim; + if (!jac.init(ctx) || !lim.init(num_joints)) { + fprintf(stderr, "kinslimits: calculator init failed\n"); + kinematicsUserFree(ctx); + hal_exit(comp_id); + return 1; + } + + std::vector minpos(num_joints, -1e9), maxpos(num_joints, 1e9); + if ((int)jerk_v.size() < num_joints) jerk_v.assign(num_joints, 1e9); + lim.updateAllLimits(vel_v.data(), acc_v.data(), + minpos.data(), maxpos.data(), jerk_v.data()); + + EmcPose start, end; + list_to_pose(start_v, &start); + list_to_pose(end_v, &end); + + /* Path parameter: XYZ arc length, falling back to the largest rotary + delta for a pure rotary move, matching what the planner uses. */ + double dx = end.tran.x - start.tran.x; + double dy = end.tran.y - start.tran.y; + double dz = end.tran.z - start.tran.z; + double target = sqrt(dx*dx + dy*dy + dz*dz); + if (target < 1e-12) { + for (int ax = 3; ax < 9; ax++) { + double d = fabs(pose_axis(end, ax) - pose_axis(start, ax)); + if (d > target) target = d; + } + } + if (target < 1e-12) { + fprintf(stderr, "kinslimits: start and end are the same pose\n"); + kinematicsUserFree(ctx); + hal_exit(comp_id); + return 1; + } + + /* tangent[a] = d(world axis a) / d(path parameter) */ + double tangent[9]; + for (int ax = 0; ax < 9; ax++) { + tangent[ax] = (pose_axis(end, ax) - pose_axis(start, ax)) / target; + } + + printf("module : %s (%s, %d joints)%s\n", module, coords, num_joints, + kinematicsUserIsIdentity(ctx) ? " [identity]" : ""); + printf("path length : %.6f (tangent units per path unit)\n", target); + printf("tangent :"); + for (int ax = 0; ax < 9; ax++) { + if (fabs(tangent[ax]) > 1e-12) printf(" %s=%.4f", AXIS_NAME[ax], tangent[ax]); + } + printf("\n\n"); + + double min_vel = 1e9, min_acc = 1e9, min_jerk = 1e9, max_cond = 1.0; + int at_vel = -1, at_acc = -1, at_jerk = -1; + double min_vel_s = 0.0; + + for (int i = 0; i < samples; i++) { + double frac = (double)i / (double)(samples - 1); + EmcPose p; + for (int ax = 0; ax < 9; ax++) { + set_pose_axis(&p, ax, + pose_axis(start, ax) + frac * (pose_axis(end, ax) - pose_axis(start, ax))); + } + + double joints[KINEMATICS_USER_MAX_JOINTS] = {0}; + if (kinematicsUserInverse(ctx, &p, joints) != 0) { + printf("sample %2d: inverse kinematics failed\n", i); + continue; + } + + double J[9][9]; + if (!jac.compute(p, J)) { + printf("sample %2d: Jacobian failed\n", i); + continue; + } + + double jpad[9] = {0}; + for (int j = 0; j < num_joints && j < 9; j++) jpad[j] = joints[j]; + + JointLimitResult r; + if (!lim.computeForTangent(J, jpad, tangent, r, singularity)) { + printf("sample %2d: limit calculation failed\n", i); + continue; + } + + printf("s=%.3f vel<=%10.3f (j%d) acc<=%10.1f (j%d) jerk<=%12.1f (j%d) cond=%.2f\n", + frac, r.max_world_vel, r.limiting_joint_vel, + r.max_world_acc, r.limiting_joint_acc, + r.max_world_jerk, r.limiting_joint_jerk, r.condition_number); + + if (r.max_world_vel < min_vel) { min_vel = r.max_world_vel; at_vel = r.limiting_joint_vel; min_vel_s = frac; } + if (r.max_world_acc < min_acc) { min_acc = r.max_world_acc; at_acc = r.limiting_joint_acc; } + if (r.max_world_jerk < min_jerk) { min_jerk = r.max_world_jerk; at_jerk = r.limiting_joint_jerk; } + if (r.condition_number > max_cond) max_cond = r.condition_number; + + if (i == 0) { + printf(" Jacobian at start (rows = joints, cols = XYZABCUVW):\n"); + for (int j = 0; j < num_joints && j < 9; j++) { + printf(" j%d:", j); + for (int ax = 0; ax < 9; ax++) printf(" %8.4f", J[j][ax]); + printf("\n"); + } + } + } + + printf("\nsegment cap : vel %.3f (joint %d at s=%.3f), acc %.1f (joint %d), jerk %.1f (joint %d)\n", + min_vel, at_vel, min_vel_s, min_acc, at_acc, min_jerk, at_jerk); + printf("worst cond : %.3f\n", max_cond); + + kinematicsUserFree(ctx); + hal_exit(comp_id); + return 0; +} From d918af6dc73c91d3fd1434a4c577eca8fd2d29c3 Mon Sep 17 00:00:00 2001 From: Luca Toniolo <10792599+grandixximo@users.noreply.github.com> Date: Fri, 4 Sep 2026 12:44:16 +1000 Subject: [PATCH 28/60] kinematics: add the parameter block form of a module A module reads its geometry from pins it created and keeps its type and iteration scratch in statics, so it can only answer for the machine as it is now, from inside realtime; a planner, a load-time check or a preview had to carry a second copy of the maths. Add the form in which the caller supplies everything: a kins_params block with the 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 with 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 under the names configs already use and copies pins in and outputs out around every call, so the maths never touches a pin. kins_single.c supplies the classic entry points for a module with one type, switchkins.c for several, dispatching a type registered with switchkinsRegisterOps() through the block and the older registrations as before, so modules convert one at a time. Every converted module exports kinsDescribe() for callers outside RT; one that does not reports itself RT-only. trivkins, 5axiskins and userkfuncs convert here. The non-RT loader moves onto kinsDescribe() and nonrt_attach() goes; jacobian.cc asks the module for its Jacobian. An iterating ops forward restarts from the pose it last solved and otherwise from the caller's estimate, keeping a result only when the solve succeeded, and the gui forward of a pure type is seeded the same way. kinslimits reports the same caps for 5axiskins to the digit. --- src/Makefile | 11 +- src/emc/kinematics/5axiskins.c | 280 +++------- src/emc/kinematics/kinematics.h | 255 +++++++++- src/emc/kinematics/kins_rt.h | 57 +++ src/emc/kinematics/kins_single.c | 155 ++++++ src/emc/kinematics/kins_util.c | 479 +++++++++++++++++- src/emc/kinematics/nonrt_kins.h | 95 ---- src/emc/kinematics/switchkins.c | 285 +++++++++-- src/emc/kinematics/switchkins.h | 27 +- src/emc/kinematics/switchkins_main.c | 42 +- src/emc/kinematics/switchkins_setup.c | 85 ++++ src/emc/kinematics/trivkins.c | 126 ++--- src/emc/kinematics/userkfuncs.c | 34 ++ .../kinematics_userspace/kinematics_user.c | 363 +++++++++---- .../kinematics_userspace/kinematics_user.h | 61 ++- src/emc/motion_planning/Submakefile | 4 +- src/emc/motion_planning/jacobian.cc | 121 +---- src/emc/motion_planning/jacobian.hh | 24 +- src/hal/components/millturn.comp | 2 +- src/hal/components/xyzab_tdr_kins.comp | 2 +- src/hal/components/xyzacb_trsrn.comp | 2 +- src/hal/components/xyzbca_trsrn.comp | 2 +- 22 files changed, 1746 insertions(+), 766 deletions(-) create mode 100644 src/emc/kinematics/kins_rt.h create mode 100644 src/emc/kinematics/kins_single.c delete mode 100644 src/emc/kinematics/nonrt_kins.h create mode 100644 src/emc/kinematics/switchkins_setup.c diff --git a/src/Makefile b/src/Makefile index 2999e03ca9e..654cc11207b 100644 --- a/src/Makefile +++ b/src/Makefile @@ -404,7 +404,7 @@ SRCHEADERS := \ emc/linuxcnc.h \ emc/kinematics/kinematics.h \ emc/kinematics/switchkins.h \ - emc/kinematics/nonrt_kins.h \ + emc/kinematics/kins_rt.h \ emc/kinematics_userspace/kinematics_user.h \ emc/nml_intf/emcmotcfg.h \ emc/ini/inifile.hh \ @@ -1132,6 +1132,7 @@ hal_lib-objs := hal/hal_lib.o $(MATHSTUB) obj-m += trivkins.o trivkins-objs := emc/kinematics/trivkins.o trivkins-objs += emc/kinematics/kins_util.o +trivkins-objs += emc/kinematics/kins_single.o obj-m += maxkins.o maxkins-objs := emc/kinematics/maxkins.o @@ -1187,6 +1188,7 @@ genhexkins-objs += $(MATHSTUB) genhexkins-objs += emc/kinematics/kins_util.o genhexkins-objs += emc/kinematics/switchkins.o genhexkins-objs += emc/kinematics/switchkins_main.o +genhexkins-objs += emc/kinematics/switchkins_setup.o genhexkins-objs += $(USERKFUNCS) obj-m += genserkins.o @@ -1197,6 +1199,7 @@ genserkins-objs += $(MATHSTUB) genserkins-objs += emc/kinematics/kins_util.o genserkins-objs += emc/kinematics/switchkins.o genserkins-objs += emc/kinematics/switchkins_main.o +genserkins-objs += emc/kinematics/switchkins_setup.o genserkins-objs += $(USERKFUNCS) obj-m += xyzac-trt-kins.o @@ -1205,6 +1208,7 @@ xyzac-trt-kins-objs += emc/kinematics/trtfuncs.o xyzac-trt-kins-objs += emc/kinematics/kins_util.o xyzac-trt-kins-objs += emc/kinematics/switchkins.o xyzac-trt-kins-objs += emc/kinematics/switchkins_main.o +xyzac-trt-kins-objs += emc/kinematics/switchkins_setup.o xyzac-trt-kins-objs += $(USERKFUNCS) obj-m += xyzbc-trt-kins.o @@ -1213,6 +1217,7 @@ xyzbc-trt-kins-objs += emc/kinematics/trtfuncs.o xyzbc-trt-kins-objs += emc/kinematics/kins_util.o xyzbc-trt-kins-objs += emc/kinematics/switchkins.o xyzbc-trt-kins-objs += emc/kinematics/switchkins_main.o +xyzbc-trt-kins-objs += emc/kinematics/switchkins_setup.o xyzbc-trt-kins-objs += $(USERKFUNCS) obj-m += scarakins.o @@ -1222,6 +1227,7 @@ scarakins-objs += $(MATHSTUB) scarakins-objs += emc/kinematics/kins_util.o scarakins-objs += emc/kinematics/switchkins.o scarakins-objs += emc/kinematics/switchkins_main.o +scarakins-objs += emc/kinematics/switchkins_setup.o scarakins-objs += $(USERKFUNCS) obj-m += pumakins.o @@ -1231,6 +1237,7 @@ pumakins-objs += $(MATHSTUB) pumakins-objs += emc/kinematics/kins_util.o pumakins-objs += emc/kinematics/switchkins.o pumakins-objs += emc/kinematics/switchkins_main.o +pumakins-objs += emc/kinematics/switchkins_setup.o pumakins-objs += $(USERKFUNCS) obj-m += three21kins.o @@ -1240,6 +1247,7 @@ three21kins-objs += $(MATHSTUB) three21kins-objs += emc/kinematics/kins_util.o three21kins-objs += emc/kinematics/switchkins.o three21kins-objs += emc/kinematics/switchkins_main.o +three21kins-objs += emc/kinematics/switchkins_setup.o three21kins-objs += $(USERKFUNCS) obj-m += 5axiskins.o @@ -1249,6 +1257,7 @@ obj-m += 5axiskins.o 5axiskins-objs += emc/kinematics/kins_util.o 5axiskins-objs += emc/kinematics/switchkins.o 5axiskins-objs += emc/kinematics/switchkins_main.o +5axiskins-objs += emc/kinematics/switchkins_setup.o 5axiskins-objs += $(USERKFUNCS) #---------------------------------------------------------------- diff --git a/src/emc/kinematics/5axiskins.c b/src/emc/kinematics/5axiskins.c index 5d8a5c7a8ec..267025f910f 100644 --- a/src/emc/kinematics/5axiskins.c +++ b/src/emc/kinematics/5axiskins.c @@ -11,10 +11,9 @@ * Copyright (c) 2007 Chris Radek * * Notes: -* 1) pivot_length hal pin must agree with mechanical -* design (including vismach simulation) and augmented -* with current tool z offset -* (typ: mechanical_pivot_length + motion.tooloffset.z) +* 1) pivot-length must agree with the mechanical design +* (including vismach simulation); the tool length comes +* in on the tool-length pin of its own * 2) C axis: spherical coordinates aziumthal angle (t or theta) * projection of radius to xy plane * 3) B axis: spherical coordinates polar angle (p or phi) @@ -42,8 +41,8 @@ * 9) Coordinates XYZBCW are required, AUV may be used * if specified with the coordinates parameter and will * be mapped one-to-one with the assigned joint. -* 10) The direction of the tilt axis is the opposite of the -* conventional axis direction. See +* 10) The direction of the tilt axis is the opposite of the +* conventional axis direction. See * https://linuxcnc.org/docs/html/gcode/machining-center.html ********************************************************************/ @@ -56,18 +55,29 @@ #include #include #include -#include #include #include #include -#include -static struct haldata { - hal_real_t pivot_length; - hal_real_t tool_length; -} *haldata; -static int fiveaxis_max_joints; +// the geometry, one pin each; the maths reads it from the block +static const kins_param_desc fiveaxis_params[] = { + { "pivot-length", KINS_PARAM_FLOAT, KINS_IN, 0, DEFAULT_PIVOT_LENGTH }, + { "tool-length", KINS_PARAM_FLOAT, KINS_IN, 0, 0.0 }, +}; +enum { P_PIVOT_LENGTH, P_TOOL_LENGTH }; + +// assignments of principal joints to axis letters, from the block +// (-1 means not defined) +#define JX (p->joint_of_axis[0]) +#define JY (p->joint_of_axis[1]) +#define JZ (p->joint_of_axis[2]) +#define JA (p->joint_of_axis[3]) +#define JB (p->joint_of_axis[4]) +#define JC (p->joint_of_axis[5]) +#define JU (p->joint_of_axis[6]) +#define JV (p->joint_of_axis[7]) +#define JW (p->joint_of_axis[8]) static PmCartesian s2r(double r, double t, double p) { // s2r: spherical coordinates to cartesian coordinates @@ -85,28 +95,18 @@ static PmCartesian s2r(double r, double t, double p) { return c; } //s2r() -// assignments of principal joints to axis letters: -// (-1 means not defined (yet)) -static int JX = -1; -static int JY = -1; -static int JZ = -1; -static int JA = -1; -static int JB = -1; -static int JC = -1; -static int JU = -1; -static int JV = -1; -static int JW = -1; - -static int fiveaxis_KinematicsForward(const double *joints, - EmcPose * pos, - const KINEMATICS_FORWARD_FLAGS * fflags, - KINEMATICS_INVERSE_FLAGS * iflags) +static int fiveaxis_forward(const kins_params *p, kins_scratch *s, + const double *joints, + EmcPose * pos, + const KINEMATICS_FORWARD_FLAGS * fflags, + KINEMATICS_INVERSE_FLAGS * iflags) { + (void)s; (void)fflags; (void)iflags; - rtapi_real pivot_length = hal_get_real(haldata->pivot_length); - rtapi_real tool_length = hal_get_real(haldata->tool_length); - PmCartesian r = s2r(pivot_length + joints[JW] + tool_length, + double pivot_length = p->geometry[P_PIVOT_LENGTH]; + double tool_length = p->geometry[P_TOOL_LENGTH]; + PmCartesian r = s2r(pivot_length + tool_length + joints[JW], joints[JC], 180.0 - joints[JB]); @@ -124,18 +124,20 @@ static int fiveaxis_KinematicsForward(const double *joints, pos->v = (JV != -1)? joints[JV] : 0; return 0; -} //fiveaxis_KinematicsForward() +} // fiveaxis_forward() -static int fiveaxis_KinematicsInverse(const EmcPose * pos, - double *joints, - const KINEMATICS_INVERSE_FLAGS * iflags, - KINEMATICS_FORWARD_FLAGS * fflags) +static int fiveaxis_inverse(const kins_params *p, kins_scratch *s, + const EmcPose * pos, + double *joints, + const KINEMATICS_INVERSE_FLAGS * iflags, + KINEMATICS_FORWARD_FLAGS * fflags) { + (void)s; (void)iflags; (void)fflags; - rtapi_real pivot_length = hal_get_real(haldata->pivot_length); - rtapi_real tool_length = hal_get_real(haldata->tool_length); - PmCartesian r = s2r(pivot_length + pos->w + tool_length, + double pivot_length = p->geometry[P_PIVOT_LENGTH]; + double tool_length = p->geometry[P_TOOL_LENGTH]; + PmCartesian r = s2r(pivot_length + tool_length + pos->w, pos->c, 180.0 - pos->b); @@ -156,21 +158,18 @@ static int fiveaxis_KinematicsInverse(const EmcPose * pos, // update joints with support for // multiple-joints per-coordinate letter: // based on computed position - position_to_mapped_joints(fiveaxis_max_joints, - &P, - joints); - return 0; -} // fiveaxis_kinematicsInverse() - -static int fiveaxis_KinematicsJacobian(const double *joints, - const EmcPose * pos, - double jac[EMCMOT_MAX_JOINTS][EMCMOT_MAX_AXIS], - const KINEMATICS_INVERSE_FLAGS * iflags) + return kinsPoseToMappedJoints(p, &P, joints); +} // fiveaxis_inverse() + +static int fiveaxis_jacobian(const kins_params *p, + const double *joints, + const EmcPose * pos, + double jac[EMCMOT_MAX_JOINTS][EMCMOT_MAX_AXIS], + const KINEMATICS_INVERSE_FLAGS * iflags) { (void)joints; (void)iflags; - rtapi_real pivot_length = hal_get_real(haldata->pivot_length); - const double R = pivot_length + pos->w; + const double R = p->geometry[P_PIVOT_LENGTH] + p->geometry[P_TOOL_LENGTH] + pos->w; const double sb = sin(TO_RAD*pos->b), cb = cos(TO_RAD*pos->b); const double sc = sin(TO_RAD*pos->c), cc = cos(TO_RAD*pos->c); double dP[EMCMOT_MAX_AXIS][EMCMOT_MAX_AXIS]; @@ -199,112 +198,15 @@ static int fiveaxis_KinematicsJacobian(const double *joints, for (a = 3; a < EMCMOT_MAX_AXIS; a++) { dP[a][a] = 1; } - return kinsJacobianFromMappedAxes(fiveaxis_max_joints, - (const double (*)[EMCMOT_MAX_AXIS])dP, - jac); -} // fiveaxis_KinematicsJacobian() - -// module constants, shared by switchkinsSetup() and nonrt_attach() -static void fiveaxis_kparms(kparms* kp) -{ - kp->kinsname = "5axiskins"; // !!! must agree with filename - kp->halprefix = "5axiskins"; // hal pin names - kp->required_coordinates = REQUIRED_COORDINATES; - kp->allow_duplicates = 1; - kp->max_joints = EMCMOT_MAX_JOINTS; -} - -// assign principal joint numbers from the coordinates string. -// No HAL involvement, so the non-RT path can use it too. -static int fiveaxis_map_joints(const char* coordinates, kparms* kp) -{ - int i,jno; - int axis_idx_for_jno[EMCMOT_MAX_JOINTS]; - int minjoints = strlen(kp->required_coordinates); - fiveaxis_max_joints = strlen(coordinates); // allow for dup coords - - if (fiveaxis_max_joints > kp->max_joints) { - rtapi_print_msg(RTAPI_MSG_ERR, - "ERROR %s: coordinates=%s requires %d joints, max joints=%d\n", - kp->kinsname, - coordinates, - fiveaxis_max_joints, - kp->max_joints); - goto error; - } - - if (map_coordinates_to_jnumbers(coordinates, - kp->max_joints, - kp->allow_duplicates, - axis_idx_for_jno)) { - goto error; - } - // require all chars in reqd_coordinates (order doesn't matter) - for (i=0; i < minjoints; i++) { - char reqd_char; - reqd_char = *(kp->required_coordinates + i); - if ( !strchr(coordinates,toupper(reqd_char)) - && !strchr(coordinates,tolower(reqd_char)) ) { - rtapi_print_msg(RTAPI_MSG_ERR, - "ERROR %s:\nrequired coordinates:%s\n" - "specified coordinates:%s\n", - kp->kinsname, kp->required_coordinates, coordinates); - goto error; - } - } - // assign principal joint numbers (first found in coordinates map) - // duplicates are handled by position_to_mapped_joints() - for (jno=0; jnopivot_length), - DEFAULT_PIVOT_LENGTH, "%s.pivot-length", kp->halprefix); - if(result < 0) goto error; - - result = hal_pin_new_real(comp_id, HAL_IN, &(haldata->tool_length), - 0.0, "%s.tool-length", kp->halprefix); - if(result < 0) goto error; - - rtapi_print("Kinematics Module %s\n",__FILE__); - rtapi_print(" module name = %s\n" - " coordinates = %s Requires: [KINS]JOINTS>=%d\n" - " sparm = %s\n", - kp->kinsname, - coordinates,fiveaxis_max_joints, - kp->sparm?kp->sparm:"NOTSPECIFIED"); - rtapi_print(" default pivot-length = %.3f\n", hal_get_real(haldata->pivot_length)); - - return 0; + return kinsJacobianFromMappedAxesP(p, (const double (*)[EMCMOT_MAX_AXIS])dP, + jac); +} // fiveaxis_jacobian() -error: - return -1; -} // fiveaxis_KinematicsSetup() +static const kins_ops fiveaxis_ops = { + .forward = fiveaxis_forward, + .inverse = fiveaxis_inverse, + .jacobian = fiveaxis_jacobian, +}; int switchkinsSetup(kparms* kp, KS* kset0, KS* kset1, KS* kset2, @@ -312,61 +214,27 @@ int switchkinsSetup(kparms* kp, KI* kinv0, KI* kinv1, KI* kinv2 ) { - fiveaxis_kparms(kp); + (void)kset0; (void)kset1; (void)kset2; + (void)kfwd0; (void)kfwd1; (void)kfwd2; + (void)kinv0; (void)kinv1; (void)kinv2; + kp->kinsname = "5axiskins"; // !!! must agree with filename + kp->halprefix = "5axiskins"; // hal pin names + kp->required_coordinates = REQUIRED_COORDINATES; + kp->allow_duplicates = 1; + kp->max_joints = EMCMOT_MAX_JOINTS; + kp->params = fiveaxis_params; + kp->nparams = sizeof(fiveaxis_params)/sizeof(fiveaxis_params[0]); if (kp->sparm && strstr(kp->sparm,"identityfirst")) { rtapi_print("\n!!! switchkins-type 0 is IDENTITY\n"); - *kset0 = identityKinematicsSetup; - *kfwd0 = identityKinematicsForward; - *kinv0 = identityKinematicsInverse; - - *kset1 = fiveaxis_KinematicsSetup; - *kfwd1 = fiveaxis_KinematicsForward; - *kinv1 = fiveaxis_KinematicsInverse; - switchkinsDeclare(0, KINSTYPE_IDENTITY); - switchkinsDeclare(1, KINSTYPE_PRIMARY); - switchkinsRegisterJacobian(1, fiveaxis_KinematicsJacobian); + switchkinsRegisterOps(0, &KINS_IDENTITY_OPS); + switchkinsRegisterOps(1, &fiveaxis_ops); } else { rtapi_print("\n!!! switchkins-type 0 is %s\n",kp->kinsname); - *kset0 = fiveaxis_KinematicsSetup; - *kfwd0 = fiveaxis_KinematicsForward; - *kinv0 = fiveaxis_KinematicsInverse; - switchkinsRegisterJacobian(0, fiveaxis_KinematicsJacobian); - - *kset1 = identityKinematicsSetup; - *kfwd1 = identityKinematicsForward; - *kinv1 = identityKinematicsInverse; - switchkinsDeclare(0, KINSTYPE_PRIMARY); - switchkinsDeclare(1, KINSTYPE_IDENTITY); + switchkinsRegisterOps(0, &fiveaxis_ops); + switchkinsRegisterOps(1, &KINS_IDENTITY_OPS); } - *kset2 = userkKinematicsSetup; - *kfwd2 = userkKinematicsForward; - *kinv2 = userkKinematicsInverse; + switchkinsRegisterOps(2, &USERK_OPS); return 0; } // switchkinsSetup() - -// Non-RT entry point: bind this copy of the module to the pins the -// running RT instance owns, then hand back the unmodified kinematics. -int nonrt_attach(const char* coordinates, nonrt_ops_t* ops, - nonrt_resolve_fn resolve, void* arg) -{ - static struct haldata nonrt_haldata; // private to this copy of the module - kparms kp = {0}; - - fiveaxis_kparms(&kp); - - haldata = &nonrt_haldata; - - if (nonrt_resolve_real(resolve, arg, &haldata->pivot_length, - "%s.pivot-length", kp.halprefix)) return -1; - - if (fiveaxis_map_joints(coordinates, &kp)) return -1; - - ops->forward = fiveaxis_KinematicsForward; - ops->inverse = fiveaxis_KinematicsInverse; - ops->is_identity = 0; - return 0; -} // nonrt_attach() - -EXPORT_SYMBOL(nonrt_attach); diff --git a/src/emc/kinematics/kinematics.h b/src/emc/kinematics/kinematics.h index 9376b7d9d6d..7de1f15b246 100644 --- a/src/emc/kinematics/kinematics.h +++ b/src/emc/kinematics/kinematics.h @@ -107,6 +107,26 @@ extern int kinematicsHome(struct EmcPose * world, extern KINEMATICS_TYPE kinematicsType(void); +/* Switchable kinematics: a module provides several kinematics, numbered +** 0..SWITCHKINS_MAX_TYPES-1, and motion runs one of them at a time. +** The count is here, not in switchkins.h, because motion and the NML +** status channel need it; it aliases KINS_MAX_TYPES below. +*/ +#define SWITCHKINS_MAX_TYPES KINS_MAX_TYPES + +/* What a kinematics type IS, declared by the module with +** switchkinsDeclare() and read back with kinematicsTypeFlags(). +** G13.1 resolves "identity" from these flags instead of assuming a +** number; a module that declares nothing leaves its types numeric-only +** and G13.1 refuses to guess. +*/ +#define KINSTYPE_IDENTITY 0x1 /* no transform: the joints are the world */ +#define KINSTYPE_PRIMARY 0x2 /* the module's working transform */ + +/* flags of a kinematics type, or -1 for a type the module does not +** provide (and for every type on a machine with plain kinematics) */ +extern int kinematicsTypeFlags(int ktype); + /* These two give the orientation of the tool and of the workpiece for a set of joint values. Each returns a rotation whose columns are that frame's axes expressed in MACHINE coordinates, the frame fixed to the bed that @@ -161,26 +181,6 @@ extern int kinematicsWorkFrame(const double *joint, PmRotationMatrix *rot, const KINEMATICS_FORWARD_FLAGS *fflags); -/* Switchable kinematics: a module provides several kinematics, numbered -** 0..SWITCHKINS_MAX_TYPES-1, and motion runs one of them at a time. -** The count is here, not in switchkins.h, because motion and the NML -** status channel need it. -*/ -#define SWITCHKINS_MAX_TYPES 9 - -/* What a kinematics type IS, declared by the module with -** switchkinsDeclare() and read back with kinematicsTypeFlags(). -** G13.1 resolves "identity" from these flags instead of assuming a -** number; a module that declares nothing leaves its types numeric-only -** and G13.1 refuses to guess. -*/ -#define KINSTYPE_IDENTITY 0x1 /* no transform: the joints are the world */ -#define KINSTYPE_PRIMARY 0x2 /* the module's working transform */ - -/* flags of a kinematics type, or -1 for a type the module does not -** provide (and for every type on a machine with plain kinematics) */ -extern int kinematicsTypeFlags(int ktype); - /* parameters for use with switchkins.c */ typedef struct kinematics_parms { char* sparm; // module string parameter passed to kins @@ -197,6 +197,8 @@ typedef struct kinematics_parms { // bitmask: 0x4 bit2: switchkins_type==2 int gui_kinstype; // may be reqd for parallel kins with vismach // to select switchkins_type for gui pins + const struct kins_param_desc_tag *params; // geometry table, see below + int nparams; } kparms; /* map letters in a coordinates string to joint numbers @@ -449,6 +451,217 @@ extern int identityKinematicsJacobian(const double *joint, double jac[EMCMOT_MAX_JOINTS][EMCMOT_MAX_AXIS], const KINEMATICS_INVERSE_FLAGS *iflags); +/* ------------------------------------------------------------------------ + Kinematics as pure functions of what the caller passes in. + + Everything above reads its geometry from HAL pins the module created and + keeps its mode and scratch in statics, so it can only answer for the + machine as it is now, from inside the module. The forms below take the + same questions with the machine described by the caller: a parameter + block naming the kinematics type, the joint map, the tool and the + geometry, and a scratch block for what an iterative method carries + between calls. Nothing is read from HAL and nothing is kept, so one copy + of the maths serves motion, a planner evaluating poses the machine has + not reached, task checking a program at load, and a tool asking what if. + + A module declares its geometry as a table of named entries. In RT the + shared code makes one HAL pin per entry, with the names configs already + use, and copies the pins into the block before every call; outside RT the + caller fills the block from wherever it likes. The maths reads + p->geometry[i] where it read a pin. + + The existing entry points stay and are supplied once, by kins_single.c + for a module with one kinematics type and by switchkins.c for one with + several, so nothing that calls kinematicsForward() changes. A module + that does not provide these forms keeps working as it did; it just cannot + be evaluated outside RT. + ------------------------------------------------------------------------ */ + +#define KINS_MAX_PARAMS 96 /* genhexkins declares 84 */ +#define KINS_MAX_TYPES 9 /* kinematics types a module may provide */ + +typedef enum { + KINS_PARAM_FLOAT = 0, + KINS_PARAM_BIT, + KINS_PARAM_S32, + KINS_PARAM_U32 +} kins_param_type; + +typedef enum { + KINS_IN = 0, /* read into the block before a call */ + KINS_OUT, /* a result, written from kins_scratch.out[] after it */ + KINS_IO /* read like an input; the pin is HAL_IO so it can be poked */ +} kins_param_dir; + +/* One entry of a module's geometry table. name follows the module's HAL + prefix. An entry with tool set is the tool length along the tool axis: + the shared code puts its value in kins_params.tool.tran.z as well, which + is what the maths should read, so that a caller outside RT can supply + the tool from the tool table without there being a pin. */ +typedef struct kins_param_desc_tag { + const char *name; + kins_param_type type; + kins_param_dir dir; + int tool; + double dflt; +} kins_param_desc; + +/* The machine, as far as the kinematics is concerned. One copy may be + shared by any number of callers: nothing writes it during a call. */ +typedef struct kins_params { + int size; /* sizeof(kins_params) */ + int ktype; /* kinematics type, 0 if one */ + int max_joints; /* joints the map covers */ + int joint_of_axis[EMCMOT_MAX_AXIS]; /* principal joint per letter */ + int joints_of_axis[EMCMOT_MAX_AXIS]; /* bit per joint, duplicates */ + EmcPose tool; /* tool offset, tool.tran.z along the tool axis */ + double geometry[KINS_MAX_PARAMS]; /* the table, in its order */ +} kins_params; + +/* What one caller carries between its own calls: the last pose an + iterative forward found, which seeds the next, and what a module reports + about its last call. Never shared between callers. */ +typedef struct kins_scratch { + EmcPose pose_seed; /* start an iterative forward here */ + int have_pose_seed; + int pose_seed_ok; /* pose_seed came from a solve that succeeded */ + double joint_seed[EMCMOT_MAX_JOINTS]; /* start an iterative inverse here */ + int have_joint_seed; + int iterations; + int failed; + double out[KINS_MAX_PARAMS]; /* the table's KINS_OUT entries */ +} kins_scratch; + +typedef int (*kins_forward_fn)(const kins_params *p, kins_scratch *s, + const double *joint, EmcPose *pos, + const KINEMATICS_FORWARD_FLAGS *fflags, + KINEMATICS_INVERSE_FLAGS *iflags); + +typedef int (*kins_inverse_fn)(const kins_params *p, kins_scratch *s, + const EmcPose *pos, double *joint, + const KINEMATICS_INVERSE_FLAGS *iflags, + KINEMATICS_FORWARD_FLAGS *fflags); + +typedef int (*kins_frame_fn)(const kins_params *p, const double *joint, + PmRotationMatrix *rot, + const KINEMATICS_FORWARD_FLAGS *fflags); + +typedef int (*kins_jacobian_fn)(const kins_params *p, const double *joint, + const EmcPose *pos, + double jac[EMCMOT_MAX_JOINTS][EMCMOT_MAX_AXIS], + const KINEMATICS_INVERSE_FLAGS *iflags); + +/* The maths of one kinematics type. forward and inverse are required; the + frames, the native rotation and the Jacobian are optional as before, and + a missing Jacobian is differenced from the inverse. fwd_iterates says the + forward starts from the pose it is handed, so the shared code seeds it + with the last answer after a switch. identity says joints are axes, which + a consumer may use to skip the maths altogether. */ +typedef struct kins_ops { + kins_forward_fn forward; + kins_inverse_fn inverse; + kins_frame_fn work; + kins_frame_fn tool; + const PmRotationMatrix *native; /* NULL means TOOL_FRAME_SPINDLE */ + kins_jacobian_fn jacobian; + int fwd_iterates; + int identity; /* joints are axes */ +} kins_ops; + +/* A module described for a caller outside RT: its table, its joint + conventions and the maths of each type. ops[t] is NULL for a type the + module still implements the old way. */ +typedef struct kins_module_info { + const char *name; + const char *halprefix; + const kins_param_desc *params; + int nparams; + const char *required_coordinates; + int max_joints; /* the most the module allows */ + int allow_duplicates; + int ntypes; + const kins_ops *ops[KINS_MAX_TYPES]; +} kins_module_info; + +/* Exported by every module that provides the forms above. coordinates and + sparm are the module parameters the RT instance was loaded with; a module + whose types depend on them replays that choice here. Meant for a copy of + the module loaded outside RT; the RT instance answers from its own state + without redoing its setup. Returns 0, or -1 with info untouched. */ +extern int kinsDescribe(const char *coordinates, const char *sparm, + kins_module_info *info); + +/* Fill a block for a module: size, the joint map from coordinates (checked + against required_coordinates, the joint limit and the duplicate rule), + ktype 0, no tool, and every geometry entry at its table default. A + caller then overwrites what it knows better. Returns 0 or -1. */ +extern int kinsParamsInit(kins_params *p, + const kins_module_info *info, + const char *coordinates); + +/* The joint map alone, into a block, with no other field touched. */ +extern int kinsParamsMapCoordinates(kins_params *p, + const char *coordinates, + int max_joints, + int allow_duplicates, + const char *required_coordinates); + +/* Reset a scratch to "no seed, nothing reported". */ +extern void kinsScratchInit(kins_scratch *s); + +/* The map helpers above, reading the map from the block instead of from + the statics that map_coordinates_to_jnumbers() fills. */ +extern int kinsMappedJointsToPose(const kins_params *p, + const double *joints, EmcPose *pos); +extern int kinsPoseToMappedJoints(const kins_params *p, + const EmcPose *pos, double *joints); +extern int kinsJacobianFromMappedAxesP(const kins_params *p, + const double dP[EMCMOT_MAX_AXIS][EMCMOT_MAX_AXIS], + double jac[EMCMOT_MAX_JOINTS][EMCMOT_MAX_AXIS]); + +/* Identity as pure functions: joints are axes through the block's map. */ +extern int kinsIdentityForward(const kins_params *p, kins_scratch *s, + const double *joint, EmcPose *pos, + const KINEMATICS_FORWARD_FLAGS *fflags, + KINEMATICS_INVERSE_FLAGS *iflags); +extern int kinsIdentityInverse(const kins_params *p, kins_scratch *s, + const EmcPose *pos, double *joint, + const KINEMATICS_INVERSE_FLAGS *iflags, + KINEMATICS_FORWARD_FLAGS *fflags); +extern int kinsIdentityFrame(const kins_params *p, const double *joint, + PmRotationMatrix *rot, + const KINEMATICS_FORWARD_FLAGS *fflags); +extern int kinsIdentityJacobian(const kins_params *p, const double *joint, + const EmcPose *pos, + double jac[EMCMOT_MAX_JOINTS][EMCMOT_MAX_AXIS], + const KINEMATICS_INVERSE_FLAGS *iflags); +extern const kins_ops KINS_IDENTITY_OPS; + +/* The five questions asked of an ops table, with the defaults applied: + identity for a missing frame, the native rotation applied to the tool + frame, and the Jacobian differenced from the inverse when there is no + closed form. These are what the RT wrappers and a caller outside RT + both go through, so both get the same answers. */ +extern int kinsOpsForward(const kins_ops *ops, const kins_params *p, + kins_scratch *s, const double *joint, EmcPose *pos, + const KINEMATICS_FORWARD_FLAGS *fflags, + KINEMATICS_INVERSE_FLAGS *iflags); +extern int kinsOpsInverse(const kins_ops *ops, const kins_params *p, + kins_scratch *s, const EmcPose *pos, double *joint, + const KINEMATICS_INVERSE_FLAGS *iflags, + KINEMATICS_FORWARD_FLAGS *fflags); +extern int kinsOpsWorkFrame(const kins_ops *ops, const kins_params *p, + const double *joint, PmRotationMatrix *rot, + const KINEMATICS_FORWARD_FLAGS *fflags); +extern int kinsOpsToolFrame(const kins_ops *ops, const kins_params *p, + const double *joint, PmRotationMatrix *rot, + const KINEMATICS_FORWARD_FLAGS *fflags); +extern int kinsOpsJacobian(const kins_ops *ops, const kins_params *p, + kins_scratch *s, const double *joint, + const EmcPose *pos, + double jac[EMCMOT_MAX_JOINTS][EMCMOT_MAX_AXIS], + const KINEMATICS_INVERSE_FLAGS *iflags); + extern int kinematicsSwitchable(void); extern int kinematicsSwitch(int switchkins_type); //NOTE: switchable kinematics may require Interp::Synch @@ -465,6 +678,8 @@ EXPORT_SYMBOL(kinematicsTypeFlags); // support for template for user-defined switchkins_type==2 +extern const kins_ops USERK_OPS; + extern int userkKinematicsSetup(const int comp_id, const char* coordinates, kparms* ksetup_parms); diff --git a/src/emc/kinematics/kins_rt.h b/src/emc/kinematics/kins_rt.h new file mode 100644 index 00000000000..96d7309c739 --- /dev/null +++ b/src/emc/kinematics/kins_rt.h @@ -0,0 +1,57 @@ +/******************************************************************** +* Description: kins_rt.h +* The RT side of a kinematics module written as pure functions: the HAL +* pins made from its geometry table, and the wrapper that supplies the +* classic entry points for a module with one kinematics type. A module +* with several types gets the same from switchkins.c. +* +* Kept apart from kinematics.h because everything here needs HAL, and +* kinematics.h is read by callers outside RT that do not. +* +* License: GPL Version 2 +********************************************************************/ +#ifndef __LINUXCNC_KINS_RT_H +#define __LINUXCNC_KINS_RT_H + +#include +#include "kinematics.h" + +/* one HAL pin handle per table entry, of whichever type the entry has */ +typedef union { + hal_real_t r; + hal_bool_t b; + hal_sint_t s; + hal_uint_t u; +} kins_pin_ref; + +/* Make one pin per table entry, named ., inputs at their + defaults. *out receives the handles, from hal_malloc(), or NULL for an + empty table. Returns 0 or -1. */ +extern int kinsParamsPinsCreate(int comp_id, const char *prefix, + const kins_param_desc *params, int nparams, + kins_pin_ref **out); + +/* Copy every input pin into p->geometry[], and the tool entry into + p->tool.tran.z as well. */ +extern void kinsParamsPinsRead(const kins_pin_ref *pins, + const kins_param_desc *params, int nparams, + kins_params *p); + +/* Copy s->out[] to every output pin. */ +extern void kinsParamsPinsWrite(const kins_pin_ref *pins, + const kins_param_desc *params, int nparams, + const kins_scratch *s); + +/* A module with one kinematics type defines this, describing itself, and + links kins_single.c, which supplies kinematicsForward() and the rest + from it. ops[0] is the maths; the other entries are ignored. */ +extern const kins_module_info kins_module; + +/* Called once from the module's rtapi_app_main() or EXTRA_SETUP(), after + hal_init() and before hal_ready(): makes the pins, builds the block for + coordinates and records the KINEMATICS_TYPE that kinematicsType() will + report. Returns 0 or -1. */ +extern int kinsSingleInit(int comp_id, const char *coordinates, + KINEMATICS_TYPE reported); + +#endif diff --git a/src/emc/kinematics/kins_single.c b/src/emc/kinematics/kins_single.c new file mode 100644 index 00000000000..58f914076d5 --- /dev/null +++ b/src/emc/kinematics/kins_single.c @@ -0,0 +1,155 @@ +/******************************************************************** +* Description: kins_single.c +* The classic kinematics entry points for a module with one kinematics +* type written as pure functions. The module defines kins_module and +* calls kinsSingleInit(); this file keeps the one RT parameter block, +* fills it from the pins before every call, and hands the call to the +* module's ops. It is the counterpart of switchkins.c for a module that +* does not switch. +* +* License: GPL Version 2 +********************************************************************/ + +#include +#include +#include + +#include +#include + +static kins_params rt_params; +static kins_scratch rt_scratch; +static kins_pin_ref *pins; +static int inited; +static KINEMATICS_TYPE reported_type = KINEMATICS_BOTH; + +static const kins_ops *ops(void) +{ + return inited ? kins_module.ops[0] : NULL; +} + +// the block sees the pins as they are now +static void read_pins(void) +{ + kinsParamsPinsRead(pins, kins_module.params, kins_module.nparams, + &rt_params); +} + +static void write_pins(void) +{ + kinsParamsPinsWrite(pins, kins_module.params, kins_module.nparams, + &rt_scratch); +} + +int kinsSingleInit(int comp_id, const char *coordinates, + KINEMATICS_TYPE reported) +{ + if (!kins_module.ops[0] || !kins_module.ops[0]->forward + || !kins_module.ops[0]->inverse) { + rtapi_print_msg(RTAPI_MSG_ERR, + "kinsSingleInit: %s supplies no forward or inverse\n", + kins_module.name ? kins_module.name : "?"); + return -1; + } + if (kinsParamsInit(&rt_params, &kins_module, coordinates)) { return -1; } + kinsScratchInit(&rt_scratch); + if (kinsParamsPinsCreate(comp_id, kins_module.halprefix, + kins_module.params, kins_module.nparams, + &pins)) { + return -1; + } + reported_type = reported; + inited = 1; + return 0; +} // kinsSingleInit() + +int kinematicsForward(const double *joint, + EmcPose *pos, + const KINEMATICS_FORWARD_FLAGS *fflags, + KINEMATICS_INVERSE_FLAGS *iflags) +{ + int r; + if (!inited) { return -1; } + read_pins(); + r = kinsOpsForward(ops(), &rt_params, &rt_scratch, joint, pos, fflags, iflags); + write_pins(); + return r; +} + +int kinematicsInverse(const EmcPose *pos, + double *joint, + const KINEMATICS_INVERSE_FLAGS *iflags, + KINEMATICS_FORWARD_FLAGS *fflags) +{ + int r; + if (!inited) { return -1; } + read_pins(); + r = kinsOpsInverse(ops(), &rt_params, &rt_scratch, pos, joint, iflags, fflags); + write_pins(); + return r; +} + +int kinematicsWorkFrame(const double *joint, + PmRotationMatrix *rot, + const KINEMATICS_FORWARD_FLAGS *fflags) +{ + if (!inited) { return -1; } + read_pins(); + return kinsOpsWorkFrame(ops(), &rt_params, joint, rot, fflags); +} + +int kinematicsToolFrame(const double *joint, + PmRotationMatrix *rot, + const KINEMATICS_FORWARD_FLAGS *fflags) +{ + if (!inited) { return -1; } + read_pins(); + return kinsOpsToolFrame(ops(), &rt_params, joint, rot, fflags); +} + +int kinematicsJacobian(const double *joint, + const EmcPose *pos, + double jac[EMCMOT_MAX_JOINTS][EMCMOT_MAX_AXIS], + const KINEMATICS_INVERSE_FLAGS *iflags) +{ + if (!inited) { return -1; } + read_pins(); + return kinsOpsJacobian(ops(), &rt_params, &rt_scratch, joint, pos, jac, iflags); +} + +KINEMATICS_TYPE kinematicsType(void) +{ + return reported_type; +} + +int kinematicsSwitchable(void) { return 0; } + +int kinematicsSwitch(int switchkins_type) +{ + (void)switchkins_type; + return 0; +} + +// The module's description, for a copy of it loaded outside RT. A module +// with one type does not depend on its parameters for its shape, so this +// is the table as declared. +int kinsDescribe(const char *coordinates, const char *sparm, + kins_module_info *info) +{ + (void)coordinates; + (void)sparm; + if (!info) { return -1; } + *info = kins_module; + info->ntypes = 1; + return 0; +} + +EXPORT_SYMBOL(kinematicsType); +EXPORT_SYMBOL(kinematicsForward); +EXPORT_SYMBOL(kinematicsInverse); +EXPORT_SYMBOL(kinematicsWorkFrame); +EXPORT_SYMBOL(kinematicsToolFrame); +EXPORT_SYMBOL(kinematicsJacobian); +EXPORT_SYMBOL(kinematicsSwitchable); +EXPORT_SYMBOL(kinematicsSwitch); +EXPORT_SYMBOL(kinsDescribe); diff --git a/src/emc/kinematics/kins_util.c b/src/emc/kinematics/kins_util.c index 2e30969fd90..efee26e2f48 100644 --- a/src/emc/kinematics/kins_util.c +++ b/src/emc/kinematics/kins_util.c @@ -48,7 +48,9 @@ #include #include #include +#include #include +#include // principal joint numbers based on module 'coordinates' parameter static int JX = -1; @@ -76,38 +78,23 @@ static int map_initialized = 0; #define MAX_COORDINATES_CHARS 32 static char used_coordinates[MAX_COORDINATES_CHARS+1]; -int map_coordinates_to_jnumbers(const char *coordinates, - const int max_joints, - const int allow_duplicates, - int axis_idx_for_jno[] ) //result +// Letters to joint numbers, in order, with the checks every caller wants: +// a valid letter set, at most max_joints of them, duplicates only where +// allowed. Fills axis_idx_for_jno (-1 past the last letter) and touches +// nothing else, so the block form and the static form share it. +static int kins_scan_coordinates(const char *coordinates, + int max_joints, + int allow_duplicates, + int axis_idx_for_jno[], + const char *errtag) { - char* errtag="map_coordinates_to_jnumbers: ERROR:\n "; - int jno=0; - bool found=0; + int jno = 0; + bool found = 0; int dups[EMCMOT_MAX_AXIS]; const char *coords = coordinates; char coord_letter[] = {'X','Y','Z','A','B','C','U','V','W'}; int i; - if (strlen(coordinates) > MAX_COORDINATES_CHARS) { - rtapi_print_msg(RTAPI_MSG_ERR, - "%s: map_coordinates_to_jnumbers too many chars:%s\n" - ,__FILE__,coordinates); - return -1; - - } - // Note: may be called multiple times for different switchkins - // types but coordinates must agree - if (used_coordinates[0] == 0) { - strcpy(used_coordinates,coordinates); - } else { - if (strcasecmp(coordinates,used_coordinates)) { - rtapi_print_msg(RTAPI_MSG_ERR, - "%s: map_coordinates_to_jnumbers altered:%s %s\n" - ,__FILE__,used_coordinates,coordinates); - return -1; - } - } for (i=0; i EMCMOT_MAX_JOINTS) ) { @@ -168,6 +155,40 @@ int map_coordinates_to_jnumbers(const char *coordinates, } } } + return 0; +} // kins_scan_coordinates() + +int map_coordinates_to_jnumbers(const char *coordinates, + const int max_joints, + const int allow_duplicates, + int axis_idx_for_jno[] ) //result +{ + char* errtag="map_coordinates_to_jnumbers: ERROR:\n "; + int jno=0; + + if (strlen(coordinates) > MAX_COORDINATES_CHARS) { + rtapi_print_msg(RTAPI_MSG_ERR, + "%s: map_coordinates_to_jnumbers too many chars:%s\n" + ,__FILE__,coordinates); + return -1; + + } + // Note: may be called multiple times for different switchkins + // types but coordinates must agree + if (used_coordinates[0] == 0) { + strcpy(used_coordinates,coordinates); + } else { + if (strcasecmp(coordinates,used_coordinates)) { + rtapi_print_msg(RTAPI_MSG_ERR, + "%s: map_coordinates_to_jnumbers altered:%s %s\n" + ,__FILE__,used_coordinates,coordinates); + return -1; + } + } + if (kins_scan_coordinates(coordinates, max_joints, allow_duplicates, + axis_idx_for_jno, errtag)) { + return -1; + } for (jno=0; jno < max_joints; jno++) { int bitnumber = 1< Axis %c\n", jno,*(p+axis_idx_for_jno[jno])); } +#ifndef ULAPI + // the module's own report of its type; this file is also built + // outside RT, where there is no module around it if (kinematicsType() != KINEMATICS_BOTH) { rtapi_print("identityKinematicsSetup: Recommend: kinstype=both\n"); } +#endif rtapi_print("\n"); } @@ -1166,3 +1191,405 @@ int identityKinematicsJacobian(const double *joint, (const double (*)[EMCMOT_MAX_AXIS])dP, jac); } // identityKinematicsJacobian() + +//---------------------------------------------------------------------- +// The parameter block. See kinematics.h for what it is for. +//---------------------------------------------------------------------- + +int kinsParamsMapCoordinates(kins_params *p, + const char *coordinates, + int max_joints, + int allow_duplicates, + const char *required_coordinates) +{ + int axis_idx_for_jno[EMCMOT_MAX_JOINTS]; + int jno, a; + + if (!p) { return -1; } + if (!coordinates) { coordinates = "XYZABCUVW"; } + + if (kins_scan_coordinates(coordinates, max_joints, allow_duplicates, + axis_idx_for_jno, + "kinsParamsMapCoordinates: ERROR:\n ")) { + return -1; + } + + // every letter the module cannot do without has to be there + for (a = 0; required_coordinates && required_coordinates[a]; a++) { + char want = required_coordinates[a]; + const char *c; + int seen = 0; + for (c = coordinates; *c; c++) { + if (*c == want || *c == want + ('a' - 'A') || *c == want - ('a' - 'A')) { + seen = 1; break; + } + } + if (!seen) { + rtapi_print_msg(RTAPI_MSG_ERR, + "kinsParamsMapCoordinates: ERROR:\n required coordinates:%s\n" + " specified coordinates:%s\n", + required_coordinates, coordinates); + return -1; + } + } + + for (a = 0; a < EMCMOT_MAX_AXIS; a++) { + p->joint_of_axis[a] = -1; + p->joints_of_axis[a] = 0; + } + p->max_joints = 0; + for (jno = 0; jno < EMCMOT_MAX_JOINTS; jno++) { + a = axis_idx_for_jno[jno]; + if (a < 0) { break; } + if (p->joint_of_axis[a] < 0) { p->joint_of_axis[a] = jno; } + p->joints_of_axis[a] |= 1 << jno; + p->max_joints = jno + 1; + } + return 0; +} // kinsParamsMapCoordinates() + +int kinsParamsInit(kins_params *p, + const kins_module_info *info, + const char *coordinates) +{ + int i; + + if (!p || !info) { return -1; } + if (info->nparams < 0 || info->nparams > KINS_MAX_PARAMS) { + rtapi_print_msg(RTAPI_MSG_ERR, + "kinsParamsInit: %s declares %d parameters, at most %d allowed\n", + info->name ? info->name : "?", info->nparams, KINS_MAX_PARAMS); + return -1; + } + + memset(p, 0, sizeof(*p)); + p->size = sizeof(*p); + p->ktype = 0; + if (!coordinates) { coordinates = info->required_coordinates; } + if (kinsParamsMapCoordinates(p, coordinates, info->max_joints, + info->allow_duplicates, + info->required_coordinates)) { + return -1; + } + for (i = 0; i < info->nparams; i++) { + p->geometry[i] = info->params[i].dflt; + if (info->params[i].tool) { p->tool.tran.z = info->params[i].dflt; } + } + return 0; +} // kinsParamsInit() + +void kinsScratchInit(kins_scratch *s) +{ + if (s) { memset(s, 0, sizeof(*s)); } +} + +int kinsMappedJointsToPose(const kins_params *p, + const double *joints, EmcPose *pos) +{ + int a; + if (!p || !joints || !pos) { return -1; } + for (a = 0; a < EMCMOT_MAX_AXIS; a++) { + int j = p->joint_of_axis[a]; + if (j < 0) { continue; } + switch (a) { + case 0: pos->tran.x = joints[j]; break; + case 1: pos->tran.y = joints[j]; break; + case 2: pos->tran.z = joints[j]; break; + case 3: pos->a = joints[j]; break; + case 4: pos->b = joints[j]; break; + case 5: pos->c = joints[j]; break; + case 6: pos->u = joints[j]; break; + case 7: pos->v = joints[j]; break; + default: pos->w = joints[j]; break; + } + } + return 0; +} // kinsMappedJointsToPose() + +static double kins_pose_coord(const EmcPose *pos, int a) +{ + switch (a) { + case 0: return pos->tran.x; + case 1: return pos->tran.y; + case 2: return pos->tran.z; + case 3: return pos->a; + case 4: return pos->b; + case 5: return pos->c; + case 6: return pos->u; + case 7: return pos->v; + default: return pos->w; + } +} + +int kinsPoseToMappedJoints(const kins_params *p, + const EmcPose *pos, double *joints) +{ + int a, jno; + if (!p || !pos || !joints) { return -1; } + for (a = 0; a < EMCMOT_MAX_AXIS; a++) { + int bits = p->joints_of_axis[a]; + if (!bits) { continue; } + for (jno = 0; jno < p->max_joints; jno++) { + if (bits & (1 << jno)) { joints[jno] = kins_pose_coord(pos, a); } + } + } + return 0; +} // kinsPoseToMappedJoints() + +int kinsJacobianFromMappedAxesP(const kins_params *p, + const double dP[EMCMOT_MAX_AXIS][EMCMOT_MAX_AXIS], + double jac[EMCMOT_MAX_JOINTS][EMCMOT_MAX_AXIS]) +{ + int a, jno, col; + if (!p || !dP || !jac) { return -1; } + kj_zero(jac); + for (a = 0; a < EMCMOT_MAX_AXIS; a++) { + int bits = p->joints_of_axis[a]; + if (!bits) { continue; } + for (jno = 0; jno < p->max_joints; jno++) { + if (!(bits & (1 << jno))) { continue; } + for (col = 0; col < EMCMOT_MAX_AXIS; col++) { jac[jno][col] = dP[a][col]; } + } + } + return 0; +} // kinsJacobianFromMappedAxesP() + +//---------------------------------------------------------------------- +// identity through the block +//---------------------------------------------------------------------- + +int kinsIdentityForward(const kins_params *p, kins_scratch *s, + const double *joint, EmcPose *pos, + const KINEMATICS_FORWARD_FLAGS *fflags, + KINEMATICS_INVERSE_FLAGS *iflags) +{ + (void)s; (void)fflags; (void)iflags; + return kinsMappedJointsToPose(p, joint, pos); +} + +int kinsIdentityInverse(const kins_params *p, kins_scratch *s, + const EmcPose *pos, double *joint, + const KINEMATICS_INVERSE_FLAGS *iflags, + KINEMATICS_FORWARD_FLAGS *fflags) +{ + (void)s; (void)iflags; (void)fflags; + return kinsPoseToMappedJoints(p, pos, joint); +} + +int kinsIdentityFrame(const kins_params *p, const double *joint, + PmRotationMatrix *rot, + const KINEMATICS_FORWARD_FLAGS *fflags) +{ + (void)p; (void)joint; (void)fflags; + *rot = TOOL_FRAME_SPINDLE; + return 0; +} + +int kinsIdentityJacobian(const kins_params *p, const double *joint, + const EmcPose *pos, + double jac[EMCMOT_MAX_JOINTS][EMCMOT_MAX_AXIS], + const KINEMATICS_INVERSE_FLAGS *iflags) +{ + double dP[EMCMOT_MAX_AXIS][EMCMOT_MAX_AXIS]; + int a, b; + (void)joint; (void)pos; (void)iflags; + for (a = 0; a < EMCMOT_MAX_AXIS; a++) { + for (b = 0; b < EMCMOT_MAX_AXIS; b++) { dP[a][b] = (a == b) ? 1.0 : 0.0; } + } + return kinsJacobianFromMappedAxesP(p, (const double (*)[EMCMOT_MAX_AXIS])dP, jac); +} + +const kins_ops KINS_IDENTITY_OPS = { + .forward = kinsIdentityForward, + .inverse = kinsIdentityInverse, + .work = kinsIdentityFrame, + .tool = kinsIdentityFrame, + .native = &TOOL_FRAME_SPINDLE, + .jacobian = kinsIdentityJacobian, + .fwd_iterates = 0, + .identity = 1, +}; + +//---------------------------------------------------------------------- +// asking an ops table, defaults applied +//---------------------------------------------------------------------- + +int kinsOpsForward(const kins_ops *ops, const kins_params *p, + kins_scratch *s, const double *joint, EmcPose *pos, + const KINEMATICS_FORWARD_FLAGS *fflags, + KINEMATICS_INVERSE_FLAGS *iflags) +{ + int r; + if (!ops || !ops->forward || !p || !s) { return -1; } + if (ops->fwd_iterates && s->have_pose_seed) { + /* no pose of our own yet: start from the caller's estimate, + which stays in *pos, rather than from a never-solved seed */ + if (s->pose_seed_ok) { *pos = s->pose_seed; } + s->have_pose_seed = 0; + } + r = ops->forward(p, s, joint, pos, fflags, iflags); + if (ops->fwd_iterates && r == 0) { + /* keep the result only when the solve succeeds */ + s->pose_seed = *pos; + s->pose_seed_ok = 1; + } + return r; +} + +int kinsOpsInverse(const kins_ops *ops, const kins_params *p, + kins_scratch *s, const EmcPose *pos, double *joint, + const KINEMATICS_INVERSE_FLAGS *iflags, + KINEMATICS_FORWARD_FLAGS *fflags) +{ + if (!ops || !ops->inverse || !p || !s) { return -1; } + return ops->inverse(p, s, pos, joint, iflags, fflags); +} + +int kinsOpsWorkFrame(const kins_ops *ops, const kins_params *p, + const double *joint, PmRotationMatrix *rot, + const KINEMATICS_FORWARD_FLAGS *fflags) +{ + if (!ops || !p || !rot) { return -1; } + if (!ops->work) { return -1; } // not supplied; not an error + return ops->work(p, joint, rot, fflags); +} + +int kinsOpsToolFrame(const kins_ops *ops, const kins_params *p, + const double *joint, PmRotationMatrix *rot, + const KINEMATICS_FORWARD_FLAGS *fflags) +{ + int r; + if (!ops || !p || !rot) { return -1; } + if (!ops->tool) { return -1; } // not supplied; not an error + r = ops->tool(p, joint, rot, fflags); + if (r) { return r; } + return toolFrameApplyNative(rot, ops->native ? ops->native + : &TOOL_FRAME_SPINDLE); +} + +int kinsOpsJacobian(const kins_ops *ops, const kins_params *p, + kins_scratch *s, const double *joint, + const EmcPose *pos, + double jac[EMCMOT_MAX_JOINTS][EMCMOT_MAX_AXIS], + const KINEMATICS_INVERSE_FLAGS *iflags) +{ + double qp[EMCMOT_MAX_JOINTS], qm[EMCMOT_MAX_JOINTS]; + KINEMATICS_INVERSE_FLAGS ifl = iflags ? *iflags : 0; + KINEMATICS_FORWARD_FLAGS ffl = 0; + EmcPose q; + int j, a; + + if (!ops || !p || !s || !joint || !pos || !jac) { return -1; } + if (ops->jacobian) { return ops->jacobian(p, joint, pos, jac, iflags); } + if (!ops->inverse) { return -1; } + + // the same differences as kinsJacobianFromInverse(), on the block form + kj_zero(jac); + for (a = 0; a < EMCMOT_MAX_AXIS; a++) { + q = *pos; + for (j = 0; j < EMCMOT_MAX_JOINTS; j++) { qp[j] = qm[j] = joint[j]; } + + *kj_coord(&q, a) += KINS_JACOBIAN_STEP; + if (ops->inverse(p, s, &q, qp, &ifl, &ffl)) { return -1; } + + *kj_coord(&q, a) -= 2 * KINS_JACOBIAN_STEP; + if (ops->inverse(p, s, &q, qm, &ifl, &ffl)) { return -1; } + + for (j = 0; j < p->max_joints && j < EMCMOT_MAX_JOINTS; j++) { + jac[j][a] = (qp[j] - qm[j]) / (2 * KINS_JACOBIAN_STEP); + } + } + return 0; +} // kinsOpsJacobian() + +//---------------------------------------------------------------------- +// the RT side of the table: one HAL pin per entry, copied into the block +// before a call and out of the scratch after it +//---------------------------------------------------------------------- + +int kinsParamsPinsCreate(int comp_id, const char *prefix, + const kins_param_desc *params, int nparams, + kins_pin_ref **out) +{ + kins_pin_ref *pins; + int i, res = 0; + + if (!out) { return -1; } + *out = NULL; + if (nparams < 0 || nparams > KINS_MAX_PARAMS) { return -1; } + if (nparams == 0) { return 0; } + if (!params || !prefix) { return -1; } + + pins = hal_malloc(nparams * sizeof(*pins)); + if (!pins) { + rtapi_print_msg(RTAPI_MSG_ERR, "kinsParamsPinsCreate: hal_malloc failed\n"); + return -1; + } + for (i = 0; i < nparams; i++) { + const kins_param_desc *d = ¶ms[i]; + hal_pdir_t dir = d->dir == KINS_OUT ? HAL_OUT : d->dir == KINS_IO ? HAL_IO : HAL_IN; + switch (d->type) { + case KINS_PARAM_FLOAT: + res += hal_pin_new_real(comp_id, dir, &pins[i].r, d->dflt, "%s.%s", prefix, d->name); + break; + case KINS_PARAM_BIT: + res += hal_pin_new_bool(comp_id, dir, &pins[i].b, d->dflt != 0, "%s.%s", prefix, d->name); + break; + case KINS_PARAM_S32: + res += hal_pin_new_si32(comp_id, dir, &pins[i].s, (rtapi_s32)d->dflt, "%s.%s", prefix, d->name); + break; + case KINS_PARAM_U32: + res += hal_pin_new_ui32(comp_id, dir, &pins[i].u, (rtapi_u32)d->dflt, "%s.%s", prefix, d->name); + break; + default: + res = -1; + } + } + if (res) { + rtapi_print_msg(RTAPI_MSG_ERR, "kinsParamsPinsCreate: pin create failed for %s\n", prefix); + return -1; + } + *out = pins; + return 0; +} // kinsParamsPinsCreate() + +void kinsParamsPinsRead(const kins_pin_ref *pins, + const kins_param_desc *params, int nparams, + kins_params *p) +{ + int i; + if (!pins || !params || !p) { return; } + for (i = 0; i < nparams && i < KINS_MAX_PARAMS; i++) { + const kins_param_desc *d = ¶ms[i]; + double v; + if (d->dir == KINS_OUT) { continue; } + switch (d->type) { + case KINS_PARAM_FLOAT: v = hal_get_real(pins[i].r); break; + case KINS_PARAM_BIT: v = hal_get_bool(pins[i].b) ? 1.0 : 0.0; break; + case KINS_PARAM_S32: v = hal_get_si32(pins[i].s); break; + case KINS_PARAM_U32: v = hal_get_ui32(pins[i].u); break; + default: v = 0; + } + p->geometry[i] = v; + if (d->tool) { p->tool.tran.z = v; } + } +} // kinsParamsPinsRead() + +void kinsParamsPinsWrite(const kins_pin_ref *pins, + const kins_param_desc *params, int nparams, + const kins_scratch *s) +{ + int i; + if (!pins || !params || !s) { return; } + for (i = 0; i < nparams && i < KINS_MAX_PARAMS; i++) { + const kins_param_desc *d = ¶ms[i]; + if (d->dir != KINS_OUT) { continue; } + switch (d->type) { + case KINS_PARAM_FLOAT: hal_set_real(pins[i].r, s->out[i]); break; + case KINS_PARAM_BIT: hal_set_bool(pins[i].b, s->out[i] != 0); break; + case KINS_PARAM_S32: hal_set_si32(pins[i].s, (rtapi_s32)s->out[i]); break; + case KINS_PARAM_U32: hal_set_ui32(pins[i].u, (rtapi_u32)s->out[i]); break; + default: break; + } + } +} // kinsParamsPinsWrite() diff --git a/src/emc/kinematics/nonrt_kins.h b/src/emc/kinematics/nonrt_kins.h deleted file mode 100644 index ed564f21b6f..00000000000 --- a/src/emc/kinematics/nonrt_kins.h +++ /dev/null @@ -1,95 +0,0 @@ -/******************************************************************** - * Description: nonrt_kins.h - * Interface a kinematics module exports so that a non-RT caller can - * evaluate it. - * - * A trajectory planner needs forward and inverse kinematics at poses - * the machine has not reached yet, which means calling them outside - * the servo thread. A module opts in by exporting nonrt_attach(). - * - * The caller dlopens the module and calls nonrt_attach() once with - * the coordinates string and a resolver callback. The module names - * each of the pins it reads, keeps the references the resolver - * returns in its own haldata, and hands back its existing forward - * and inverse. The kinematics code itself does not change. - * - * A reference does not point into the RT instance's pin. The - * resolver creates an input pin on the caller's own component and - * connects it to the signal the RT pin reads, so the reference - * belongs to the caller and rewiring cannot strand it. - * - * Name lookup belongs to the caller, userspace code linked against - * liblinuxcnchal. This file is compiled into an RT module, which - * has no business walking the HAL name space and would risk binding - * against rtlib's copy of the same symbols. - * - * Resolve input pins only. Output pins and scratch storage stay - * private to the non-RT copy, or the two copies write to each - * other's state. - * - * Author: LinuxCNC - * License: GPL Version 2 - * System: Linux - * - * Copyright (c) 2024 All rights reserved. - ********************************************************************/ - -#ifndef NONRT_KINS_H -#define NONRT_KINS_H - -#include - -#include -#include -#include -#include - -/* Supplied by the caller. Finds 'pin_name' in HAL, checks that it has - type 'type', and writes to 'out' a reference carrying that pin's - value. The reference is to storage the caller owns, not to the named - pin itself. Returns 0 on success. */ -typedef int (*nonrt_resolve_fn)(const char *pin_name, - hal_type_t type, - hal_refs_u *out, - void *arg); - -/* Filled in by nonrt_attach(). A module that reports is_identity has - joints equal to axes and the caller needs no module code at all, so - forward and inverse may be left NULL. */ -typedef struct { - int (*forward)(const double *joints, EmcPose *pos, - const KINEMATICS_FORWARD_FLAGS *fflags, - KINEMATICS_INVERSE_FLAGS *iflags); - int (*inverse)(const EmcPose *pos, double *joints, - const KINEMATICS_INVERSE_FLAGS *iflags, - KINEMATICS_FORWARD_FLAGS *fflags); - int is_identity; -} nonrt_ops_t; - -/* Exported by a participating module: - int nonrt_attach(const char *coordinates, nonrt_ops_t *ops, - nonrt_resolve_fn resolve, void *arg); - Returns 0 on success. */ - -/* Convenience for the common case: resolve one float pin, by printf - style name, into a haldata field. */ -static inline int nonrt_resolve_real(nonrt_resolve_fn resolve, void *arg, - hal_real_t *dst, const char *fmt, ...) -{ - char name[HAL_NAME_LEN + 1]; - hal_refs_u ref; - va_list ap; - - if (!resolve || !dst) return -1; - - va_start(ap, fmt); - rtapi_vsnprintf(name, sizeof(name), fmt, ap); - va_end(ap); - - if (resolve(name, HAL_FLOAT, &ref, arg) != 0) return -1; - - *dst = ref.r; - return 0; -} - -#endif /* NONRT_KINS_H */ diff --git a/src/emc/kinematics/switchkins.c b/src/emc/kinematics/switchkins.c index 472394cefdd..24b432262fb 100644 --- a/src/emc/kinematics/switchkins.c +++ b/src/emc/kinematics/switchkins.c @@ -27,10 +27,12 @@ * Using modules must supply function: switchkinsSetup() */ #include +#include #include #include #include "switchkins.h" +#include //********************************************************************* // kinematic functions (default=0 for err detection): @@ -46,6 +48,15 @@ static KTI ktinvs[SWITCHKINS_MAX_TYPES] = {NULL}; static KJ kjacs[SWITCHKINS_MAX_TYPES] = {NULL}; static PmRotationMatrix knative[SWITCHKINS_MAX_TYPES]; +// types written as pure functions (see kinematics.h): the maths of each, +// the one RT parameter block they all read, a scratch per type, and the +// pins made from the module's table +static const kins_ops *kops[SWITCHKINS_MAX_TYPES] = {NULL}; +static kins_params rt_params; +static kins_scratch rt_scratch[SWITCHKINS_MAX_TYPES]; +static kins_pin_ref *pins; +static int inited; + // types provided, counted in rtapi_app_main() once they are all in static int kins_count; static int register_error; @@ -102,6 +113,36 @@ static void get_lastpose(int ktype, EmcPose* pos) pos->w = lastpose[ktype].w; } // get_lastpose() +// the block sees the pins as they are now, and the type asked for +static void read_block(int ktype) +{ + rt_params.ktype = ktype; + kinsParamsPinsRead(pins, kp.params, kp.nparams, &rt_params); +} + +static void write_block(int ktype) +{ + kinsParamsPinsWrite(pins, kp.params, kp.nparams, &rt_scratch[ktype]); +} + +// the forward of one type, whichever way it was provided, from the pose +// it is handed: no seeding, which is the caller's business +static int call_forward(int ktype, const double *joint, EmcPose *pos, + const KINEMATICS_FORWARD_FLAGS *fflags, + KINEMATICS_INVERSE_FLAGS *iflags) +{ + int r; + if (kops[ktype]) { + read_block(ktype); + r = kops[ktype]->forward(&rt_params, &rt_scratch[ktype], + joint, pos, fflags, iflags); + write_block(ktype); + return r; + } + if (!kfwds[ktype]) { return -1; } + return kfwds[ktype](joint, pos, fflags, iflags); +} + static int gui_forward_kins(const double *joints, const EmcPose* estimate) { // the hexapod vismach gui uses these hal pins to @@ -113,7 +154,7 @@ static int gui_forward_kins(const double *joints, const EmcPose* estimate) KINEMATICS_INVERSE_FLAGS iflags; if ( kp.gui_kinstype < 0 || kp.gui_kinstype >= kins_count - || !kfwds[kp.gui_kinstype]) { + || (!kfwds[kp.gui_kinstype] && !kops[kp.gui_kinstype])) { rtapi_print_msg(RTAPI_MSG_ERR, "gui_forward_kins BAD gui_kinstype <%d>\n", kp.gui_kinstype); @@ -123,8 +164,8 @@ static int gui_forward_kins(const double *joints, const EmcPose* estimate) // no pose of our own yet, start from the caller's lastpose[kp.gui_kinstype] = *estimate; } - res = kfwds[kp.gui_kinstype](joints, &lastpose[kp.gui_kinstype], - &fflags, &iflags); + res = call_forward(kp.gui_kinstype, joints, &lastpose[kp.gui_kinstype], + &fflags, &iflags); lastpose_ok[kp.gui_kinstype] = (res == 0); hal_set_real(swdata->gui_x, lastpose[kp.gui_kinstype].tran.x); hal_set_real(swdata->gui_y, lastpose[kp.gui_kinstype].tran.y); @@ -163,6 +204,10 @@ int kinematicsSwitch(int new_switchkins_type) if (fwd_iterates[switchkins_type] && lastpose_ok[switchkins_type]) { use_lastpose[switchkins_type] = 1; // restarting a kins types } + // a pure type keeps the same restart pose in its own scratch + if (kops[switchkins_type] && kops[switchkins_type]->fwd_iterates) { + rt_scratch[switchkins_type].have_pose_seed = 1; + } return 0; // 0==> no error } // kinematicsSwitch() @@ -174,26 +219,37 @@ int kinematicsForward(const double *joint, int r; EmcPose estimate = *pos; // the caller's guess, the only one we get - if ( fwd_iterates[switchkins_type] - && use_lastpose[switchkins_type] - && lastpose_ok[switchkins_type]) { - // initialize iterative forward kins (ok for identity too) - get_lastpose(switchkins_type,pos); - use_lastpose[switchkins_type] = 0; - } - if ( switchkins_type < 0 || switchkins_type >= kins_count - || !kfwds[switchkins_type]) { + || (!kfwds[switchkins_type] && !kops[switchkins_type])) { rtapi_print_msg(RTAPI_MSG_ERR, "switchkins: Forward BAD switchkins_type \n", switchkins_type); return -1; } - r = kfwds[switchkins_type](joint, pos, fflags, iflags); - if (fwd_iterates[switchkins_type]) { - save_lastpose(switchkins_type,pos); - lastpose_ok[switchkins_type] = (r == 0); + + if (kops[switchkins_type]) { + read_block(switchkins_type); + r = kinsOpsForward(kops[switchkins_type], &rt_params, + &rt_scratch[switchkins_type], + joint, pos, fflags, iflags); + write_block(switchkins_type); + // the gui forward below starts from here, as it did for the + // older form + if (kops[switchkins_type]->fwd_iterates) {save_lastpose(switchkins_type,pos);} + } else { + if ( fwd_iterates[switchkins_type] + && use_lastpose[switchkins_type] + && lastpose_ok[switchkins_type]) { + // initialize iterative forward kins (ok for identity too) + get_lastpose(switchkins_type,pos); + use_lastpose[switchkins_type] = 0; + } + r = kfwds[switchkins_type](joint, pos, fflags, iflags); + if (fwd_iterates[switchkins_type]) { + save_lastpose(switchkins_type,pos); + lastpose_ok[switchkins_type] = (r == 0); + } } if (r) return r; @@ -223,12 +279,20 @@ int kinematicsInverse(const EmcPose * pos, if ( switchkins_type < 0 || switchkins_type >= kins_count - || !kinvs[switchkins_type]) { + || (!kinvs[switchkins_type] && !kops[switchkins_type])) { rtapi_print_msg(RTAPI_MSG_ERR, "switchkins: Inverse BAD switchkins_type \n", switchkins_type); return -1; } + if (kops[switchkins_type]) { + read_block(switchkins_type); + r = kinsOpsInverse(kops[switchkins_type], &rt_params, + &rt_scratch[switchkins_type], + pos, joint, iflags, fflags); + write_block(switchkins_type); + return r; + } r = kinvs[switchkins_type](pos, joint, iflags, fflags); return r; } // kinematicsInverse() @@ -239,9 +303,13 @@ int kinematicsToolFrame(const double *joint, { int r; - if ( switchkins_type < 0 - || switchkins_type >= kins_count - || !ktools[switchkins_type]) { + if (switchkins_type < 0 || switchkins_type >= kins_count) { return -1; } + if (kops[switchkins_type]) { + read_block(switchkins_type); + return kinsOpsToolFrame(kops[switchkins_type], &rt_params, + joint, rot, fflags); + } + if (!ktools[switchkins_type]) { return -1; // this type does not supply one; not an error } r = ktools[switchkins_type](joint, rot, fflags); @@ -256,9 +324,13 @@ int kinematicsWorkFrame(const double *joint, PmRotationMatrix *rot, const KINEMATICS_FORWARD_FLAGS *fflags) { - if ( switchkins_type < 0 - || switchkins_type >= kins_count - || !kworks[switchkins_type]) { + if (switchkins_type < 0 || switchkins_type >= kins_count) { return -1; } + if (kops[switchkins_type]) { + read_block(switchkins_type); + return kinsOpsWorkFrame(kops[switchkins_type], &rt_params, + joint, rot, fflags); + } + if (!kworks[switchkins_type]) { return -1; // this type does not supply one; not an error } // no native rotation here: the work frame has no tool axis to point the @@ -275,10 +347,12 @@ int kinematicsToolFrameInverse(const PmCartesian *axis_in_work, int *free_directions, double *tool_spin) { - if ( switchkins_type < 0 - || switchkins_type >= kins_count - || !ktools[switchkins_type] - || !kworks[switchkins_type]) { + if (switchkins_type < 0 || switchkins_type >= kins_count) { return -1; } + if (kops[switchkins_type]) { + if (!kops[switchkins_type]->tool || !kops[switchkins_type]->work) { + return -1; // this type does not report its frames, so it cannot answer + } + } else if (!ktools[switchkins_type] || !kworks[switchkins_type]) { return -1; // this type does not report its frames, so it cannot answer } @@ -307,6 +381,12 @@ int kinematicsJacobian(const double *joint, if (switchkins_type < 0 || switchkins_type >= kins_count) { return -1; } + if (kops[switchkins_type]) { + read_block(switchkins_type); + return kinsOpsJacobian(kops[switchkins_type], &rt_params, + &rt_scratch[switchkins_type], + joint, world, jac, iflags); + } // a closed form is exact and knows its own singular poses if (kjacs[switchkins_type]) { return kjacs[switchkins_type](joint, world, jac, iflags); @@ -333,7 +413,7 @@ int switchkinsRegister(int ktype, KS kset, KF kfwd, KI kinv) register_error = 1; return -1; } - if (ksetups[ktype] || kfwds[ktype] || kinvs[ktype]) { + if (ksetups[ktype] || kfwds[ktype] || kinvs[ktype] || kops[ktype]) { rtapi_print_msg(RTAPI_MSG_ERR, "switchkinsRegister: switchkins-type %d" " already provided\n", ktype); @@ -346,6 +426,65 @@ int switchkinsRegister(int ktype, KS kset, KF kfwd, KI kinv) return 0; } // switchkinsRegister() +int switchkinsDeclare(int ktype, int flags) +{ + if (ktype < 0 || ktype >= SWITCHKINS_MAX_TYPES) { + rtapi_print_msg(RTAPI_MSG_ERR, + "switchkinsDeclare: BAD switchkins_type <%d>" + " (must be 0..%d)\n", + ktype, SWITCHKINS_MAX_TYPES - 1); + register_error = 1; + return -1; + } + ktype_flags[ktype] = flags; + return 0; +} // switchkinsDeclare() + +int kinematicsTypeFlags(int ktype) +{ + if ( ktype < 0 + || ktype >= kins_count + || (!kfwds[ktype] && !kops[ktype])) { return -1; } + return ktype_flags[ktype]; +} // kinematicsTypeFlags() + +int switchkinsRegisterOps(int ktype, const kins_ops *ops) +{ + if (ktype < 0 || ktype >= SWITCHKINS_MAX_TYPES) { + rtapi_print_msg(RTAPI_MSG_ERR, + "switchkinsRegisterOps: BAD switchkins_type <%d>" + " (must be 0..%d)\n", + ktype, SWITCHKINS_MAX_TYPES - 1); + register_error = 1; + return -1; + } + if (ksetups[ktype] || kfwds[ktype] || kinvs[ktype] || kops[ktype]) { + rtapi_print_msg(RTAPI_MSG_ERR, + "switchkinsRegisterOps: switchkins-type %d" + " already provided\n", ktype); + register_error = 1; + return -1; + } + if (!ops || !ops->forward || !ops->inverse) { + rtapi_print_msg(RTAPI_MSG_ERR, + "switchkinsRegisterOps: switchkins-type %d" + " has no forward or inverse\n", ktype); + register_error = 1; + return -1; + } + if (ops->tool && ops->native && !toolFrameIsProper(ops->native)) { + rtapi_print_msg(RTAPI_MSG_ERR, + "switchkinsRegisterOps: switchkins-type %d" + " declared a rotation that is not orthonormal with" + " determinant +1\n", ktype); + register_error = 1; + return -1; + } + kops[ktype] = ops; + if (ops->identity) { ktype_flags[ktype] |= KINSTYPE_IDENTITY; } + return 0; +} // switchkinsRegisterOps() + int switchkinsRegisterFrames(int ktype, KT kwork, KT ktool, const PmRotationMatrix *native) { @@ -400,26 +539,6 @@ int switchkinsRegisterToolFrameInverse(int ktype, KTI kinv) return 0; } // switchkinsRegisterToolFrameInverse() -int switchkinsDeclare(int ktype, int flags) -{ - if (ktype < 0 || ktype >= SWITCHKINS_MAX_TYPES) { - rtapi_print_msg(RTAPI_MSG_ERR, - "switchkinsDeclare: BAD switchkins_type <%d>" - " (must be 0..%d)\n", - ktype, SWITCHKINS_MAX_TYPES - 1); - register_error = 1; - return -1; - } - ktype_flags[ktype] = flags; - return 0; -} // switchkinsDeclare() - -int kinematicsTypeFlags(int ktype) -{ - if (ktype < 0 || ktype >= kins_count || !kfwds[ktype]) { return -1; } - return ktype_flags[ktype]; -} // kinematicsTypeFlags() - EXPORT_SYMBOL(kinematicsSwitchable); EXPORT_SYMBOL(kinematicsSwitch); EXPORT_SYMBOL(kinematicsType); @@ -435,7 +554,46 @@ EXPORT_SYMBOL(switchkinsRegisterToolFrameInverse); EXPORT_SYMBOL(switchkinsDeclare); EXPORT_SYMBOL(kinematicsTypeFlags); EXPORT_SYMBOL(switchkinsRegisterJacobian); +EXPORT_SYMBOL(switchkinsRegisterOps); EXPORT_SYMBOL(switchkinsInit); +EXPORT_SYMBOL(switchkinsDescribe); +EXPORT_SYMBOL(switchkinsDescribeSetup); + +//********************************************************************* +// the module as registered so far, described for a caller outside RT +int switchkinsDescribeSetup(const kparms *k, kins_module_info *info) +{ + int i, n = 0; + + if (!k || !info) { return -1; } + if (k->nparams < 0 || k->nparams > KINS_MAX_PARAMS + || (k->nparams > 0 && !k->params)) { + rtapi_print_msg(RTAPI_MSG_ERR, + "switchkins: %s declares a bad parameter table\n", + k->kinsname ? k->kinsname : "?"); + return -1; + } + memset(info, 0, sizeof(*info)); + info->name = k->kinsname; + info->halprefix = k->halprefix ? k->halprefix : k->kinsname; + info->params = k->params; + info->nparams = k->nparams; + info->required_coordinates = k->required_coordinates; + info->max_joints = k->max_joints; + info->allow_duplicates = k->allow_duplicates; + for (i=0; i < SWITCHKINS_MAX_TYPES; i++) { + info->ops[i] = kops[i]; + if (ksetups[i] || kfwds[i] || kinvs[i] || kops[i]) { n = i + 1; } + } + info->ntypes = n; + return 0; +} // switchkinsDescribeSetup() + +int switchkinsDescribe(kins_module_info *info) +{ + if (!inited) { return -1; } + return switchkinsDescribeSetup(&kp, info); +} // switchkinsDescribe() //********************************************************************* // The caller owns the hal component: it does hal_init() before this and @@ -470,7 +628,7 @@ int switchkinsInit(const int comp_id, // the highest type registered sets the count for (i=0; i < SWITCHKINS_MAX_TYPES; i++) { - if (ksetups[i] || kfwds[i] || kinvs[i]) { kins_count = i + 1; } + if (ksetups[i] || kfwds[i] || kinvs[i] || kops[i]) { kins_count = i + 1; } } if (!kins_count) { emsg = "no switchkins-types provided"; goto error; } @@ -517,6 +675,7 @@ int switchkinsInit(const int comp_id, // a type left out below the highest one provided is a gap, not a count for (i=0; i < kins_count; i++) { + if (kops[i]) { continue; } if (ksetups[i] && kfwds[i] && kinvs[i]) { continue; } rtapi_print_msg(RTAPI_MSG_ERR, "switchkins: switchkins-type %d incomplete:%s%s%s\n", @@ -550,10 +709,36 @@ int switchkinsInit(const int comp_id, if (!coordinates) {coordinates = kp.required_coordinates;} + // the pure types share one block and one set of pins from the table + if (kp.params || kp.nparams) { + kins_module_info mi; + if (switchkinsDescribeSetup(&kp, &mi)) { emsg = "bad table"; goto error; } + if (kinsParamsInit(&rt_params, &mi, coordinates)) { + emsg = "coordinates"; goto error; + } + if (kinsParamsPinsCreate(comp_id, kp.halprefix, kp.params, kp.nparams, + &pins)) { + emsg = "table pin create fail"; goto error; + } + } else { + for (i=0; i < kins_count; i++) { + if (kops[i]) { + kins_module_info mi; + if (switchkinsDescribeSetup(&kp, &mi)) { emsg = "bad table"; goto error; } + if (kinsParamsInit(&rt_params, &mi, coordinates)) { + emsg = "coordinates"; goto error; + } + break; + } + } + } + for (i=0; i < SWITCHKINS_MAX_TYPES; i++) { kinsScratchInit(&rt_scratch[i]); } + for (i=0; i < kins_count; i++) { - ksetups[i](comp_id,coordinates,&kp); + if (ksetups[i]) { ksetups[i](comp_id,coordinates,&kp); } } + inited = 1; return 0; error: diff --git a/src/emc/kinematics/switchkins.h b/src/emc/kinematics/switchkins.h index c7114262403..c5f87b35117 100644 --- a/src/emc/kinematics/switchkins.h +++ b/src/emc/kinematics/switchkins.h @@ -7,7 +7,10 @@ #include "kinematics.h" //SWITCHKINS_MAX_TYPES (max number of types a module may provide) -//is in kinematics.h: motion and the NML status channel need it too +//is in kinematics.h as KINS_MAX_TYPES: motion and the NML +//status channel need it too +//max number of switchkins types a module may provide: +#define SWITCHKINS_MAX_TYPES KINS_MAX_TYPES // KinematicsFORWARD functions typedef int (*KF)(const double *joint, @@ -85,10 +88,32 @@ typedef int (*KJ)(const double *joint, // otherwise the generic differences of its own inverse. extern int switchkinsRegisterJacobian(int ktype, KJ kjac); +// provide one switchkins-type written as pure functions (see kinematics.h), +// before switchkinsInit(). Its pins come from the table in kparms, shared +// by every type of the module, so it has no setup function. A type may be +// provided this way or through switchkinsRegister(), not both. +extern int switchkinsRegisterOps(int ktype, const kins_ops *ops); + // create the hal pins and start on type 0; the caller owns the hal // component and does hal_init() before and hal_ready() after extern int switchkinsInit(const int comp_id, kparms* ksetup_parms, const char* coordinates ); + +// Fill kp with the defaults, run the module's switchkinsSetup() and +// register the three types it may return, so that every type goes in by +// one route. In switchkins_setup.c, which a module links only if it +// defines switchkinsSetup(); a halcompile component that registers its +// types itself does not. Returns 0 or -1. +extern int switchkinsRunSetup(kparms* kp, const char* sparm); + +// The module as the core knows it after switchkinsInit(): its table and +// the ops of every type, NULL for one provided the old way. Behind +// kinsDescribe() for the RT instance; a copy outside RT that has not been +// initialised is described by switchkins_setup.c after a replay of setup. +// Returns 0, or -1 before switchkinsInit(). +extern int switchkinsDescribe(kins_module_info *info); +extern int switchkinsDescribeSetup(const kparms *kp, kins_module_info *info); + #endif diff --git a/src/emc/kinematics/switchkins_main.c b/src/emc/kinematics/switchkins_main.c index 4a4cc05153c..8ab98b54223 100644 --- a/src/emc/kinematics/switchkins_main.c +++ b/src/emc/kinematics/switchkins_main.c @@ -19,9 +19,10 @@ /* switchkins_main.c provides rtapi_app_main() for kinematics modules * built around switchkins.c. A module that gets its rtapi_app_main() * from somewhere else (a halcompile component, for instance) links -* switchkins.c alone and calls switchkinsInit() itself. +* switchkins.c without this file and calls switchkinsInit() itself. * -* Using modules must supply function: switchkinsSetup() +* Using modules must supply function: switchkinsSetup(), which +* switchkinsRunSetup() in switchkins_setup.c runs. */ #include #include @@ -41,43 +42,8 @@ static int comp_id = -1; int rtapi_app_main(void) { kparms kp; - KS ksetup[3] = {NULL}; - KF kfwd[3] = {NULL}; - KI kinv[3] = {NULL}; - int i; - // defaults prior to switchkinsSetup() call - kp.kinsname = NULL; - kp.halprefix = NULL; - kp.required_coordinates = ""; - kp.max_joints = 0; // Setup must supply - kp.allow_duplicates = 0; - kp.fwd_iterates_mask = 0; - kp.gui_kinstype = -1; // negative means: not used - - kp.sparm = sparm; // module parm passed to kins - - // switchkinsSetup() provides types 0,1,2 and may also call - // switchkinsRegister() for any others - if (switchkinsSetup(&kp, - &ksetup[0], &ksetup[1], &ksetup[2], - &kfwd[0], &kfwd[1], &kfwd[2], - &kinv[0], &kinv[1], &kinv[2])) { - rtapi_print_msg(RTAPI_MSG_ERR,"\nSwitchkins FAIL:\n"); - return -1; - } - - // the types switchkinsSetup() supplied go in by the same route as - // any other, so that providing one twice is caught - for (i=0; i < 3; i++) { - if (!ksetup[i] && !kfwd[i] && !kinv[i]) { continue; } - if (switchkinsRegister(i, ksetup[i], kfwd[i], kinv[i])) { return -1; } - } - - if (!kp.kinsname) { - rtapi_print_msg(RTAPI_MSG_ERR,"\nSwitchkins FAIL:\n"); - return -1; - } + if (switchkinsRunSetup(&kp, sparm)) { return -1; } comp_id = hal_init(kp.kinsname); if (comp_id < 0) return comp_id; diff --git a/src/emc/kinematics/switchkins_setup.c b/src/emc/kinematics/switchkins_setup.c new file mode 100644 index 00000000000..b44b7192412 --- /dev/null +++ b/src/emc/kinematics/switchkins_setup.c @@ -0,0 +1,85 @@ +/* + License GPL Version 2 +*/ + +/* switchkins_setup.c: the part of a switchkins module that depends on the +* module supplying switchkinsSetup(). Kept apart from switchkins.c so +* that a halcompile component, which registers its types itself and has +* no switchkinsSetup(), can link the core without it. +* +* switchkinsRunSetup() is what rtapi_app_main() and EXTRA_SETUP() call +* before switchkinsInit(). kinsDescribe() is the description a copy of +* the module loaded outside RT answers with: the RT instance describes +* itself from its own state, a fresh copy replays setup first, so the +* types come out the way the module parameters decide them. +*/ +#include +#include +#include + +#include + +int switchkinsRunSetup(kparms* kp, const char* sparm) +{ + KS ksetup[3] = {NULL}; + KF kfwd[3] = {NULL}; + KI kinv[3] = {NULL}; + int i; + + if (!kp) { return -1; } + memset(kp, 0, sizeof(*kp)); + + // defaults prior to switchkinsSetup() call + kp->kinsname = NULL; + kp->halprefix = NULL; + kp->required_coordinates = ""; + kp->max_joints = 0; // Setup must supply + kp->allow_duplicates = 0; + kp->fwd_iterates_mask = 0; + kp->gui_kinstype = -1; // negative means: not used + + kp->sparm = (char*)sparm; // module parm passed to kins + + // switchkinsSetup() provides types 0,1,2 and may also call + // switchkinsRegister() or switchkinsRegisterOps() for any others + if (switchkinsSetup(kp, + &ksetup[0], &ksetup[1], &ksetup[2], + &kfwd[0], &kfwd[1], &kfwd[2], + &kinv[0], &kinv[1], &kinv[2])) { + rtapi_print_msg(RTAPI_MSG_ERR,"\nSwitchkins FAIL:\n"); + return -1; + } + + // the types switchkinsSetup() supplied go in by the same route as + // any other, so that providing one twice is caught + for (i=0; i < 3; i++) { + if (!ksetup[i] && !kfwd[i] && !kinv[i]) { continue; } + if (switchkinsRegister(i, ksetup[i], kfwd[i], kinv[i])) { return -1; } + } + + if (!kp->kinsname) { + rtapi_print_msg(RTAPI_MSG_ERR,"\nSwitchkins FAIL:\n"); + return -1; + } + return 0; +} // switchkinsRunSetup() + +int kinsDescribe(const char *coordinates, const char *sparm, + kins_module_info *info) +{ + static kparms kp; + (void)coordinates; // the map is the caller's business, see kinsParamsInit() + + if (!info) { return -1; } + + // the RT instance knows itself already + if (switchkinsDescribe(info) == 0) { return 0; } + + // a copy outside RT: register the types the way the module would + if (switchkinsRunSetup(&kp, sparm)) { return -1; } + if (switchkinsDescribeSetup(&kp, info)) { return -1; } + return 0; +} // kinsDescribe() + +EXPORT_SYMBOL(switchkinsRunSetup); +EXPORT_SYMBOL(kinsDescribe); diff --git a/src/emc/kinematics/trivkins.c b/src/emc/kinematics/trivkins.c index 0690aa9ee39..2de2368614a 100644 --- a/src/emc/kinematics/trivkins.c +++ b/src/emc/kinematics/trivkins.c @@ -10,63 +10,27 @@ * ********************************************************************/ -#include #include /* RTAPI realtime OS API */ #include /* RTAPI realtime module decls */ -#include #include #include #include #include -#include "nonrt_kins.h" - - -#define SET(f) pos->f = joints[i] - -int kinematicsForward(const double *joints, - EmcPose * pos, - const KINEMATICS_FORWARD_FLAGS * fflags, - KINEMATICS_INVERSE_FLAGS * iflags) -{ - return identityKinematicsForward(joints, pos, fflags, iflags); -} - -int kinematicsInverse(const EmcPose * pos, - double *joints, - const KINEMATICS_INVERSE_FLAGS * iflags, - KINEMATICS_FORWARD_FLAGS * fflags) -{ - return identityKinematicsInverse(pos, joints, iflags, fflags); -} - -int kinematicsToolFrame(const double *joints, - PmRotationMatrix *rot, - const KINEMATICS_FORWARD_FLAGS *fflags) -{ - return identityKinematicsToolFrame(joints, rot, fflags); -} - -int kinematicsWorkFrame(const double *joints, - PmRotationMatrix *rot, - const KINEMATICS_FORWARD_FLAGS *fflags) -{ - return identityKinematicsWorkFrame(joints, rot, fflags); -} - -int kinematicsJacobian(const double *joints, - const EmcPose *pos, - double jac[EMCMOT_MAX_JOINTS][EMCMOT_MAX_AXIS], - const KINEMATICS_INVERSE_FLAGS *iflags) -{ - return identityKinematicsJacobian(joints, pos, jac, iflags); -} - -static KINEMATICS_TYPE ktype = -1; - -KINEMATICS_TYPE kinematicsType() -{ - return ktype; -} +#include + +// joints are axes, through whatever map coordinates= gives; the maths is +// the shared identity and the entry points come from kins_single.c +const kins_module_info kins_module = { + .name = "trivkins", + .halprefix = "trivkins", + .params = NULL, + .nparams = 0, + .required_coordinates = "", + .max_joints = EMCMOT_MAX_JOINTS, + .allow_duplicates = 1, + .ntypes = 1, + .ops = { &KINS_IDENTITY_OPS }, +}; #define TRIVKINS_DEFAULT_COORDINATES "XYZABCUVW" static char *coordinates = TRIVKINS_DEFAULT_COORDINATES; @@ -75,19 +39,40 @@ RTAPI_MP_STRING(coordinates, "Existing Axes"); static char *kinstype = "1"; // use KINEMATICS_IDENTITY RTAPI_MP_STRING(kinstype, "Kinematics Type (Identity,Both)"); -KINS_NOT_SWITCHABLE -EXPORT_SYMBOL(kinematicsType); -EXPORT_SYMBOL(kinematicsForward); -EXPORT_SYMBOL(kinematicsInverse); -EXPORT_SYMBOL(kinematicsToolFrame); -EXPORT_SYMBOL(kinematicsWorkFrame); -EXPORT_SYMBOL(kinematicsJacobian); MODULE_LICENSE("GPL"); static int comp_id; +// say so when the joints are not in axis order, and which type suits that +static void show_map(KINEMATICS_TYPE ktype) +{ + kins_params p; + int a, unconventional = 0; + + if (kinsParamsInit(&p, &kins_module, coordinates)) { return; } + for (a = 0; a < EMCMOT_MAX_AXIS; a++) { + if (p.joint_of_axis[a] >= 0 && p.joint_of_axis[a] != a) { unconventional = 1; } + if (p.joints_of_axis[a] & (p.joints_of_axis[a] - 1)) { unconventional = 1; } + } + if (!unconventional || !strcasecmp(coordinates, "xz")) { return; } + + rtapi_print("\ntrivkins: coordinates:%s\n", coordinates); + for (a = 0; a < EMCMOT_MAX_AXIS; a++) { + int j; + for (j = 0; j < p.max_joints; j++) { + if (p.joints_of_axis[a] & (1 << j)) { + rtapi_print(" Joint %d ==> Axis %c\n", j, "XYZABCUVW"[a]); + } + } + } + if (ktype != KINEMATICS_BOTH) { + rtapi_print("trivkins: Recommend: kinstype=both\n"); + } + rtapi_print("\n"); +} + int rtapi_app_main(void) { - kparms ksetup; + KINEMATICS_TYPE ktype; switch (*kinstype) { case 'b': case 'B': ktype = KINEMATICS_BOTH; break; @@ -99,29 +84,14 @@ int rtapi_app_main(void) { comp_id = hal_init("trivkins"); if(comp_id < 0) return comp_id; - // see typedef for KS KinematicsSETUP: - ksetup.max_joints = EMCMOT_MAX_JOINTS; - ksetup.allow_duplicates = 1; - if (identityKinematicsSetup(comp_id, coordinates, &ksetup)) { - return -1; //setup failed + if (kinsSingleInit(comp_id, coordinates, ktype)) { + hal_exit(comp_id); + return -1; } + show_map(ktype); hal_ready(comp_id); return 0; } void rtapi_app_exit(void) { hal_exit(comp_id); } - -// Non-RT entry point: joints are axes, so a non-RT caller needs no -// module code at all and reads nothing from HAL. -int nonrt_attach(const char* coordinates, nonrt_ops_t* ops, - nonrt_resolve_fn resolve, void* arg) -{ - (void)coordinates; (void)resolve; (void)arg; - ops->forward = NULL; - ops->inverse = NULL; - ops->is_identity = 1; - return 0; -} - -EXPORT_SYMBOL(nonrt_attach); diff --git a/src/emc/kinematics/userkfuncs.c b/src/emc/kinematics/userkfuncs.c index 81aa4c2d942..0e51ed651e5 100644 --- a/src/emc/kinematics/userkfuncs.c +++ b/src/emc/kinematics/userkfuncs.c @@ -2,6 +2,12 @@ ** switchable kinematics functions. ** License GPL Version 2 ** +** Two forms are here. USERK_OPS is the current one: identity through +** the parameter block, with no state of its own, registered by a module +** with switchkinsRegisterOps(2, &USERK_OPS). The functions below it are +** the older form, kept for the modules that still register their types +** through switchkinsSetup()'s out parameters. +** ** Example Usage (for customizing the genser-switchkins module): ** (works with rtpreempt only rtai --> Makefile needs work) ** @@ -24,6 +30,34 @@ // #include "genserkins.h" //includes gomath,hal //********************************************************************** +// the current form: pure functions of the block + +static int userk_forward(const kins_params *p, kins_scratch *s, + const double *joint, EmcPose *world, + const KINEMATICS_FORWARD_FLAGS *fflags, + KINEMATICS_INVERSE_FLAGS *iflags) +{ + // replace with the machine's own forward; the block carries the + // geometry (p->geometry[]), the joint map and the tool + return kinsIdentityForward(p, s, joint, world, fflags, iflags); +} + +static int userk_inverse(const kins_params *p, kins_scratch *s, + const EmcPose *world, double *joint, + const KINEMATICS_INVERSE_FLAGS *iflags, + KINEMATICS_FORWARD_FLAGS *fflags) +{ + return kinsIdentityInverse(p, s, world, joint, iflags, fflags); +} + +const kins_ops USERK_OPS = { + .forward = userk_forward, + .inverse = userk_inverse, + // .work, .tool, .native and .jacobian are optional, see kinematics.h +}; + +//********************************************************************** +// the older form // static local variables and functions go here static int userk_inited = 0; diff --git a/src/emc/kinematics_userspace/kinematics_user.c b/src/emc/kinematics_userspace/kinematics_user.c index 69abd527dac..b1ccec60eca 100644 --- a/src/emc/kinematics_userspace/kinematics_user.c +++ b/src/emc/kinematics_userspace/kinematics_user.c @@ -2,14 +2,17 @@ * Description: kinematics_user.c * Non-RT loader for kinematics modules * - * Loads a kinematics .so with dlopen and calls the nonrt_attach() it - * exports, so this process evaluates the kinematics the machine is - * running, at whatever poses it likes. See nonrt_kins.h. + * Loads a kinematics .so with dlopen, asks it to describe itself through + * kinsDescribe(), and evaluates its kinematics through the parameter + * block (see kinematics.h). The block is filled from HAL: one input pin + * of the caller's component per table entry, connected to the signal the + * RT instance's pin reads, so the values are the live ones; and the tool + * from motion's own tooloffset pins where motion is loaded, so that the + * tool the module sees is the one motion has, whether or not the config + * netted it to the module's pin. * - * Identity kinematics needs no module code: the module says so through - * nonrt_ops_t and this file maps joints to axes directly. A module - * exporting no nonrt_attach() is not an error either; the context comes - * back flagged rt_only. + * A module exporting no kinsDescribe() is not an error; the context comes + * back flagged rt_only and answers nothing. * * Author: LinuxCNC * License: GPL Version 2 @@ -19,31 +22,30 @@ ********************************************************************/ #include "kinematics_user.h" -#include #include #include #include #include -#include +#include #include "config.h" /* EMC2_HOME */ -typedef int (*nonrt_attach_fn)(const char *coordinates, nonrt_ops_t *ops, - nonrt_resolve_fn resolve, void *arg); +typedef int (*kins_describe_fn)(const char *coordinates, const char *sparm, + kins_module_info *info); -/* One per value a kinematics module reads is a generous bound. */ -#define MAX_MADE_SIGNALS 16 -#define MAX_BOUND_PINS 16 +#define MAX_BOUND_PINS (KINS_MAX_PARAMS + AXIS_COUNT) +#define MAX_MADE_SIGNALS MAX_BOUND_PINS struct KinematicsUserContext { int initialized; - int rt_only; /* 1 if the module exports no nonrt_attach() */ - int is_identity; /* 1 for identity kinematics: no module code needed */ + int rt_only; /* 1 if the module exports no kinsDescribe() */ KINEMATICS_TYPE kins_type; void *rt_handle; /* dlopen handle */ - nonrt_ops_t ops; + kins_module_info info; + kins_params params; + kins_scratch scratch; + int ktype; /* kinematics type being evaluated */ int num_joints; - int joint_to_axis[KINEMATICS_USER_MAX_JOINTS]; /* identity path only */ char module_name[64]; int comp_id; /* the caller's component, owns the pins made here */ const char *prefix; /* its name, which those pin names start with */ @@ -51,6 +53,10 @@ struct KinematicsUserContext { int num_made_signals; hal_refs_u *cell; /* HAL storage those pins are made against */ int num_cells; + int cell_of_param[KINS_MAX_PARAMS]; /* -1 if not bound */ + int cell_of_tool[AXIS_COUNT]; /* motion.tooloffset.*, -1 if absent */ + int tool_param; /* the table's tool entry, -1 if none */ + int warned_tool; }; /* ======================================================================== @@ -58,16 +64,16 @@ struct KinematicsUserContext { * ======================================================================== */ /* - * Give a kinematics module a reference to a value it asked for. + * Give the block a reference to a value it needs. * * The reference is to a pin of ours rather than into the RT instance's, - * so that its lifetime is ours: see nonrt_kins.h. Ours is connected to - * the signal the RT pin reads, or, when the RT pin has no signal, to one - * made here and removed again in kinematicsUserFree(). + * so that its lifetime is ours. Ours is connected to the signal the RT + * pin reads, or, when the RT pin has no signal, to one made here and + * removed again in kinematicsUserFree(). * * The reference has to 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 and the module gets what a cell holds once + * against hal_malloc() cells and the block reads what a cell holds once * the connection is in place. */ static int make_signal(KinematicsUserContext *ctx, const char *pin_name, @@ -100,23 +106,30 @@ static int new_pin(int comp_id, hal_type_t type, hal_refs_u *out, case HAL_FLOAT: return hal_pin_new_real(comp_id, HAL_IN, &out->r, 0.0, "%s", name); case HAL_S32: return hal_pin_new_si32(comp_id, HAL_IN, &out->s, 0, "%s", name); case HAL_U32: return hal_pin_new_ui32(comp_id, HAL_IN, &out->u, 0, "%s", name); - case HAL_S64: return hal_pin_new_sint(comp_id, HAL_IN, &out->s, 0, "%s", name); - case HAL_U64: return hal_pin_new_uint(comp_id, HAL_IN, &out->u, 0, "%s", name); default: break; } return -1; } -static int bind_pin(const char *pin_name, hal_type_t type, - hal_refs_u *out, void *arg) +/* Does a pin of this name exist? Silent: absence is an answer, not an error. */ +static int pin_exists(const char *pin_name) +{ + hal_query_t q; + memset(&q, 0, sizeof(q)); + q.name = pin_name; + q.qtype = HAL_QTYPE_PIN; + return hal_getref_p(&q) == 0; +} + +/* Bind pin_name; returns the cell index, or -1. */ +static int bind_pin(KinematicsUserContext *ctx, const char *pin_name, + hal_type_t type) { - KinematicsUserContext *ctx = (KinematicsUserContext *)arg; char signal[HAL_NAME_LEN + 1]; char mine[HAL_NAME_LEN + 1]; hal_refs_u *cell; hal_query_t q; - - if (!ctx || !pin_name || !out) return -1; + int idx; memset(&q, 0, sizeof(q)); q.name = pin_name; @@ -149,7 +162,8 @@ static int bind_pin(const char *pin_name, hal_type_t type, fprintf(stderr, "kinematicsUserInit: too many pins to bind\n"); return -1; } - cell = &ctx->cell[ctx->num_cells++]; + idx = ctx->num_cells; + cell = &ctx->cell[idx]; if (new_pin(ctx->comp_id, type, cell, mine) != 0) { fprintf(stderr, "kinematicsUserInit: cannot create pin '%s'\n", mine); @@ -160,30 +174,103 @@ static int bind_pin(const char *pin_name, hal_type_t type, mine, signal); return -1; } + ctx->num_cells++; + return idx; +} - *out = *cell; - return 0; +static hal_type_t hal_type_of(kins_param_type t) +{ + switch (t) { + case KINS_PARAM_BIT: return HAL_BIT; + case KINS_PARAM_S32: return HAL_S32; + case KINS_PARAM_U32: return HAL_U32; + default: return HAL_FLOAT; + } } -/* ======================================================================== - * Identity joint mapping - * ======================================================================== */ +static double cell_value(const hal_refs_u *cell, kins_param_type t) +{ + switch (t) { + case KINS_PARAM_BIT: return hal_get_bool(cell->b) ? 1.0 : 0.0; + case KINS_PARAM_S32: return hal_get_si32(cell->s); + case KINS_PARAM_U32: return hal_get_ui32(cell->u); + default: return hal_get_real(cell->r); + } +} -static void fill_identity_joint_map(KinematicsUserContext *ctx, const char *coords) +/* Bind every input of the table, and motion's tool where motion is there. */ +static int bind_all(KinematicsUserContext *ctx) { - int i, j = 0; - for (i = 0; i < KINEMATICS_USER_MAX_JOINTS; i++) ctx->joint_to_axis[i] = -1; - if (!coords) return; - for (; *coords && j < ctx->num_joints; coords++) { - int axis; - switch (tolower((unsigned char)*coords)) { - case 'x': axis = 0; break; case 'y': axis = 1; break; - case 'z': axis = 2; break; case 'a': axis = 3; break; - case 'b': axis = 4; break; case 'c': axis = 5; break; - case 'u': axis = 6; break; case 'v': axis = 7; break; - case 'w': axis = 8; break; default: continue; - } - ctx->joint_to_axis[j++] = axis; + static const char letter[AXIS_COUNT] = { 'x','y','z','a','b','c','u','v','w' }; + char name[HAL_NAME_LEN + 1]; + int i; + + for (i = 0; i < KINS_MAX_PARAMS; i++) ctx->cell_of_param[i] = -1; + for (i = 0; i < AXIS_COUNT; i++) ctx->cell_of_tool[i] = -1; + ctx->tool_param = -1; + + for (i = 0; i < ctx->info.nparams; i++) { + const kins_param_desc *d = &ctx->info.params[i]; + if (d->dir == KINS_OUT) continue; + if (d->tool) ctx->tool_param = i; + snprintf(name, sizeof(name), "%s.%s", ctx->info.halprefix, d->name); + ctx->cell_of_param[i] = bind_pin(ctx, name, hal_type_of(d->type)); + if (ctx->cell_of_param[i] < 0) return -1; + } + + /* motion publishes the tool it applies; take it from there when it is + loaded, so the module sees the tool whether or not the config netted + it through. Under halrun with the module alone there is no motion, + and the module's own tool entry is all there is. */ + for (i = 0; i < AXIS_COUNT; i++) { + snprintf(name, sizeof(name), "motion.tooloffset.%c", letter[i]); + if (!pin_exists(name)) continue; + ctx->cell_of_tool[i] = bind_pin(ctx, name, HAL_FLOAT); + if (ctx->cell_of_tool[i] < 0) return -1; + } + return 0; +} + +/* The block sees the pins as they are now. */ +static void refresh(KinematicsUserContext *ctx) +{ + int i; + double tool[AXIS_COUNT]; + int have_motion_tool = 0; + + for (i = 0; i < ctx->info.nparams; i++) { + int c = ctx->cell_of_param[i]; + if (c < 0) continue; + ctx->params.geometry[i] = cell_value(&ctx->cell[c], ctx->info.params[i].type); + } + if (ctx->tool_param >= 0) { + ctx->params.tool.tran.z = ctx->params.geometry[ctx->tool_param]; + } + + for (i = 0; i < AXIS_COUNT; i++) { + int c = ctx->cell_of_tool[i]; + tool[i] = 0.0; + if (c < 0) continue; + tool[i] = hal_get_real(ctx->cell[c].r); + have_motion_tool = 1; + } + if (!have_motion_tool) return; + + /* the module's pin and motion disagree: the config lost the tool + somewhere between them. Say so once; motion's value is the one + being cut with. */ + if (ctx->tool_param >= 0 && !ctx->warned_tool + && fabs(tool[AXIS_Z] - ctx->params.geometry[ctx->tool_param]) > 1e-9) { + fprintf(stderr, + "kinematics_user: %s.%s is %.6g but motion.tooloffset.z is %.6g;" + " using motion's value\n", + ctx->info.halprefix, ctx->info.params[ctx->tool_param].name, + ctx->params.geometry[ctx->tool_param], tool[AXIS_Z]); + ctx->warned_tool = 1; + } + for (i = 0; i < AXIS_COUNT; i++) emcPoseSetAxis(&ctx->params.tool, i, tool[i]); + if (ctx->tool_param >= 0) { + ctx->params.geometry[ctx->tool_param] = tool[AXIS_Z]; } } @@ -193,11 +280,12 @@ static void fill_identity_joint_map(KinematicsUserContext *ctx, const char *coor static int load_module(KinematicsUserContext *ctx, const char *module_name, - const char *coordinates) + const char *coordinates, + const char *sparm) { char module_path[512]; void *handle; - nonrt_attach_fn attach; + kins_describe_fn describe; snprintf(module_path, sizeof(module_path), "%s/rtlib/%s.so", EMC2_HOME, module_name); @@ -210,18 +298,18 @@ static int load_module(KinematicsUserContext *ctx, } ctx->rt_handle = handle; - attach = (nonrt_attach_fn)dlsym(handle, "nonrt_attach"); - if (!attach) { - fprintf(stderr, "kinematicsUserInit: '%s' exports no nonrt_attach\n", - module_name); + describe = (kins_describe_fn)dlsym(handle, "kinsDescribe"); + if (!describe) { + fprintf(stderr, "kinematicsUserInit: '%s' exports no kinsDescribe;" + " it cannot be evaluated outside RT\n", module_name); dlclose(handle); ctx->rt_handle = NULL; ctx->rt_only = 1; return -1; } - if (attach(coordinates, &ctx->ops, bind_pin, ctx) != 0) { - fprintf(stderr, "kinematicsUserInit: nonrt_attach failed for '%s'\n", + if (describe(coordinates, sparm, &ctx->info) != 0) { + fprintf(stderr, "kinematicsUserInit: kinsDescribe failed for '%s'\n", module_name); dlclose(handle); ctx->rt_handle = NULL; @@ -229,21 +317,28 @@ static int load_module(KinematicsUserContext *ctx, return -1; } - if (ctx->ops.is_identity) { - ctx->is_identity = 1; - ctx->kins_type = KINEMATICS_IDENTITY; - return 0; + if (ctx->info.ntypes < 1 || !ctx->info.ops[0]) { + fprintf(stderr, "kinematicsUserInit: '%s' has no type 0 in the" + " parameter block form\n", module_name); + dlclose(handle); + ctx->rt_handle = NULL; + ctx->rt_only = 1; + return -1; } - if (!ctx->ops.forward || !ctx->ops.inverse) { - fprintf(stderr, "kinematicsUserInit: '%s' set no fwd/inv\n", module_name); + if (kinsParamsInit(&ctx->params, &ctx->info, coordinates) != 0) { + fprintf(stderr, "kinematicsUserInit: '%s' refuses coordinates '%s'\n", + module_name, coordinates ? coordinates : "(default)"); dlclose(handle); ctx->rt_handle = NULL; ctx->rt_only = 1; return -1; } + kinsScratchInit(&ctx->scratch); - ctx->kins_type = KINEMATICS_BOTH; + ctx->ktype = 0; + ctx->kins_type = ctx->info.ops[0]->identity ? KINEMATICS_IDENTITY + : KINEMATICS_BOTH; return 0; } @@ -251,11 +346,12 @@ static int load_module(KinematicsUserContext *ctx, * Public API * ======================================================================== */ -KinematicsUserContext* kinematicsUserInit(const char* kins_type, - int num_joints, - const char* coordinates, - int comp_id, - const char* prefix) +KinematicsUserContext* kinematicsUserInitSparm(const char* kins_type, + int num_joints, + const char* coordinates, + const char* sparm, + int comp_id, + const char* prefix) { KinematicsUserContext *ctx; @@ -280,59 +376,124 @@ KinematicsUserContext* kinematicsUserInit(const char* kins_type, } strncpy(ctx->module_name, kins_type, sizeof(ctx->module_name) - 1); - load_module(ctx, kins_type, coordinates); - - if (ctx->is_identity) { - fill_identity_joint_map(ctx, coordinates); + if (load_module(ctx, kins_type, coordinates, sparm) == 0) { + if (bind_all(ctx) != 0) { + fprintf(stderr, "kinematicsUserInit: cannot bind the pins of '%s'\n", + kins_type); + ctx->rt_only = 1; + } } ctx->initialized = 1; return ctx; } +KinematicsUserContext* kinematicsUserInit(const char* kins_type, + int num_joints, + const char* coordinates, + int comp_id, + const char* prefix) +{ + return kinematicsUserInitSparm(kins_type, num_joints, coordinates, NULL, + comp_id, prefix); +} + +int kinematicsUserSetType(KinematicsUserContext* ctx, int ktype) +{ + if (!ctx || !ctx->initialized || ctx->rt_only) return -1; + if (ktype < 0 || ktype >= ctx->info.ntypes || !ctx->info.ops[ktype]) { + return -1; + } + ctx->ktype = ktype; + ctx->params.ktype = ktype; + kinsScratchInit(&ctx->scratch); + ctx->kins_type = ctx->info.ops[ktype]->identity ? KINEMATICS_IDENTITY + : KINEMATICS_BOTH; + return 0; +} + +int kinematicsUserGetNumTypes(KinematicsUserContext* ctx) +{ + if (!ctx || !ctx->initialized || ctx->rt_only) return 0; + return ctx->info.ntypes; +} + int kinematicsUserInverse(KinematicsUserContext* ctx, const EmcPose* world, double* joints) { + KINEMATICS_INVERSE_FLAGS iflags = 0; + KINEMATICS_FORWARD_FLAGS fflags = 0; + double j[EMCMOT_MAX_JOINTS]; + int i; + if (!ctx || !ctx->initialized || !world || !joints) return -1; + if (ctx->rt_only) return -1; - if (ctx->is_identity) { - int i; - for (i = 0; i < ctx->num_joints; i++) { - int ax = ctx->joint_to_axis[i]; - joints[i] = (ax >= 0) ? emcPoseGetAxis(world, ax) : 0.0; - } - return 0; + refresh(ctx); + for (i = 0; i < EMCMOT_MAX_JOINTS; i++) j[i] = 0.0; + if (kinsOpsInverse(ctx->info.ops[ctx->ktype], &ctx->params, &ctx->scratch, + world, j, &iflags, &fflags) != 0) { + return -1; } - - if (ctx->rt_only) return -1; - return ctx->ops.inverse(world, joints, NULL, NULL); + for (i = 0; i < ctx->num_joints; i++) joints[i] = j[i]; + return 0; } int kinematicsUserForward(KinematicsUserContext* ctx, const double* joints, EmcPose* world) { + KINEMATICS_INVERSE_FLAGS iflags = 0; + KINEMATICS_FORWARD_FLAGS fflags = 0; + double j[EMCMOT_MAX_JOINTS]; + int i; + if (!ctx || !ctx->initialized || !joints || !world) return -1; + if (ctx->rt_only) return -1; - if (ctx->is_identity) { - int i; - memset(world, 0, sizeof(*world)); - for (i = 0; i < ctx->num_joints; i++) { - int ax = ctx->joint_to_axis[i]; - if (ax >= 0) emcPoseSetAxis(world, ax, joints[i]); - } - return 0; + refresh(ctx); + for (i = 0; i < EMCMOT_MAX_JOINTS; i++) { + j[i] = (i < ctx->num_joints) ? joints[i] : 0.0; } + memset(world, 0, sizeof(*world)); + return kinsOpsForward(ctx->info.ops[ctx->ktype], &ctx->params, &ctx->scratch, + j, world, &fflags, &iflags); +} +int kinematicsUserJacobian(KinematicsUserContext* ctx, + const EmcPose* world, + double J[KINEMATICS_USER_MAX_JOINTS][AXIS_COUNT]) +{ + KINEMATICS_INVERSE_FLAGS iflags = 0; + KINEMATICS_FORWARD_FLAGS fflags = 0; + double jac[EMCMOT_MAX_JOINTS][EMCMOT_MAX_AXIS]; + double j[EMCMOT_MAX_JOINTS]; + int r, a; + + if (!ctx || !ctx->initialized || !world || !J) return -1; if (ctx->rt_only) return -1; - return ctx->ops.forward(joints, world, NULL, NULL); + + refresh(ctx); + for (r = 0; r < EMCMOT_MAX_JOINTS; r++) j[r] = 0.0; + if (kinsOpsInverse(ctx->info.ops[ctx->ktype], &ctx->params, &ctx->scratch, + world, j, &iflags, &fflags) != 0) { + return -1; + } + if (kinsOpsJacobian(ctx->info.ops[ctx->ktype], &ctx->params, &ctx->scratch, + j, world, jac, &iflags) != 0) { + return -1; + } + for (r = 0; r < KINEMATICS_USER_MAX_JOINTS; r++) { + for (a = 0; a < AXIS_COUNT; a++) J[r][a] = jac[r][a]; + } + return 0; } int kinematicsUserIsIdentity(KinematicsUserContext* ctx) { - if (!ctx || !ctx->initialized) return 0; - return ctx->is_identity; + if (!ctx || !ctx->initialized || ctx->rt_only) return 0; + return ctx->info.ops[ctx->ktype]->identity; } int kinematicsUserGetNumJoints(KinematicsUserContext* ctx) @@ -355,8 +516,16 @@ const char* kinematicsUserGetModuleName(KinematicsUserContext* ctx) int kinematicsUserRefreshParams(KinematicsUserContext* ctx) { - (void)ctx; - return 0; /* nothing to refresh: the bound pins are the live values */ + if (!ctx || !ctx->initialized || ctx->rt_only) return -1; + refresh(ctx); + return 0; +} + +const kins_params* kinematicsUserParams(KinematicsUserContext* ctx) +{ + if (!ctx || !ctx->initialized || ctx->rt_only) return NULL; + refresh(ctx); + return &ctx->params; } int kinematicsUserIsRtOnly(KinematicsUserContext* ctx) diff --git a/src/emc/kinematics_userspace/kinematics_user.h b/src/emc/kinematics_userspace/kinematics_user.h index d01d8a8d277..0a7187537f9 100644 --- a/src/emc/kinematics_userspace/kinematics_user.h +++ b/src/emc/kinematics_userspace/kinematics_user.h @@ -6,10 +6,11 @@ * the RT kinematics interface. Used by the 9D planner to compute joint * positions from world coordinates without requiring RT kernel calls. * - * The kinematics module is loaded into this process and given input pins - * belonging to the caller's HAL component, connected to the same signals - * the running RT instance reads. Its own forward and inverse then work on - * live values, unmodified. + * The kinematics module is loaded into this process and evaluated through + * its parameter block form (see kinematics.h). The block is filled from + * input pins belonging to the caller's HAL component, connected to the + * same signals the running RT instance reads, and from motion's tool + * offset pins where motion is loaded, so the maths runs on live values. * * Author: LinuxCNC * License: GPL Version 2 @@ -62,6 +63,30 @@ KinematicsUserContext* kinematicsUserInit(const char* kins_type, int comp_id, const char* prefix); +/** + * As kinematicsUserInit(), with the module's sparm= parameter as well, for + * a module whose kinematics types depend on it (5axiskins identityfirst). + */ +KinematicsUserContext* kinematicsUserInitSparm(const char* kins_type, + int num_joints, + const char* coordinates, + const char* sparm, + int comp_id, + const char* prefix); + +/** + * Select which kinematics type of a switchable module to evaluate. + * Type 0 is selected after init. + * + * @return 0, or -1 if the module has no such type in the block form + */ +int kinematicsUserSetType(KinematicsUserContext* ctx, int ktype); + +/** + * How many kinematics types the module has (1 for one that does not switch). + */ +int kinematicsUserGetNumTypes(KinematicsUserContext* ctx); + /** * Perform inverse kinematics (world coords -> joint positions) * @@ -86,6 +111,24 @@ int kinematicsUserForward(KinematicsUserContext* ctx, const double* joints, EmcPose* world); +/** + * The Jacobian at a pose, J[joint][axis] = d joint / d axis, from the + * module's closed form where it has one and by differencing its inverse + * where it does not. The inverse is run at the pose first, so the + * derivative is taken on the solution branch the module picks there. + * + * @return 0 on success, -1 on failure + */ +int kinematicsUserJacobian(KinematicsUserContext* ctx, + const EmcPose* world, + double J[KINEMATICS_USER_MAX_JOINTS][AXIS_COUNT]); + +/** + * The parameter block as it stands, refreshed from HAL first. For + * reporting; the block belongs to the context. + */ +const kins_params* kinematicsUserParams(KinematicsUserContext* ctx); + /** * Check if kinematics type is identity (world coords = joint coords) * @@ -119,20 +162,18 @@ KINEMATICS_TYPE kinematicsUserGetType(KinematicsUserContext* ctx); const char* kinematicsUserGetModuleName(KinematicsUserContext* ctx); /** - * Refresh kinematics parameters (no-op) - * - * The bound pins read the live values, so there is nothing to fetch. - * This function is kept for API compatibility but does nothing. + * Copy the bound pins into the block now. Every evaluation does this + * itself; call it only to observe the values. * * @param ctx Kinematics context - * @return 0 always + * @return 0, or -1 for an RT-only context */ int kinematicsUserRefreshParams(KinematicsUserContext* ctx); /** * Check if this context is RT-only * - * An RT-only module exports no nonrt_attach() and so cannot be evaluated + * An RT-only module exports no kinsDescribe() and so cannot be evaluated * outside RT. Planner 2 is unavailable for such modules. * * @param ctx Kinematics context diff --git a/src/emc/motion_planning/Submakefile b/src/emc/motion_planning/Submakefile index 553849e7ba5..3a8ccdf737a 100644 --- a/src/emc/motion_planning/Submakefile +++ b/src/emc/motion_planning/Submakefile @@ -8,9 +8,11 @@ LIBKINSLIMITS_CXXSRCS := $(addprefix emc/motion_planning/, \ joint_limits.cc \ ) +# kins_util.c is the shared kinematics code the modules link; the loader +# needs the same block helpers and ops dispatch on this side of dlopen. LIBKINSLIMITS_CSRCS := $(addprefix emc/kinematics_userspace/, \ kinematics_user.c \ - ) + ) emc/kinematics/kins_util.c USERSRCS += $(LIBKINSLIMITS_CXXSRCS) $(LIBKINSLIMITS_CSRCS) diff --git a/src/emc/motion_planning/jacobian.cc b/src/emc/motion_planning/jacobian.cc index a7d5a7661e7..ba8c69fdd42 100644 --- a/src/emc/motion_planning/jacobian.cc +++ b/src/emc/motion_planning/jacobian.cc @@ -12,7 +12,6 @@ #include "jacobian.hh" #include #include -#include namespace motion_planning { @@ -38,128 +37,12 @@ bool JacobianCalculator::init(KinematicsUserContext* kins_ctx) { return true; } -void JacobianCalculator::computeTrivkins(double J[9][9]) { - // Zero the matrix - std::memset(J, 0, sizeof(double) * 9 * 9); - - // For trivkins, the Jacobian is identity (with axis mapping) - // Since trivkins maps: joint[i] = world_axis[mapped_axis[i]] - // The Jacobian is: J[joint][axis] = 1 if axis == mapped_axis[joint], else 0 - - // For a simple XYZ trivkins: - // J[0][AXIS_X] = 1 (joint 0 = X) - // J[1][AXIS_Y] = 1 (joint 1 = Y) - // J[2][AXIS_Z] = 1 (joint 2 = Z) - // etc. - - // We need to query the kinematics context for the mapping. - // Since the context is opaque, we use inverse kinematics to determine - // the mapping. - - // Test each axis: perturb it and see which joint changes - EmcPose zero_pose; - ZERO_EMC_POSE(zero_pose); - double zero_joints[9]; - kinematicsUserInverse(kins_ctx_, &zero_pose, zero_joints); - - for (int axis = 0; axis < AXIS_COUNT; axis++) { - EmcPose test_pose = zero_pose; - emcPoseSetAxis(&test_pose, axis, 1.0); - - double test_joints[9]; - kinematicsUserInverse(kins_ctx_, &test_pose, test_joints); - - for (int joint = 0; joint < num_joints_; joint++) { - double delta = test_joints[joint] - zero_joints[joint]; - if (std::fabs(delta) > 0.5) { - // This axis maps to this joint - J[joint][axis] = 1.0; - } - } - } -} - -bool JacobianCalculator::computeNumerical(const EmcPose& pose, double J[9][9]) { - // Zero the matrix - std::memset(J, 0, sizeof(double) * 9 * 9); - - // Compute joints at nominal pose - double joints_center[9]; - if (kinematicsUserInverse(kins_ctx_, &pose, joints_center) != 0) { - return false; - } - - // Perturb each axis and compute derivatives - for (int axis = 0; axis < AXIS_COUNT; axis++) { - // Choose perturbation size based on axis type - double delta = (axis < 3 || axis >= 6) ? DELTA_LINEAR : DELTA_ROTARY; - - // Positive perturbation - EmcPose pose_plus = pose; - double val_plus = emcPoseGetAxis(&pose_plus, axis) + delta; - emcPoseSetAxis(&pose_plus, axis, val_plus); - - double joints_plus[9]; - if (kinematicsUserInverse(kins_ctx_, &pose_plus, joints_plus) != 0) { - // Kinematics failed - use one-sided difference - for (int joint = 0; joint < num_joints_; joint++) { - J[joint][axis] = (joints_plus[joint] - joints_center[joint]) / delta; - } - continue; - } - - // Negative perturbation - EmcPose pose_minus = pose; - double val_minus = emcPoseGetAxis(&pose_minus, axis) - delta; - emcPoseSetAxis(&pose_minus, axis, val_minus); - - double joints_minus[9]; - if (kinematicsUserInverse(kins_ctx_, &pose_minus, joints_minus) != 0) { - // Use forward difference - for (int joint = 0; joint < num_joints_; joint++) { - J[joint][axis] = (joints_plus[joint] - joints_center[joint]) / delta; - } - continue; - } - - // Central difference (most accurate) - for (int joint = 0; joint < num_joints_; joint++) { - J[joint][axis] = (joints_plus[joint] - joints_minus[joint]) / (2.0 * delta); - } - } - - // Check for NaN/Inf values and replace with safe defaults - bool had_nan = false; - for (int joint = 0; joint < num_joints_; joint++) { - for (int axis = 0; axis < AXIS_COUNT; axis++) { - if (!std::isfinite(J[joint][axis])) { - // Replace NaN/Inf with 0 (assume no coupling) - J[joint][axis] = 0.0; - had_nan = true; - } - } - } - - // If we had NaN values, the Jacobian may be unreliable - // Return true anyway but the condition number check will catch issues - (void)had_nan; // Could log this in debug mode - - return true; -} - bool JacobianCalculator::compute(const EmcPose& pose, double J[9][9]) { if (!kins_ctx_) { return false; } - - if (is_identity_) { - // For trivkins, use the fast identity computation - computeTrivkins(J); - return true; - } else { - // For non-trivial kinematics, use numerical differentiation - return computeNumerical(pose, J); - } + std::memset(J, 0, sizeof(double) * 9 * 9); + return kinematicsUserJacobian(kins_ctx_, &pose, J) == 0; } double JacobianCalculator::conditionNumber(const double J[9][9]) { diff --git a/src/emc/motion_planning/jacobian.hh b/src/emc/motion_planning/jacobian.hh index 8713e89f180..2db3cf00d3e 100644 --- a/src/emc/motion_planning/jacobian.hh +++ b/src/emc/motion_planning/jacobian.hh @@ -3,7 +3,8 @@ * Jacobian calculation for userspace kinematics trajectory planning * * Computes the Jacobian matrix relating world velocities to joint - * velocities. For trivkins this is the identity matrix. + * velocities, from the module's own closed form through the non-RT + * kinematics loader. * * Author: LinuxCNC * License: GPL Version 2 @@ -25,8 +26,8 @@ namespace motion_planning { * Computes the Jacobian matrix J where: * joint_velocities = J × world_velocities * - * For trivkins, J is the identity matrix (with appropriate axis mapping). - * For non-trivial kinematics, J is computed via numerical differentiation. + * The module answers: a closed form where it has one, its inverse + * differenced where it does not. See kinematicsUserJacobian(). */ class JacobianCalculator { public: @@ -72,26 +73,9 @@ public: bool isIdentity() const { return is_identity_; } private: - /** - * Compute Jacobian for trivkins (identity with axis mapping) - */ - void computeTrivkins(double J[9][9]); - - /** - * Compute Jacobian via numerical differentiation - * Uses central differences: J[j][a] = (f(x+h) - f(x-h)) / (2h) - */ - bool computeNumerical(const EmcPose& pose, double J[9][9]); - KinematicsUserContext* kins_ctx_; bool is_identity_; int num_joints_; - - // Perturbation size for numerical differentiation (mm or degrees) - // Must be large enough for kinematics to produce stable results - // but small enough for accurate derivatives - static constexpr double DELTA_LINEAR = 0.1; // 0.1 mm - static constexpr double DELTA_ROTARY = 0.1; // 0.1 degrees }; } // namespace motion_planning diff --git a/src/hal/components/millturn.comp b/src/hal/components/millturn.comp index 161e8abebba..abbff00a375 100644 --- a/src/hal/components/millturn.comp +++ b/src/hal/components/millturn.comp @@ -123,7 +123,7 @@ static int turnKinematicsJacobian(const double *j, // halcompile has done hal_init() and does hal_ready() after this returns, // which is what switchkinsInit() expects EXTRA_SETUP() { - kparms kp; + kparms kp = {0}; (void)__comp_inst; (void)prefix; (void)extra_arg; kp.kinsname = "millturn"; diff --git a/src/hal/components/xyzab_tdr_kins.comp b/src/hal/components/xyzab_tdr_kins.comp index 0387d73c85a..c5d4db81f66 100644 --- a/src/hal/components/xyzab_tdr_kins.comp +++ b/src/hal/components/xyzab_tdr_kins.comp @@ -222,7 +222,7 @@ static int tdrKinematicsJacobian(const double *j, // halcompile has done hal_init() and does hal_ready() after this returns, // which is what switchkinsInit() expects EXTRA_SETUP() { - kparms kp; + kparms kp = {0}; (void)__comp_inst; (void)prefix; (void)extra_arg; kp.kinsname = "xyzab_tdr_kins"; diff --git a/src/hal/components/xyzacb_trsrn.comp b/src/hal/components/xyzacb_trsrn.comp index 45a8a9af4f9..3d26044943d 100644 --- a/src/hal/components/xyzacb_trsrn.comp +++ b/src/hal/components/xyzacb_trsrn.comp @@ -595,7 +595,7 @@ static int toolKinematicsJacobian(const double *j, // halcompile has done hal_init() and does hal_ready() after this returns, // which is what switchkinsInit() expects EXTRA_SETUP() { - kparms kp; + kparms kp = {0}; (void)__comp_inst; (void)prefix; (void)extra_arg; kp.kinsname = "xyzacb_trsrn"; diff --git a/src/hal/components/xyzbca_trsrn.comp b/src/hal/components/xyzbca_trsrn.comp index 75e6f7ea0f4..075e3cbfeb2 100644 --- a/src/hal/components/xyzbca_trsrn.comp +++ b/src/hal/components/xyzbca_trsrn.comp @@ -598,7 +598,7 @@ static int toolKinematicsJacobian(const double *j, // halcompile has done hal_init() and does hal_ready() after this returns, // which is what switchkinsInit() expects EXTRA_SETUP() { - kparms kp; + kparms kp = {0}; (void)__comp_inst; (void)prefix; (void)extra_arg; kp.kinsname = "xyzbca_trsrn"; From d4119269816361c7ab4f832389f371415fbf0b3d Mon Sep 17 00:00:00 2001 From: Luca Toniolo <10792599+grandixximo@users.noreply.github.com> Date: Fri, 4 Sep 2026 12:48:41 +1000 Subject: [PATCH 29/60] trtfuncs, xyzac-trt-kins, xyzbc-trt-kins, maxkins: move onto the parameter 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. --- src/Makefile | 2 + src/emc/kinematics/kinematics.h | 57 +--- src/emc/kinematics/maxkins.c | 131 ++++----- src/emc/kinematics/trtfuncs.c | 403 +++++++++++----------------- src/emc/kinematics/xyzac-trt-kins.c | 48 ++-- src/emc/kinematics/xyzbc-trt-kins.c | 48 ++-- 6 files changed, 265 insertions(+), 424 deletions(-) diff --git a/src/Makefile b/src/Makefile index 654cc11207b..bd58516acd1 100644 --- a/src/Makefile +++ b/src/Makefile @@ -1136,6 +1136,8 @@ trivkins-objs += emc/kinematics/kins_single.o obj-m += maxkins.o maxkins-objs := emc/kinematics/maxkins.o +maxkins-objs += emc/kinematics/kins_util.o +maxkins-objs += emc/kinematics/kins_single.o obj-m += rotatekins.o rotatekins-objs := emc/kinematics/rotatekins.o diff --git a/src/emc/kinematics/kinematics.h b/src/emc/kinematics/kinematics.h index 7de1f15b246..ec31358075f 100644 --- a/src/emc/kinematics/kinematics.h +++ b/src/emc/kinematics/kinematics.h @@ -529,6 +529,7 @@ typedef struct kins_scratch { int have_joint_seed; int iterations; int failed; + double aux[8]; /* whatever else a module carries between calls */ double out[KINS_MAX_PARAMS]; /* the table's KINS_OUT entries */ } kins_scratch; @@ -694,57 +695,11 @@ extern int userkKinematicsInverse(const struct EmcPose * world, const KINEMATICS_INVERSE_FLAGS * iflags, KINEMATICS_FORWARD_FLAGS * fflags); //********************************************************************* -// xyzac,xyzbc; -extern int trtKinematicsSetup(const int comp_id, - const char* coordinates, - kparms* ksetup_parms); - -extern int xyzacKinematicsForward(const double *joints, - EmcPose * pos, - const KINEMATICS_FORWARD_FLAGS * fflags, - KINEMATICS_INVERSE_FLAGS * iflags); - -extern int xyzacKinematicsInverse(const EmcPose * pos, - double *joints, - const KINEMATICS_INVERSE_FLAGS * iflags, - KINEMATICS_FORWARD_FLAGS * fflags); - -extern int xyzacKinematicsToolFrame(const double *joints, - PmRotationMatrix *rot, - const KINEMATICS_FORWARD_FLAGS *fflags); - -extern int xyzacKinematicsWorkFrame(const double *joints, - PmRotationMatrix *rot, - const KINEMATICS_FORWARD_FLAGS *fflags); - -extern int xyzacKinematicsJacobian(const double *joints, - const EmcPose *pos, - double jac[EMCMOT_MAX_JOINTS][EMCMOT_MAX_AXIS], - const KINEMATICS_INVERSE_FLAGS *iflags); - - -extern int xyzbcKinematicsForward(const double *joints, - EmcPose * pos, - const KINEMATICS_FORWARD_FLAGS * fflags, - KINEMATICS_INVERSE_FLAGS * iflags); - -extern int xyzbcKinematicsInverse(const EmcPose * pos, - double *joints, - const KINEMATICS_INVERSE_FLAGS * iflags, - KINEMATICS_FORWARD_FLAGS * fflags); - -extern int xyzbcKinematicsToolFrame(const double *joints, - PmRotationMatrix *rot, - const KINEMATICS_FORWARD_FLAGS *fflags); - -extern int xyzbcKinematicsWorkFrame(const double *joints, - PmRotationMatrix *rot, - const KINEMATICS_FORWARD_FLAGS *fflags); - -extern int xyzbcKinematicsJacobian(const double *joints, - const EmcPose *pos, - double jac[EMCMOT_MAX_JOINTS][EMCMOT_MAX_AXIS], - const KINEMATICS_INVERSE_FLAGS *iflags); +// xyzac,xyzbc (trtfuncs.c): one geometry table, the maths of each machine +extern const kins_param_desc TRT_PARAMS[]; +extern const int TRT_NPARAMS; +extern const kins_ops XYZAC_OPS; +extern const kins_ops XYZBC_OPS; //********************************************************************* #ifdef __cplusplus diff --git a/src/emc/kinematics/maxkins.c b/src/emc/kinematics/maxkins.c index 2da69858e44..38862871d74 100644 --- a/src/emc/kinematics/maxkins.c +++ b/src/emc/kinematics/maxkins.c @@ -6,13 +6,13 @@ * * Author: Chris Radek * License: GPL Version 2 -* +* * Copyright (c) 2007 Chris Radek ********************************************************************/ /******************************************************************** -* Note: The direction of the B axis is the opposite of the -* conventional axis direction. See +* Note: The direction of the B axis is the opposite of the +* conventional axis direction. See * https://linuxcnc.org/docs/html/gcode/machining-center.html ********************************************************************/ @@ -21,6 +21,7 @@ #include #include #include /* these decls */ +#include #define d2r(d) ((d)*PM_PI/180.0) #define r2d(r) ((r)*180.0/PM_PI) @@ -29,27 +30,33 @@ #define hypot(a,b) (sqrt((a)*(a)+(b)*(b))) #endif -static struct haldata { - hal_real_t pivot_length; - hal_real_t tool_length; - hal_bool_t conventional_directions; //default is false -} *haldata; - -int kinematicsForward(const double *joints, - EmcPose * pos, - const KINEMATICS_FORWARD_FLAGS * fflags, - KINEMATICS_INVERSE_FLAGS * iflags) +// the geometry, one pin each; the maths reads it from the block +static const kins_param_desc max_params[] = { + { "pivot-length", KINS_PARAM_FLOAT, KINS_IO, 0, 0.666 }, + { "conventional-directions", KINS_PARAM_BIT, KINS_IN, 0, 0 }, // default is unconventional + { "tool-length", KINS_PARAM_FLOAT, KINS_IN, 0, 0 }, +}; +enum { P_PIVOT_LENGTH, P_CON, P_TOOL_LENGTH }; + +#define CON(p) ((p)->geometry[P_CON] != 0 ? 1.0 : -1.0) + +static int max_forward(const kins_params *p, kins_scratch *s, + const double *joints, + EmcPose * pos, + const KINEMATICS_FORWARD_FLAGS * fflags, + KINEMATICS_INVERSE_FLAGS * iflags) { + (void)s; (void)fflags; (void)iflags; - rtapi_real con = hal_get_bool(haldata->conventional_directions) ? 1.0 : -1.0; - rtapi_real pivot_length = hal_get_real(haldata->pivot_length); - rtapi_real tool_length = hal_get_real(haldata->tool_length); + const double con = CON(p); + const double pivot_length = p->geometry[P_PIVOT_LENGTH]; + const double tool_length = p->geometry[P_TOOL_LENGTH]; // B correction - const double zb = (pivot_length + joints[8] + tool_length) * cos(d2r(joints[4])); - const double xb = (pivot_length + joints[8] + tool_length) * sin(d2r(joints[4])); + const double zb = (pivot_length + tool_length + joints[8]) * cos(d2r(joints[4])); + const double xb = (pivot_length + tool_length + joints[8]) * sin(d2r(joints[4])); // U correction const double zv = joints[6] * sin(d2r(joints[4])); @@ -81,22 +88,24 @@ int kinematicsForward(const double *joints, return 0; } -int kinematicsInverse(const EmcPose * pos, - double *joints, - const KINEMATICS_INVERSE_FLAGS * iflags, - KINEMATICS_FORWARD_FLAGS * fflags) +static int max_inverse(const kins_params *p, kins_scratch *s, + const EmcPose * pos, + double *joints, + const KINEMATICS_INVERSE_FLAGS * iflags, + KINEMATICS_FORWARD_FLAGS * fflags) { + (void)s; (void)iflags; (void)fflags; - rtapi_real con = hal_get_bool(haldata->conventional_directions) ? 1.0 : -1.0; - rtapi_real pivot_length = hal_get_real(haldata->pivot_length); - rtapi_real tool_length = hal_get_real(haldata->tool_length); + const double con = CON(p); + const double pivot_length = p->geometry[P_PIVOT_LENGTH]; + const double tool_length = p->geometry[P_TOOL_LENGTH]; // B correction - const double zb = (pivot_length + pos->w + tool_length) * cos(d2r(pos->b)); - const double xb = (pivot_length + pos->w + tool_length) * sin(d2r(pos->b)); - + const double zb = (pivot_length + tool_length + pos->w) * cos(d2r(pos->b)); + const double xb = (pivot_length + tool_length + pos->w) * sin(d2r(pos->b)); + // C correction const double xyr = hypot(pos->tran.x, pos->tran.y); const double xytheta = atan2(pos->tran.y, pos->tran.x) - d2r(pos->c); @@ -121,18 +130,18 @@ int kinematicsInverse(const EmcPose * pos, return 0; } -int kinematicsJacobian(const double *joints, - const EmcPose * pos, - double jac[EMCMOT_MAX_JOINTS][EMCMOT_MAX_AXIS], - const KINEMATICS_INVERSE_FLAGS * iflags) +static int max_jacobian(const kins_params *p, const double *joints, + const EmcPose * pos, + double jac[EMCMOT_MAX_JOINTS][EMCMOT_MAX_AXIS], + const KINEMATICS_INVERSE_FLAGS * iflags) { - rtapi_real con = hal_get_bool(haldata->conventional_directions) ? 1.0 : -1.0; - rtapi_real pivot_length = hal_get_real(haldata->pivot_length); + const double con = CON(p); + const double pivot_length = p->geometry[P_PIVOT_LENGTH]; const double k = M_PI/180; const double sb = sin(d2r(pos->b)), cb = cos(d2r(pos->b)); const double sc = sin(d2r(pos->c)), cc = cos(d2r(pos->c)); const double x = pos->tran.x, y = pos->tran.y; - const double R = pivot_length + pos->w; + const double R = pivot_length + p->geometry[P_TOOL_LENGTH] + pos->w; int j, a; (void)joints; @@ -141,9 +150,9 @@ int kinematicsJacobian(const double *joints, for (a = 0; a < EMCMOT_MAX_AXIS; a++) { jac[j][a] = 0; } } - // kinematicsInverse() with the polar form expanded: rotating (x, y) - // by -c is x*cos(c) + y*sin(c) and y*cos(c) - x*sin(c), and the - // B and U corrections are what they are written as + // max_inverse() with the polar form expanded: rotating (x, y) by -c + // is x*cos(c) + y*sin(c) and y*cos(c) - x*sin(c), and the B and U + // corrections are what they are written as jac[0][0] = cc; jac[0][1] = sc; jac[0][4] = (con * R * cb - pos->u * sb) * k; @@ -165,40 +174,40 @@ int kinematicsJacobian(const double *joints, return 0; } -KINEMATICS_TYPE kinematicsType() -{ - return KINEMATICS_BOTH; -} +static const kins_ops max_ops = { + .forward = max_forward, + .inverse = max_inverse, + .jacobian = max_jacobian, +}; + +// joints 0..8 are X..W in order, always; the entry points come from +// kins_single.c +const kins_module_info kins_module = { + .name = "maxkins", + .halprefix = "maxkins", + .params = max_params, + .nparams = sizeof(max_params)/sizeof(max_params[0]), + .required_coordinates = "XYZABCUVW", + .max_joints = 9, + .allow_duplicates = 0, + .ntypes = 1, + .ops = { &max_ops }, +}; -KINS_NOT_SWITCHABLE -EXPORT_SYMBOL(kinematicsType); -EXPORT_SYMBOL(kinematicsInverse); -EXPORT_SYMBOL(kinematicsForward); -EXPORT_SYMBOL(kinematicsJacobian); MODULE_LICENSE("GPL"); static int comp_id; int rtapi_app_main(void) { - int result; comp_id = hal_init("maxkins"); if(comp_id < 0) return comp_id; - haldata = hal_malloc(sizeof(*haldata)); - if(!haldata) { result = -ENOMEM; goto error; } - - result = hal_pin_new_real(comp_id, HAL_IO, &(haldata->pivot_length), 0.666, "maxkins.pivot-length"); - result += hal_pin_new_real(comp_id, HAL_IN, &(haldata->tool_length), 0.0, "maxkins.tool-length"); - // default is unconventional - result += hal_pin_new_bool(comp_id, HAL_IN, &(haldata->conventional_directions), 0, "maxkins.conventional-directions"); - - if(result < 0) goto error; + if (kinsSingleInit(comp_id, "XYZABCUVW", KINEMATICS_BOTH)) { + hal_exit(comp_id); + return -1; + } hal_ready(comp_id); return 0; - -error: - hal_exit(comp_id); - return result; } void rtapi_app_exit(void) { hal_exit(comp_id); } diff --git a/src/emc/kinematics/trtfuncs.c b/src/emc/kinematics/trtfuncs.c index 6f77bfd92ad..a2c5f6e057f 100644 --- a/src/emc/kinematics/trtfuncs.c +++ b/src/emc/kinematics/trtfuncs.c @@ -25,150 +25,67 @@ * This mill has a tilting table (B axis) and horizontal rotary * mounted to the table (C axis). * -* Note: The directions of the rotational axes are the opposite of the -* conventional axis directions. See +* Note: The directions of the rotational axes are the opposite of the +* conventional axis directions. See * https://linuxcnc.org/docs/html/gcode/machining-center.html - +* +* Written as pure functions of the parameter block (see kinematics.h): +* the geometry is the table below, the joint map comes from the block, +* and the tool length is p->tool.tran.z. ********************************************************************/ #include -#include -#include -#include #include #include -static int trtfuncs_max_joints; - -// joint number assignments (-1 ==> not assigned) -static int JX = -1; -static int JY = -1; -static int JZ = -1; - -static int JA = -1; -static int JB = -1; -static int JC = -1; - -static int JU = -1; -static int JV = -1; -static int JW = -1; - -struct haldata { - hal_real_t x_rot_point; - hal_real_t y_rot_point; - hal_real_t z_rot_point; - hal_real_t x_offset; - hal_real_t y_offset; - hal_real_t z_offset; - hal_real_t tool_offset; - hal_bool_t conventional_directions; // default: false -} *haldata; - - -int trtKinematicsSetup(const int comp_id, - const char* coordinates, - kparms* kp) -{ - int i,jno,res=0; - int axis_idx_for_jno[EMCMOT_MAX_JOINTS]; - int rqdjoints = strlen(kp->required_coordinates); - - if (rqdjoints > kp->max_joints) { - rtapi_print_msg(RTAPI_MSG_ERR, - "ERROR %s: supports %d joints, <%s> requires %d\n", - kp->kinsname, - kp->max_joints, - coordinates, - rqdjoints); - goto error; - } - trtfuncs_max_joints = kp->max_joints; - - if (map_coordinates_to_jnumbers(coordinates, - kp->max_joints, - kp->allow_duplicates, - axis_idx_for_jno)) { - goto error; - } - // require all chars in reqd_coords (order doesn't matter) - for (i=0; i < rqdjoints; i++) { - char reqd_char; - reqd_char = *(kp->required_coordinates + i); - if ( !strchr(coordinates,toupper(reqd_char)) - && !strchr(coordinates,tolower(reqd_char)) ) { - rtapi_print_msg(RTAPI_MSG_ERR, - "ERROR %s:\nrequired coordinates:%s\n" - "specified coordinates:%s\n", - kp->kinsname, kp->required_coordinates, coordinates); - goto error; - } - } - - // assign principal joint numbers (first found in coordinates map) - // duplicates are handled by position_to_mapped_joints() - for (jno=0; jno < EMCMOT_MAX_JOINTS; jno++) { - if (axis_idx_for_jno[jno] == 0 && JX==-1) {JX = jno;} - if (axis_idx_for_jno[jno] == 1 && JY==-1) {JY = jno;} - if (axis_idx_for_jno[jno] == 2 && JZ==-1) {JZ = jno;} - if (axis_idx_for_jno[jno] == 3 && JA==-1) {JA = jno;} - if (axis_idx_for_jno[jno] == 4 && JB==-1) {JB = jno;} - if (axis_idx_for_jno[jno] == 5 && JC==-1) {JC = jno;} - if (axis_idx_for_jno[jno] == 6 && JU==-1) {JU = jno;} - if (axis_idx_for_jno[jno] == 7 && JV==-1) {JV = jno;} - if (axis_idx_for_jno[jno] == 8 && JW==-1) {JW = jno;} - } - - rtapi_print("%s coordinates=%s assigns:\n", kp->kinsname,coordinates); - for (jno=0; jno Axis %c\n", - jno,"XYZABCUVW"[axis_idx_for_jno[jno]]); - } - - haldata = hal_malloc(sizeof(struct haldata)); - if (!haldata) {goto error;} - - res += hal_pin_new_real(comp_id, HAL_IN, &(haldata->x_rot_point), - 0.0, "%s.x-rot-point",kp->halprefix); - res += hal_pin_new_real(comp_id, HAL_IN, &(haldata->y_rot_point), - 0.0, "%s.y-rot-point",kp->halprefix); - res += hal_pin_new_real(comp_id, HAL_IN, &(haldata->z_rot_point), - 0.0, "%s.z-rot-point",kp->halprefix); - res += hal_pin_new_real(comp_id, HAL_IN, &(haldata->x_offset), - 0.0, "%s.x-offset",kp->halprefix); - res += hal_pin_new_real(comp_id, HAL_IN, &(haldata->y_offset), - 0.0, "%s.y-offset",kp->halprefix); - res += hal_pin_new_real(comp_id, HAL_IN, &(haldata->z_offset), - 0.0, "%s.z-offset",kp->halprefix); - res += hal_pin_new_real(comp_id, HAL_IN, &(haldata->tool_offset), - 0.0, "%s.tool-offset",kp->halprefix); - res += hal_pin_new_bool(comp_id, HAL_IN, &(haldata->conventional_directions), - 0, "%s.conventional-directions", kp->halprefix); - if (res) {goto error;} - return 0; - -error: - rtapi_print_msg(RTAPI_MSG_ERR,"trtKinematicsSetup() FAIL\n"); - return -1; -} // trtKinematicsSetup() - -int xyzacKinematicsForward(const double *joints, - EmcPose * pos, - const KINEMATICS_FORWARD_FLAGS * fflags, - KINEMATICS_INVERSE_FLAGS * iflags) +// the geometry both machines share, one pin each +const kins_param_desc TRT_PARAMS[] = { + { "x-rot-point", KINS_PARAM_FLOAT, KINS_IN, 0, 0.0 }, + { "y-rot-point", KINS_PARAM_FLOAT, KINS_IN, 0, 0.0 }, + { "z-rot-point", KINS_PARAM_FLOAT, KINS_IN, 0, 0.0 }, + { "x-offset", KINS_PARAM_FLOAT, KINS_IN, 0, 0.0 }, + { "y-offset", KINS_PARAM_FLOAT, KINS_IN, 0, 0.0 }, + { "z-offset", KINS_PARAM_FLOAT, KINS_IN, 0, 0.0 }, + { "tool-offset", KINS_PARAM_FLOAT, KINS_IN, 1, 0.0 }, + { "conventional-directions", KINS_PARAM_BIT, KINS_IN, 0, 0.0 }, // default: false +}; +const int TRT_NPARAMS = sizeof(TRT_PARAMS)/sizeof(TRT_PARAMS[0]); + +enum { TRT_XR, TRT_YR, TRT_ZR, TRT_XO, TRT_YO, TRT_ZO, TRT_TOOL, TRT_CON }; + +// joint number assignments from the block (-1 ==> not assigned) +#define JX (p->joint_of_axis[0]) +#define JY (p->joint_of_axis[1]) +#define JZ (p->joint_of_axis[2]) +#define JA (p->joint_of_axis[3]) +#define JB (p->joint_of_axis[4]) +#define JC (p->joint_of_axis[5]) +#define JU (p->joint_of_axis[6]) +#define JV (p->joint_of_axis[7]) +#define JW (p->joint_of_axis[8]) + +// the direction sign the conventional-directions pin selects +#define CON(p) ((p)->geometry[TRT_CON] != 0 ? 1.0 : -1.0) + +static int xyzac_forward(const kins_params *p, kins_scratch *s, + const double *joints, + EmcPose * pos, + const KINEMATICS_FORWARD_FLAGS * fflags, + KINEMATICS_INVERSE_FLAGS * iflags) { + (void)s; (void)fflags; (void)iflags; - const double x_rot_point = hal_get_real(haldata->x_rot_point); - const double y_rot_point = hal_get_real(haldata->y_rot_point); - const double z_rot_point = hal_get_real(haldata->z_rot_point); - const double dt = hal_get_real(haldata->tool_offset); - const double dy = hal_get_real(haldata->y_offset); - const double dz = hal_get_real(haldata->z_offset) + dt; + const double x_rot_point = p->geometry[TRT_XR]; + const double y_rot_point = p->geometry[TRT_YR]; + const double z_rot_point = p->geometry[TRT_ZR]; + const double dt = p->tool.tran.z; + const double dy = p->geometry[TRT_YO]; + const double dz = p->geometry[TRT_ZO] + dt; const double a_rad = joints[JA]*TO_RAD; const double c_rad = joints[JC]*TO_RAD; - rtapi_real con = hal_get_bool(haldata->conventional_directions) ? 1.0 : -1.0; + const double con = CON(p); pos->tran.x = + cos(c_rad) * (joints[JX] - x_rot_point) - con * sin(c_rad) * cos(a_rad) * (joints[JY] - dy - y_rot_point) @@ -198,25 +115,27 @@ int xyzacKinematicsForward(const double *joints, pos->w = (JW != -1)? joints[JW] : 0; return 0; -} // xyzacKinematicsForward() +} // xyzac_forward() -int xyzacKinematicsInverse(const EmcPose * pos, - double *joints, - const KINEMATICS_INVERSE_FLAGS * iflags, - KINEMATICS_FORWARD_FLAGS * fflags) +static int xyzac_inverse(const kins_params *p, kins_scratch *s, + const EmcPose * pos, + double *joints, + const KINEMATICS_INVERSE_FLAGS * iflags, + KINEMATICS_FORWARD_FLAGS * fflags) { + (void)s; (void)iflags; (void)fflags; - const double x_rot_point = hal_get_real(haldata->x_rot_point); - const double y_rot_point = hal_get_real(haldata->y_rot_point); - const double z_rot_point = hal_get_real(haldata->z_rot_point); - const double dy = hal_get_real(haldata->y_offset); - const double dt = hal_get_real(haldata->tool_offset); - const double dz = hal_get_real(haldata->z_offset) + dt; + const double x_rot_point = p->geometry[TRT_XR]; + const double y_rot_point = p->geometry[TRT_YR]; + const double z_rot_point = p->geometry[TRT_ZR]; + const double dy = p->geometry[TRT_YO]; + const double dt = p->tool.tran.z; + const double dz = p->geometry[TRT_ZO] + dt; const double a_rad = pos->a*TO_RAD; const double c_rad = pos->c*TO_RAD; - rtapi_real con = hal_get_bool(haldata->conventional_directions) ? 1.0 : -1.0; + const double con = CON(p); EmcPose P; // computed position @@ -253,16 +172,12 @@ int xyzacKinematicsInverse(const EmcPose * pos, // update joints with support for // multiple-joints per-coordinate letter: // based on computed position - position_to_mapped_joints(trtfuncs_max_joints, - &P, - joints); - - return 0; -} // xyzacKinematicsInverse() + return kinsPoseToMappedJoints(p, &P, joints); +} // xyzac_inverse() -int xyzacKinematicsWorkFrame(const double *joints, - PmRotationMatrix *rot, - const KINEMATICS_FORWARD_FLAGS *fflags) +static int xyzac_work_frame(const kins_params *p, const double *joints, + PmRotationMatrix *rot, + const KINEMATICS_FORWARD_FLAGS *fflags) { (void)fflags; // the forward transform's coefficients for a displacement of the X, Y and @@ -271,7 +186,7 @@ int xyzacKinematicsWorkFrame(const double *joints, const double a_rad = joints[JA]*TO_RAD; const double c_rad = joints[JC]*TO_RAD; - rtapi_real con = hal_get_bool(haldata->conventional_directions) ? 1.0 : -1.0; + const double con = CON(p); rot->x.x = cos(c_rad); rot->y.x = con * sin(c_rad); @@ -286,32 +201,21 @@ int xyzacKinematicsWorkFrame(const double *joints, rot->z.z = cos(a_rad); return 0; -} // xyzacKinematicsWorkFrame() - -int xyzacKinematicsToolFrame(const double *joints, - PmRotationMatrix *rot, - const KINEMATICS_FORWARD_FLAGS *fflags) -{ - (void)joints; - (void)fflags; - // both rotaries carry the work, so the tool never turns in the machine - *rot = TOOL_FRAME_SPINDLE; - return 0; -} // xyzacKinematicsToolFrame() +} // xyzac_work_frame() -int xyzacKinematicsJacobian(const double *joints, - const EmcPose *pos, - double jac[EMCMOT_MAX_JOINTS][EMCMOT_MAX_AXIS], - const KINEMATICS_INVERSE_FLAGS *iflags) +static int xyzac_jacobian(const kins_params *p, const double *joints, + const EmcPose *pos, + double jac[EMCMOT_MAX_JOINTS][EMCMOT_MAX_AXIS], + const KINEMATICS_INVERSE_FLAGS *iflags) { (void)joints; (void)iflags; - const double x_rot_point = hal_get_real(haldata->x_rot_point); - const double y_rot_point = hal_get_real(haldata->y_rot_point); - const double z_rot_point = hal_get_real(haldata->z_rot_point); - const double dy = hal_get_real(haldata->y_offset); - const double dt = hal_get_real(haldata->tool_offset); - const double dz = hal_get_real(haldata->z_offset) + dt; + const double x_rot_point = p->geometry[TRT_XR]; + const double y_rot_point = p->geometry[TRT_YR]; + const double z_rot_point = p->geometry[TRT_ZR]; + const double dy = p->geometry[TRT_YO]; + const double dt = p->tool.tran.z; + const double dz = p->geometry[TRT_ZO] + dt; const double sa = sin(pos->a*TO_RAD), ca = cos(pos->a*TO_RAD); const double sc = sin(pos->c*TO_RAD), cc = cos(pos->c*TO_RAD); const double X = pos->tran.x - x_rot_point; @@ -320,14 +224,14 @@ int xyzacKinematicsJacobian(const double *joints, double dP[EMCMOT_MAX_AXIS][EMCMOT_MAX_AXIS]; int a, b; - rtapi_real con = hal_get_bool(haldata->conventional_directions) ? 1.0 : -1.0; + const double con = CON(p); for (a = 0; a < EMCMOT_MAX_AXIS; a++) { for (b = 0; b < EMCMOT_MAX_AXIS; b++) { dP[a][b] = 0; } } - // the computed position P of xyzacKinematicsInverse(), differentiated: - // its coefficients for x, y and z, and the same expressions with the + // the computed position P of xyzac_inverse(), differentiated: its + // coefficients for x, y and z, and the same expressions with the // rotation taken a quarter turn on for a and for c dP[0][0] = cc; dP[0][1] = con * sc; @@ -347,29 +251,41 @@ int xyzacKinematicsJacobian(const double *joints, for (a = 3; a < EMCMOT_MAX_AXIS; a++) { dP[a][a] = 1; } - return kinsJacobianFromMappedAxes(trtfuncs_max_joints, - (const double (*)[EMCMOT_MAX_AXIS])dP, - jac); -} // xyzacKinematicsJacobian() - -int xyzbcKinematicsForward(const double *joints, - EmcPose * pos, - const KINEMATICS_FORWARD_FLAGS * fflags, - KINEMATICS_INVERSE_FLAGS * iflags) + return kinsJacobianFromMappedAxesP(p, (const double (*)[EMCMOT_MAX_AXIS])dP, + jac); +} // xyzac_jacobian() + +// both rotaries carry the work, so the tool never turns in the machine: +// the tool frame is the shared identity one +const kins_ops XYZAC_OPS = { + .forward = xyzac_forward, + .inverse = xyzac_inverse, + .work = xyzac_work_frame, + .tool = kinsIdentityFrame, + .native = &TOOL_FRAME_SPINDLE, + .jacobian = xyzac_jacobian, +}; + +static int xyzbc_forward(const kins_params *p, kins_scratch *s, + const double *joints, + EmcPose * pos, + const KINEMATICS_FORWARD_FLAGS * fflags, + KINEMATICS_INVERSE_FLAGS * iflags) { + (void)s; (void)fflags; (void)iflags; // Note: 'principal' joints are used - const double x_rot_point = hal_get_real(haldata->x_rot_point); - const double y_rot_point = hal_get_real(haldata->y_rot_point); - const double z_rot_point = hal_get_real(haldata->z_rot_point); - const double dx = hal_get_real(haldata->x_offset); - const double dt = hal_get_real(haldata->tool_offset); - const double dz = hal_get_real(haldata->z_offset) + dt; + const double x_rot_point = p->geometry[TRT_XR]; + const double y_rot_point = p->geometry[TRT_YR]; + const double z_rot_point = p->geometry[TRT_ZR]; + const double dx = p->geometry[TRT_XO]; + const double dt = p->tool.tran.z; + const double dz = p->geometry[TRT_ZO] + dt; const double b_rad = joints[JB]*TO_RAD; const double c_rad = joints[JC]*TO_RAD; - rtapi_real con = hal_get_bool(haldata->conventional_directions) ? 1.0 : -1.0; + const double con = CON(p); pos->tran.x = cos(c_rad) * cos(b_rad) * (joints[JX] - dx - x_rot_point) - con * sin(c_rad) * (joints[JY] - y_rot_point) @@ -398,25 +314,27 @@ int xyzbcKinematicsForward(const double *joints, pos->w = (JW != -1)? joints[JW] : 0; return 0; -} // xyzbcKinematicsForward() +} // xyzbc_forward() -int xyzbcKinematicsInverse(const EmcPose * pos, - double *joints, - const KINEMATICS_INVERSE_FLAGS * iflags, - KINEMATICS_FORWARD_FLAGS * fflags) +static int xyzbc_inverse(const kins_params *p, kins_scratch *s, + const EmcPose * pos, + double *joints, + const KINEMATICS_INVERSE_FLAGS * iflags, + KINEMATICS_FORWARD_FLAGS * fflags) { + (void)s; (void)iflags; (void)fflags; - const double x_rot_point = hal_get_real(haldata->x_rot_point); - const double y_rot_point = hal_get_real(haldata->y_rot_point); - const double z_rot_point = hal_get_real(haldata->z_rot_point); - const double dx = hal_get_real(haldata->x_offset); - const double dt = hal_get_real(haldata->tool_offset); - const double dz = hal_get_real(haldata->z_offset) + dt; + const double x_rot_point = p->geometry[TRT_XR]; + const double y_rot_point = p->geometry[TRT_YR]; + const double z_rot_point = p->geometry[TRT_ZR]; + const double dx = p->geometry[TRT_XO]; + const double dt = p->tool.tran.z; + const double dz = p->geometry[TRT_ZO] + dt; const double b_rad = pos->b*TO_RAD; const double c_rad = pos->c*TO_RAD; - rtapi_real con = hal_get_bool(haldata->conventional_directions) ? 1.0 : -1.0; + const double con = CON(p); // the offsets seen from the tilted table: the same rotation the // forward applies to them, in the same sense @@ -453,23 +371,19 @@ int xyzbcKinematicsInverse(const EmcPose * pos, // update joints with support for // multiple-joints per-coordinate letter: // based on computed position - position_to_mapped_joints(trtfuncs_max_joints, - &P, - joints); + return kinsPoseToMappedJoints(p, &P, joints); +} // xyzbc_inverse() - return 0; -} // xyzbcKinematicsInverse() - -int xyzbcKinematicsWorkFrame(const double *joints, - PmRotationMatrix *rot, - const KINEMATICS_FORWARD_FLAGS *fflags) +static int xyzbc_work_frame(const kins_params *p, const double *joints, + PmRotationMatrix *rot, + const KINEMATICS_FORWARD_FLAGS *fflags) { (void)fflags; - // see the comment in xyzacKinematicsWorkFrame() + // see the comment in xyzac_work_frame() const double b_rad = joints[JB]*TO_RAD; const double c_rad = joints[JC]*TO_RAD; - rtapi_real con = hal_get_bool(haldata->conventional_directions) ? 1.0 : -1.0; + const double con = CON(p); rot->x.x = cos(c_rad) * cos(b_rad); rot->y.x = con * sin(c_rad) * cos(b_rad); @@ -484,32 +398,21 @@ int xyzbcKinematicsWorkFrame(const double *joints, rot->z.z = cos(b_rad); return 0; -} // xyzbcKinematicsWorkFrame() - -int xyzbcKinematicsToolFrame(const double *joints, - PmRotationMatrix *rot, - const KINEMATICS_FORWARD_FLAGS *fflags) -{ - (void)joints; - (void)fflags; - // both rotaries carry the work, so the tool never turns in the machine - *rot = TOOL_FRAME_SPINDLE; - return 0; -} // xyzbcKinematicsToolFrame() +} // xyzbc_work_frame() -int xyzbcKinematicsJacobian(const double *joints, - const EmcPose *pos, - double jac[EMCMOT_MAX_JOINTS][EMCMOT_MAX_AXIS], - const KINEMATICS_INVERSE_FLAGS *iflags) +static int xyzbc_jacobian(const kins_params *p, const double *joints, + const EmcPose *pos, + double jac[EMCMOT_MAX_JOINTS][EMCMOT_MAX_AXIS], + const KINEMATICS_INVERSE_FLAGS *iflags) { (void)joints; (void)iflags; - const double x_rot_point = hal_get_real(haldata->x_rot_point); - const double y_rot_point = hal_get_real(haldata->y_rot_point); - const double z_rot_point = hal_get_real(haldata->z_rot_point); - const double dx = hal_get_real(haldata->x_offset); - const double dt = hal_get_real(haldata->tool_offset); - const double dz = hal_get_real(haldata->z_offset) + dt; + const double x_rot_point = p->geometry[TRT_XR]; + const double y_rot_point = p->geometry[TRT_YR]; + const double z_rot_point = p->geometry[TRT_ZR]; + const double dx = p->geometry[TRT_XO]; + const double dt = p->tool.tran.z; + const double dz = p->geometry[TRT_ZO] + dt; const double sb = sin(pos->b*TO_RAD), cb = cos(pos->b*TO_RAD); const double sc = sin(pos->c*TO_RAD), cc = cos(pos->c*TO_RAD); const double X = pos->tran.x - x_rot_point; @@ -518,14 +421,14 @@ int xyzbcKinematicsJacobian(const double *joints, double dP[EMCMOT_MAX_AXIS][EMCMOT_MAX_AXIS]; int a, b; - rtapi_real con = hal_get_bool(haldata->conventional_directions) ? 1.0 : -1.0; + const double con = CON(p); for (a = 0; a < EMCMOT_MAX_AXIS; a++) { for (b = 0; b < EMCMOT_MAX_AXIS; b++) { dP[a][b] = 0; } } - // see the comment in xyzacKinematicsJacobian(); dpx and dpz of the - // inverse depend on b as well + // see the comment in xyzac_jacobian(); dpx and dpz of the inverse + // depend on b as well dP[0][0] = cc * cb; dP[0][1] = con * sc * cb; dP[0][2] = - con * sb; @@ -544,7 +447,15 @@ int xyzbcKinematicsJacobian(const double *joints, for (a = 3; a < EMCMOT_MAX_AXIS; a++) { dP[a][a] = 1; } - return kinsJacobianFromMappedAxes(trtfuncs_max_joints, - (const double (*)[EMCMOT_MAX_AXIS])dP, - jac); -} // xyzbcKinematicsJacobian() + return kinsJacobianFromMappedAxesP(p, (const double (*)[EMCMOT_MAX_AXIS])dP, + jac); +} // xyzbc_jacobian() + +const kins_ops XYZBC_OPS = { + .forward = xyzbc_forward, + .inverse = xyzbc_inverse, + .work = xyzbc_work_frame, + .tool = kinsIdentityFrame, + .native = &TOOL_FRAME_SPINDLE, + .jacobian = xyzbc_jacobian, +}; diff --git a/src/emc/kinematics/xyzac-trt-kins.c b/src/emc/kinematics/xyzac-trt-kins.c index b6b35538f25..3fc14fc9de4 100644 --- a/src/emc/kinematics/xyzac-trt-kins.c +++ b/src/emc/kinematics/xyzac-trt-kins.c @@ -4,11 +4,12 @@ * * NOTEs: * 1) specify all kparms items -* 2) specify 3 KS,KF,KI functions for switchkins_type=0,1,2 -* 3) the 0th switchkins_type is the startup default -* 4) sparm is a module string parameter for configuration -* 5) The directions of the rotational axes are the opposite of the +* 2) the 0th switchkins_type is the startup default +* 3) sparm is a module string parameter for configuration +* 4) The directions of the rotational axes are the opposite of the * conventional axis directions. +* 5) the maths and the geometry table are in trtfuncs.c, written as +* pure functions of the parameter block (see kinematics.h) */ #include @@ -23,47 +24,28 @@ int switchkinsSetup(kparms* kp, KI* kinv0, KI* kinv1, KI* kinv2 ) { + (void)kset0; (void)kset1; (void)kset2; + (void)kfwd0; (void)kfwd1; (void)kfwd2; + (void)kinv0; (void)kinv1; (void)kinv2; kp->kinsname = "xyzac-trt-kins"; // !!! must agree with filename kp->halprefix = "xyzac-trt-kins"; // hal pin names kp->required_coordinates = "xyzac"; kp->allow_duplicates = 1; kp->max_joints = EMCMOT_MAX_JOINTS; + kp->params = TRT_PARAMS; + kp->nparams = TRT_NPARAMS; if (kp->sparm && strstr(kp->sparm,"identityfirst")) { rtapi_print("\n!!! switchkins-type 0 is IDENTITY\n"); - *kset0 = identityKinematicsSetup; - *kfwd0 = identityKinematicsForward; - *kinv0 = identityKinematicsInverse; - - *kset1 = trtKinematicsSetup; // trt: xyzac,xyzbc - *kfwd1 = xyzacKinematicsForward; - *kinv1 = xyzacKinematicsInverse; - switchkinsRegisterFrames(1, xyzacKinematicsWorkFrame, - xyzacKinematicsToolFrame, - &TOOL_FRAME_SPINDLE); - switchkinsDeclare(0, KINSTYPE_IDENTITY); - switchkinsDeclare(1, KINSTYPE_PRIMARY); - switchkinsRegisterJacobian(1, xyzacKinematicsJacobian); + switchkinsRegisterOps(0, &KINS_IDENTITY_OPS); + switchkinsRegisterOps(1, &XYZAC_OPS); } else { rtapi_print("\n!!! switchkins-type 0 is %s\n",kp->kinsname); - *kset0 = trtKinematicsSetup; // trt: xyzac,xyzbc - *kfwd0 = xyzacKinematicsForward; - *kinv0 = xyzacKinematicsInverse; - switchkinsRegisterFrames(0, xyzacKinematicsWorkFrame, - xyzacKinematicsToolFrame, - &TOOL_FRAME_SPINDLE); - switchkinsRegisterJacobian(0, xyzacKinematicsJacobian); - - *kset1 = identityKinematicsSetup; - *kfwd1 = identityKinematicsForward; - *kinv1 = identityKinematicsInverse; - switchkinsDeclare(0, KINSTYPE_PRIMARY); - switchkinsDeclare(1, KINSTYPE_IDENTITY); + switchkinsRegisterOps(0, &XYZAC_OPS); + switchkinsRegisterOps(1, &KINS_IDENTITY_OPS); } - *kset2 = userkKinematicsSetup; - *kfwd2 = userkKinematicsForward; - *kinv2 = userkKinematicsInverse; + switchkinsRegisterOps(2, &USERK_OPS); return 0; } diff --git a/src/emc/kinematics/xyzbc-trt-kins.c b/src/emc/kinematics/xyzbc-trt-kins.c index 401311e4398..45c41b448dd 100644 --- a/src/emc/kinematics/xyzbc-trt-kins.c +++ b/src/emc/kinematics/xyzbc-trt-kins.c @@ -4,11 +4,12 @@ * * NOTEs: * 1) specify all kparms items -* 2) specify 3 KS,KF,KI functions for switchkins_type=0,1,2 -* 3) the 0th switchkins_type is the startup default -* 4) sparm is a module string parameter for configuration -* 5) The directions of the rotational axes are the opposite of the +* 2) the 0th switchkins_type is the startup default +* 3) sparm is a module string parameter for configuration +* 4) The directions of the rotational axes are the opposite of the * conventional axis directions. +* 5) the maths and the geometry table are in trtfuncs.c, written as +* pure functions of the parameter block (see kinematics.h) */ #include @@ -23,47 +24,28 @@ int switchkinsSetup(kparms* kp, KI* kinv0, KI* kinv1, KI* kinv2 ) { + (void)kset0; (void)kset1; (void)kset2; + (void)kfwd0; (void)kfwd1; (void)kfwd2; + (void)kinv0; (void)kinv1; (void)kinv2; kp->kinsname = "xyzbc-trt-kins"; // !!! must agree with filename kp->halprefix = "xyzbc-trt-kins"; // hal pin names kp->required_coordinates = "xyzbc"; kp->allow_duplicates = 1; kp->max_joints = EMCMOT_MAX_JOINTS; + kp->params = TRT_PARAMS; + kp->nparams = TRT_NPARAMS; if (kp->sparm && strstr(kp->sparm,"identityfirst")) { rtapi_print("\n!!! switchkins-type 0 is IDENTITY\n"); - *kset0 = identityKinematicsSetup; - *kfwd0 = identityKinematicsForward; - *kinv0 = identityKinematicsInverse; - - *kset1 = trtKinematicsSetup; // trt: xyzac,xyzbc - *kfwd1 = xyzbcKinematicsForward; - *kinv1 = xyzbcKinematicsInverse; - switchkinsRegisterFrames(1, xyzbcKinematicsWorkFrame, - xyzbcKinematicsToolFrame, - &TOOL_FRAME_SPINDLE); - switchkinsDeclare(0, KINSTYPE_IDENTITY); - switchkinsDeclare(1, KINSTYPE_PRIMARY); - switchkinsRegisterJacobian(1, xyzbcKinematicsJacobian); + switchkinsRegisterOps(0, &KINS_IDENTITY_OPS); + switchkinsRegisterOps(1, &XYZBC_OPS); } else { rtapi_print("\n!!! switchkins-type 0 is %s\n",kp->kinsname); - *kset0 = trtKinematicsSetup; // trt: xyzac,xyzbc - *kfwd0 = xyzbcKinematicsForward; - *kinv0 = xyzbcKinematicsInverse; - switchkinsRegisterFrames(0, xyzbcKinematicsWorkFrame, - xyzbcKinematicsToolFrame, - &TOOL_FRAME_SPINDLE); - switchkinsRegisterJacobian(0, xyzbcKinematicsJacobian); - - *kset1 = identityKinematicsSetup; - *kfwd1 = identityKinematicsForward; - *kinv1 = identityKinematicsInverse; - switchkinsDeclare(0, KINSTYPE_PRIMARY); - switchkinsDeclare(1, KINSTYPE_IDENTITY); + switchkinsRegisterOps(0, &XYZBC_OPS); + switchkinsRegisterOps(1, &KINS_IDENTITY_OPS); } - *kset2 = userkKinematicsSetup; - *kfwd2 = userkKinematicsForward; - *kinv2 = userkKinematicsInverse; + switchkinsRegisterOps(2, &USERK_OPS); return 0; } From 5a0daf705aedb686473a86258d2fe19474e08ab1 Mon Sep 17 00:00:00 2001 From: Luca Toniolo <10792599+grandixximo@users.noreply.github.com> Date: Fri, 4 Sep 2026 12:57:51 +1000 Subject: [PATCH 30/60] corexykins, rotatekins, rosekins, tripodkins, scorbot-kins, the deltas, matrixkins, userkins: move onto the parameter block Each becomes a kins_module over one ops table and links kins_single.c. The maths is unchanged: where it read a pin it reads the block, what it kept between calls it keeps in the scratch, so rosekins counts its turns per caller and reports them as declared outputs, and tripodkins keeps its HAL_IO pins. kinematicsHome() goes from corexykins and rotatekins; nothing called it. The two deltas share their maths with a python module through a header whose geometry was statics; it is a struct the caller passes now, python API unchanged. matrixkins's nine coefficients were HAL parameters and are pins of the same names. userkins includes kins_util.c and kins_single.c by name so halcompile builds it on its own, and kins_single.c joins the sources installed in share/linuxcnc. --- .gitignore | 1 + debian/linuxcnc-uspace-dev.install | 1 + src/Makefile | 19 +- src/emc/kinematics/Submakefile | 3 +- src/emc/kinematics/corexykins.c | 72 +++--- src/emc/kinematics/lineardeltakins-common.h | 46 ++-- src/emc/kinematics/lineardeltakins.c | 94 ++++---- src/emc/kinematics/lineardeltakins.cc | 14 +- src/emc/kinematics/rosekins.c | 110 +++++---- src/emc/kinematics/rotarydeltakins-common.h | 62 +++-- src/emc/kinematics/rotarydeltakins.c | 116 ++++----- src/emc/kinematics/rotarydeltakins.cc | 15 +- src/emc/kinematics/rotatekins.c | 84 ++++--- src/emc/kinematics/scorbot-kins.c | 127 +++------- src/emc/kinematics/tripodkins.c | 246 +++++--------------- src/hal/components/Submakefile | 1 + src/hal/components/matrixkins.comp | 196 ++++++++-------- src/hal/components/userkins.comp | 174 +++++++------- 18 files changed, 650 insertions(+), 731 deletions(-) diff --git a/.gitignore b/.gitignore index 18eb2868130..19647ebbccd 100644 --- a/.gitignore +++ b/.gitignore @@ -23,6 +23,7 @@ share/desktop-directories/linuxcnc-doc.directory share/linuxcnc/mesa_modbus.c.tmpl share/linuxcnc/switchkins.c share/linuxcnc/kins_util.c +share/linuxcnc/kins_single.c src/modules.order /configs/*/emc.nml !/configs/common/emc.nml diff --git a/debian/linuxcnc-uspace-dev.install b/debian/linuxcnc-uspace-dev.install index 39c124d3532..251c401a9e9 100644 --- a/debian/linuxcnc-uspace-dev.install +++ b/debian/linuxcnc-uspace-dev.install @@ -7,3 +7,4 @@ usr/share/linuxcnc/Makefile.modinc usr/share/linuxcnc/mesa_modbus.c.tmpl usr/share/linuxcnc/switchkins.c usr/share/linuxcnc/kins_util.c +usr/share/linuxcnc/kins_single.c diff --git a/src/Makefile b/src/Makefile index bd58516acd1..2cef5765d25 100644 --- a/src/Makefile +++ b/src/Makefile @@ -788,7 +788,7 @@ ifeq ($(BUILD_GUI),yes) endif $(FILE) ../src/hal/drivers/mesa-hostmot2/modbus/*.tmpl $(DESTDIR)$(prefix)/share/linuxcnc/ - $(FILE) ../src/emc/kinematics/switchkins.c ../src/emc/kinematics/kins_util.c $(DESTDIR)$(prefix)/share/linuxcnc/ + $(FILE) ../src/emc/kinematics/switchkins.c ../src/emc/kinematics/kins_util.c ../src/emc/kinematics/kins_single.c $(DESTDIR)$(prefix)/share/linuxcnc/ install-kernel-indep: install-python install-python: install-dirs @@ -918,6 +918,9 @@ endif # "kbuild" system. $(BASEPWD) is used here, instead of relative paths, because # that's what kbuild seems to require +# A component built in tree includes the shared kinematics sources by the +# bare names the out-of-tree build resolves in share/linuxcnc +RTFLAGS += -I$(BASEPWD)/emc/kinematics EXTRA_CFLAGS := $(filter-out -ffast-math,$(RTFLAGS)) -D__MODULE__ \ -I$(BASEPWD)/../include -I$(BASEPWD) \ -DSEQUENTIAL_SUPPORT -DHAL_SUPPORT -DDYNAMIC_PLCSIZE -DRT_SUPPORT -DOLD_TIMERS_MONOS_SUPPORT -DMODBUS_IO_MASTER \ @@ -1141,15 +1144,23 @@ maxkins-objs += emc/kinematics/kins_single.o obj-m += rotatekins.o rotatekins-objs := emc/kinematics/rotatekins.o +rotatekins-objs += emc/kinematics/kins_util.o +rotatekins-objs += emc/kinematics/kins_single.o obj-m += tripodkins.o tripodkins-objs := emc/kinematics/tripodkins.o +tripodkins-objs += emc/kinematics/kins_util.o +tripodkins-objs += emc/kinematics/kins_single.o obj-m += corexykins.o corexykins-objs := emc/kinematics/corexykins.o +corexykins-objs += emc/kinematics/kins_util.o +corexykins-objs += emc/kinematics/kins_single.o obj-m += lineardeltakins.o lineardeltakins-objs := emc/kinematics/lineardeltakins.o +lineardeltakins-objs += emc/kinematics/kins_util.o +lineardeltakins-objs += emc/kinematics/kins_single.o obj-m += pentakins.o pentakins-objs := emc/kinematics/pentakins.o @@ -1158,14 +1169,20 @@ pentakins-objs += $(MATHSTUB) obj-m += rotarydeltakins.o rotarydeltakins-objs := emc/kinematics/rotarydeltakins.o +rotarydeltakins-objs += emc/kinematics/kins_util.o +rotarydeltakins-objs += emc/kinematics/kins_single.o rotarydeltakins-objs += libposemath/_posemath.o rotarydeltakins-objs += $(MATHSTUB) obj-m += rosekins.o rosekins-objs := emc/kinematics/rosekins.o +rosekins-objs += emc/kinematics/kins_util.o +rosekins-objs += emc/kinematics/kins_single.o obj-m += scorbot-kins.o scorbot-kins-objs := emc/kinematics/scorbot-kins.o +scorbot-kins-objs += emc/kinematics/kins_util.o +scorbot-kins-objs += emc/kinematics/kins_single.o ifeq ($(origin userkfuncs), undefined) # use template: diff --git a/src/emc/kinematics/Submakefile b/src/emc/kinematics/Submakefile index 7e2f2d84b4b..c71c18696e2 100644 --- a/src/emc/kinematics/Submakefile +++ b/src/emc/kinematics/Submakefile @@ -39,7 +39,8 @@ PYTARGETS += $(RDELTAMODULE) # in-tree ones link it. EMCKINEMATICSSRCS = \ ../share/linuxcnc/switchkins.c \ - ../share/linuxcnc/kins_util.c + ../share/linuxcnc/kins_util.c \ + ../share/linuxcnc/kins_single.c $(EMCKINEMATICSSRCS): ../share/linuxcnc/%.c: ./emc/kinematics/%.c $(ECHO) Copying switchkins source $(notdir $@) diff --git a/src/emc/kinematics/corexykins.c b/src/emc/kinematics/corexykins.c index 473a2ceede1..353ae0f5a84 100644 --- a/src/emc/kinematics/corexykins.c +++ b/src/emc/kinematics/corexykins.c @@ -8,12 +8,15 @@ #include #include #include +#include -int kinematicsForward(const double *joints - ,EmcPose *pos - ,const KINEMATICS_FORWARD_FLAGS *fflags - ,KINEMATICS_INVERSE_FLAGS *iflags - ) { +static int corexy_forward(const kins_params *p, kins_scratch *s, + const double *joints, EmcPose *pos, + const KINEMATICS_FORWARD_FLAGS *fflags, + KINEMATICS_INVERSE_FLAGS *iflags) +{ + (void)p; + (void)s; (void)fflags; (void)iflags; pos->tran.x = 0.5 * (joints[0] + joints[1]); @@ -29,11 +32,13 @@ int kinematicsForward(const double *joints return 0; } -int kinematicsInverse(const EmcPose *pos - ,double *joints - ,const KINEMATICS_INVERSE_FLAGS *iflags - ,KINEMATICS_FORWARD_FLAGS *fflags - ) { +static int corexy_inverse(const kins_params *p, kins_scratch *s, + const EmcPose *pos, double *joints, + const KINEMATICS_INVERSE_FLAGS *iflags, + KINEMATICS_FORWARD_FLAGS *fflags) +{ + (void)p; + (void)s; (void)iflags; (void)fflags; joints[0] = pos->tran.x + pos->tran.y; @@ -49,12 +54,13 @@ int kinematicsInverse(const EmcPose *pos return 0; } -int kinematicsJacobian(const double *joints, - const EmcPose *pos, - double jac[EMCMOT_MAX_JOINTS][EMCMOT_MAX_AXIS], - const KINEMATICS_INVERSE_FLAGS *iflags) +static int corexy_jacobian(const kins_params *p, const double *joints, + const EmcPose *pos, + double jac[EMCMOT_MAX_JOINTS][EMCMOT_MAX_AXIS], + const KINEMATICS_INVERSE_FLAGS *iflags) { int j, a; + (void)p; (void)joints; (void)pos; (void)iflags; @@ -68,23 +74,26 @@ int kinematicsJacobian(const double *joints, return 0; } -int kinematicsHome(EmcPose *world - ,double *joint - ,KINEMATICS_FORWARD_FLAGS *fflags - ,KINEMATICS_INVERSE_FLAGS *iflags - ) { - *fflags = 0; - *iflags = 0; - return kinematicsForward(joint, world, fflags, iflags); -} +static const kins_ops corexy_ops = { + .forward = corexy_forward, + .inverse = corexy_inverse, + .jacobian = corexy_jacobian, +}; -KINEMATICS_TYPE kinematicsType() { return KINEMATICS_BOTH; } +// no geometry: the belts are what they are. Joints 0..8 are the nine +// letters in order; the entry points come from kins_single.c +const kins_module_info kins_module = { + .name = "corexykins", + .halprefix = "corexykins", + .params = NULL, + .nparams = 0, + .required_coordinates = "XYZABCUVW", + .max_joints = 9, + .allow_duplicates = 0, + .ntypes = 1, + .ops = { &corexy_ops }, +}; -KINS_NOT_SWITCHABLE -EXPORT_SYMBOL(kinematicsType); -EXPORT_SYMBOL(kinematicsForward); -EXPORT_SYMBOL(kinematicsInverse); -EXPORT_SYMBOL(kinematicsJacobian); MODULE_LICENSE("GPL"); static int comp_id; @@ -92,6 +101,11 @@ int rtapi_app_main(void) { comp_id = hal_init("corexykins"); if(comp_id < 0) return comp_id; + if (kinsSingleInit(comp_id, "XYZABCUVW", KINEMATICS_BOTH)) { + hal_exit(comp_id); + return -1; + } + hal_ready(comp_id); return 0; } diff --git a/src/emc/kinematics/lineardeltakins-common.h b/src/emc/kinematics/lineardeltakins-common.h index 6e0b0037375..203343a20e1 100644 --- a/src/emc/kinematics/lineardeltakins-common.h +++ b/src/emc/kinematics/lineardeltakins-common.h @@ -32,10 +32,16 @@ // common routines used by the userspace kinematics and the realtime kinematics // user must include a math.h-type header first // Inspired by Marlin delta firmware and https://gist.github.com/kastner/5279172 +// +// The geometry is a value the caller holds and passes in, so the same +// routines serve the realtime module through its parameter block and the +// python module through its own copy. #include -static double L, R; -static double Ax, Ay, Bx, By, Cx, Cy, L2; +typedef struct { + double L, R; + double Ax, Ay, Bx, By, Cx, Cy, L2; +} lineardelta_geometry; #define SQ3 (sqrt(3)) @@ -44,31 +50,30 @@ static double Ax, Ay, Bx, By, Cx, Cy, L2; static double sq(double x) { return x*x; } -static void set_geometry(double r_, double l_) +static void lineardelta_set_geometry(lineardelta_geometry *g, double r_, double l_) { - if(L == l_ && R == r_) return; - - L = l_; - R = r_; + g->L = l_; + g->R = r_; - L2 = sq(L); + g->L2 = sq(g->L); - Ax = 0.0; - Ay = R; + g->Ax = 0.0; + g->Ay = g->R; - Bx = -SIN_60 * R; - By = -COS_60 * R; + g->Bx = -SIN_60 * g->R; + g->By = -COS_60 * g->R; - Cx = SIN_60 * R; - Cy = -COS_60 * R; + g->Cx = SIN_60 * g->R; + g->Cy = -COS_60 * g->R; } -static int kinematics_inverse(const EmcPose *pos, double *joints) +static int lineardelta_inverse(const lineardelta_geometry *g, + const EmcPose *pos, double *joints) { double x = pos->tran.x, y = pos->tran.y, z = pos->tran.z; - joints[0] = z + sqrt(L2 - sq(Ax-x) - sq(Ay-y)); - joints[1] = z + sqrt(L2 - sq(Bx-x) - sq(By-y)); - joints[2] = z + sqrt(L2 - sq(Cx-x) - sq(Cy-y)); + joints[0] = z + sqrt(g->L2 - sq(g->Ax-x) - sq(g->Ay-y)); + joints[1] = z + sqrt(g->L2 - sq(g->Bx-x) - sq(g->By-y)); + joints[2] = z + sqrt(g->L2 - sq(g->Cx-x) - sq(g->Cy-y)); joints[3] = pos->a; joints[4] = pos->b; joints[5] = pos->c; @@ -80,11 +85,14 @@ static int kinematics_inverse(const EmcPose *pos, double *joints) ? -1 : 0; } -static int kinematics_forward(const double *joints, EmcPose *pos) +static int lineardelta_forward(const lineardelta_geometry *g, + const double *joints, EmcPose *pos) { double q1 = joints[0]; double q2 = joints[1]; double q3 = joints[2]; + const double Ay = g->Ay, Bx = g->Bx, By = g->By, Cx = g->Cx, Cy = g->Cy; + const double L = g->L; double den = (By-Ay)*Cx-(Cy-Ay)*Bx; diff --git a/src/emc/kinematics/lineardeltakins.c b/src/emc/kinematics/lineardeltakins.c index 541643fef74..8aa454258d8 100644 --- a/src/emc/kinematics/lineardeltakins.c +++ b/src/emc/kinematics/lineardeltakins.c @@ -18,52 +18,67 @@ #include #include #include +#include #include "lineardeltakins-common.h" -static struct haldata -{ - hal_real_t r; - hal_real_t l; -} *haldata; +// the two lengths, one pin each +static const kins_param_desc ld_params[] = { + { "R", KINS_PARAM_FLOAT, KINS_IN, 0, DELTA_RADIUS }, + { "L", KINS_PARAM_FLOAT, KINS_IN, 0, DELTA_DIAGONAL_ROD }, +}; +enum { P_R, P_L }; static int comp_id; -int kinematicsForward(const double * joints, +// the tower positions follow from the block's two lengths +static void geometry_of(const kins_params *p, lineardelta_geometry *g) +{ + lineardelta_set_geometry(g, p->geometry[P_R], p->geometry[P_L]); +} + +static int ld_forward(const kins_params *p, kins_scratch *s, + const double * joints, EmcPose * pos, const KINEMATICS_FORWARD_FLAGS * fflags, KINEMATICS_INVERSE_FLAGS * iflags) { + lineardelta_geometry g; + (void)s; (void)fflags; (void)iflags; - set_geometry(hal_get_real(haldata->r), hal_get_real(haldata->l)); - return kinematics_forward(joints, pos); + geometry_of(p, &g); + return lineardelta_forward(&g, joints, pos); } -int kinematicsInverse(const EmcPose *pos, double *joints, - const KINEMATICS_INVERSE_FLAGS *iflags, - KINEMATICS_FORWARD_FLAGS *fflags) { +static int ld_inverse(const kins_params *p, kins_scratch *s, + const EmcPose *pos, double *joints, + const KINEMATICS_INVERSE_FLAGS *iflags, + KINEMATICS_FORWARD_FLAGS *fflags) { + lineardelta_geometry g; + (void)s; (void)iflags; (void)fflags; - set_geometry(hal_get_real(haldata->r), hal_get_real(haldata->l)); - return kinematics_inverse(pos, joints); + geometry_of(p, &g); + return lineardelta_inverse(&g, pos, joints); } -int kinematicsJacobian(const double *joints, +static int ld_jacobian(const kins_params *p, const double *joints, const EmcPose *pos, double jac[EMCMOT_MAX_JOINTS][EMCMOT_MAX_AXIS], const KINEMATICS_INVERSE_FLAGS *iflags) { + lineardelta_geometry g; double x = pos->tran.x, y = pos->tran.y, z = pos->tran.z; int i, j, a; (void)iflags; - set_geometry(hal_get_real(haldata->r), hal_get_real(haldata->l)); + geometry_of(p, &g); for (j = 0; j < EMCMOT_MAX_JOINTS; j++) { for (a = 0; a < EMCMOT_MAX_AXIS; a++) { jac[j][a] = 0; } } // each carriage is the platform height plus the rise of its rod, and // the rise changes with the horizontal offset from the tower for (i = 0; i < 3; i++) { - double tx = (i == 0) ? Ax : (i == 1) ? Bx : Cx; - double ty = (i == 0) ? Ay : (i == 1) ? By : Cy; + double tx = (i == 0) ? g.Ax : (i == 1) ? g.Bx : g.Cx; + double ty = (i == 0) ? g.Ay : (i == 1) ? g.By : g.Cy; double rise = joints[i] - z; if (rise <= 0) { return -1; } jac[i][0] = (tx - x)/rise; @@ -74,32 +89,38 @@ int kinematicsJacobian(const double *joints, return 0; } -KINEMATICS_TYPE kinematicsType() -{ - return KINEMATICS_BOTH; -} +static const kins_ops ld_ops = { + .forward = ld_forward, + .inverse = ld_inverse, + .jacobian = ld_jacobian, +}; + +// three towers for the three linear coordinates, the rest passed +// through; the entry points come from kins_single.c +const kins_module_info kins_module = { + .name = "lineardeltakins", + .halprefix = "lineardeltakins", + .params = ld_params, + .nparams = sizeof(ld_params)/sizeof(ld_params[0]), + .required_coordinates = "XYZABCUVW", + .max_joints = 9, + .allow_duplicates = 0, + .ntypes = 1, + .ops = { &ld_ops }, +}; int rtapi_app_main(void) { - int retval; - comp_id = hal_init("lineardeltakins"); if(comp_id < 0) return comp_id; - haldata = hal_malloc(sizeof(*haldata)); - if(!haldata) { retval = -ENOMEM; goto error; } - - if((retval = hal_pin_new_real(comp_id, HAL_IN, &haldata->r, DELTA_RADIUS, "lineardeltakins.R")) < 0) - goto error; - if((retval = hal_pin_new_real(comp_id, HAL_IN, &haldata->l, DELTA_DIAGONAL_ROD, "lineardeltakins.L")) < 0) - goto error; + if (kinsSingleInit(comp_id, "XYZABCUVW", KINEMATICS_BOTH)) { + hal_exit(comp_id); + return -1; + } hal_ready(comp_id); return 0; - -error: - hal_exit(comp_id); - return retval; } void rtapi_app_exit(void) @@ -107,9 +128,4 @@ void rtapi_app_exit(void) hal_exit(comp_id); } -KINS_NOT_SWITCHABLE -EXPORT_SYMBOL(kinematicsType); -EXPORT_SYMBOL(kinematicsForward); -EXPORT_SYMBOL(kinematicsInverse); -EXPORT_SYMBOL(kinematicsJacobian); MODULE_LICENSE("GPL"); diff --git a/src/emc/kinematics/lineardeltakins.cc b/src/emc/kinematics/lineardeltakins.cc index 351746081c7..780542ba380 100644 --- a/src/emc/kinematics/lineardeltakins.cc +++ b/src/emc/kinematics/lineardeltakins.cc @@ -21,11 +21,19 @@ using namespace boost::python; #define isnan(x) std::isnan(x) #include "lineardeltakins-common.h" +// the python module keeps one geometry, set from python +static lineardelta_geometry geometry; + +static void set_geometry(double r, double l) +{ + lineardelta_set_geometry(&geometry, r, l); +} + static object forward(double j0, double j1, double j2) { double joints[9] = {j0, j1, j2}; EmcPose pos; - int result = kinematics_forward(joints, &pos); + int result = lineardelta_forward(&geometry, joints, &pos); if(result == 0) return make_tuple(pos.tran.x, pos.tran.y, pos.tran.z); return object(); @@ -35,7 +43,7 @@ static object inverse(double x, double y, double z) { double joints[9]; EmcPose pos = {{x,y,z}, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0}; - int result = kinematics_inverse(&pos, joints); + int result = lineardelta_inverse(&geometry, &pos, joints); if(result == 0) return make_tuple(joints[0], joints[1], joints[2]); return object(); @@ -43,7 +51,7 @@ static object inverse(double x, double y, double z) static object get_geometry() { - return make_tuple(R, L); + return make_tuple(geometry.R, geometry.L); } #pragma GCC diagnostic push diff --git a/src/emc/kinematics/rosekins.c b/src/emc/kinematics/rosekins.c index 9f73fbc3f9d..1622542154f 100644 --- a/src/emc/kinematics/rosekins.c +++ b/src/emc/kinematics/rosekins.c @@ -21,29 +21,36 @@ #include #include #include +#include -KINS_NOT_SWITCHABLE -EXPORT_SYMBOL(kinematicsType); -EXPORT_SYMBOL(kinematicsInverse); -EXPORT_SYMBOL(kinematicsForward); -EXPORT_SYMBOL(kinematicsJacobian); MODULE_LICENSE("GPL"); #ifndef hypot #define hypot(a,b) (sqrt((a)*(a)+(b)*(b))) #endif -static struct haldata { - hal_real_t revolutions; - hal_real_t theta_degrees; - hal_real_t bigtheta_degrees; -} *haldata; - -int kinematicsForward(const double *joints, - EmcPose * pos, - const KINEMATICS_FORWARD_FLAGS * fflags, - KINEMATICS_INVERSE_FLAGS * iflags) +// the inverse reports the turn count it keeps and the angles it saw +static const kins_param_desc rose_params[] = { + { "revolutions", KINS_PARAM_FLOAT, KINS_OUT, 0, 0.0 }, + { "theta_degrees", KINS_PARAM_FLOAT, KINS_OUT, 0, 0.0 }, + { "bigtheta_degrees", KINS_PARAM_FLOAT, KINS_OUT, 0, 0.0 }, +}; +enum { O_REVOLUTIONS, O_THETA, O_BIGTHETA }; + +// what the inverse carries from one call to the next: the quadrant it +// last saw and the turns it has counted. In the scratch, so that each +// caller counts its own. +#define OLDQUAD(s) ((s)->aux[0]) +#define REVOLUTIONS(s) ((s)->aux[1]) + +static int rose_forward(const kins_params *p, kins_scratch *s, + const double *joints, + EmcPose * pos, + const KINEMATICS_FORWARD_FLAGS * fflags, + KINEMATICS_INVERSE_FLAGS * iflags) { + (void)p; + (void)s; (void)fflags; (void)iflags; double radius,z,theta; @@ -65,18 +72,20 @@ int kinematicsForward(const double *joints, return 0; } -int kinematicsInverse(const EmcPose * pos, - double *joints, - const KINEMATICS_INVERSE_FLAGS * iflags, - KINEMATICS_FORWARD_FLAGS * fflags) +static int rose_inverse(const kins_params *p, kins_scratch *s, + const EmcPose * pos, + double *joints, + const KINEMATICS_INVERSE_FLAGS * iflags, + KINEMATICS_FORWARD_FLAGS * fflags) { + (void)p; (void)iflags; (void)fflags; // There is a potential problem when accumulating bigtheta -- loss of // precision based on size of mantissa -- but in practice, it is probably ok - static int oldquad; - static int revolutions; + int oldquad = (int)OLDQUAD(s); + int revolutions = (int)REVOLUTIONS(s); double theta,bigtheta; int nowquad = 0; @@ -95,9 +104,9 @@ int kinematicsInverse(const EmcPose * pos, theta = atan2(y,x); bigtheta = theta + PM_2_PI * revolutions; - hal_set_real(haldata->revolutions, revolutions); - hal_set_real(haldata->theta_degrees, theta * TO_DEG); - hal_set_real(haldata->bigtheta_degrees, bigtheta * TO_DEG); + s->out[O_REVOLUTIONS] = revolutions; + s->out[O_THETA] = theta * TO_DEG; + s->out[O_BIGTHETA] = bigtheta * TO_DEG; joints[0] = hypot(x,y); joints[1] = z; @@ -109,19 +118,21 @@ int kinematicsInverse(const EmcPose * pos, joints[7] = 0; joints[8] = 0; - oldquad = nowquad; + OLDQUAD(s) = nowquad; + REVOLUTIONS(s) = revolutions; return 0; } -int kinematicsJacobian(const double *joints, - const EmcPose *pos, - double jac[EMCMOT_MAX_JOINTS][EMCMOT_MAX_AXIS], - const KINEMATICS_INVERSE_FLAGS *iflags) +static int rose_jacobian(const kins_params *p, const double *joints, + const EmcPose *pos, + double jac[EMCMOT_MAX_JOINTS][EMCMOT_MAX_AXIS], + const KINEMATICS_INVERSE_FLAGS *iflags) { double x = pos->tran.x, y = pos->tran.y; double r2 = x*x + y*y; double r = sqrt(r2); int j, a; + (void)p; (void)joints; (void)iflags; // on the axis the angle is undefined and its rate unbounded @@ -136,34 +147,39 @@ int kinematicsJacobian(const double *joints, return 0; } -KINEMATICS_TYPE kinematicsType() -{ - return KINEMATICS_BOTH; -} +static const kins_ops rose_ops = { + .forward = rose_forward, + .inverse = rose_inverse, + .jacobian = rose_jacobian, +}; + +// joints 0..2 are radius, z and the unwrapped angle; the entry points +// come from kins_single.c +const kins_module_info kins_module = { + .name = "rosekins", + .halprefix = "rosekins", + .params = rose_params, + .nparams = sizeof(rose_params)/sizeof(rose_params[0]), + .required_coordinates = "XYZ", + .max_joints = 3, + .allow_duplicates = 0, + .ntypes = 1, + .ops = { &rose_ops }, +}; static int comp_id; void rtapi_app_exit(void) { hal_exit(comp_id); } int rtapi_app_main(void) { - int ans; comp_id = hal_init("rosekins"); if(comp_id < 0) return comp_id; - haldata = hal_malloc(sizeof(*haldata)); - if(!haldata) { ans = -ENOMEM; goto error; } - - if((ans = hal_pin_new_real(comp_id, HAL_OUT, &(haldata->revolutions), 0.0, "rosekins.revolutions")) < 0) - goto error; - if((ans = hal_pin_new_real(comp_id, HAL_OUT, &(haldata->theta_degrees), 0.0, "rosekins.theta_degrees")) < 0) - goto error; - if((ans = hal_pin_new_real(comp_id, HAL_OUT, &(haldata->bigtheta_degrees), 0.0, "rosekins.bigtheta_degrees")) < 0) - goto error; + if (kinsSingleInit(comp_id, "XYZ", KINEMATICS_BOTH)) { + hal_exit(comp_id); + return -1; + } hal_ready(comp_id); return 0; - -error: - hal_exit(comp_id); - return ans; } diff --git a/src/emc/kinematics/rotarydeltakins-common.h b/src/emc/kinematics/rotarydeltakins-common.h index 59cc872200c..95c6ce95c9e 100644 --- a/src/emc/kinematics/rotarydeltakins-common.h +++ b/src/emc/kinematics/rotarydeltakins-common.h @@ -40,6 +40,10 @@ positive, the Z coordinate will get more negative. Joint zero is the one whose thigh swings in the YZ plane. + + The geometry is a value the caller holds and passes in, so the same + routines serve the realtime module through its parameter block and the + python module through its own copy. */ #ifndef LINUXCNCROTARYDELTAKINS_COMMON_H @@ -47,17 +51,19 @@ #include -// distance from origin to a hip joint -static double platformradius; +typedef struct { + // distance from origin to a hip joint + double platformradius; -// thigh connects the hip to the knee -static double thighlength; + // thigh connects the hip to the knee + double thighlength; -// shin (the parallelogram) connects the knee to the foot -static double shinlength; + // shin (the parallelogram) connects the knee to the foot + double shinlength; -// distance from center of foot (controlled point) to an ankle joint -static double footradius; + // distance from center of foot (controlled point) to an ankle joint + double footradius; +} rotarydelta_geometry; #ifndef sq #define sq(a) ((a)*(a)) @@ -66,15 +72,21 @@ static double footradius; #define D2R(d) ((d)*M_PI/180.) #endif -static void set_geometry(double pfr, double tl, double sl, double fr) { - platformradius = pfr; - thighlength = tl; - shinlength = sl; - footradius = fr; +static void rotarydelta_set_geometry(rotarydelta_geometry *g, + double pfr, double tl, double sl, double fr) { + g->platformradius = pfr; + g->thighlength = tl; + g->shinlength = sl; + g->footradius = fr; } // Given three hip joint angles, find the controlled point -static int kinematics_forward(const double *joints, EmcPose *pos) { +static int rotarydelta_forward(const rotarydelta_geometry *g, + const double *joints, EmcPose *pos) { + const double platformradius = g->platformradius; + const double thighlength = g->thighlength; + const double shinlength = g->shinlength; + const double footradius = g->footradius; double j0 = joints[0], j1 = joints[1], @@ -139,7 +151,12 @@ static int kinematics_forward(const double *joints, EmcPose *pos) { // Given controlled point, find joint zero's angle // (J0 is the easy one in the ZY plane) -static int inverse_j0(double x, double y, double z, double *theta) { +static int rotarydelta_inverse_j0(const rotarydelta_geometry *g, + double x, double y, double z, double *theta) { + const double platformradius = g->platformradius; + const double thighlength = g->thighlength; + const double shinlength = g->shinlength; + const double footradius = g->footradius; double a, b, d, knee_y, knee_z; a = 0.5 * (sq(x) + sq(y - footradius) + sq(z) + sq(thighlength) - @@ -157,25 +174,26 @@ static int inverse_j0(double x, double y, double z, double *theta) { return 0; } -static void rotate(double *x, double *y, double theta) { +static void rotarydelta_rotate(double *x, double *y, double theta) { double xx, yy; xx = *x, yy = *y; *x = xx * cos(theta) - yy * sin(theta); *y = xx * sin(theta) + yy * cos(theta); } -static int kinematics_inverse(const EmcPose *pos, double *joints) { +static int rotarydelta_inverse(const rotarydelta_geometry *g, + const EmcPose *pos, double *joints) { double xr, yr; - if(inverse_j0(pos->tran.x, pos->tran.y, pos->tran.z, &joints[0])) return -1; + if(rotarydelta_inverse_j0(g, pos->tran.x, pos->tran.y, pos->tran.z, &joints[0])) return -1; // now use symmetry property to get the other two just as easily... xr = pos->tran.x; yr = pos->tran.y; - rotate(&xr, &yr, -2*M_PI/3); - if(inverse_j0(xr, yr, pos->tran.z, &joints[1])) return -1; + rotarydelta_rotate(&xr, &yr, -2*M_PI/3); + if(rotarydelta_inverse_j0(g, xr, yr, pos->tran.z, &joints[1])) return -1; xr = pos->tran.x; yr = pos->tran.y; - rotate(&xr, &yr, 2*M_PI/3); - if(inverse_j0(xr, yr, pos->tran.z, &joints[2])) return -1; + rotarydelta_rotate(&xr, &yr, 2*M_PI/3); + if(rotarydelta_inverse_j0(g, xr, yr, pos->tran.z, &joints[2])) return -1; joints[3] = pos->a; joints[4] = pos->b; diff --git a/src/emc/kinematics/rotarydeltakins.c b/src/emc/kinematics/rotarydeltakins.c index a2f52c10c1c..4cee9c38173 100644 --- a/src/emc/kinematics/rotarydeltakins.c +++ b/src/emc/kinematics/rotarydeltakins.c @@ -19,75 +19,90 @@ #include #include #include +#include #include "rotarydeltakins-common.h" -static struct haldata -{ - hal_real_t pfr; - hal_real_t tl; - hal_real_t sl; - hal_real_t fr; -} *haldata; +// the four lengths, one pin each +static const kins_param_desc rd_params[] = { + { "platformradius", KINS_PARAM_FLOAT, KINS_IN, 0, RDELTA_PFR }, + { "thighlength", KINS_PARAM_FLOAT, KINS_IN, 0, RDELTA_TL }, + { "shinlength", KINS_PARAM_FLOAT, KINS_IN, 0, RDELTA_SL }, + { "footradius", KINS_PARAM_FLOAT, KINS_IN, 0, RDELTA_FR }, +}; +enum { P_PFR, P_TL, P_SL, P_FR }; static int comp_id; -int kinematicsForward(const double * joints, +static void geometry_of(const kins_params *p, rotarydelta_geometry *g) +{ + rotarydelta_set_geometry(g, p->geometry[P_PFR], p->geometry[P_TL], + p->geometry[P_SL], p->geometry[P_FR]); +} + +static int rd_forward(const kins_params *p, kins_scratch *s, + const double * joints, EmcPose * pos, const KINEMATICS_FORWARD_FLAGS * fflags, KINEMATICS_INVERSE_FLAGS * iflags) { + rotarydelta_geometry g; + (void)s; (void)fflags; (void)iflags; - set_geometry(hal_get_real(haldata->pfr), hal_get_real(haldata->tl), hal_get_real(haldata->sl), hal_get_real(haldata->fr)); - return kinematics_forward(joints, pos); + geometry_of(p, &g); + return rotarydelta_forward(&g, joints, pos); } -int kinematicsInverse(const EmcPose *pos, double *joints, - const KINEMATICS_INVERSE_FLAGS *iflags, - KINEMATICS_FORWARD_FLAGS *fflags) { +static int rd_inverse(const kins_params *p, kins_scratch *s, + const EmcPose *pos, double *joints, + const KINEMATICS_INVERSE_FLAGS *iflags, + KINEMATICS_FORWARD_FLAGS *fflags) { + rotarydelta_geometry g; + (void)s; (void)iflags; (void)fflags; - set_geometry(hal_get_real(haldata->pfr), hal_get_real(haldata->tl), hal_get_real(haldata->sl), hal_get_real(haldata->fr)); - return kinematics_inverse(pos, joints); + geometry_of(p, &g); + return rotarydelta_inverse(&g, pos, joints); } -int kinematicsJacobian(const double *joints, +static int rd_jacobian(const kins_params *p, const double *joints, const EmcPose *pos, double jac[EMCMOT_MAX_JOINTS][EMCMOT_MAX_AXIS], const KINEMATICS_INVERSE_FLAGS *iflags) { + rotarydelta_geometry g; int i, j, a; (void)iflags; - set_geometry(hal_get_real(haldata->pfr), hal_get_real(haldata->tl), hal_get_real(haldata->sl), hal_get_real(haldata->fr)); + geometry_of(p, &g); for (j = 0; j < EMCMOT_MAX_JOINTS; j++) { for (a = 0; a < EMCMOT_MAX_AXIS; a++) { jac[j][a] = 0; } } // The foot stays a shin length from each knee, so along a leg the // motion of the foot and the motion of the knee agree: // (P - K) . dP = (P - K) . dK/dq dq - // K is the knee less the foot offset, written as kinematics_forward() + // K is the knee less the foot offset, written as rotarydelta_forward() // writes it, and q the hip angle that swings it. for (i = 0; i < 3; i++) { double q = D2R(joints[i]); - double reach = platformradius - footradius + thighlength * cos(q); + double reach = g.platformradius - g.footradius + g.thighlength * cos(q); double kx, ky, kz, dkx, dky, dkz, px, py, pz, denom; switch (i) { case 0: kx = 0; ky = -reach; - dkx = 0; dky = thighlength * sin(q); + dkx = 0; dky = g.thighlength * sin(q); break; case 1: kx = reach * 0.5 * sqrt(3); ky = reach * 0.5; - dkx = -thighlength * sin(q) * 0.5 * sqrt(3); - dky = -thighlength * sin(q) * 0.5; + dkx = -g.thighlength * sin(q) * 0.5 * sqrt(3); + dky = -g.thighlength * sin(q) * 0.5; break; default: kx = -reach * 0.5 * sqrt(3); ky = reach * 0.5; - dkx = thighlength * sin(q) * 0.5 * sqrt(3); - dky = -thighlength * sin(q) * 0.5; + dkx = g.thighlength * sin(q) * 0.5 * sqrt(3); + dky = -g.thighlength * sin(q) * 0.5; break; } - kz = -thighlength * sin(q); - dkz = -thighlength * cos(q); + kz = -g.thighlength * sin(q); + dkz = -g.thighlength * cos(q); px = pos->tran.x - kx; py = pos->tran.y - ky; pz = pos->tran.z - kz; @@ -103,36 +118,38 @@ int kinematicsJacobian(const double *joints, return 0; } -KINEMATICS_TYPE kinematicsType() -{ - return KINEMATICS_BOTH; -} +static const kins_ops rd_ops = { + .forward = rd_forward, + .inverse = rd_inverse, + .jacobian = rd_jacobian, +}; + +// three hips for the three linear coordinates, the rest passed through; +// the entry points come from kins_single.c +const kins_module_info kins_module = { + .name = "rotarydeltakins", + .halprefix = "rotarydeltakins", + .params = rd_params, + .nparams = sizeof(rd_params)/sizeof(rd_params[0]), + .required_coordinates = "XYZABCUVW", + .max_joints = 9, + .allow_duplicates = 0, + .ntypes = 1, + .ops = { &rd_ops }, +}; int rtapi_app_main(void) { - int retval; - comp_id = hal_init("rotarydeltakins"); if(comp_id < 0) return comp_id; - haldata = hal_malloc(sizeof(*haldata)); - if(!haldata) { retval = -ENOMEM; goto error; } - - if((retval = hal_pin_new_real(comp_id, HAL_IN, &haldata->pfr, RDELTA_PFR, "rotarydeltakins.platformradius")) < 0) - goto error; - if((retval = hal_pin_new_real(comp_id, HAL_IN, &haldata->tl, RDELTA_TL, "rotarydeltakins.thighlength")) < 0) - goto error; - if((retval = hal_pin_new_real(comp_id, HAL_IN, &haldata->sl, RDELTA_SL, "rotarydeltakins.shinlength")) < 0) - goto error; - if((retval = hal_pin_new_real(comp_id, HAL_IN, &haldata->fr, RDELTA_FR, "rotarydeltakins.footradius")) < 0) - goto error; + if (kinsSingleInit(comp_id, "XYZABCUVW", KINEMATICS_BOTH)) { + hal_exit(comp_id); + return -1; + } hal_ready(comp_id); return 0; - -error: - hal_exit(comp_id); - return retval; } void rtapi_app_exit(void) @@ -140,9 +157,4 @@ void rtapi_app_exit(void) hal_exit(comp_id); } -KINS_NOT_SWITCHABLE -EXPORT_SYMBOL(kinematicsType); -EXPORT_SYMBOL(kinematicsForward); -EXPORT_SYMBOL(kinematicsInverse); -EXPORT_SYMBOL(kinematicsJacobian); MODULE_LICENSE("GPL"); diff --git a/src/emc/kinematics/rotarydeltakins.cc b/src/emc/kinematics/rotarydeltakins.cc index 49a26b3153e..a8227573ab9 100644 --- a/src/emc/kinematics/rotarydeltakins.cc +++ b/src/emc/kinematics/rotarydeltakins.cc @@ -20,11 +20,19 @@ #include using namespace boost::python; +// the python module keeps one geometry, set from python +static rotarydelta_geometry geometry; + +static void set_geometry(double pfr, double tl, double sl, double fr) +{ + rotarydelta_set_geometry(&geometry, pfr, tl, sl, fr); +} + static object forward(double j0, double j1, double j2) { double joints[9] = {j0, j1, j2}; EmcPose pos; - int result = kinematics_forward(joints, &pos); + int result = rotarydelta_forward(&geometry, joints, &pos); if(result == 0) return make_tuple(pos.tran.x, pos.tran.y, pos.tran.z); return object(); @@ -34,7 +42,7 @@ static object inverse(double x, double y, double z) { double joints[9]; EmcPose pos = {{x,y,z}, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0}; - int result = kinematics_inverse(&pos, joints); + int result = rotarydelta_inverse(&geometry, &pos, joints); if(result == 0) return make_tuple(joints[0], joints[1], joints[2]); return object(); @@ -42,7 +50,8 @@ static object inverse(double x, double y, double z) static object get_geometry() { - return make_tuple(platformradius, thighlength, shinlength, footradius); + return make_tuple(geometry.platformradius, geometry.thighlength, + geometry.shinlength, geometry.footradius); } #pragma GCC diagnostic push diff --git a/src/emc/kinematics/rotatekins.c b/src/emc/kinematics/rotatekins.c index b5b648b4b38..6fe38d8c11a 100644 --- a/src/emc/kinematics/rotatekins.c +++ b/src/emc/kinematics/rotatekins.c @@ -7,7 +7,7 @@ * Author: Chris Radek * License: GPL Version 2 * System: Linux -* +* * Copyright (c) 2006 All rights reserved. * ********************************************************************/ @@ -17,12 +17,16 @@ #include #include #include /* these decls */ +#include -int kinematicsForward(const double *joints, - EmcPose * pos, - const KINEMATICS_FORWARD_FLAGS * fflags, - KINEMATICS_INVERSE_FLAGS * iflags) +static int rotate_forward(const kins_params *p, kins_scratch *s, + const double *joints, + EmcPose * pos, + const KINEMATICS_FORWARD_FLAGS * fflags, + KINEMATICS_INVERSE_FLAGS * iflags) { + (void)p; + (void)s; (void)fflags; (void)iflags; double c_rad = -joints[5]*M_PI/180; @@ -39,11 +43,14 @@ int kinematicsForward(const double *joints, return 0; } -int kinematicsInverse(const EmcPose * pos, - double *joints, - const KINEMATICS_INVERSE_FLAGS * iflags, - KINEMATICS_FORWARD_FLAGS * fflags) +static int rotate_inverse(const kins_params *p, kins_scratch *s, + const EmcPose * pos, + double *joints, + const KINEMATICS_INVERSE_FLAGS * iflags, + KINEMATICS_FORWARD_FLAGS * fflags) { + (void)p; + (void)s; (void)iflags; (void)fflags; double c_rad = pos->c*M_PI/180; @@ -60,14 +67,15 @@ int kinematicsInverse(const EmcPose * pos, return 0; } -int kinematicsJacobian(const double *joints, - const EmcPose *pos, - double jac[EMCMOT_MAX_JOINTS][EMCMOT_MAX_AXIS], - const KINEMATICS_INVERSE_FLAGS *iflags) +static int rotate_jacobian(const kins_params *p, const double *joints, + const EmcPose *pos, + double jac[EMCMOT_MAX_JOINTS][EMCMOT_MAX_AXIS], + const KINEMATICS_INVERSE_FLAGS *iflags) { double c_rad = pos->c*M_PI/180; double cc = cos(c_rad), sc = sin(c_rad); int j, a; + (void)p; (void)joints; (void)iflags; for (j = 0; j < EMCMOT_MAX_JOINTS; j++) { @@ -83,38 +91,40 @@ int kinematicsJacobian(const double *joints, return 0; } -/* implemented for these kinematics as giving joints preference */ -int kinematicsHome(EmcPose * world, - double *joint, - KINEMATICS_FORWARD_FLAGS * fflags, - KINEMATICS_INVERSE_FLAGS * iflags) -{ - *fflags = 0; - *iflags = 0; +static const kins_ops rotate_ops = { + .forward = rotate_forward, + .inverse = rotate_inverse, + .jacobian = rotate_jacobian, +}; - return kinematicsForward(joint, world, fflags, iflags); -} +// no geometry; joints 0..8 are the nine letters in order, and the entry +// points come from kins_single.c +const kins_module_info kins_module = { + .name = "rotatekins", + .halprefix = "rotatekins", + .params = NULL, + .nparams = 0, + .required_coordinates = "XYZABCUVW", + .max_joints = 9, + .allow_duplicates = 0, + .ntypes = 1, + .ops = { &rotate_ops }, +}; -KINEMATICS_TYPE kinematicsType() -{ - return KINEMATICS_BOTH; -} - -KINS_NOT_SWITCHABLE -EXPORT_SYMBOL(kinematicsType); -EXPORT_SYMBOL(kinematicsForward); -EXPORT_SYMBOL(kinematicsInverse); -EXPORT_SYMBOL(kinematicsJacobian); MODULE_LICENSE("GPL"); int comp_id; int rtapi_app_main(void) { comp_id = hal_init("rotatekins"); - if(comp_id > 0) { - hal_ready(comp_id); - return 0; + if(comp_id < 0) return comp_id; + + if (kinsSingleInit(comp_id, "XYZABCUVW", KINEMATICS_BOTH)) { + hal_exit(comp_id); + return -1; } - return comp_id; + + hal_ready(comp_id); + return 0; } void rtapi_app_exit(void) { hal_exit(comp_id); } diff --git a/src/emc/kinematics/scorbot-kins.c b/src/emc/kinematics/scorbot-kins.c index b7f933a3b71..43e96638389 100644 --- a/src/emc/kinematics/scorbot-kins.c +++ b/src/emc/kinematics/scorbot-kins.c @@ -43,6 +43,7 @@ #include #include #include +#include // @@ -75,12 +76,15 @@ static void compute_j1_cartesian_location(double j0, EmcPose *j1_cart) { // Forward kinematics takes the joint positions and computes the cartesian // coordinates of the controlled point. -int kinematicsForward( +static int scorbot_forward( + const kins_params *p, kins_scratch *s, const double *joints, EmcPose *pose, const KINEMATICS_FORWARD_FLAGS *fflags, KINEMATICS_INVERSE_FLAGS *iflags ) { + (void)p; + (void)s; (void)fflags; (void)iflags; EmcPose j1_vector; // the vector from j0 ("base") to joint 1 ("shoulder", end of link 0) @@ -89,16 +93,13 @@ int kinematicsForward( double r; - // rtapi_print("fwd: j0=%f, j1=%f, j2=%f\n", joints[0], joints[1], joints[2]); compute_j1_cartesian_location(joints[0], &j1_vector); - // rtapi_print("fwd: j1=(%f, %f, %f)\n", j1_vector.tran.x, j1_vector.tran.y, j1_vector.tran.z); // Link 1 connects j1 (shoulder) to j2 (elbow). r = L1_LENGTH * cos(TO_RAD * joints[1]); j2_vector.tran.x = r * cos(TO_RAD * joints[0]); j2_vector.tran.y = r * sin(TO_RAD * joints[0]); j2_vector.tran.z = L1_LENGTH * sin(TO_RAD * joints[1]); - // rtapi_print("fwd: j2=(%f, %f, %f)\n", j2_vector.tran.x, j2_vector.tran.y, j2_vector.tran.z); // Link 2 connects j2 (elbow) to j3 (wrist). // J3 is the controlled point. @@ -106,13 +107,11 @@ int kinematicsForward( j3_vector.tran.x = r * cos(TO_RAD * joints[0]); j3_vector.tran.y = r * sin(TO_RAD * joints[0]); j3_vector.tran.z = L2_LENGTH * sin(TO_RAD * joints[2]); - // rtapi_print("fwd: j3=(%f, %f, %f)\n", j3_vector.tran.x, j3_vector.tran.y, j3_vector.tran.z); // The end-effector location is the sum of the linkage vectors. pose->tran.x = j1_vector.tran.x + j2_vector.tran.x + j3_vector.tran.x; pose->tran.y = j1_vector.tran.y + j2_vector.tran.y + j3_vector.tran.y; pose->tran.z = j1_vector.tran.z + j2_vector.tran.z + j3_vector.tran.z; - // rtapi_print("fwd: pose=(%f, %f, %f)\n", pose->tran.x, pose->tran.y, pose->tran.z); // A and B are wrist roll and pitch, handled in hal by external kinematics pose->a = joints[3]; @@ -134,15 +133,17 @@ int kinematicsForward( // is the horizontal distance (ie, in the XY plane) of the controlled // point from J0. // -int kinematicsInverse( +static int scorbot_inverse( + const kins_params *p, kins_scratch *s, const EmcPose *pose, double *joints, const KINEMATICS_INVERSE_FLAGS *iflags, KINEMATICS_FORWARD_FLAGS *fflags ) { + (void)p; + (void)s; (void)iflags; (void)fflags; - // EmcPose j1_cart; double distance_to_cp, distance_to_center; double r_j1, z_j1; // (r_j1, z_j1) is the location of J1 in the RZ plane double r_cp, z_cp; // (r_cp, z_cp) is the location of the controlled point in the RZ plane @@ -152,16 +153,10 @@ int kinematicsInverse( // the location of J2, this is what we're trying to find double z_j2; - // rtapi_print("inv: x=%f, y=%f, z=%f\n", pose->tran.x, pose->tran.y, pose->tran.z); - // J0 is easy. Project the (X, Y, Z) of the pose onto the Z=0 plane. // J0 points at the projected (X, Y) point. tan(J0) = Y/X // J0 then defines the plane that the rest of the arm operates in. joints[0] = TO_DEG * atan2(pose->tran.y, pose->tran.x); - // rtapi_print("inv: j0=%f\n", joints[0]); - - // compute_j1_cartesian_location(joints[0], &j1_cart); - // rtapi_print("inv: j1=(X=%f, Y=%f, Z=%f)\n", j1_cart.tran.x, j1_cart.tran.y, j1_cart.tran.z); // FIXME: Until i figure the wrist differential out, the controlled // point will be the location of the wrist joint, J3/J4. @@ -175,19 +170,16 @@ int kinematicsInverse( // of J0. This is just a known, static vector. r_j1 = L0_HORIZONTAL_DISTANCE; z_j1 = L0_VERTICAL_DISTANCE; - // rtapi_print("inv: r_j1=%f, z_j1=%f\n", r_j1, z_j1); // (r_cp, z_cp) is the location of J3 (the controlled point), again in // the plane defined by the angle of J0, with the origin of the // machine. r_cp = sqrt(pow(pose->tran.x, 2) + pow(pose->tran.y, 2)); z_cp = pose->tran.z; - // rtapi_print("inv: r_cp=%f, z_cp=%f (controlled point)\n", r_cp, z_cp); // translate so (r_j1, z_j1) is the origin of the coordinate system r_cp -= r_j1; z_cp -= z_j1; - // rtapi_print("inv: r_cp=%f, z_cp=%f (translated controlled point)\n", r_cp, z_cp); // // Now the origin (aka J1), J2, and CP define a triangle in the RZ plane. @@ -206,86 +198,23 @@ int kinematicsInverse( distance_to_cp = sqrt(pow(r_cp, 2) + pow(z_cp, 2)); distance_to_center = distance_to_cp / 2; - // rtapi_print("inv: distance to cp: %f\n", distance_to_cp); // find the angle of the vector from the origin to the CP angle_to_cp = TO_DEG * acos(r_cp / distance_to_cp); if (z_cp < 0) { angle_to_cp *= -1; } - // rtapi_print("inv: angle to cp: %f\n", angle_to_cp); // find the angle (Center, J1, J2) j1_angle = TO_DEG * acos(distance_to_center / L1_LENGTH); - // rtapi_print("inv: j1 angle: %f\n", j1_angle); joints[1] = angle_to_cp + j1_angle; - // rtapi_print("inv: j1: %f\n", joints[1]); // now we can compute the location of J2 z_j2 = L1_LENGTH * sin(TO_RAD * joints[1]); - // rtapi_print("inv: r_j2=%f, z_j2=%f (translated j2)\n", r_j2, z_j2); joints[2] = -1.0 * TO_DEG * asin((z_j2 - z_cp) / L2_LENGTH); - -#if 0 - // Distance between controlled point and the location of j1. These two - // points are separated by link 1, joint 1, and link 2. - distance_between_centers = sqrt(pow((r2 - r1), 2) + pow((z2 - z1), 2)); - - if (distance_between_centers > (L1_LENGTH + L2_LENGTH)) { - // trying to reach too far - return GO_RESULT_RANGE_ERROR; - } - - if (distance_between_centers < fabs(L1_LENGTH - L2_LENGTH)) { - // trying to reach too far into armpit - return GO_RESULT_RANGE_ERROR; - } - - delta = (1.0 / 4.0) * sqrt((distance_between_centers + L1_LENGTH + L2_LENGTH) * (distance_between_centers + L1_LENGTH - L2_LENGTH) * (distance_between_centers - L1_LENGTH + L2_LENGTH) * (L1_LENGTH + L2_LENGTH - distance_between_centers)); - - ir1 = ((r1 + r2) / 2) + (((r2 - r1) * (pow(L1_LENGTH, 2) - pow(L2_LENGTH, 2)))/(2 * pow(distance_between_centers, 2))) + ((2 * (z1 - z2) * delta) / pow(distance_between_centers, 2)); - ir2 = ((r1 + r2) / 2) + (((r2 - r1) * (pow(L1_LENGTH, 2) - pow(L2_LENGTH, 2)))/(2 * pow(distance_between_centers, 2))) - ((2 * (z1 - z2) * delta) / pow(distance_between_centers, 2)); - - iz1 = ((z1 + z2) / 2) + (((z2 - z1) * (pow(L1_LENGTH, 2) - pow(L2_LENGTH, 2)))/(2 * pow(distance_between_centers, 2))) - ((2 * (r1 - r2) * delta) / pow(distance_between_centers, 2)); - iz2 = ((z1 + z2) / 2) + (((z2 - z1) * (pow(L1_LENGTH, 2) - pow(L2_LENGTH, 2)))/(2 * pow(distance_between_centers, 2))) + ((2 * (r1 - r2) * delta) / pow(distance_between_centers, 2)); - - - // (ir1, iz1) is one intersection point, (ir2, iz2) is the other. - // These are the possible locations of the J2 joint. - // FIXME: For now we arbitrarily pick the one with the bigger Z. - - if (iz1 > iz2) { - j2_r = ir1; - j2_z = iz1; - } else { - j2_r = ir2; - j2_z = iz2; - } - // rtapi_print("inv: j2_r=%f, j2_z=%f (J2, intersection point)\n", j2_r, j2_z); - - // Make J1 point at J2 (j2_r, j2_z). - { - double l1_r = j2_r - r1; - joints[1] = TO_DEG * acos(l1_r / L1_LENGTH); - // rtapi_print("inv: l1_r=%f, j1=%f\n", l1_r, joints[1]); - } - - // Make J2 point at the controlled point. - { - double l2_r = r2 - j2_r; - double j2; - j2 = TO_DEG * acos(l2_r / L2_LENGTH); - if (j2_z > pose->tran.z) { - j2 *= -1; - } - joints[2] = j2; - // rtapi_print("inv: l2_r=%f, j2=%f\n", l2_r, joints[2]); - } -#endif - // A and B are wrist roll and pitch, handled in hal by external kinematics joints[3] = pose->a; joints[4] = pose->b; @@ -294,13 +223,14 @@ int kinematicsInverse( } -int kinematicsJacobian( +static int scorbot_jacobian( + const kins_params *p, const double *joints, const EmcPose *pose, double jac[EMCMOT_MAX_JOINTS][EMCMOT_MAX_AXIS], const KINEMATICS_INVERSE_FLAGS *iflags ) { - // kinematicsInverse() above, differentiated step by step in the same + // scorbot_inverse() above, differentiated step by step in the same // order, each quantity carried as its gradient over (x, y, z) const double x = pose->tran.x, y = pose->tran.y; const double rho2 = x*x + y*y; @@ -310,6 +240,7 @@ int kinematicsJacobian( double q; int i, j, a; + (void)p; (void)joints; (void)iflags; if (rho2 <= 0) { return -1; } @@ -364,15 +295,26 @@ int kinematicsJacobian( return 0; } -KINEMATICS_TYPE kinematicsType(void) { - return KINEMATICS_BOTH; -} +static const kins_ops scorbot_ops = { + .forward = scorbot_forward, + .inverse = scorbot_inverse, + .jacobian = scorbot_jacobian, +}; + +// the arm's dimensions are the constants above; no geometry pins. The +// entry points come from kins_single.c +const kins_module_info kins_module = { + .name = "scorbot-kins", + .halprefix = "scorbot-kins", + .params = NULL, + .nparams = 0, + .required_coordinates = "XYZAB", + .max_joints = 5, + .allow_duplicates = 0, + .ntypes = 1, + .ops = { &scorbot_ops }, +}; -KINS_NOT_SWITCHABLE -EXPORT_SYMBOL(kinematicsType); -EXPORT_SYMBOL(kinematicsForward); -EXPORT_SYMBOL(kinematicsInverse); -EXPORT_SYMBOL(kinematicsJacobian); MODULE_LICENSE("GPL"); static int comp_id; @@ -382,6 +324,10 @@ int rtapi_app_main(void) { if (comp_id < 0) { return comp_id; } + if (kinsSingleInit(comp_id, "XYZAB", KINEMATICS_BOTH)) { + hal_exit(comp_id); + return -1; + } hal_ready(comp_id); return 0; } @@ -389,4 +335,3 @@ int rtapi_app_main(void) { void rtapi_app_exit(void) { hal_exit(comp_id); } - diff --git a/src/emc/kinematics/tripodkins.c b/src/emc/kinematics/tripodkins.c index c58d726dd46..6f373cae2c8 100644 --- a/src/emc/kinematics/tripodkins.c +++ b/src/emc/kinematics/tripodkins.c @@ -4,10 +4,10 @@ * * Derived from a work by Fred Proctor * -* Author: +* Author: * License: GPL Version 2 * System: Linux -* +* * Copyright (c) 2004 All rights reserved. * * Last change: @@ -67,19 +67,15 @@ #include #include #include /* these decls */ +#include -/* ident tag */ -#ifndef __GNUC__ -#ifndef __attribute__ -#define __attribute__(x) -#endif -#endif - -static struct haldata { - hal_real_t bx; - hal_real_t cx; - hal_real_t cy; -} *haldata = NULL; +// the base geometry, one pin each, poked from HAL as before +static const kins_param_desc tripod_params[] = { + { "Bx", KINS_PARAM_FLOAT, KINS_IO, 0, 1.0 }, + { "Cx", KINS_PARAM_FLOAT, KINS_IO, 0, 1.0 }, + { "Cy", KINS_PARAM_FLOAT, KINS_IO, 0, 1.0 }, +}; +enum { P_BX, P_CX, P_CY }; #define sq(x) ((x)*(x)) @@ -123,11 +119,13 @@ static struct haldata { solutions. Positive means the tripod is above the xy plane, negative means below. */ -int kinematicsForward(const double * joints, - EmcPose * pos, - const KINEMATICS_FORWARD_FLAGS * fflags, - KINEMATICS_INVERSE_FLAGS * iflags) +static int tripod_forward(const kins_params *p, kins_scratch *s_, + const double * joints, + EmcPose * pos, + const KINEMATICS_FORWARD_FLAGS * fflags, + KINEMATICS_INVERSE_FLAGS * iflags) { + (void)s_; (void)iflags; #define AD (joints[0]) #define BD (joints[1]) @@ -137,9 +135,9 @@ int kinematicsForward(const double * joints, #define Dz (pos->tran.z) double P, Q, R; double s, t, u; - rtapi_real Bx = hal_get_real(haldata->bx); - rtapi_real Cx = hal_get_real(haldata->cx); - rtapi_real Cy = hal_get_real(haldata->cy); + const double Bx = p->geometry[P_BX]; + const double Cx = p->geometry[P_CX]; + const double Cy = p->geometry[P_CY]; P = sq(AD); Q = sq(BD) - sq(Bx); @@ -183,11 +181,13 @@ int kinematicsForward(const double * joints, #undef Dz } -int kinematicsInverse(const EmcPose * pos, - double * joints, - const KINEMATICS_INVERSE_FLAGS * iflags, - KINEMATICS_FORWARD_FLAGS * fflags) +static int tripod_inverse(const kins_params *p, kins_scratch *s, + const EmcPose * pos, + double * joints, + const KINEMATICS_INVERSE_FLAGS * iflags, + KINEMATICS_FORWARD_FLAGS * fflags) { + (void)s; (void)iflags; #define AD (joints[0]) #define BD (joints[1]) @@ -195,9 +195,9 @@ int kinematicsInverse(const EmcPose * pos, #define Dx (pos->tran.x) #define Dy (pos->tran.y) #define Dz (pos->tran.z) - rtapi_real Bx = hal_get_real(haldata->bx); - rtapi_real Cx = hal_get_real(haldata->cx); - rtapi_real Cy = hal_get_real(haldata->cy); + const double Bx = p->geometry[P_BX]; + const double Cx = p->geometry[P_CX]; + const double Cy = p->geometry[P_CY]; AD = sqrt(sq(Dx) + sq(Dy) + sq(Dz)); BD = sqrt(sq(Dx - Bx) + sq(Dy) + sq(Dz)); @@ -218,14 +218,14 @@ int kinematicsInverse(const EmcPose * pos, #undef Dz } -int kinematicsJacobian(const double * joints, - const EmcPose * pos, - double jac[EMCMOT_MAX_JOINTS][EMCMOT_MAX_AXIS], - const KINEMATICS_INVERSE_FLAGS * iflags) +static int tripod_jacobian(const kins_params *p, const double * joints, + const EmcPose * pos, + double jac[EMCMOT_MAX_JOINTS][EMCMOT_MAX_AXIS], + const KINEMATICS_INVERSE_FLAGS * iflags) { - rtapi_real Bx = hal_get_real(haldata->bx); - rtapi_real Cx = hal_get_real(haldata->cx); - rtapi_real Cy = hal_get_real(haldata->cy); + const double Bx = p->geometry[P_BX]; + const double Cx = p->geometry[P_CX]; + const double Cy = p->geometry[P_CY]; /* the three strut base points, in the order of the joints */ const double base[3][2] = { {0, 0}, {Bx, 0}, {Cx, Cy} }; int i, j, a; @@ -249,170 +249,40 @@ int kinematicsJacobian(const double * joints, return 0; } -KINEMATICS_TYPE kinematicsType() -{ - return KINEMATICS_BOTH; -} - -#ifdef MAIN - -#include -#include - -/* - Interactive testing of kins. - - Syntax: a.out -*/ -int main(int argc, char *argv[]) -{ -#ifndef BUFFERLEN -#define BUFFERLEN 256 -#endif - char buffer[BUFFERLEN]; - char cmd[BUFFERLEN]; - EmcPose pos, vel; - double joints[3]={0.0,0.0,0.0}, jointvels[3]={0.0,0.0,0.0}; - char inverse; - char flags; - KINEMATICS_FORWARD_FLAGS fflags; - - inverse = 0; /* forwards, by default */ - flags = 0; /* didn't provide flags */ - fflags = 0; /* above xy plane, by default */ - if (argc != 4 || - 1 != sscanf(argv[1], "%lf", &Bx) || - 1 != sscanf(argv[2], "%lf", &Cx) || - 1 != sscanf(argv[3], "%lf", &Cy)) { - fprintf(stderr, "syntax: %s Bx Cx Cy\n", argv[0]); - return 1; - } - - while (! feof(stdin)) { - if (inverse) { - printf("inv> "); - } - else { - printf("fwd> "); - } - fflush(stdout); - - if (NULL == fgets(buffer, BUFFERLEN, stdin)) { - break; - } - if (1 != sscanf(buffer, "%255s", cmd)) { - continue; - } - - if (! strcmp(cmd, "quit")) { - break; - } - if (! strcmp(cmd, "i")) { - inverse = 1; - continue; - } - if (! strcmp(cmd, "f")) { - inverse = 0; - continue; - } - if (! strcmp(cmd, "ff")) { - if (1 != sscanf(buffer, "%*s %lu", &fflags)) { - printf("need forward flag\n"); - } - continue; - } - - if (inverse) { /* inverse kins */ - if (3 != sscanf(buffer, "%lf %lf %lf", - &pos.tran.x, - &pos.tran.y, - &pos.tran.z)) { - printf("need X Y Z\n"); - continue; - } - if (0 != kinematicsInverse(&pos, joints, NULL, &fflags)) { - printf("inverse kin error\n"); - } - else { - printf("%f\t%f\t%f\n", joints[0], joints[1], joints[2]); - if (0 != kinematicsForward(joints, &pos, &fflags, NULL)) { - printf("forward kin error\n"); - } - else { - printf("%f\t%f\t%f\n", pos.tran.x, pos.tran.y, pos.tran.z); - } - } - } - else { /* forward kins */ - if (flags) { - if (4 != sscanf(buffer, "%lf %lf %lf %lu", - &joints[0], - &joints[1], - &joints[2], - &fflags)) { - printf("need 3 strut values and flag\n"); - continue; - } - } - else { - if (3 != sscanf(buffer, "%lf %lf %lf", - &joints[0], - &joints[1], - &joints[2])) { - printf("need 3 strut values\n"); - continue; - } - } - if (0 != kinematicsForward(joints, &pos, &fflags, NULL)) { - printf("forward kin error\n"); - } - else { - printf("%f\t%f\t%f\n", pos.tran.x, pos.tran.y, pos.tran.z); - if (0 != kinematicsInverse(&pos, joints, NULL, &fflags)) { - printf("inverse kin error\n"); - } - else { - printf("%f\t%f\t%f\n", joints[0], joints[1], joints[2]); - } - } - } - } /* end while (! feof(stdin)) */ - - return 0; -} - -#endif /* MAIN */ - -KINS_NOT_SWITCHABLE -EXPORT_SYMBOL(kinematicsType); -EXPORT_SYMBOL(kinematicsForward); -EXPORT_SYMBOL(kinematicsInverse); -EXPORT_SYMBOL(kinematicsJacobian); +static const kins_ops tripod_ops = { + .forward = tripod_forward, + .inverse = tripod_inverse, + .jacobian = tripod_jacobian, +}; + +// three struts for three coordinates; the entry points come from +// kins_single.c +const kins_module_info kins_module = { + .name = "tripodkins", + .halprefix = "tripodkins", + .params = tripod_params, + .nparams = sizeof(tripod_params)/sizeof(tripod_params[0]), + .required_coordinates = "XYZ", + .max_joints = 3, + .allow_duplicates = 0, + .ntypes = 1, + .ops = { &tripod_ops }, +}; MODULE_LICENSE("GPL"); - - static int comp_id; int rtapi_app_main(void) { - int res = 0; - comp_id = hal_init("tripodkins"); if(comp_id < 0) return comp_id; - haldata = hal_malloc(sizeof(struct haldata)); - if(!haldata) goto error; - - if((res = hal_pin_new_real(comp_id, HAL_IO, &(haldata->bx), 1.0, "tripodkins.Bx")) < 0) goto error; - if((res = hal_pin_new_real(comp_id, HAL_IO, &(haldata->cx), 1.0, "tripodkins.Cx")) < 0) goto error; - if((res = hal_pin_new_real(comp_id, HAL_IO, &(haldata->cy), 1.0, "tripodkins.Cy")) < 0) goto error; + if (kinsSingleInit(comp_id, "XYZ", KINEMATICS_BOTH)) { + hal_exit(comp_id); + return -1; + } hal_ready(comp_id); return 0; - -error: - hal_exit(comp_id); - return res; } void rtapi_app_exit(void) { hal_exit(comp_id); } diff --git a/src/hal/components/Submakefile b/src/hal/components/Submakefile index 8ad4ee1740e..865b70ece96 100644 --- a/src/hal/components/Submakefile +++ b/src/hal/components/Submakefile @@ -98,6 +98,7 @@ obj-m += $(patsubst hal/drivers/%.comp, %.o, $(patsubst hal/components/%.comp, % # -extra-objs. The list is expanded when the .mak is written, # so it has to be defined in this file (which the .mak depends on). SWITCHKINS_OBJS := emc/kinematics/switchkins.o emc/kinematics/kins_util.o +matrixkins-extra-objs := emc/kinematics/kins_util.o emc/kinematics/kins_single.o millturn-extra-objs := $(SWITCHKINS_OBJS) xyzab_tdr_kins-extra-objs := $(SWITCHKINS_OBJS) xyzacb_trsrn-extra-objs := $(SWITCHKINS_OBJS) diff --git a/src/hal/components/matrixkins.comp b/src/hal/components/matrixkins.comp index 8bf76899c8e..f3b1834bbb1 100644 --- a/src/hal/components/matrixkins.comp +++ b/src/hal/components/matrixkins.comp @@ -40,7 +40,7 @@ mechanical issues, including: 3. Parallelism between spindle rotational axis and Z movement. 4. Perpendicularity between spindle rotational axis and X/Y movement. -The matrix coefficients are set by parameters C_xx .. C_zz. +The matrix coefficients are set by the pins C_xx .. C_zz. For 3 axis machine, the equations become: .... @@ -152,7 +152,7 @@ Specify matrixkins in LinuxCNC INI file as: KINEMATICS=matrixkins ---- -In your HAL configuration file, set the parameters C_xx .. C_zz: +In your HAL configuration file, set the pins C_xx .. C_zz: [source,hal] ---- @@ -167,7 +167,7 @@ setp matrixkins.C_zy 0 # Skew Y axis towards Z axis setp matrixkins.C_zz 1 # Z axis scale ---- -The parameters can be modified during runtime using halcmd. +The pins can be modified during runtime using halcmd. To avoid sudden movements, it is better to turn off machine power before changes. If recalibration is performed with already existing calibration being in effect, @@ -180,68 +180,30 @@ option extra_setup; license "GPL"; ;; -static struct haldata { - hal_real_t C_xx; - hal_real_t C_xy; - hal_real_t C_xz; - hal_real_t C_yx; - hal_real_t C_yy; - hal_real_t C_yz; - hal_real_t C_zx; - hal_real_t C_zy; - hal_real_t C_zz; -} *haldata; - -EXTRA_SETUP() { - (void)__comp_inst; - (void)prefix; - (void)extra_arg; - int res=0; - - // inherit comp_id from rtapi_main() - if (comp_id < 0) goto error; - - haldata = hal_malloc(sizeof(struct haldata)); - if (!haldata) goto error; - - res |= hal_param_new_real(comp_id, HAL_RW, &haldata->C_xx, 1.0, "matrixkins.C_xx"); - res |= hal_param_new_real(comp_id, HAL_RW, &haldata->C_xy, 0.0, "matrixkins.C_xy"); - res |= hal_param_new_real(comp_id, HAL_RW, &haldata->C_xz, 0.0, "matrixkins.C_xz"); - res |= hal_param_new_real(comp_id, HAL_RW, &haldata->C_yx, 0.0, "matrixkins.C_yx"); - res |= hal_param_new_real(comp_id, HAL_RW, &haldata->C_yy, 1.0, "matrixkins.C_yy"); - res |= hal_param_new_real(comp_id, HAL_RW, &haldata->C_yz, 0.0, "matrixkins.C_yz"); - res |= hal_param_new_real(comp_id, HAL_RW, &haldata->C_zx, 0.0, "matrixkins.C_zx"); - res |= hal_param_new_real(comp_id, HAL_RW, &haldata->C_zy, 0.0, "matrixkins.C_zy"); - res |= hal_param_new_real(comp_id, HAL_RW, &haldata->C_zz, 1.0, "matrixkins.C_zz"); - - if (res) goto error; - - rtapi_print("*** %s setup ok\n",__FILE__); - return 0; -error: - rtapi_print("\n!!! %s setup failed res=%d\n\n",__FILE__,res); - return -1; -} - #include -#include - -KINS_NOT_SWITCHABLE -EXPORT_SYMBOL(kinematicsType); -EXPORT_SYMBOL(kinematicsInverse); -EXPORT_SYMBOL(kinematicsJacobian); -EXPORT_SYMBOL(kinematicsForward); - -KINEMATICS_TYPE kinematicsType() -{ - return KINEMATICS_BOTH; -} - -int kinematicsForward(const double *j, - EmcPose * pos, - const KINEMATICS_FORWARD_FLAGS * fflags, - KINEMATICS_INVERSE_FLAGS * iflags) +#include + +// the calibration matrix, one pin each; the maths reads it from the block +static const kins_param_desc matrix_params[] = { + { "C_xx", KINS_PARAM_FLOAT, KINS_IN, 0, 1.0 }, + { "C_xy", KINS_PARAM_FLOAT, KINS_IN, 0, 0.0 }, + { "C_xz", KINS_PARAM_FLOAT, KINS_IN, 0, 0.0 }, + { "C_yx", KINS_PARAM_FLOAT, KINS_IN, 0, 0.0 }, + { "C_yy", KINS_PARAM_FLOAT, KINS_IN, 0, 1.0 }, + { "C_yz", KINS_PARAM_FLOAT, KINS_IN, 0, 0.0 }, + { "C_zx", KINS_PARAM_FLOAT, KINS_IN, 0, 0.0 }, + { "C_zy", KINS_PARAM_FLOAT, KINS_IN, 0, 0.0 }, + { "C_zz", KINS_PARAM_FLOAT, KINS_IN, 0, 1.0 }, +}; +enum { C_XX, C_XY, C_XZ, C_YX, C_YY, C_YZ, C_ZX, C_ZY, C_ZZ }; + +static int matrix_forward(const kins_params *p, kins_scratch *s, + const double *j, + EmcPose * pos, + const KINEMATICS_FORWARD_FLAGS * fflags, + KINEMATICS_INVERSE_FLAGS * iflags) { + (void)s; (void)fflags; (void)iflags; // For forward kinematics (joint to axis position) we @@ -251,20 +213,20 @@ int kinematicsForward(const double *j, // https://ardoris.wordpress.com/2008/07/18/general-formula-for-the-inverse-of-a-3x3-matrix/ // https://en.wikipedia.org/wiki/Invertible_matrix#Inversion_of_3_%C3%97_3_matrices - rtapi_real a = hal_get_real(haldata->C_xx); - rtapi_real b = hal_get_real(haldata->C_xy); - rtapi_real c = hal_get_real(haldata->C_xz); - rtapi_real d = hal_get_real(haldata->C_yx); - rtapi_real e = hal_get_real(haldata->C_yy); - rtapi_real f = hal_get_real(haldata->C_yz); - rtapi_real g = hal_get_real(haldata->C_zx); - rtapi_real h = hal_get_real(haldata->C_zy); - rtapi_real i = hal_get_real(haldata->C_zz); - - rtapi_real det = a * (e * i - f * h) - - b * (d * i - f * g) - + c * (d * h - e * g); - rtapi_real invdet = 1.0 / det; + const double a = p->geometry[C_XX]; + const double b = p->geometry[C_XY]; + const double c = p->geometry[C_XZ]; + const double d = p->geometry[C_YX]; + const double e = p->geometry[C_YY]; + const double f = p->geometry[C_YZ]; + const double g = p->geometry[C_ZX]; + const double h = p->geometry[C_ZY]; + const double i = p->geometry[C_ZZ]; + + const double det = a * (e * i - f * h) + - b * (d * i - f * g) + + c * (d * h - e * g); + const double invdet = 1.0 / det; // Apply inverse matrix transform to the 3 cartesian coordinates pos->tran.x = invdet * ( (e * i - f * h) * j[0] @@ -290,22 +252,24 @@ int kinematicsForward(const double *j, return 0; } -int kinematicsInverse(const EmcPose * pos, - double *j, - const KINEMATICS_INVERSE_FLAGS * iflags, - KINEMATICS_FORWARD_FLAGS * fflags) +static int matrix_inverse(const kins_params *p, kins_scratch *s, + const EmcPose * pos, + double *j, + const KINEMATICS_INVERSE_FLAGS * iflags, + KINEMATICS_FORWARD_FLAGS * fflags) { + (void)s; (void)iflags; (void)fflags; - rtapi_real a = hal_get_real(haldata->C_xx); - rtapi_real b = hal_get_real(haldata->C_xy); - rtapi_real c = hal_get_real(haldata->C_xz); - rtapi_real d = hal_get_real(haldata->C_yx); - rtapi_real e = hal_get_real(haldata->C_yy); - rtapi_real f = hal_get_real(haldata->C_yz); - rtapi_real g = hal_get_real(haldata->C_zx); - rtapi_real h = hal_get_real(haldata->C_zy); - rtapi_real i = hal_get_real(haldata->C_zz); + const double a = p->geometry[C_XX]; + const double b = p->geometry[C_XY]; + const double c = p->geometry[C_XZ]; + const double d = p->geometry[C_YX]; + const double e = p->geometry[C_YY]; + const double f = p->geometry[C_YZ]; + const double g = p->geometry[C_ZX]; + const double h = p->geometry[C_ZY]; + const double i = p->geometry[C_ZZ]; // Apply matrix transform to the 3 cartesian coordinates j[0] = pos->tran.x * a + pos->tran.y * b + pos->tran.z * c; @@ -323,10 +287,10 @@ int kinematicsInverse(const EmcPose * pos, return 0; } -int kinematicsJacobian(const double *j, - const EmcPose * pos, - double jac[EMCMOT_MAX_JOINTS][EMCMOT_MAX_AXIS], - const KINEMATICS_INVERSE_FLAGS * iflags) +static int matrix_jacobian(const kins_params *p, const double *j, + const EmcPose * pos, + double jac[EMCMOT_MAX_JOINTS][EMCMOT_MAX_AXIS], + const KINEMATICS_INVERSE_FLAGS * iflags) { int r, c; (void)j; @@ -337,15 +301,41 @@ int kinematicsJacobian(const double *j, } // the inverse is the calibration matrix itself, so its derivative is // that matrix, and the pass-through axes are ones - jac[0][0] = hal_get_real(haldata->C_xx); - jac[0][1] = hal_get_real(haldata->C_xy); - jac[0][2] = hal_get_real(haldata->C_xz); - jac[1][0] = hal_get_real(haldata->C_yx); - jac[1][1] = hal_get_real(haldata->C_yy); - jac[1][2] = hal_get_real(haldata->C_yz); - jac[2][0] = hal_get_real(haldata->C_zx); - jac[2][1] = hal_get_real(haldata->C_zy); - jac[2][2] = hal_get_real(haldata->C_zz); + jac[0][0] = p->geometry[C_XX]; + jac[0][1] = p->geometry[C_XY]; + jac[0][2] = p->geometry[C_XZ]; + jac[1][0] = p->geometry[C_YX]; + jac[1][1] = p->geometry[C_YY]; + jac[1][2] = p->geometry[C_YZ]; + jac[2][0] = p->geometry[C_ZX]; + jac[2][1] = p->geometry[C_ZY]; + jac[2][2] = p->geometry[C_ZZ]; for (r = 3; r < 9; r++) { jac[r][r] = 1; } return 0; } + +static const kins_ops matrix_ops = { + .forward = matrix_forward, + .inverse = matrix_inverse, + .jacobian = matrix_jacobian, +}; + +// the entry points come from kins_single.c, linked in +const kins_module_info kins_module = { + .name = "matrixkins", + .halprefix = "matrixkins", + .params = matrix_params, + .nparams = sizeof(matrix_params)/sizeof(matrix_params[0]), + .required_coordinates = "XYZABCUVW", + .max_joints = 9, + .allow_duplicates = 0, + .ntypes = 1, + .ops = { &matrix_ops }, +}; + +// halcompile has done hal_init() and does hal_ready() after this returns, +// which is what kinsSingleInit() expects +EXTRA_SETUP() { + (void)__comp_inst; (void)prefix; (void)extra_arg; + return kinsSingleInit(comp_id, "XYZABCUVW", KINEMATICS_BOTH); +} diff --git a/src/hal/components/userkins.comp b/src/hal/components/userkins.comp index ac0c003369d..f382b0f0544 100644 --- a/src/hal/components/userkins.comp +++ b/src/hal/components/userkins.comp @@ -16,9 +16,8 @@ where '2.8' is the branch name (use 'master' for the master branch). For a RIP (run-in-place) build, the file is located in the git tree as: `src/hal/components/userkins.comp`. -Edit the functions kinematicsForward() and kinematicsInverse() as required. - -If required, add HAL pins following examples in the template code. +Edit the functions userkins_forward() and userkins_inverse() as required, +and list the geometry the maths needs in the *userkins_params* table. Build and install the component using halcompile: @@ -50,16 +49,18 @@ change all instances of `userkins` to `mykins`. === NOTES +* The kinematics are written as functions of a parameter block, see + kinematics.h: the geometry is declared once in the *userkins_params* + table, one HAL pin is made per entry, and the maths reads + *p->geometry[]* where it would have read a pin. The classic entry + points (kinematicsForward() and the rest) are supplied by kins_single.c, + included below, so nothing here touches HAL and the same maths can be + evaluated outside realtime. * The *fpin* pin is included to satisfy the requirements of the halcompile utility but it is not accessible to kinematics functions. -* HAL pins and parameters needed in kinematics functions (kinematicsForward(), - kinematicsInverse()) must be setup in the *EXTRA_SETUP()* function, which - halcompile runs once when the module is loaded, before the component is - made ready. """; // The fpin pin is not accessible in kinematics functions. -// Use EXTRA_SETUP() for pins and params used by kinematics. pin out si32 fpin=0"pin to demonstrate use of a conventional (non-kinematics) function fdemo"; option period no; option extra_setup; @@ -69,20 +70,22 @@ author "Dewey Garrett"; ;; #include - -static struct haldata { - // Example pin pointers - hal_uint_t in; - hal_uint_t out; - // Example parameters - hal_real_t param_rw; - hal_real_t param_ro; -} *haldata; -// hal pin/param types: -// hal_bool_t boolean bit -// hal_uint_t unsigned integer -// hal_sint_t signed integer -// hal_real_t floating point (double precision) +#include + +// the shared code for a module with one kinematics type, compiled in so +// that halcompile builds this file on its own +#include +#include + +// The geometry, one HAL pin per entry, named userkins.. An entry +// is an input (read into p->geometry[] before every call), an output +// (written from s->out[] after it), or an input that can be poked +// (KINS_IO). The example pair below echoes 'in' to 'out'. +static const kins_param_desc userkins_params[] = { + { "in", KINS_PARAM_U32, KINS_IN, 0, 0 }, + { "out", KINS_PARAM_U32, KINS_OUT, 0, 0 }, +}; +enum { P_IN, P_OUT }; FUNCTION(fdemo) { // This function can be added to a thread (addf) for @@ -93,61 +96,16 @@ FUNCTION(fdemo) { fpin_set(fpin + 1); } -EXTRA_SETUP() { - (void)__comp_inst; - (void)prefix; - (void)extra_arg; -#define HAL_PREFIX "userkins" - int res=0; - - // inherit comp_id from rtapi_main() - if (comp_id < 0) goto error; - - haldata = hal_malloc(sizeof(struct haldata)); - if (!haldata) goto error; - - // hal pin examples: - res += hal_pin_new_ui32(comp_id, HAL_IN , &(haldata->in) , 0, "%s.in" , HAL_PREFIX); - res += hal_pin_new_ui32(comp_id, HAL_OUT, &(haldata->out), 0, "%s.out", HAL_PREFIX); - - // hal parameter examples: - res += hal_param_new_real(comp_id, HAL_RW, &haldata->param_rw, 0.0, "%s.param-rw", HAL_PREFIX); - res += hal_param_new_real(comp_id, HAL_RO, &haldata->param_ro, 0.0, "%s.param-ro", HAL_PREFIX); - - if (res) goto error; - rtapi_print("*** %s setup ok\n",__FILE__); - return 0; -error: - rtapi_print("\n!!! %s setup failed res=%d\n\n",__FILE__,res); - return -1; -#undef HAL_PREFIX -} - -KINS_NOT_SWITCHABLE -// see millturn.comp for example of switchable kinematics - -EXPORT_SYMBOL(kinematicsType); -EXPORT_SYMBOL(kinematicsInverse); -EXPORT_SYMBOL(kinematicsJacobian); -EXPORT_SYMBOL(kinematicsForward); - -KINEMATICS_TYPE kinematicsType() -{ - return KINEMATICS_IDENTITY; // set as required - // Note: If kinematics are identity, using KINEMATICS_BOTH - // may be used in order to allow a gui to display - // joint values in preview prior to homing -} // kinematicsType() - -static bool is_ready=0; -int kinematicsForward(const double *j, - EmcPose * pos, - const KINEMATICS_FORWARD_FLAGS * fflags, - KINEMATICS_INVERSE_FLAGS * iflags) +static int userkins_forward(const kins_params *p, kins_scratch *s, + const double *j, + EmcPose * pos, + const KINEMATICS_FORWARD_FLAGS * fflags, + KINEMATICS_INVERSE_FLAGS * iflags) { + (void)p; + (void)s; (void)fflags; (void)iflags; - static bool gave_msg; // [KINS]JOINTS=3 pos->tran.x = j[0]; // X coordinate pos->tran.y = j[1]; // Y coordinate @@ -160,23 +118,17 @@ int kinematicsForward(const double *j, pos->v = 0; pos->w = 0; - if (hal_get_ui32(haldata->in) && !is_ready && !gave_msg) { - rtapi_print_msg(RTAPI_MSG_ERR, - "%s The 'in' pin not echoed until Inverse called\n", - __FILE__); - gave_msg=1; - } return 0; -} // kinematicsForward() +} // userkins_forward() -int kinematicsInverse(const EmcPose * pos, - double *j, - const KINEMATICS_INVERSE_FLAGS * iflags, - KINEMATICS_FORWARD_FLAGS * fflags) +static int userkins_inverse(const kins_params *p, kins_scratch *s, + const EmcPose * pos, + double *j, + const KINEMATICS_INVERSE_FLAGS * iflags, + KINEMATICS_FORWARD_FLAGS * fflags) { (void)iflags; (void)fflags; - is_ready = 1; // Inverse is not called until homed for KINEMATICS_BOTH // Update the kinematic joints specified by the // [KINS]JOINTS setting (3 required for this template). @@ -189,25 +141,26 @@ int kinematicsInverse(const EmcPose * pos, j[1] = pos->tran.y; // joint 1 j[2] = pos->tran.z; // joint 2 - //example hal pin update (homing reqd before kinematicsInverse) - hal_set_ui32(haldata->out, hal_get_ui32(haldata->in)); //dereference - //read from param example: hal_set_ui32(haldata->out, hal_get_real(haldata->param_rw)); + // example output: echo the 'in' pin to the 'out' pin + s->out[P_OUT] = p->geometry[P_IN]; return 0; -} // kinematicsInverse() +} // userkins_inverse() -int kinematicsJacobian(const double *j, - const EmcPose * pos, - double jac[EMCMOT_MAX_JOINTS][EMCMOT_MAX_AXIS], - const KINEMATICS_INVERSE_FLAGS * iflags) +static int userkins_jacobian(const kins_params *p, const double *j, + const EmcPose * pos, + double jac[EMCMOT_MAX_JOINTS][EMCMOT_MAX_AXIS], + const KINEMATICS_INVERSE_FLAGS * iflags) { int r, c; + (void)p; (void)j; (void)pos; (void)iflags; // How each joint responds to each pose coordinate, the derivative of - // kinematicsInverse(): for this template joint 0 follows x, joint 1 + // userkins_inverse(): for this template joint 0 follows x, joint 1 // follows y and joint 2 follows z, each one for one. See kinematics.h. + // Leave .jacobian out of the ops below to have it differenced instead. for (r = 0; r < EMCMOT_MAX_JOINTS; r++) { for (c = 0; c < EMCMOT_MAX_AXIS; c++) { jac[r][c] = 0; } } @@ -215,4 +168,33 @@ int kinematicsJacobian(const double *j, jac[1][1] = 1; jac[2][2] = 1; return 0; -} // kinematicsJacobian() +} // userkins_jacobian() + +static const kins_ops userkins_ops = { + .forward = userkins_forward, + .inverse = userkins_inverse, + .jacobian = userkins_jacobian, + // .work, .tool and .native report the frames, see kinematics.h +}; + +const kins_module_info kins_module = { + .name = "userkins", + .halprefix = "userkins", + .params = userkins_params, + .nparams = sizeof(userkins_params)/sizeof(userkins_params[0]), + .required_coordinates = "XYZ", + .max_joints = 3, + .allow_duplicates = 0, + .ntypes = 1, + .ops = { &userkins_ops }, +}; + +// halcompile has done hal_init() and does hal_ready() after this returns, +// which is what kinsSingleInit() expects. KINEMATICS_IDENTITY is what +// kinematicsType() reports; use KINEMATICS_BOTH for a machine whose +// joints are not the axes, or to let a gui display joint values in the +// preview before homing. +EXTRA_SETUP() { + (void)__comp_inst; (void)prefix; (void)extra_arg; + return kinsSingleInit(comp_id, "XYZ", KINEMATICS_IDENTITY); +} From 7df4d68b352311706165e8e33899fbba0a808bc6 Mon Sep 17 00:00:00 2001 From: Luca Toniolo <10792599+grandixximo@users.noreply.github.com> Date: Fri, 4 Sep 2026 13:00:55 +1000 Subject: [PATCH 31/60] scarakins, pumakins, three21kins: move onto the parameter block 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. --- src/emc/kinematics/pumakins.c | 151 ++++++++++++------------------ src/emc/kinematics/scarakins.c | 155 +++++++++++++------------------ src/emc/kinematics/three21kins.c | 129 +++++++++++-------------- 3 files changed, 183 insertions(+), 252 deletions(-) diff --git a/src/emc/kinematics/pumakins.c b/src/emc/kinematics/pumakins.c index 53aca15c6f7..09dfb8193ab 100644 --- a/src/emc/kinematics/pumakins.c +++ b/src/emc/kinematics/pumakins.c @@ -24,9 +24,15 @@ #include "pumakins.h" #include -struct haldata { - hal_real_t a2, a3, d3, d4, d6; -} *haldata = NULL; +// the five dimensions, one pin each; the maths reads them from the block +static const kins_param_desc puma_params[] = { + { "A2", KINS_PARAM_FLOAT, KINS_IN, 0, DEFAULT_PUMA560_A2 }, + { "A3", KINS_PARAM_FLOAT, KINS_IN, 0, DEFAULT_PUMA560_A3 }, + { "D3", KINS_PARAM_FLOAT, KINS_IN, 0, DEFAULT_PUMA560_D3 }, + { "D4", KINS_PARAM_FLOAT, KINS_IN, 0, DEFAULT_PUMA560_D4 }, + { "D6", KINS_PARAM_FLOAT, KINS_IN, 0, DEFAULT_PUMA560_D6 }, +}; +enum { P_A2, P_A3, P_D3, P_D4, P_D6 }; /* the difference of two angles, brought into (-pi, pi] so that a joint a whole turn from the formula still matches it */ @@ -107,11 +113,13 @@ static void pumaFlangeRotation(const double * joint, PmRotationMatrix * rot) *rot = hom.rot; } // pumaFlangeRotation() -static int pumaKinematicsForward(const double * joint, - EmcPose * world, - const KINEMATICS_FORWARD_FLAGS * fflags, - KINEMATICS_INVERSE_FLAGS * iflags) +static int puma_forward(const kins_params *p, kins_scratch *s, + const double * joint, + EmcPose * world, + const KINEMATICS_FORWARD_FLAGS * fflags, + KINEMATICS_INVERSE_FLAGS * iflags) { + (void)s; (void)fflags; double s1, s2, s3; double c1, c2, c3; @@ -135,10 +143,10 @@ static int pumaKinematicsForward(const double * joint, s23 = c2 * s3 + s2 * c3; c23 = c2 * c3 - s2 * s3; - rtapi_real PUMA_A2 = hal_get_real(haldata->a2); - rtapi_real PUMA_A3 = hal_get_real(haldata->a3); - rtapi_real PUMA_D3 = hal_get_real(haldata->d3); - rtapi_real PUMA_D4 = hal_get_real(haldata->d4); + const double PUMA_A2 = p->geometry[P_A2]; + const double PUMA_A3 = p->geometry[P_A3]; + const double PUMA_D3 = p->geometry[P_D3]; + const double PUMA_D4 = p->geometry[P_D4]; /* Calculate term to be used in definition of... */ /* position vector. */ @@ -191,7 +199,7 @@ static int pumaKinematicsForward(const double * joint, *iflags |= PUMA_WRIST_FLIP; } } - rtapi_real PUMA_D6 = hal_get_real(haldata->d6); + const double PUMA_D6 = p->geometry[P_D6]; /* add effect of d6 parameter */ hom.tran.x = hom.tran.x + hom.rot.z.x*PUMA_D6; hom.tran.y = hom.tran.y + hom.rot.z.y*PUMA_D6; @@ -210,32 +218,25 @@ static int pumaKinematicsForward(const double * joint, return 0; } -static int pumaKinematicsToolFrame(const double * joint, - PmRotationMatrix * rot, - const KINEMATICS_FORWARD_FLAGS * fflags) +static int puma_tool_frame(const kins_params *p, const double * joint, + PmRotationMatrix * rot, + const KINEMATICS_FORWARD_FLAGS * fflags) { + (void)p; (void)fflags; - // answers in the flange frame; switchkins applies the declared half turn + // answers in the flange frame; the declared half turn is applied by + // the shared code pumaFlangeRotation(joint, rot); return 0; -} // pumaKinematicsToolFrame() +} // puma_tool_frame() -static int pumaKinematicsWorkFrame(const double * joint, - PmRotationMatrix * rot, - const KINEMATICS_FORWARD_FLAGS * fflags) -{ - (void)joint; - (void)fflags; - // the arm carries the tool and nothing carries the work - *rot = TOOL_FRAME_SPINDLE; - return 0; -} // pumaKinematicsWorkFrame() - -static int pumaKinematicsInverse(const EmcPose * world, - double * joint, - const KINEMATICS_INVERSE_FLAGS * iflags, - KINEMATICS_FORWARD_FLAGS * fflags) +static int puma_inverse(const kins_params *p, kins_scratch *s, + const EmcPose * world, + double * joint, + const KINEMATICS_INVERSE_FLAGS * iflags, + KINEMATICS_FORWARD_FLAGS * fflags) { + (void)s; PmHomogeneous hom; PmPose worldPose; PmRpy rpy; @@ -271,11 +272,11 @@ static int pumaKinematicsInverse(const EmcPose * world, pmRpyQuatConvert(&rpy,&worldPose.rot); pmPoseHomConvert(&worldPose, &hom); - rtapi_real PUMA_A2 = hal_get_real(haldata->a2); - rtapi_real PUMA_A3 = hal_get_real(haldata->a3); - rtapi_real PUMA_D3 = hal_get_real(haldata->d3); - rtapi_real PUMA_D4 = hal_get_real(haldata->d4); - rtapi_real PUMA_D6 = hal_get_real(haldata->d6); + const double PUMA_A2 = p->geometry[P_A2]; + const double PUMA_A3 = p->geometry[P_A3]; + const double PUMA_D3 = p->geometry[P_D3]; + const double PUMA_D4 = p->geometry[P_D4]; + const double PUMA_D6 = p->geometry[P_D6]; /* remove effect of d6 parameter */ px = hom.tran.x - PUMA_D6*hom.rot.z.x; @@ -388,29 +389,18 @@ static int pumaKinematicsInverse(const EmcPose * world, return 0; } -int pumaKinematicsSetup(const int comp_id, - const char* coordinates, - kparms* kp) -{ - (void)coordinates; - int res=0; - - haldata = hal_malloc(sizeof(*haldata)); - if (!haldata) goto error; - - - res += hal_pin_new_real(comp_id, HAL_IN, &(haldata->a2), DEFAULT_PUMA560_A2, "%s.A2", kp->halprefix); - res += hal_pin_new_real(comp_id, HAL_IN, &(haldata->a3), DEFAULT_PUMA560_A3, "%s.A3", kp->halprefix); - res += hal_pin_new_real(comp_id, HAL_IN, &(haldata->d3), DEFAULT_PUMA560_D3, "%s.D3", kp->halprefix); - res += hal_pin_new_real(comp_id, HAL_IN, &(haldata->d4), DEFAULT_PUMA560_D4, "%s.D4", kp->halprefix); - res += hal_pin_new_real(comp_id, HAL_IN, &(haldata->d6), DEFAULT_PUMA560_D6, "%s.D6", kp->halprefix); - if (res) { goto error; } - - return 0; - -error: - return -1; -} // pumaKinematicsSetup() +// the arm carries the tool and nothing carries the work, so the work frame +// is the shared identity one. The maths is the ISO 9787 flange frame, so +// the tool axis it produces runs holder towards tip, the opposite of the +// convention; the declared half turn puts it right. No closed form +// Jacobian: the shared code differences the inverse. +static const kins_ops puma_ops = { + .forward = puma_forward, + .inverse = puma_inverse, + .work = kinsIdentityFrame, + .tool = puma_tool_frame, + .native = &TOOL_FRAME_FLANGE, +}; int switchkinsSetup(kparms* kp, KS* kset0, KS* kset1, KS* kset2, @@ -418,49 +408,28 @@ int switchkinsSetup(kparms* kp, KI* kinv0, KI* kinv1, KI* kinv2 ) { + (void)kset0; (void)kset1; (void)kset2; + (void)kfwd0; (void)kfwd1; (void)kfwd2; + (void)kinv0; (void)kinv1; (void)kinv2; kp->kinsname = "pumakins"; // !!! must agree with filename kp->halprefix = "pumakins"; // hal pin names kp->required_coordinates = "xyzabc"; kp->allow_duplicates = 0; kp->max_joints = strlen(kp->required_coordinates); + kp->params = puma_params; + kp->nparams = sizeof(puma_params)/sizeof(puma_params[0]); if (kp->sparm && strstr(kp->sparm,"identityfirst")) { rtapi_print("\n!!! switchkins-type 0 is IDENTITY\n"); - *kset0 = identityKinematicsSetup; - *kfwd0 = identityKinematicsForward; - *kinv0 = identityKinematicsInverse; - - *kset1 = pumaKinematicsSetup; - *kfwd1 = pumaKinematicsForward; - *kinv1 = pumaKinematicsInverse; - // the maths is the ISO 9787 flange frame, so the tool axis it produces - // runs holder towards tip, the opposite of the convention - switchkinsRegisterFrames(1, pumaKinematicsWorkFrame, - pumaKinematicsToolFrame, - &TOOL_FRAME_FLANGE); - switchkinsDeclare(0, KINSTYPE_IDENTITY); - switchkinsDeclare(1, KINSTYPE_PRIMARY); + switchkinsRegisterOps(0, &KINS_IDENTITY_OPS); + switchkinsRegisterOps(1, &puma_ops); } else { rtapi_print("\n!!! switchkins-type 0 is %s\n",kp->kinsname); - *kset0 = pumaKinematicsSetup; - *kfwd0 = pumaKinematicsForward; - *kinv0 = pumaKinematicsInverse; - // the maths is the ISO 9787 flange frame, so the tool axis it produces - // runs holder towards tip, the opposite of the convention - switchkinsRegisterFrames(0, pumaKinematicsWorkFrame, - pumaKinematicsToolFrame, - &TOOL_FRAME_FLANGE); - - *kset1 = identityKinematicsSetup; - *kfwd1 = identityKinematicsForward; - *kinv1 = identityKinematicsInverse; - switchkinsDeclare(0, KINSTYPE_PRIMARY); - switchkinsDeclare(1, KINSTYPE_IDENTITY); + switchkinsRegisterOps(0, &puma_ops); + switchkinsRegisterOps(1, &KINS_IDENTITY_OPS); } - *kset2 = userkKinematicsSetup; - *kfwd2 = userkKinematicsForward; - *kinv2 = userkKinematicsInverse; + switchkinsRegisterOps(2, &USERK_OPS); return 0; } // switchkinsSetup() diff --git a/src/emc/kinematics/scarakins.c b/src/emc/kinematics/scarakins.c index 338f14a668d..c6137331d7f 100644 --- a/src/emc/kinematics/scarakins.c +++ b/src/emc/kinematics/scarakins.c @@ -22,10 +22,6 @@ #include -static struct scara_data { - hal_real_t d1, d2, d3, d4, d5, d6; -} *haldata = NULL; - /* key dimensions joint[0] = Entire arm rotates around a vertical axis at its inner end @@ -62,13 +58,32 @@ static struct scara_data { on the value of joint[3]. */ +#define DEFAULT_D1 490 +#define DEFAULT_D2 340 +#define DEFAULT_D3 50 +#define DEFAULT_D4 250 +#define DEFAULT_D5 50 +#define DEFAULT_D6 50 + +// the six dimensions, one pin each; the maths reads them from the block +static const kins_param_desc scara_params[] = { + { "D1", KINS_PARAM_FLOAT, KINS_IN, 0, DEFAULT_D1 }, + { "D2", KINS_PARAM_FLOAT, KINS_IN, 0, DEFAULT_D2 }, + { "D3", KINS_PARAM_FLOAT, KINS_IN, 0, DEFAULT_D3 }, + { "D4", KINS_PARAM_FLOAT, KINS_IN, 0, DEFAULT_D4 }, + { "D5", KINS_PARAM_FLOAT, KINS_IN, 0, DEFAULT_D5 }, + { "D6", KINS_PARAM_FLOAT, KINS_IN, 0, DEFAULT_D6 }, +}; +enum { P_D1, P_D2, P_D3, P_D4, P_D5, P_D6 }; + /* joint[0], joint[1] and joint[3] are in degrees and joint[2] is in length units */ -static -int scaraKinematicsForward(const double * joint, - EmcPose * world, - const KINEMATICS_FORWARD_FLAGS * fflags, - KINEMATICS_INVERSE_FLAGS * iflags) +static int scara_forward(const kins_params *p, kins_scratch *s, + const double * joint, + EmcPose * world, + const KINEMATICS_FORWARD_FLAGS * fflags, + KINEMATICS_INVERSE_FLAGS * iflags) { + (void)s; (void)fflags; double a0, a1, a3; double x, y, z, c; @@ -83,12 +98,12 @@ int scaraKinematicsForward(const double * joint, a1 = a1 + a0; a3 = a3 + a1; - rtapi_real D1 = hal_get_real(haldata->d1); - rtapi_real D2 = hal_get_real(haldata->d2); - rtapi_real D3 = hal_get_real(haldata->d3); - rtapi_real D4 = hal_get_real(haldata->d4); - rtapi_real D5 = hal_get_real(haldata->d5); - rtapi_real D6 = hal_get_real(haldata->d6); + const double D1 = p->geometry[P_D1]; + const double D2 = p->geometry[P_D2]; + const double D3 = p->geometry[P_D3]; + const double D4 = p->geometry[P_D4]; + const double D5 = p->geometry[P_D5]; + const double D6 = p->geometry[P_D6]; x = D2*cos(a0) + D4*cos(a1) + D6*cos(a3); y = D2*sin(a0) + D4*sin(a1) + D6*sin(a3); @@ -109,13 +124,15 @@ int scaraKinematicsForward(const double * joint, world->b = joint[5]; return (0); -} //scaraKinematicsForward() +} // scara_forward() -static int scaraKinematicsInverse(const EmcPose * world, - double * joint, - const KINEMATICS_INVERSE_FLAGS * iflags, - KINEMATICS_FORWARD_FLAGS * fflags) +static int scara_inverse(const kins_params *p, kins_scratch *s, + const EmcPose * world, + double * joint, + const KINEMATICS_INVERSE_FLAGS * iflags, + KINEMATICS_FORWARD_FLAGS * fflags) { + (void)s; double a3; double q0, q1; double xt, yt, rsq, cc; @@ -129,12 +146,12 @@ static int scaraKinematicsInverse(const EmcPose * world, /* convert degrees to radians */ a3 = c * ( PM_PI / 180 ); - rtapi_real D1 = hal_get_real(haldata->d1); - rtapi_real D2 = hal_get_real(haldata->d2); - rtapi_real D3 = hal_get_real(haldata->d3); - rtapi_real D4 = hal_get_real(haldata->d4); - rtapi_real D5 = hal_get_real(haldata->d5); - rtapi_real D6 = hal_get_real(haldata->d6); + const double D1 = p->geometry[P_D1]; + const double D2 = p->geometry[P_D2]; + const double D3 = p->geometry[P_D3]; + const double D4 = p->geometry[P_D4]; + const double D5 = p->geometry[P_D5]; + const double D6 = p->geometry[P_D6]; /* center of end effector (correct for D6) */ xt = x - D6*cos(a3); @@ -176,17 +193,17 @@ static int scaraKinematicsInverse(const EmcPose * world, *fflags = 0; return (0); -} // scaraKinematicsInverse() +} // scara_inverse() -static int scaraKinematicsJacobian(const double * joint, - const EmcPose * world, - double jac[EMCMOT_MAX_JOINTS][EMCMOT_MAX_AXIS], - const KINEMATICS_INVERSE_FLAGS * iflags) +static int scara_jacobian(const kins_params *p, const double * joint, + const EmcPose * world, + double jac[EMCMOT_MAX_JOINTS][EMCMOT_MAX_AXIS], + const KINEMATICS_INVERSE_FLAGS * iflags) { (void)iflags; - rtapi_real D2 = hal_get_real(haldata->d2); - rtapi_real D4 = hal_get_real(haldata->d4); - rtapi_real D6 = hal_get_real(haldata->d6); + const double D2 = p->geometry[P_D2]; + const double D4 = p->geometry[P_D4]; + const double D6 = p->geometry[P_D6]; const double a3 = world->c * (PM_PI / 180); const double q1 = joint[1] * (PM_PI / 180); const double xt = world->tran.x - D6*cos(a3); @@ -228,38 +245,13 @@ static int scaraKinematicsJacobian(const double * joint, jac[4][3] = 1; jac[5][4] = 1; return 0; -} // scaraKinematicsJacobian() - -#define DEFAULT_D1 490 -#define DEFAULT_D2 340 -#define DEFAULT_D3 50 -#define DEFAULT_D4 250 -#define DEFAULT_D5 50 -#define DEFAULT_D6 50 - -static int scaraKinematicsSetup(const int comp_id, - const char* coordinates, - kparms* kp) -{ - (void)coordinates; - int res=0; - - haldata = hal_malloc(sizeof(*haldata)); - if (!haldata) goto error; - - res += hal_pin_new_real(comp_id, HAL_IN, &(haldata->d1), DEFAULT_D1, "%s.D1", kp->halprefix); - res += hal_pin_new_real(comp_id, HAL_IN, &(haldata->d2), DEFAULT_D2, "%s.D2", kp->halprefix); - res += hal_pin_new_real(comp_id, HAL_IN, &(haldata->d3), DEFAULT_D3, "%s.D3", kp->halprefix); - res += hal_pin_new_real(comp_id, HAL_IN, &(haldata->d4), DEFAULT_D4, "%s.D4", kp->halprefix); - res += hal_pin_new_real(comp_id, HAL_IN, &(haldata->d5), DEFAULT_D5, "%s.D5", kp->halprefix); - res += hal_pin_new_real(comp_id, HAL_IN, &(haldata->d6), DEFAULT_D6, "%s.D6", kp->halprefix); - if (res) { goto error; } - - return 0; +} // scara_jacobian() -error: - return -1; -} // scaraKinematicsSetup() +static const kins_ops scara_ops = { + .forward = scara_forward, + .inverse = scara_inverse, + .jacobian = scara_jacobian, +}; int switchkinsSetup(kparms* kp, KS* kset0, KS* kset1, KS* kset2, @@ -267,41 +259,28 @@ int switchkinsSetup(kparms* kp, KI* kinv0, KI* kinv1, KI* kinv2 ) { + (void)kset0; (void)kset1; (void)kset2; + (void)kfwd0; (void)kfwd1; (void)kfwd2; + (void)kinv0; (void)kinv1; (void)kinv2; kp->kinsname = "scarakins"; // !!! must agree with filename kp->halprefix = "scarakins"; // hal pin names kp->required_coordinates = "xyzabc"; // ab are scaragui table tilts kp->allow_duplicates = 0; kp->max_joints = strlen(kp->required_coordinates); + kp->params = scara_params; + kp->nparams = sizeof(scara_params)/sizeof(scara_params[0]); if (kp->sparm && strstr(kp->sparm,"identityfirst")) { rtapi_print("\n!!! switchkins-type 0 is IDENTITY\n"); - *kset0 = identityKinematicsSetup; - *kfwd0 = identityKinematicsForward; - *kinv0 = identityKinematicsInverse; - - *kset1 = scaraKinematicsSetup; - *kfwd1 = scaraKinematicsForward; - *kinv1 = scaraKinematicsInverse; - switchkinsDeclare(0, KINSTYPE_IDENTITY); - switchkinsDeclare(1, KINSTYPE_PRIMARY); - switchkinsRegisterJacobian(1, scaraKinematicsJacobian); + switchkinsRegisterOps(0, &KINS_IDENTITY_OPS); + switchkinsRegisterOps(1, &scara_ops); } else { rtapi_print("\n!!! switchkins-type 0 is %s\n",kp->kinsname); - *kset0 = scaraKinematicsSetup; - *kfwd0 = scaraKinematicsForward; - *kinv0 = scaraKinematicsInverse; - switchkinsRegisterJacobian(0, scaraKinematicsJacobian); - - *kset1 = identityKinematicsSetup; - *kfwd1 = identityKinematicsForward; - *kinv1 = identityKinematicsInverse; - switchkinsDeclare(0, KINSTYPE_PRIMARY); - switchkinsDeclare(1, KINSTYPE_IDENTITY); + switchkinsRegisterOps(0, &scara_ops); + switchkinsRegisterOps(1, &KINS_IDENTITY_OPS); } - *kset2 = userkKinematicsSetup; - *kfwd2 = userkKinematicsForward; - *kinv2 = userkKinematicsInverse; + switchkinsRegisterOps(2, &USERK_OPS); return 0; } // switchkinsSetup() diff --git a/src/emc/kinematics/three21kins.c b/src/emc/kinematics/three21kins.c index 039001a8624..f647f18e318 100644 --- a/src/emc/kinematics/three21kins.c +++ b/src/emc/kinematics/three21kins.c @@ -27,9 +27,18 @@ /* flags for forward kinematics */ #define THREE21_REACH 0x01 -struct haldata { - hal_real_t a1, a2, a3, d1, d2, d3, d4, d6; -} *haldata = NULL; +// the eight dimensions, one pin each; the maths reads them from the block +static const kins_param_desc three21_params[] = { + { "A1", KINS_PARAM_FLOAT, KINS_IN, 0, DEFAULT_THREE21_A1 }, + { "A2", KINS_PARAM_FLOAT, KINS_IN, 0, DEFAULT_THREE21_A2 }, + { "A3", KINS_PARAM_FLOAT, KINS_IN, 0, DEFAULT_THREE21_A3 }, + { "D1", KINS_PARAM_FLOAT, KINS_IN, 0, DEFAULT_THREE21_D1 }, + { "D2", KINS_PARAM_FLOAT, KINS_IN, 0, DEFAULT_THREE21_D2 }, + { "D3", KINS_PARAM_FLOAT, KINS_IN, 0, DEFAULT_THREE21_D3 }, + { "D4", KINS_PARAM_FLOAT, KINS_IN, 0, DEFAULT_THREE21_D4 }, + { "D6", KINS_PARAM_FLOAT, KINS_IN, 0, DEFAULT_THREE21_D6 }, +}; +enum { P_A1, P_A2, P_A3, P_D1, P_D2, P_D3, P_D4, P_D6 }; /* the difference of two angles, brought into (-pi, pi] so that a joint a whole turn from the formula still matches it */ @@ -41,20 +50,22 @@ static double angleDiff(double a, double b) return d; } -static int three21KinematicsForward(const double * joint, - EmcPose * world, - const KINEMATICS_FORWARD_FLAGS * fflags, - KINEMATICS_INVERSE_FLAGS * iflags) +static int three21_forward(const kins_params *p, kins_scratch *s, + const double * joint, + EmcPose * world, + const KINEMATICS_FORWARD_FLAGS * fflags, + KINEMATICS_INVERSE_FLAGS * iflags) { + (void)s; (void)fflags; - double a1 = hal_get_real(haldata->a1); - double a2 = hal_get_real(haldata->a2); - double a3 = hal_get_real(haldata->a3); - double d1 = hal_get_real(haldata->d1); - double d2 = hal_get_real(haldata->d2); - double d3 = hal_get_real(haldata->d3); - double d4 = hal_get_real(haldata->d4); - double d6 = hal_get_real(haldata->d6); + double a1 = p->geometry[P_A1]; + double a2 = p->geometry[P_A2]; + double a3 = p->geometry[P_A3]; + double d1 = p->geometry[P_D1]; + double d2 = p->geometry[P_D2]; + double d3 = p->geometry[P_D3]; + double d4 = p->geometry[P_D4]; + double d6 = p->geometry[P_D6]; double s1, s2, s3, s4, s5, s6; double c1, c2, c3, c4, c5, c6; @@ -189,23 +200,25 @@ static int three21KinematicsForward(const double * joint, return 0; } -static int three21KinematicsInverse(const EmcPose * world, - double * joint, - const KINEMATICS_INVERSE_FLAGS * iflags, - KINEMATICS_FORWARD_FLAGS * fflags) +static int three21_inverse(const kins_params *p, kins_scratch *s, + const EmcPose * world, + double * joint, + const KINEMATICS_INVERSE_FLAGS * iflags, + KINEMATICS_FORWARD_FLAGS * fflags) { + (void)s; PmHomogeneous hom; PmPose worldPose; PmRpy rpy; - double a1 = hal_get_real(haldata->a1); - double a2 = hal_get_real(haldata->a2); - double a3 = hal_get_real(haldata->a3); - double d1 = hal_get_real(haldata->d1); - double d2 = hal_get_real(haldata->d2); - double d3 = hal_get_real(haldata->d3); - double d4 = hal_get_real(haldata->d4); - double d6 = hal_get_real(haldata->d6); + double a1 = p->geometry[P_A1]; + double a2 = p->geometry[P_A2]; + double a3 = p->geometry[P_A3]; + double d1 = p->geometry[P_D1]; + double d2 = p->geometry[P_D2]; + double d3 = p->geometry[P_D3]; + double d4 = p->geometry[P_D4]; + double d6 = p->geometry[P_D6]; double t1, t2, t3; double k; @@ -348,31 +361,12 @@ static int three21KinematicsInverse(const EmcPose * world, return 0; } -int three21KinematicsSetup(const int comp_id, - const char* coordinates, - kparms* kp) -{ - (void)coordinates; - int res=0; - - haldata = hal_malloc(sizeof(*haldata)); - if (!haldata) goto error; - - res += hal_pin_new_real(comp_id, HAL_IN, &(haldata->a1), DEFAULT_THREE21_A1, "%s.A1", kp->halprefix); - res += hal_pin_new_real(comp_id, HAL_IN, &(haldata->a2), DEFAULT_THREE21_A2, "%s.A2", kp->halprefix); - res += hal_pin_new_real(comp_id, HAL_IN, &(haldata->a3), DEFAULT_THREE21_A3, "%s.A3", kp->halprefix); - res += hal_pin_new_real(comp_id, HAL_IN, &(haldata->d1), DEFAULT_THREE21_D1, "%s.D1", kp->halprefix); - res += hal_pin_new_real(comp_id, HAL_IN, &(haldata->d2), DEFAULT_THREE21_D2, "%s.D2", kp->halprefix); - res += hal_pin_new_real(comp_id, HAL_IN, &(haldata->d3), DEFAULT_THREE21_D3, "%s.D3", kp->halprefix); - res += hal_pin_new_real(comp_id, HAL_IN, &(haldata->d4), DEFAULT_THREE21_D4, "%s.D4", kp->halprefix); - res += hal_pin_new_real(comp_id, HAL_IN, &(haldata->d6), DEFAULT_THREE21_D6, "%s.D6", kp->halprefix); - if (res) { goto error; } - - return 0; - -error: - return -1; -} +// no frames reported and no closed form Jacobian: the shared code +// differences the inverse +static const kins_ops three21_ops = { + .forward = three21_forward, + .inverse = three21_inverse, +}; int switchkinsSetup(kparms* kp, KS* kset0, KS* kset1, KS* kset2, @@ -380,39 +374,28 @@ int switchkinsSetup(kparms* kp, KI* kinv0, KI* kinv1, KI* kinv2 ) { + (void)kset0; (void)kset1; (void)kset2; + (void)kfwd0; (void)kfwd1; (void)kfwd2; + (void)kinv0; (void)kinv1; (void)kinv2; kp->kinsname = "three21kins"; kp->halprefix = "three21kins"; kp->required_coordinates = "xyzabc"; kp->allow_duplicates = 0; kp->max_joints = strlen(kp->required_coordinates); + kp->params = three21_params; + kp->nparams = sizeof(three21_params)/sizeof(three21_params[0]); if (kp->sparm && strstr(kp->sparm,"identityfirst")) { rtapi_print("\n!!! switchkins-type 0 is IDENTITY\n"); - *kset0 = identityKinematicsSetup; - *kfwd0 = identityKinematicsForward; - *kinv0 = identityKinematicsInverse; - - *kset1 = three21KinematicsSetup; - *kfwd1 = three21KinematicsForward; - *kinv1 = three21KinematicsInverse; - switchkinsDeclare(0, KINSTYPE_IDENTITY); - switchkinsDeclare(1, KINSTYPE_PRIMARY); + switchkinsRegisterOps(0, &KINS_IDENTITY_OPS); + switchkinsRegisterOps(1, &three21_ops); } else { rtapi_print("\n!!! switchkins-type 0 is %s\n",kp->kinsname); - *kset0 = three21KinematicsSetup; - *kfwd0 = three21KinematicsForward; - *kinv0 = three21KinematicsInverse; - - *kset1 = identityKinematicsSetup; - *kfwd1 = identityKinematicsForward; - *kinv1 = identityKinematicsInverse; - switchkinsDeclare(0, KINSTYPE_PRIMARY); - switchkinsDeclare(1, KINSTYPE_IDENTITY); + switchkinsRegisterOps(0, &three21_ops); + switchkinsRegisterOps(1, &KINS_IDENTITY_OPS); } - *kset2 = userkKinematicsSetup; - *kfwd2 = userkKinematicsForward; - *kinv2 = userkKinematicsInverse; + switchkinsRegisterOps(2, &USERK_OPS); return 0; } From 2f99d4d284e8420ff1d1a56ea01854b0345bcb7c Mon Sep 17 00:00:00 2001 From: Luca Toniolo <10792599+grandixximo@users.noreply.github.com> Date: Fri, 4 Sep 2026 13:06:35 +1000 Subject: [PATCH 32/60] millturn, xyzab_tdr_kins, xyzacb_trsrn, xyzbca_trsrn: move onto the parameter 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. --- src/hal/components/Submakefile | 2 +- src/hal/components/millturn.comp | 103 +++--- src/hal/components/xyzab_tdr_kins.comp | 173 +++++---- src/hal/components/xyzacb_trsrn.comp | 470 +++++++++++-------------- src/hal/components/xyzbca_trsrn.comp | 436 ++++++++++------------- 5 files changed, 540 insertions(+), 644 deletions(-) diff --git a/src/hal/components/Submakefile b/src/hal/components/Submakefile index 865b70ece96..d8975f3f8d1 100644 --- a/src/hal/components/Submakefile +++ b/src/hal/components/Submakefile @@ -97,7 +97,7 @@ obj-m += $(patsubst hal/drivers/%.comp, %.o, $(patsubst hal/components/%.comp, % # A component that links objects besides its own names them here as # -extra-objs. The list is expanded when the .mak is written, # so it has to be defined in this file (which the .mak depends on). -SWITCHKINS_OBJS := emc/kinematics/switchkins.o emc/kinematics/kins_util.o +SWITCHKINS_OBJS := emc/kinematics/switchkins.o emc/kinematics/switchkins_setup.o emc/kinematics/kins_util.o matrixkins-extra-objs := emc/kinematics/kins_util.o emc/kinematics/kins_single.o millturn-extra-objs := $(SWITCHKINS_OBJS) xyzab_tdr_kins-extra-objs := $(SWITCHKINS_OBJS) diff --git a/src/hal/components/millturn.comp b/src/hal/components/millturn.comp index abbff00a375..b841ba3f0e0 100644 --- a/src/hal/components/millturn.comp +++ b/src/hal/components/millturn.comp @@ -26,7 +26,6 @@ chapter (docs/src/motion/switchkins.txt) """; // The fpin pin is not accessible in kinematics functions. -// Use the *_setup() function for pins and params used by kinematics. pin out si32 fpin=0"pin to demonstrate use of a conventional (non-kinematics) function fdemo"; option period no; option extra_setup; @@ -49,22 +48,16 @@ FUNCTION(fdemo) { fpin_set(fpin + 1); } -// the turn kinematics need no hal pins of their own -static int turnKinematicsSetup(const int comp_id, - const char* coords, - kparms* kp) -{ - (void)comp_id; - (void)coords; - (void)kp; - return 0; -} // turnKinematicsSetup() - -static int turnKinematicsForward(const double *j, - EmcPose * pos, - const KINEMATICS_FORWARD_FLAGS * fflags, - KINEMATICS_INVERSE_FLAGS * iflags) +// the turn kinematics: no geometry, written as pure functions of the +// parameter block (see kinematics.h) +static int turn_forward(const kins_params *p, kins_scratch *s, + const double *j, + EmcPose * pos, + const KINEMATICS_FORWARD_FLAGS * fflags, + KINEMATICS_INVERSE_FLAGS * iflags) { + (void)p; + (void)s; (void)fflags; (void)iflags; @@ -81,13 +74,16 @@ static int turnKinematicsForward(const double *j, pos->w = 0; return 0; -} // turnKinematicsForward() +} // turn_forward() -static int turnKinematicsInverse(const EmcPose * pos, - double *j, - const KINEMATICS_INVERSE_FLAGS * iflags, - KINEMATICS_FORWARD_FLAGS * fflags) +static int turn_inverse(const kins_params *p, kins_scratch *s, + const EmcPose * pos, + double *j, + const KINEMATICS_INVERSE_FLAGS * iflags, + KINEMATICS_FORWARD_FLAGS * fflags) { + (void)p; + (void)s; (void)iflags; (void)fflags; @@ -97,53 +93,62 @@ static int turnKinematicsInverse(const EmcPose * pos, j[3] = pos->a; return 0; -} // turnKinematicsInverse() +} // turn_inverse() -static int turnKinematicsJacobian(const double *j, - const EmcPose * pos, - double jac[EMCMOT_MAX_JOINTS][EMCMOT_MAX_AXIS], - const KINEMATICS_INVERSE_FLAGS * iflags) +static int turn_jacobian(const kins_params *p, const double *j, + const EmcPose * pos, + double jac[EMCMOT_MAX_JOINTS][EMCMOT_MAX_AXIS], + const KINEMATICS_INVERSE_FLAGS * iflags) { int R, C; + (void)p; (void)j; (void)pos; (void)iflags; for (R = 0; R < EMCMOT_MAX_JOINTS; R++) { for (C = 0; C < EMCMOT_MAX_AXIS; C++) { jac[R][C] = 0; } } - // the derivative of turnKinematicsInverse(): which joint follows which - // pose coordinate, and in which sense + // the derivative of turn_inverse(): which joint follows which pose + // coordinate, and in which sense jac[2][0] = 1; jac[1][1] = -1; jac[0][2] = 1; jac[3][3] = 1; return 0; -} // turnKinematicsJacobian() +} // turn_jacobian() + +static const kins_ops turn_ops = { + .forward = turn_forward, + .inverse = turn_inverse, + .jacobian = turn_jacobian, +}; + +int switchkinsSetup(kparms* kp, + KS* kset0, KS* kset1, KS* kset2, + KF* kfwd0, KF* kfwd1, KF* kfwd2, + KI* kinv0, KI* kinv1, KI* kinv2 + ) +{ + (void)kset0; (void)kset1; (void)kset2; + (void)kfwd0; (void)kfwd1; (void)kfwd2; + (void)kinv0; (void)kinv1; (void)kinv2; + kp->kinsname = "millturn"; + kp->halprefix = "millturn"; + kp->required_coordinates = "xyza"; + kp->allow_duplicates = 0; + kp->max_joints = strlen(kp->required_coordinates); + + switchkinsRegisterOps(0, &KINS_IDENTITY_OPS); + switchkinsRegisterOps(1, &turn_ops); + return 0; +} // switchkinsSetup() // halcompile has done hal_init() and does hal_ready() after this returns, // which is what switchkinsInit() expects EXTRA_SETUP() { - kparms kp = {0}; + kparms kp; (void)__comp_inst; (void)prefix; (void)extra_arg; - kp.kinsname = "millturn"; - kp.halprefix = "millturn"; - kp.required_coordinates = "xyza"; - kp.allow_duplicates = 0; - kp.fwd_iterates_mask = 0; - kp.gui_kinstype = -1; - kp.sparm = NULL; - kp.max_joints = strlen(kp.required_coordinates); - - if (switchkinsRegister(0, identityKinematicsSetup, - identityKinematicsForward, - identityKinematicsInverse)) { return -1; } - if (switchkinsRegister(1, turnKinematicsSetup, - turnKinematicsForward, - turnKinematicsInverse)) { return -1; } - if (switchkinsRegisterJacobian(1, turnKinematicsJacobian)) { return -1; } - - if (switchkinsDeclare(0, KINSTYPE_IDENTITY)) { return -1; } - + if (switchkinsRunSetup(&kp, NULL)) { return -1; } return switchkinsInit(comp_id, &kp, coordinates); } // EXTRA_SETUP() diff --git a/src/hal/components/xyzab_tdr_kins.comp b/src/hal/components/xyzab_tdr_kins.comp index c5d4db81f66..ce21a7d8159 100644 --- a/src/hal/components/xyzab_tdr_kins.comp +++ b/src/hal/components/xyzab_tdr_kins.comp @@ -44,55 +44,33 @@ author "David Mueller"; static char *coordinates; RTAPI_MP_STRING(coordinates, "Axes-to-joints-ordering"); -static struct haldata { - hal_real_t tool_offset_z; - hal_real_t x_offset; - hal_real_t z_offset; - hal_real_t x_rot_point; - hal_real_t y_rot_point; - hal_real_t z_rot_point; -} *tdrdata; - -static int tdrKinematicsSetup(const int comp_id, - const char* coords, - kparms* kp) -{ - int res = 0; - (void)coords; - - tdrdata = hal_malloc(sizeof(*tdrdata)); - if (!tdrdata) return -1; - - res += hal_pin_new_real(comp_id, HAL_IN, &tdrdata->tool_offset_z, 0.0, - "%s.tool-offset-z", kp->halprefix); - res += hal_pin_new_real(comp_id, HAL_IN, &tdrdata->x_offset, 0.0, - "%s.x-offset", kp->halprefix); - res += hal_pin_new_real(comp_id, HAL_IN, &tdrdata->z_offset, 0.0, - "%s.z-offset", kp->halprefix); - res += hal_pin_new_real(comp_id, HAL_IN, &tdrdata->x_rot_point, 0.0, - "%s.x-rot-point", kp->halprefix); - res += hal_pin_new_real(comp_id, HAL_IN, &tdrdata->y_rot_point, 0.0, - "%s.y-rot-point", kp->halprefix); - res += hal_pin_new_real(comp_id, HAL_IN, &tdrdata->z_rot_point, 0.0, - "%s.z-rot-point", kp->halprefix); - if (res) return -1; - - return 0; -} // tdrKinematicsSetup() - -static int tdrKinematicsForward(const double *j, - EmcPose * pos, - const KINEMATICS_FORWARD_FLAGS * fflags, - KINEMATICS_INVERSE_FLAGS * iflags) +// the geometry, one pin each; the maths reads it from the block (see +// kinematics.h), and the tool length from p->tool.tran.z +static const kins_param_desc tdr_params[] = { + { "tool-offset-z", KINS_PARAM_FLOAT, KINS_IN, 1, 0.0 }, + { "x-offset", KINS_PARAM_FLOAT, KINS_IN, 0, 0.0 }, + { "z-offset", KINS_PARAM_FLOAT, KINS_IN, 0, 0.0 }, + { "x-rot-point", KINS_PARAM_FLOAT, KINS_IN, 0, 0.0 }, + { "y-rot-point", KINS_PARAM_FLOAT, KINS_IN, 0, 0.0 }, + { "z-rot-point", KINS_PARAM_FLOAT, KINS_IN, 0, 0.0 }, +}; +enum { P_TOOL, P_XO, P_ZO, P_XR, P_YR, P_ZR }; + +static int tdr_forward(const kins_params *p, kins_scratch *s, + const double *j, + EmcPose * pos, + const KINEMATICS_FORWARD_FLAGS * fflags, + KINEMATICS_INVERSE_FLAGS * iflags) { + (void)s; (void)fflags; (void)iflags; - double x_rot_point = hal_get_real(tdrdata->x_rot_point); - double y_rot_point = hal_get_real(tdrdata->y_rot_point); - double z_rot_point = hal_get_real(tdrdata->z_rot_point); + double x_rot_point = p->geometry[P_XR]; + double y_rot_point = p->geometry[P_YR]; + double z_rot_point = p->geometry[P_ZR]; - double dz = hal_get_real(tdrdata->z_offset); - double dt = hal_get_real(tdrdata->tool_offset_z); + double dz = p->geometry[P_ZO]; + double dt = p->tool.tran.z; // substitutions as used in mathematical documentation // including degree -> radians angle conversion @@ -125,22 +103,24 @@ static int tdrKinematicsForward(const double *j, pos->w = 0; return 0; -} // tdrKinematicsForward() +} // tdr_forward() -static int tdrKinematicsInverse(const EmcPose * pos, - double *j, - const KINEMATICS_INVERSE_FLAGS * iflags, - KINEMATICS_FORWARD_FLAGS * fflags) +static int tdr_inverse(const kins_params *p, kins_scratch *s, + const EmcPose * pos, + double *j, + const KINEMATICS_INVERSE_FLAGS * iflags, + KINEMATICS_FORWARD_FLAGS * fflags) { + (void)s; (void)iflags; (void)fflags; - double x_rot_point = hal_get_real(tdrdata->x_rot_point); - double y_rot_point = hal_get_real(tdrdata->y_rot_point); - double z_rot_point = hal_get_real(tdrdata->z_rot_point); + double x_rot_point = p->geometry[P_XR]; + double y_rot_point = p->geometry[P_YR]; + double z_rot_point = p->geometry[P_ZR]; - double dx = hal_get_real(tdrdata->x_offset); - double dz = hal_get_real(tdrdata->z_offset); - double dt = hal_get_real(tdrdata->tool_offset_z); + double dx = p->geometry[P_XO]; + double dz = p->geometry[P_ZO]; + double dt = p->tool.tran.z; // substitutions as used in mathematical documentation // including degree -> radians angle conversion @@ -167,21 +147,21 @@ static int tdrKinematicsInverse(const EmcPose * pos, j[4] = pos->b; return 0; -} // tdrKinematicsInverse() +} // tdr_inverse() -static int tdrKinematicsJacobian(const double *j, - const EmcPose * pos, - double jac[EMCMOT_MAX_JOINTS][EMCMOT_MAX_AXIS], - const KINEMATICS_INVERSE_FLAGS * iflags) +static int tdr_jacobian(const kins_params *p, const double *j, + const EmcPose * pos, + double jac[EMCMOT_MAX_JOINTS][EMCMOT_MAX_AXIS], + const KINEMATICS_INVERSE_FLAGS * iflags) { (void)j; (void)iflags; - double x_rot_point = hal_get_real(tdrdata->x_rot_point); - double y_rot_point = hal_get_real(tdrdata->y_rot_point); - double z_rot_point = hal_get_real(tdrdata->z_rot_point); - double dx = hal_get_real(tdrdata->x_offset); - double dz = hal_get_real(tdrdata->z_offset); - double dt = hal_get_real(tdrdata->tool_offset_z); + double x_rot_point = p->geometry[P_XR]; + double y_rot_point = p->geometry[P_YR]; + double z_rot_point = p->geometry[P_ZR]; + double dx = p->geometry[P_XO]; + double dz = p->geometry[P_ZO]; + double dt = p->tool.tran.z; double sa = sin(pos->a*TO_RAD); double ca = cos(pos->a*TO_RAD); double sb = sin(pos->b*TO_RAD); @@ -195,9 +175,9 @@ static int tdrKinematicsJacobian(const double *j, for (C = 0; C < EMCMOT_MAX_AXIS; C++) { jac[R][C] = 0; } } - // tdrKinematicsInverse() differentiated: its coefficients of qx, qy - // and qz for the linear columns, and the same terms with a or b - // advanced a quarter turn for the rotary columns + // tdr_inverse() differentiated: its coefficients of qx, qy and qz for + // the linear columns, and the same terms with a or b advanced a + // quarter turn for the rotary columns jac[0][0] = cb; jac[0][1] = sa*sb; jac[0][2] = -ca*sb; @@ -217,33 +197,42 @@ static int tdrKinematicsJacobian(const double *j, jac[3][3] = 1; jac[4][4] = 1; return 0; -} // tdrKinematicsJacobian() +} // tdr_jacobian() + +static const kins_ops tdr_ops = { + .forward = tdr_forward, + .inverse = tdr_inverse, + .jacobian = tdr_jacobian, +}; + +int switchkinsSetup(kparms* kp, + KS* kset0, KS* kset1, KS* kset2, + KF* kfwd0, KF* kfwd1, KF* kfwd2, + KI* kinv0, KI* kinv1, KI* kinv2 + ) +{ + (void)kset0; (void)kset1; (void)kset2; + (void)kfwd0; (void)kfwd1; (void)kfwd2; + (void)kinv0; (void)kinv1; (void)kinv2; + kp->kinsname = "xyzab_tdr_kins"; + kp->halprefix = "xyzab_tdr_kins"; + kp->required_coordinates = "xyzab"; + kp->allow_duplicates = 0; + kp->max_joints = strlen(kp->required_coordinates); + kp->params = tdr_params; + kp->nparams = sizeof(tdr_params)/sizeof(tdr_params[0]); + + switchkinsRegisterOps(0, &KINS_IDENTITY_OPS); + switchkinsRegisterOps(1, &tdr_ops); + return 0; +} // switchkinsSetup() // halcompile has done hal_init() and does hal_ready() after this returns, // which is what switchkinsInit() expects EXTRA_SETUP() { - kparms kp = {0}; + kparms kp; (void)__comp_inst; (void)prefix; (void)extra_arg; - kp.kinsname = "xyzab_tdr_kins"; - kp.halprefix = "xyzab_tdr_kins"; - kp.required_coordinates = "xyzab"; - kp.allow_duplicates = 0; - kp.fwd_iterates_mask = 0; - kp.gui_kinstype = -1; - kp.sparm = NULL; - kp.max_joints = strlen(kp.required_coordinates); - - if (switchkinsRegister(0, identityKinematicsSetup, - identityKinematicsForward, - identityKinematicsInverse)) { return -1; } - if (switchkinsRegister(1, tdrKinematicsSetup, - tdrKinematicsForward, - tdrKinematicsInverse)) { return -1; } - if (switchkinsRegisterJacobian(1, tdrKinematicsJacobian)) { return -1; } - - if (switchkinsDeclare(0, KINSTYPE_IDENTITY)) { return -1; } - if (switchkinsDeclare(1, KINSTYPE_PRIMARY)) { return -1; } - + if (switchkinsRunSetup(&kp, NULL)) { return -1; } return switchkinsInit(comp_id, &kp, coordinates); } // EXTRA_SETUP() diff --git a/src/hal/components/xyzacb_trsrn.comp b/src/hal/components/xyzacb_trsrn.comp index 3d26044943d..8fb6d8dae39 100644 --- a/src/hal/components/xyzacb_trsrn.comp +++ b/src/hal/components/xyzacb_trsrn.comp @@ -25,85 +25,46 @@ author "David Mueller"; static char *coordinates; RTAPI_MP_STRING(coordinates, "Axes-to-joints-ordering"); -static struct haldata { - // these should be parameters really but we want to be able to - // change them for demonstration purposes - hal_real_t y_pivot; - hal_real_t z_pivot; - hal_real_t x_offset; - hal_real_t y_offset; - hal_real_t y_rot_axis; - hal_real_t z_rot_axis; - hal_real_t pre_rot; - hal_real_t nut_angle; - hal_real_t prim_angle; - hal_real_t sec_angle; - - // Parameters used for xyzacb_trsrn kinematics: - - // Declare hal pin pointers used for xyzacb_trsrn kinematics: - - hal_real_t tool_offset_z; -} *haldata; - -// the pins are shared by the TCP and TOOL kinematics; the TOOL type has -// no setup routine of its own -static int trsrnKinematicsSetup(const int comp_id, - const char* coords, - kparms* kp) -{ - int res = 0; - (void)coords; - - haldata = hal_malloc(sizeof(struct haldata)); - if (!haldata) return -1; - - res += hal_pin_new_real(comp_id, HAL_IN, &haldata->tool_offset_z, 0.0, "%s.tool-offset-z" ,kp->halprefix); - res += hal_pin_new_real(comp_id, HAL_IN, &haldata->y_pivot, 0.0, "%s.y-pivot" ,kp->halprefix); - res += hal_pin_new_real(comp_id, HAL_IN, &haldata->z_pivot, 0.0, "%s.z-pivot" ,kp->halprefix); - res += hal_pin_new_real(comp_id, HAL_IN, &haldata->x_offset, 0.0, "%s.x-offset" ,kp->halprefix); - res += hal_pin_new_real(comp_id, HAL_IN, &haldata->y_offset, 0.0, "%s.y-offset" ,kp->halprefix); - res += hal_pin_new_real(comp_id, HAL_IN, &haldata->y_rot_axis, 0.0, "%s.y-rot-axis" ,kp->halprefix); - res += hal_pin_new_real(comp_id, HAL_IN, &haldata->z_rot_axis, 0.0, "%s.z-rot-axis" ,kp->halprefix); - res += hal_pin_new_real(comp_id, HAL_IN, &haldata->pre_rot, 0.0, "%s.pre-rot" ,kp->halprefix); - res += hal_pin_new_real(comp_id, HAL_IN, &haldata->nut_angle, 0.0, "%s.nut-angle" ,kp->halprefix); - res += hal_pin_new_real(comp_id, HAL_IN, &haldata->prim_angle, 0.0, "%s.primary-angle" ,kp->halprefix); - res += hal_pin_new_real(comp_id, HAL_IN, &haldata->sec_angle, 0.0, "%s.secondary-angle" ,kp->halprefix); - if (res) return -1; - - return 0; -} // trsrnKinematicsSetup() - -static int toolKinematicsSetup(const int comp_id, - const char* coords, - kparms* kp) -{ - (void)comp_id; - (void)coords; - (void)kp; - return 0; // pins created by trsrnKinematicsSetup() -} // toolKinematicsSetup() +// The geometry of the universal spindle head, one pin each, shared by the +// TCP and TOOL kinematics; the maths reads it from the block (see +// kinematics.h) and the tool length from p->tool.tran.z. The two angle +// pins are what the TOOL kinematics uses in place of the head joints: +// the remap writes them. +static const kins_param_desc trsrn_params[] = { + { "tool-offset-z", KINS_PARAM_FLOAT, KINS_IN, 1, 0.0 }, + { "y-pivot", KINS_PARAM_FLOAT, KINS_IN, 0, 0.0 }, + { "z-pivot", KINS_PARAM_FLOAT, KINS_IN, 0, 0.0 }, + { "x-offset", KINS_PARAM_FLOAT, KINS_IN, 0, 0.0 }, + { "y-offset", KINS_PARAM_FLOAT, KINS_IN, 0, 0.0 }, + { "y-rot-axis", KINS_PARAM_FLOAT, KINS_IN, 0, 0.0 }, + { "z-rot-axis", KINS_PARAM_FLOAT, KINS_IN, 0, 0.0 }, + { "pre-rot", KINS_PARAM_FLOAT, KINS_IN, 0, 0.0 }, + { "nut-angle", KINS_PARAM_FLOAT, KINS_IN, 0, 0.0 }, + { "primary-angle", KINS_PARAM_FLOAT, KINS_IN, 0, 0.0 }, + { "secondary-angle", KINS_PARAM_FLOAT, KINS_IN, 0, 0.0 }, +}; +enum { P_TOOL, P_PIVOT, P_ZPIVOT, P_XO, P_YO, P_ROT_AXIS, P_ZROT_AXIS, + P_PRE_ROT, P_NUT, P_PRIM, P_SEC }; + +// geometric offsets of the universal spindle head as defined in the ini file +#define GEOMETRY(p) \ + const double Ly = (p)->geometry[P_PIVOT]; \ + const double Lz = (p)->geometry[P_ZPIVOT]; \ + const double Dx = (p)->geometry[P_XO]; \ + const double Dy = (p)->geometry[P_YO]; \ + const double Dray = (p)->geometry[P_ROT_AXIS] - (Dy + Ly); \ + const double Draz = (p)->geometry[P_ZROT_AXIS] - Lz; \ + const double tc = (p)->geometry[P_PRE_ROT]; \ + const double nu = (p)->geometry[P_NUT]; /* degrees */ \ + const double theta_1 = (p)->geometry[P_PRIM]; /* degrees */ \ + const double theta_2 = (p)->geometry[P_SEC]; /* degrees */ \ + const double Dt = (p)->tool.tran.z /* tool-length offset if G43 is used */ // tool_kins==0: TCP kinematics, using the current spindle joint positions // tool_kins==1: TOOL kinematics, using the angles calculated in remap.py -static int trsrnForward(const double *j, EmcPose * pos, int tool_kins) +static int trsrnForward(const kins_params *p, const double *j, EmcPose * pos, int tool_kins) { - // START of custom variable declaration for Forward kinematics - - // geometric offsets of the universal spindle head as defined in the ini file - double Ly = hal_get_real(haldata->y_pivot); - double Lz = hal_get_real(haldata->z_pivot); - double Dx = hal_get_real(haldata->x_offset); - double Dy = hal_get_real(haldata->y_offset); - double Dray = hal_get_real(haldata->y_rot_axis) - (Dy + Ly); - double Draz = hal_get_real(haldata->z_rot_axis) - Lz; - double tc = hal_get_real(haldata->pre_rot); - double nu = hal_get_real(haldata->nut_angle); // degrees - double theta_1 = hal_get_real(haldata->prim_angle); // degrees - double theta_2 = hal_get_real(haldata->sec_angle); // degrees - - // tool-length offset if G43 is used (offset as defined in the tool editor) - double Dt = hal_get_real(haldata->tool_offset_z); + GEOMETRY(p); // variables used in both, TCP and TOOL kinematics double Sw = sin(j[3]*TO_RAD); @@ -130,8 +91,6 @@ static int trsrnForward(const double *j, EmcPose * pos, int tool_kins) double Py = j[1]; double Pz = j[2]; - // END of custom variable declaration for Forward kinematics - if (!tool_kins) { // ========================= TCP kinematics FORWARD // in TCP we use the current positions of the spindle joints Ss = sin(j[4]*TO_RAD); @@ -144,32 +103,32 @@ static int trsrnForward(const double *j, EmcPose * pos, int tool_kins) s = Cs + Cv*Cv*(1-Cs); t = Sv*Cv*(1-Cs); - pos->tran.x = - (Cp*SvSs - Sp*t)*(Dt + Lz) - - Cp*Dx - + (Cp*CvSs + Sp*r)*Ly - + Dy*Sp - + Dx + pos->tran.x = - (Cp*SvSs - Sp*t)*(Dt + Lz) + - Cp*Dx + + (Cp*CvSs + Sp*r)*Ly + + Dy*Sp + + Dx + Px; - pos->tran.y = - Cp*Cw*Dy - - Cw*Dx*Sp - - Cw*(Dray - Py) - - (Cw*Sp*SvSs + Cp*Cw*t - Sw*s)*(Dt + Lz) - + (CvSs*Cw*Sp - Cp*Cw*r + Sw*t)*Ly - + (Draz - Pz)*Sw - + Dray - + Dy + pos->tran.y = - Cp*Cw*Dy + - Cw*Dx*Sp + - Cw*(Dray - Py) + - (Cw*Sp*SvSs + Cp*Cw*t - Sw*s)*(Dt + Lz) + + (CvSs*Cw*Sp - Cp*Cw*r + Sw*t)*Ly + + (Draz - Pz)*Sw + + Dray + + Dy + Ly; - pos->tran.z = - Cp*Dy*Sw - - Dx*Sp*Sw - - Cw*(Draz - Pz) - - (Sp*SvSs*Sw + Cp*Sw*t + Cw*s)*(Dt + Lz) - + (CvSs*Sp*Sw - Cp*Sw*r - Cw*t)*Ly - - (Dray - Py)*Sw - + Draz - + Dt - + Lz; + pos->tran.z = - Cp*Dy*Sw + - Dx*Sp*Sw + - Cw*(Draz - Pz) + - (Sp*SvSs*Sw + Cp*Sw*t + Cw*s)*(Dt + Lz) + + (CvSs*Sp*Sw - Cp*Sw*r - Cw*t)*Ly + - (Dray - Py)*Sw + + Draz + + Dt + + Lz; pos->a = j[3]; pos->b = j[4]; @@ -187,27 +146,27 @@ static int trsrnForward(const double *j, EmcPose * pos, int tool_kins) s = Cs + Cv*Cv*(1-Cs); t = Sv*Cv*(1-Cs); - pos->tran.x = ((Cs*Ctc - CvSs*Stc)*Cp - (Ctc*CvSs + Stc*r)*Sp)*(Dx + Px) - - (Cs*Ctc - CvSs*Stc)*Dx - + ((Ctc*CvSs + Stc*r)*Cp - + (Cs*Ctc - CvSs*Stc)*Sp)*(Dy + Ly + Py) - - (Ctc*CvSs + Stc*r)*Dy - - (Ctc*SvSs - Stc*t)*(Lz + Pz) + pos->tran.x = ((Cs*Ctc - CvSs*Stc)*Cp - (Ctc*CvSs + Stc*r)*Sp)*(Dx + Px) + - (Cs*Ctc - CvSs*Stc)*Dx + + ((Ctc*CvSs + Stc*r)*Cp + + (Cs*Ctc - CvSs*Stc)*Sp)*(Dy + Ly + Py) + - (Ctc*CvSs + Stc*r)*Dy + - (Ctc*SvSs - Stc*t)*(Lz + Pz) - Ly*Stc; - pos->tran.y = - ((Ctc*CvSs + Cs*Stc)*Cp - (CvSs*Stc - Ctc*r)*Sp)*(Dx + Px) - + (Ctc*CvSs + Cs*Stc)*Dx - - ((CvSs*Stc - Ctc*r)*Cp - + (Ctc*CvSs + Cs*Stc)*Sp)*(Dy + Ly + Py) - + (CvSs*Stc - Ctc*r)*Dy - - Ctc*Ly + pos->tran.y = - ((Ctc*CvSs + Cs*Stc)*Cp - (CvSs*Stc - Ctc*r)*Sp)*(Dx + Px) + + (Ctc*CvSs + Cs*Stc)*Dx + - ((CvSs*Stc - Ctc*r)*Cp + + (Ctc*CvSs + Cs*Stc)*Sp)*(Dy + Ly + Py) + + (CvSs*Stc - Ctc*r)*Dy + - Ctc*Ly + (Stc*SvSs + Ctc*t)*(Lz + Pz); - pos->tran.z = (Cp*SvSs - Sp*t)*(Dx + Px) - + (Sp*SvSs + Cp*t)*(Dy + Ly + Py) - - Dx*SvSs - + (Lz + Pz)*s - - Dy*t + pos->tran.z = (Cp*SvSs - Sp*t)*(Dx + Px) + + (Sp*SvSs + Cp*t)*(Dy + Ly + Py) + - Dx*SvSs + + (Lz + Pz)*s + - Dy*t - Lz; pos->a = j[3]; @@ -222,47 +181,35 @@ static int trsrnForward(const double *j, EmcPose * pos, int tool_kins) return 0; } // trsrnForward() -static int tcpKinematicsForward(const double *j, +static int tcpKinematicsForward(const kins_params *p, kins_scratch *s, + const double *j, EmcPose * pos, const KINEMATICS_FORWARD_FLAGS * fflags, KINEMATICS_INVERSE_FLAGS * iflags) { + (void)s; (void)fflags; (void)iflags; - return trsrnForward(j, pos, 0); + return trsrnForward(p, j, pos, 0); } // tcpKinematicsForward() -static int toolKinematicsForward(const double *j, +static int toolKinematicsForward(const kins_params *p, kins_scratch *s, + const double *j, EmcPose * pos, const KINEMATICS_FORWARD_FLAGS * fflags, KINEMATICS_INVERSE_FLAGS * iflags) { + (void)s; (void)fflags; (void)iflags; - return trsrnForward(j, pos, 1); + return trsrnForward(p, j, pos, 1); } // toolKinematicsForward() -static int trsrnInverse(const EmcPose * pos, double *j, int tool_kins) +// The inverses read the rotary angles from the joint argument, where the +// machine is, as they always have. +static int trsrnInverse(const kins_params *p, const EmcPose * pos, double *j, int tool_kins) { - // START of custom variable declaration for Forward kinematics - - // geometric offsets of the universal spindle head as defined in the ini file - double Ly = hal_get_real(haldata->y_pivot); - double Lz = hal_get_real(haldata->z_pivot); - double Dx = hal_get_real(haldata->x_offset); - double Dy = hal_get_real(haldata->y_offset); - double Dray = hal_get_real(haldata->y_rot_axis) - (Dy + Ly); - double Draz = hal_get_real(haldata->z_rot_axis) - Lz; - double tc = hal_get_real(haldata->pre_rot); - double nu = hal_get_real(haldata->nut_angle); // degrees - double theta_1 = hal_get_real(haldata->prim_angle); // degrees - double theta_2 = hal_get_real(haldata->sec_angle); // degrees - - // tool-length offset if G43 is used (offset as defined in the tool editor) - double Dt = hal_get_real(haldata->tool_offset_z); - - // substitutions as used in mathematical documentation - // including degree -> radians angle conversion + GEOMETRY(p); // variables used in both, TCP and TOOL kinematics double Sw = sin(j[3]*TO_RAD); @@ -271,7 +218,7 @@ static int trsrnInverse(const EmcPose * pos, double *j, int tool_kins) double Cv = cos(nu*TO_RAD); double Stc = sin(tc); double Ctc = cos(tc); - + // in TCP we use the current positions of the spindle joints // in TOOL we will use the angle values calculated in remap.py double Ss = 0; @@ -286,10 +233,8 @@ static int trsrnInverse(const EmcPose * pos, double *j, int tool_kins) // onLy used to be consistent with math in documentation double Qx = pos->tran.x; - double Qy = pos->tran.y; - double Qz = pos->tran.z; - - // END of custom variable declaration for Forward kinematics + double Qy = pos->tran.y; + double Qz = pos->tran.z; if (!tool_kins) { // ========================= TCP kinematics INVERSE // in TCP we use the current positions of the spindle joints @@ -303,25 +248,25 @@ static int trsrnInverse(const EmcPose * pos, double *j, int tool_kins) s = Cs + Cv*Cv*(1-Cs); t = Sv*Cv*(1-Cs); - j[0] = (Cp*SvSs - Sp*t)*(Dt + Lz) - + Cp*Dx - - (Cp*CvSs + Sp*r)*Ly - - Dy*Sp - - Dx + j[0] = (Cp*SvSs - Sp*t)*(Dt + Lz) + + Cp*Dx + - (Cp*CvSs + Sp*r)*Ly + - Dy*Sp + - Dx + Qx; - j[1] = Cp*Dy - + Dx*Sp - - Cw*(Dray + Dy + Ly - Qy) - + (Sp*SvSs + Cp*t)*(Dt + Lz) - - (CvSs*Sp - Cp*r)*Ly - - (Draz + Dt + Lz - Qz)*Sw + j[1] = Cp*Dy + + Dx*Sp + - Cw*(Dray + Dy + Ly - Qy) + + (Sp*SvSs + Cp*t)*(Dt + Lz) + - (CvSs*Sp - Cp*r)*Ly + - (Draz + Dt + Lz - Qz)*Sw + Dray; - j[2] = (Dt + Lz)*s - + Ly*t - - Cw*(Draz + Dt + Lz - Qz) - + (Dray + Dy + Ly - Qy)*Sw + j[2] = (Dt + Lz)*s + + Ly*t + - Cw*(Draz + Dt + Lz - Qz) + + (Dray + Dy + Ly - Qy)*Sw + Draz; j[3] = pos->a; @@ -329,80 +274,84 @@ static int trsrnInverse(const EmcPose * pos, double *j, int tool_kins) j[5] = pos->c; } else { // ========================= TOOL kinematics INVERSE - // in TOOL kinematics we use the articulated joint positions from the TWP - Ss = sin(theta_2*TO_RAD); - Cs = cos(theta_2*TO_RAD); - Sp = sin(theta_1*TO_RAD); - Cp = cos(theta_1*TO_RAD); - CvSs = Cv*Ss; - SvSs = Sv*Ss; - r = Cs + Sv*Sv*(1-Cs); - s = Cs + Cv*Cv*(1-Cs); - t = Sv*Cv*(1-Cs); - - j[0] = Cp*Dx - - (Cp*CvSs + Sp*r)*Ly - + (Cp*SvSs - Sp*t)*Lz - + ((Cp*Cs - CvSs*Sp)*Ctc - - (Cp*CvSs + Sp*r)*Stc)*Qx - - ((Cp*CvSs + Sp*r)*Ctc + (Cp*Cs - CvSs*Sp)*Stc)*Qy - + (Cp*SvSs - Sp*t)*Qz - - Dy*Sp - - Dx; - - j[1] = Cp*Dy - - (CvSs*Sp - Cp*r)*Ly - + (Sp*SvSs + Cp*t)*Lz - + ((Cp*CvSs + Cs*Sp)*Ctc - (CvSs*Sp - Cp*r)*Stc)*Qx - - ((CvSs*Sp - Cp*r)*Ctc + (Cp*CvSs + Cs*Sp)*Stc)*Qy - + (Sp*SvSs + Cp*t)*Qz - + Dx*Sp - - Dy - - Ly; - - j[2] = - (Ctc*SvSs - Stc*t)*Qx - + (Stc*SvSs + Ctc*t)*Qy - + Lz*s - + Qz*s - + Ly*t - - Lz; - - j[3] = pos->a; - j[4] = pos->b; - j[5] = pos->c; + // in TOOL kinematics we use the articulated joint positions from the TWP + Ss = sin(theta_2*TO_RAD); + Cs = cos(theta_2*TO_RAD); + Sp = sin(theta_1*TO_RAD); + Cp = cos(theta_1*TO_RAD); + CvSs = Cv*Ss; + SvSs = Sv*Ss; + r = Cs + Sv*Sv*(1-Cs); + s = Cs + Cv*Cv*(1-Cs); + t = Sv*Cv*(1-Cs); + + j[0] = Cp*Dx + - (Cp*CvSs + Sp*r)*Ly + + (Cp*SvSs - Sp*t)*Lz + + ((Cp*Cs - CvSs*Sp)*Ctc + - (Cp*CvSs + Sp*r)*Stc)*Qx + - ((Cp*CvSs + Sp*r)*Ctc + (Cp*Cs - CvSs*Sp)*Stc)*Qy + + (Cp*SvSs - Sp*t)*Qz + - Dy*Sp + - Dx; + + j[1] = Cp*Dy + - (CvSs*Sp - Cp*r)*Ly + + (Sp*SvSs + Cp*t)*Lz + + ((Cp*CvSs + Cs*Sp)*Ctc - (CvSs*Sp - Cp*r)*Stc)*Qx + - ((CvSs*Sp - Cp*r)*Ctc + (Cp*CvSs + Cs*Sp)*Stc)*Qy + + (Sp*SvSs + Cp*t)*Qz + + Dx*Sp + - Dy + - Ly; + + j[2] = - (Ctc*SvSs - Stc*t)*Qx + + (Stc*SvSs + Ctc*t)*Qy + + Lz*s + + Qz*s + + Ly*t + - Lz; + + j[3] = pos->a; + j[4] = pos->b; + j[5] = pos->c; } return 0; } // trsrnInverse() -static int tcpKinematicsInverse(const EmcPose * pos, +static int tcpKinematicsInverse(const kins_params *p, kins_scratch *s, + const EmcPose * pos, double *j, const KINEMATICS_INVERSE_FLAGS * iflags, KINEMATICS_FORWARD_FLAGS * fflags) { + (void)s; (void)iflags; (void)fflags; - return trsrnInverse(pos, j, 0); + return trsrnInverse(p, pos, j, 0); } // tcpKinematicsInverse() -static int toolKinematicsInverse(const EmcPose * pos, +static int toolKinematicsInverse(const kins_params *p, kins_scratch *s, + const EmcPose * pos, double *j, const KINEMATICS_INVERSE_FLAGS * iflags, KINEMATICS_FORWARD_FLAGS * fflags) { + (void)s; (void)iflags; (void)fflags; - return trsrnInverse(pos, j, 1); + return trsrnInverse(p, pos, j, 1); } // toolKinematicsInverse() // The head answers in the convention already, so the native rotation -// registered with these frames is TOOL_FRAME_SPINDLE. -static int tcpKinematicsToolFrame(const double *j, +// declared with these frames is TOOL_FRAME_SPINDLE. +static int tcpKinematicsToolFrame(const kins_params *p, const double *j, PmRotationMatrix *rot, const KINEMATICS_FORWARD_FLAGS *fflags) { (void)fflags; - double nu = hal_get_real(haldata->nut_angle); // degrees + double nu = p->geometry[P_NUT]; // degrees double Sv = sin(nu*TO_RAD); double Cv = cos(nu*TO_RAD); double Ss = sin(j[4]*TO_RAD); @@ -437,10 +386,11 @@ static int tcpKinematicsToolFrame(const double *j, return 0; } // tcpKinematicsToolFrame() -static int tcpKinematicsWorkFrame(const double *j, +static int tcpKinematicsWorkFrame(const kins_params *p, const double *j, PmRotationMatrix *rot, const KINEMATICS_FORWARD_FLAGS *fflags) { + (void)p; (void)fflags; double Sw = sin(j[3]*TO_RAD); double Cw = cos(j[3]*TO_RAD); @@ -456,23 +406,15 @@ static int tcpKinematicsWorkFrame(const double *j, return 0; } // tcpKinematicsWorkFrame() -static int tcpKinematicsJacobian(const double *j, +static int tcpKinematicsJacobian(const kins_params *p, const double *j, const EmcPose * pos, double jac[EMCMOT_MAX_JOINTS][EMCMOT_MAX_AXIS], const KINEMATICS_INVERSE_FLAGS * iflags) { (void)j; (void)iflags; - - // the same geometry as trsrnInverse(), read the same way - double Ly = hal_get_real(haldata->y_pivot); - double Lz = hal_get_real(haldata->z_pivot); - double Dx = hal_get_real(haldata->x_offset); - double Dy = hal_get_real(haldata->y_offset); - double Dray = hal_get_real(haldata->y_rot_axis) - (Dy + Ly); - double Draz = hal_get_real(haldata->z_rot_axis) - Lz; - double nu = hal_get_real(haldata->nut_angle); // degrees - double Dt = hal_get_real(haldata->tool_offset_z); + GEOMETRY(p); + (void)tc; (void)theta_1; (void)theta_2; double Sv = sin(nu*TO_RAD); double Cv = cos(nu*TO_RAD); @@ -539,7 +481,7 @@ static int tcpKinematicsJacobian(const double *j, return 0; } // tcpKinematicsJacobian() -static int toolKinematicsJacobian(const double *j, +static int toolKinematicsJacobian(const kins_params *p, const double *j, const EmcPose * pos, double jac[EMCMOT_MAX_JOINTS][EMCMOT_MAX_AXIS], const KINEMATICS_INVERSE_FLAGS * iflags) @@ -550,10 +492,10 @@ static int toolKinematicsJacobian(const double *j, // the head angles come from pins, so the inverse is linear in the pose // and the rows are its coefficients - double tc = hal_get_real(haldata->pre_rot); - double nu = hal_get_real(haldata->nut_angle); // degrees - double theta_1 = hal_get_real(haldata->prim_angle); // degrees - double theta_2 = hal_get_real(haldata->sec_angle); // degrees + double tc = p->geometry[P_PRE_ROT]; + double nu = p->geometry[P_NUT]; // degrees + double theta_1 = p->geometry[P_PRIM]; // degrees + double theta_2 = p->geometry[P_SEC]; // degrees double Sv = sin(nu*TO_RAD); double Cv = cos(nu*TO_RAD); @@ -592,43 +534,55 @@ static int toolKinematicsJacobian(const double *j, return 0; } // toolKinematicsJacobian() +static const kins_ops tcp_ops = { + .forward = tcpKinematicsForward, + .inverse = tcpKinematicsInverse, + .work = tcpKinematicsWorkFrame, + .tool = tcpKinematicsToolFrame, + .native = &TOOL_FRAME_SPINDLE, + .jacobian = tcpKinematicsJacobian, +}; + +// the tool kinematics report in tool axes, so the tool is square with the +// world by construction and nothing turns the work against it +static const kins_ops tool_ops = { + .forward = toolKinematicsForward, + .inverse = toolKinematicsInverse, + .work = kinsIdentityFrame, + .tool = kinsIdentityFrame, + .native = &TOOL_FRAME_SPINDLE, + .jacobian = toolKinematicsJacobian, +}; + +int switchkinsSetup(kparms* kp, + KS* kset0, KS* kset1, KS* kset2, + KF* kfwd0, KF* kfwd1, KF* kfwd2, + KI* kinv0, KI* kinv1, KI* kinv2 + ) +{ + (void)kset0; (void)kset1; (void)kset2; + (void)kfwd0; (void)kfwd1; (void)kfwd2; + (void)kinv0; (void)kinv1; (void)kinv2; + kp->kinsname = "xyzacb_trsrn"; + kp->halprefix = "xyzacb_trsrn_kins"; + kp->required_coordinates = "xyzabc"; + kp->allow_duplicates = 0; + kp->max_joints = strlen(kp->required_coordinates); + kp->params = trsrn_params; + kp->nparams = sizeof(trsrn_params)/sizeof(trsrn_params[0]); + + switchkinsRegisterOps(0, &KINS_IDENTITY_OPS); + switchkinsRegisterOps(1, &tcp_ops); + switchkinsRegisterOps(2, &tool_ops); + return 0; +} // switchkinsSetup() + // halcompile has done hal_init() and does hal_ready() after this returns, // which is what switchkinsInit() expects EXTRA_SETUP() { - kparms kp = {0}; + kparms kp; (void)__comp_inst; (void)prefix; (void)extra_arg; - kp.kinsname = "xyzacb_trsrn"; - kp.halprefix = "xyzacb_trsrn_kins"; - kp.required_coordinates = "xyzabc"; - kp.allow_duplicates = 0; - kp.fwd_iterates_mask = 0; - kp.gui_kinstype = -1; - kp.sparm = NULL; - kp.max_joints = strlen(kp.required_coordinates); - - if (switchkinsRegister(0, identityKinematicsSetup, - identityKinematicsForward, - identityKinematicsInverse)) { return -1; } - if (switchkinsRegister(1, trsrnKinematicsSetup, - tcpKinematicsForward, - tcpKinematicsInverse)) { return -1; } - if (switchkinsRegister(2, toolKinematicsSetup, - toolKinematicsForward, - toolKinematicsInverse)) { return -1; } - if (switchkinsRegisterFrames(1, tcpKinematicsWorkFrame, - tcpKinematicsToolFrame, - &TOOL_FRAME_SPINDLE)) { return -1; } - if (switchkinsRegisterJacobian(1, tcpKinematicsJacobian)) { return -1; } - // the tool kinematics report in tool axes, so the tool is square with - // the world by construction and nothing turns the work against it - if (switchkinsRegisterFrames(2, identityKinematicsWorkFrame, - identityKinematicsToolFrame, - &TOOL_FRAME_SPINDLE)) { return -1; } - if (switchkinsRegisterJacobian(2, toolKinematicsJacobian)) { return -1; } - - if (switchkinsDeclare(0, KINSTYPE_IDENTITY)) { return -1; } - if (switchkinsDeclare(1, KINSTYPE_PRIMARY)) { return -1; } - + if (switchkinsRunSetup(&kp, NULL)) { return -1; } return switchkinsInit(comp_id, &kp, coordinates); } // EXTRA_SETUP() diff --git a/src/hal/components/xyzbca_trsrn.comp b/src/hal/components/xyzbca_trsrn.comp index 075e3cbfeb2..151fde7a352 100644 --- a/src/hal/components/xyzbca_trsrn.comp +++ b/src/hal/components/xyzbca_trsrn.comp @@ -25,85 +25,46 @@ author "David Mueller"; static char *coordinates; RTAPI_MP_STRING(coordinates, "Axes-to-joints-ordering"); -static struct haldata { - // these should be parameters really but we want to be able to - // change them for demonstration purposes - hal_real_t x_pivot; - hal_real_t z_pivot; - hal_real_t x_offset; - hal_real_t y_offset; - hal_real_t x_rot_axis; - hal_real_t z_rot_axis; - hal_real_t pre_rot; - hal_real_t nut_angle; - hal_real_t prim_angle; - hal_real_t sec_angle; - - // Parameters used for xyzbca_trsrn kinematics: - - // Declare hal pin pointers used for xyzbca_trsrn kinematics: - - hal_real_t tool_offset_z; -} *haldata; - -// the pins are shared by the TCP and TOOL kinematics; the TOOL type has -// no setup routine of its own -static int trsrnKinematicsSetup(const int comp_id, - const char* coords, - kparms* kp) -{ - int res = 0; - (void)coords; - - haldata = hal_malloc(sizeof(struct haldata)); - if (!haldata) return -1; - - res += hal_pin_new_real(comp_id, HAL_IN, &haldata->tool_offset_z, 0.0, "%s.tool-offset-z", kp->halprefix); - res += hal_pin_new_real(comp_id, HAL_IN, &haldata->x_pivot, 0.0, "%s.x-pivot", kp->halprefix); - res += hal_pin_new_real(comp_id, HAL_IN, &haldata->z_pivot, 0.0, "%s.z-pivot", kp->halprefix); - res += hal_pin_new_real(comp_id, HAL_IN, &haldata->x_offset, 0.0, "%s.x-offset", kp->halprefix); - res += hal_pin_new_real(comp_id, HAL_IN, &haldata->y_offset, 0.0, "%s.y-offset", kp->halprefix); - res += hal_pin_new_real(comp_id, HAL_IN, &haldata->x_rot_axis, 0.0, "%s.x-rot-axis", kp->halprefix); - res += hal_pin_new_real(comp_id, HAL_IN, &haldata->z_rot_axis, 0.0, "%s.z-rot-axis", kp->halprefix); - res += hal_pin_new_real(comp_id, HAL_IN, &haldata->pre_rot, 0.0, "%s.pre-rot", kp->halprefix); - res += hal_pin_new_real(comp_id, HAL_IN, &haldata->nut_angle, 0.0, "%s.nut-angle", kp->halprefix); - res += hal_pin_new_real(comp_id, HAL_IN, &haldata->prim_angle, 0.0, "%s.primary-angle", kp->halprefix); - res += hal_pin_new_real(comp_id, HAL_IN, &haldata->sec_angle, 0.0, "%s.secondary-angle", kp->halprefix); - if (res) return -1; - - return 0; -} // trsrnKinematicsSetup() - -static int toolKinematicsSetup(const int comp_id, - const char* coords, - kparms* kp) -{ - (void)comp_id; - (void)coords; - (void)kp; - return 0; // pins created by trsrnKinematicsSetup() -} // toolKinematicsSetup() +// The geometry of the universal spindle head, one pin each, shared by the +// TCP and TOOL kinematics; the maths reads it from the block (see +// kinematics.h) and the tool length from p->tool.tran.z. The two angle +// pins are what the TOOL kinematics uses in place of the head joints: +// the remap writes them. +static const kins_param_desc trsrn_params[] = { + { "tool-offset-z", KINS_PARAM_FLOAT, KINS_IN, 1, 0.0 }, + { "x-pivot", KINS_PARAM_FLOAT, KINS_IN, 0, 0.0 }, + { "z-pivot", KINS_PARAM_FLOAT, KINS_IN, 0, 0.0 }, + { "x-offset", KINS_PARAM_FLOAT, KINS_IN, 0, 0.0 }, + { "y-offset", KINS_PARAM_FLOAT, KINS_IN, 0, 0.0 }, + { "x-rot-axis", KINS_PARAM_FLOAT, KINS_IN, 0, 0.0 }, + { "z-rot-axis", KINS_PARAM_FLOAT, KINS_IN, 0, 0.0 }, + { "pre-rot", KINS_PARAM_FLOAT, KINS_IN, 0, 0.0 }, + { "nut-angle", KINS_PARAM_FLOAT, KINS_IN, 0, 0.0 }, + { "primary-angle", KINS_PARAM_FLOAT, KINS_IN, 0, 0.0 }, + { "secondary-angle", KINS_PARAM_FLOAT, KINS_IN, 0, 0.0 }, +}; +enum { P_TOOL, P_PIVOT, P_ZPIVOT, P_XO, P_YO, P_ROT_AXIS, P_ZROT_AXIS, + P_PRE_ROT, P_NUT, P_PRIM, P_SEC }; + +// geometric offsets of the universal spindle head as defined in the ini file +#define GEOMETRY(p) \ + const double Lx = (p)->geometry[P_PIVOT]; \ + const double Lz = (p)->geometry[P_ZPIVOT]; \ + const double Dx = (p)->geometry[P_XO]; \ + const double Dy = (p)->geometry[P_YO]; \ + const double Drax = (p)->geometry[P_ROT_AXIS] - Lx - Dx; \ + const double Draz = (p)->geometry[P_ZROT_AXIS] - Lz; \ + const double tc = (p)->geometry[P_PRE_ROT]; \ + const double nu = (p)->geometry[P_NUT]; /* degrees */ \ + const double theta_1 = (p)->geometry[P_PRIM]; /* degrees */ \ + const double theta_2 = (p)->geometry[P_SEC]; /* degrees */ \ + const double Dt = (p)->tool.tran.z /* tool-length offset if G43 is used */ // tool_kins==0: TCP kinematics, using the current spindle joint positions // tool_kins==1: TOOL kinematics, using the angles calculated in remap.py -static int trsrnForward(const double *j, EmcPose * pos, int tool_kins) +static int trsrnForward(const kins_params *p, const double *j, EmcPose * pos, int tool_kins) { - // START of custom variable declaration for Forward kinematics - - // geometric offsets of the universal spindle head as defined in the ini file - double Lx = hal_get_real(haldata->x_pivot); - double Lz = hal_get_real(haldata->z_pivot); - double Dx = hal_get_real(haldata->x_offset); - double Dy = hal_get_real(haldata->y_offset); - double Drax = hal_get_real(haldata->x_rot_axis) - Lx- Dx; - double Draz = hal_get_real(haldata->z_rot_axis) - Lz; - double tc = hal_get_real(haldata->pre_rot); - double nu = hal_get_real(haldata->nut_angle); // degrees - double theta_1 = hal_get_real(haldata->prim_angle); // degrees - double theta_2 = hal_get_real(haldata->sec_angle); // degrees - - // tool-length offset if G43 is used (offset as defined in the tool editor) - double Dt = hal_get_real(haldata->tool_offset_z); + GEOMETRY(p); // variables used in both, TCP and TOOL kinematics double Sw = sin(j[4]*TO_RAD); @@ -130,9 +91,6 @@ static int trsrnForward(const double *j, EmcPose * pos, int tool_kins) double Py = j[1]; double Pz = j[2]; - // END of custom variable declaration for Forward kinematics - - if (!tool_kins) { // ========================= TCP kinematics FORWARD // in TCP we use the current positions of the spindle joints Ss = sin(j[3]*TO_RAD); @@ -144,37 +102,33 @@ static int trsrnForward(const double *j, EmcPose * pos, int tool_kins) r = Cs + Sv*Sv*(1-Cs); s = Cs + Cv*Cv*(1-Cs); t = Sv*Cv*(1-Cs); - // onLy used to be consistent with math in documentation - Px = j[0]; - Py = j[1]; - Pz = j[2]; - - pos->tran.x = - Cp*Cw*Dx - + Cw*Dy*Sp - - Cw*(Drax - Px) - - (Cw*Sp*SvSs + Cp*Cw*t + Sw*s)*(Dt + Lz) - + (CvSs*Cw*Sp - Cp*Cw*r - Sw*t)*Lx - - (Draz - Pz)*Sw - + Drax - + Dx + + pos->tran.x = - Cp*Cw*Dx + + Cw*Dy*Sp + - Cw*(Drax - Px) + - (Cw*Sp*SvSs + Cp*Cw*t + Sw*s)*(Dt + Lz) + + (CvSs*Cw*Sp - Cp*Cw*r - Sw*t)*Lx + - (Draz - Pz)*Sw + + Drax + + Dx + Lx; - pos->tran.y = (Cp*SvSs - Sp*t)*(Dt + Lz) - - Cp*Dy - - (Cp*CvSs + Sp*r)*Lx - - Dx*Sp - + Dy + pos->tran.y = (Cp*SvSs - Sp*t)*(Dt + Lz) + - Cp*Dy + - (Cp*CvSs + Sp*r)*Lx + - Dx*Sp + + Dy + Py; - pos->tran.z = Cp*Dx*Sw - - Dy*Sp*Sw - - Cw*(Draz - Pz) - + (Sp*SvSs*Sw + Cp*Sw*t - Cw*s)*(Dt + Lz) - - (CvSs*Sp*Sw - Cp*Sw*r + Cw*t)*Lx - + (Drax - Px)*Sw - + Draz - + Dt - + Lz; + pos->tran.z = Cp*Dx*Sw + - Dy*Sp*Sw + - Cw*(Draz - Pz) + + (Sp*SvSs*Sw + Cp*Sw*t - Cw*s)*(Dt + Lz) + - (CvSs*Sp*Sw - Cp*Sw*r + Cw*t)*Lx + + (Drax - Px)*Sw + + Draz + + Dt + + Lz; pos->a = j[3]; pos->b = j[4]; @@ -192,27 +146,25 @@ static int trsrnForward(const double *j, EmcPose * pos, int tool_kins) s = Cs + Cv*Cv*(1-Cs); t = Sv*Cv*(1-Cs); - pos->tran.x = - ((CvSs*Stc - Ctc*r)*Cp + (Ctc*CvSs + Cs*Stc)*Sp)*(Dx + Lx + Px) - + (CvSs*Stc - Ctc*r)*Dx - + ((Ctc*CvSs + Cs*Stc)*Cp - (CvSs*Stc - Ctc*r)*Sp)*(Dy + Py) - - (Ctc*CvSs + Cs*Stc)*Dy - - Ctc*Lx + pos->tran.x = - ((CvSs*Stc - Ctc*r)*Cp + (Ctc*CvSs + Cs*Stc)*Sp)*(Dx + Lx + Px) + + (CvSs*Stc - Ctc*r)*Dx + + ((Ctc*CvSs + Cs*Stc)*Cp - (CvSs*Stc - Ctc*r)*Sp)*(Dy + Py) + - (Ctc*CvSs + Cs*Stc)*Dy + - Ctc*Lx + (Stc*SvSs + Ctc*t)*(Lz + Pz); - - pos->tran.y = - ((Ctc*CvSs + Stc*r)*Cp + (Cs*Ctc - CvSs*Stc)*Sp)*(Dx + Lx + Px) - + (Ctc*CvSs + Stc*r)*Dx - + ((Cs*Ctc - CvSs*Stc)*Cp - (Ctc*CvSs + Stc*r)*Sp)*(Dy + Py) - - (Cs*Ctc - CvSs*Stc)*Dy - + (Ctc*SvSs - Stc*t)*(Lz + Pz) + pos->tran.y = - ((Ctc*CvSs + Stc*r)*Cp + (Cs*Ctc - CvSs*Stc)*Sp)*(Dx + Lx + Px) + + (Ctc*CvSs + Stc*r)*Dx + + ((Cs*Ctc - CvSs*Stc)*Cp - (Ctc*CvSs + Stc*r)*Sp)*(Dy + Py) + - (Cs*Ctc - CvSs*Stc)*Dy + + (Ctc*SvSs - Stc*t)*(Lz + Pz) + Lx*Stc; - - pos->tran.z = (Sp*SvSs + Cp*t)*(Dx + Lx + Px) - - (Cp*SvSs - Sp*t)*(Dy + Py) - + Dy*SvSs - + (Lz + Pz)*s - - Dx*t + pos->tran.z = (Sp*SvSs + Cp*t)*(Dx + Lx + Px) + - (Cp*SvSs - Sp*t)*(Dy + Py) + + Dy*SvSs + + (Lz + Pz)*s + - Dx*t - Lz; pos->a = j[3]; @@ -227,44 +179,35 @@ static int trsrnForward(const double *j, EmcPose * pos, int tool_kins) return 0; } // trsrnForward() -static int tcpKinematicsForward(const double *j, +static int tcpKinematicsForward(const kins_params *p, kins_scratch *s, + const double *j, EmcPose * pos, const KINEMATICS_FORWARD_FLAGS * fflags, KINEMATICS_INVERSE_FLAGS * iflags) { + (void)s; (void)fflags; (void)iflags; - return trsrnForward(j, pos, 0); + return trsrnForward(p, j, pos, 0); } // tcpKinematicsForward() -static int toolKinematicsForward(const double *j, +static int toolKinematicsForward(const kins_params *p, kins_scratch *s, + const double *j, EmcPose * pos, const KINEMATICS_FORWARD_FLAGS * fflags, KINEMATICS_INVERSE_FLAGS * iflags) { + (void)s; (void)fflags; (void)iflags; - return trsrnForward(j, pos, 1); + return trsrnForward(p, j, pos, 1); } // toolKinematicsForward() -static int trsrnInverse(const EmcPose * pos, double *j, int tool_kins) +// The inverses read the rotary angles from the joint argument, where the +// machine is, as they always have. +static int trsrnInverse(const kins_params *p, const EmcPose * pos, double *j, int tool_kins) { - // START of custom variable declaration for Forward kinematics - - // geometric offsets of the universal spindle head as defined in the ini file - double Lx = hal_get_real(haldata->x_pivot); - double Lz = hal_get_real(haldata->z_pivot); - double Dx = hal_get_real(haldata->x_offset); - double Dy = hal_get_real(haldata->y_offset); - double Drax = hal_get_real(haldata->x_rot_axis) - Lx - Dx; - double Draz = hal_get_real(haldata->z_rot_axis) - Lz; - double tc = hal_get_real(haldata->pre_rot); - double nu = hal_get_real(haldata->nut_angle); // degrees - double theta_1 = hal_get_real(haldata->prim_angle); // degrees - double theta_2 = hal_get_real(haldata->sec_angle); // degrees - - // tool-length offset if G43 is used (offset as defined in the tool editor) - double Dt = hal_get_real(haldata->tool_offset_z); + GEOMETRY(p); // variables used in both, TCP and TOOL kinematics double Sw = sin(j[4]*TO_RAD); @@ -288,11 +231,8 @@ static int trsrnInverse(const EmcPose * pos, double *j, int tool_kins) // onLy used to be consistent with math in documentation double Qx = pos->tran.x; - double Qy = pos->tran.y; - double Qz = pos->tran.z; - - // END of custom variable declaration for Forward kinematics - + double Qy = pos->tran.y; + double Qz = pos->tran.z; if (!tool_kins) { // ========================= TCP kinematics INVERSE // in TCP we use the current positions of the spindle joints @@ -304,27 +244,27 @@ static int trsrnInverse(const EmcPose * pos, double *j, int tool_kins) SvSs = Sv*Ss; r = Cs + Sv*Sv*(1-Cs); s = Cs + Cv*Cv*(1-Cs); - t = Sv*Cv*(1-Cs); - - j[0] = Cp*Dx - - Dy*Sp - - Cw*(Drax + Dx + Lx - Qx) - + (Sp*SvSs + Cp*t)*(Dt + Lz) - - (CvSs*Sp - Cp*r)*Lx - + (Draz + Dt + Lz - Qz)*Sw + t = Sv*Cv*(1-Cs); + + j[0] = Cp*Dx + - Dy*Sp + - Cw*(Drax + Dx + Lx - Qx) + + (Sp*SvSs + Cp*t)*(Dt + Lz) + - (CvSs*Sp - Cp*r)*Lx + + (Draz + Dt + Lz - Qz)*Sw + Drax; - j[1] = - (Cp*SvSs - Sp*t)*(Dt + Lz) - + Cp*Dy - + (Cp*CvSs + Sp*r)*Lx - + Dx*Sp - - Dy + j[1] = - (Cp*SvSs - Sp*t)*(Dt + Lz) + + Cp*Dy + + (Cp*CvSs + Sp*r)*Lx + + Dx*Sp + - Dy + Qy; - j[2] = (Dt + Lz)*s - + Lx*t - - Cw*(Draz + Dt + Lz - Qz) - - (Drax + Dx + Lx - Qx)*Sw + j[2] = (Dt + Lz)*s + + Lx*t + - Cw*(Draz + Dt + Lz - Qz) + - (Drax + Dx + Lx - Qx)*Sw + Draz; j[3] = pos->a; @@ -342,32 +282,31 @@ static int trsrnInverse(const EmcPose * pos, double *j, int tool_kins) r = Cs + Sv*Sv*(1-Cs); s = Cs + Cv*Cv*(1-Cs); t = Sv*Cv*(1-Cs); - - j[0] = Cp*Dx - - (CvSs*Sp - Cp*r)*Lx - + (Sp*SvSs + Cp*t)*Lz - - ((CvSs*Sp - Cp*r)*Ctc + (Cp*CvSs + Cs*Sp)*Stc)*Qx - - ((Cp*CvSs + Cs*Sp)*Ctc - (CvSs*Sp - Cp*r)*Stc)*Qy - + (Sp*SvSs + Cp*t)*Qz - - Dy*Sp - - Dx + + j[0] = Cp*Dx + - (CvSs*Sp - Cp*r)*Lx + + (Sp*SvSs + Cp*t)*Lz + - ((CvSs*Sp - Cp*r)*Ctc + (Cp*CvSs + Cs*Sp)*Stc)*Qx + - ((Cp*CvSs + Cs*Sp)*Ctc - (CvSs*Sp - Cp*r)*Stc)*Qy + + (Sp*SvSs + Cp*t)*Qz + - Dy*Sp + - Dx - Lx; - j[1] = Cp*Dy - + (Cp*CvSs + Sp*r)*Lx - - (Cp*SvSs - Sp*t)*Lz - + ((Cp*CvSs + Sp*r)*Ctc + (Cp*Cs - CvSs*Sp)*Stc)*Qx - + ((Cp*Cs - CvSs*Sp)*Ctc - (Cp*CvSs + Sp*r)*Stc)*Qy - - (Cp*SvSs - Sp*t)*Qz - + Dx*Sp + j[1] = Cp*Dy + + (Cp*CvSs + Sp*r)*Lx + - (Cp*SvSs - Sp*t)*Lz + + ((Cp*CvSs + Sp*r)*Ctc + (Cp*Cs - CvSs*Sp)*Stc)*Qx + + ((Cp*Cs - CvSs*Sp)*Ctc - (Cp*CvSs + Sp*r)*Stc)*Qy + - (Cp*SvSs - Sp*t)*Qz + + Dx*Sp - Dy; - - j[2] = (Stc*SvSs + Ctc*t)*Qx - + (Ctc*SvSs - Stc*t)*Qy - + Lz*s - + Qz*s - + Lx*t + j[2] = (Stc*SvSs + Ctc*t)*Qx + + (Ctc*SvSs - Stc*t)*Qy + + Lz*s + + Qz*s + + Lx*t - Lz; j[3] = pos->a; @@ -378,34 +317,38 @@ static int trsrnInverse(const EmcPose * pos, double *j, int tool_kins) return 0; } // trsrnInverse() -static int tcpKinematicsInverse(const EmcPose * pos, +static int tcpKinematicsInverse(const kins_params *p, kins_scratch *s, + const EmcPose * pos, double *j, const KINEMATICS_INVERSE_FLAGS * iflags, KINEMATICS_FORWARD_FLAGS * fflags) { + (void)s; (void)iflags; (void)fflags; - return trsrnInverse(pos, j, 0); + return trsrnInverse(p, pos, j, 0); } // tcpKinematicsInverse() -static int toolKinematicsInverse(const EmcPose * pos, +static int toolKinematicsInverse(const kins_params *p, kins_scratch *s, + const EmcPose * pos, double *j, const KINEMATICS_INVERSE_FLAGS * iflags, KINEMATICS_FORWARD_FLAGS * fflags) { + (void)s; (void)iflags; (void)fflags; - return trsrnInverse(pos, j, 1); + return trsrnInverse(p, pos, j, 1); } // toolKinematicsInverse() // The head answers in the convention already, so the native rotation -// registered with these frames is TOOL_FRAME_SPINDLE. -static int tcpKinematicsToolFrame(const double *j, +// declared with these frames is TOOL_FRAME_SPINDLE. +static int tcpKinematicsToolFrame(const kins_params *p, const double *j, PmRotationMatrix *rot, const KINEMATICS_FORWARD_FLAGS *fflags) { (void)fflags; - double nu = hal_get_real(haldata->nut_angle); // degrees + double nu = p->geometry[P_NUT]; // degrees double Sv = sin(nu*TO_RAD); double Cv = cos(nu*TO_RAD); double Ss = sin(j[3]*TO_RAD); @@ -440,10 +383,11 @@ static int tcpKinematicsToolFrame(const double *j, return 0; } // tcpKinematicsToolFrame() -static int tcpKinematicsWorkFrame(const double *j, +static int tcpKinematicsWorkFrame(const kins_params *p, const double *j, PmRotationMatrix *rot, const KINEMATICS_FORWARD_FLAGS *fflags) { + (void)p; (void)fflags; double Sw = sin(j[4]*TO_RAD); double Cw = cos(j[4]*TO_RAD); @@ -459,23 +403,15 @@ static int tcpKinematicsWorkFrame(const double *j, return 0; } // tcpKinematicsWorkFrame() -static int tcpKinematicsJacobian(const double *j, +static int tcpKinematicsJacobian(const kins_params *p, const double *j, const EmcPose * pos, double jac[EMCMOT_MAX_JOINTS][EMCMOT_MAX_AXIS], const KINEMATICS_INVERSE_FLAGS * iflags) { (void)j; (void)iflags; - - // the same geometry as trsrnInverse(), read the same way - double Lx = hal_get_real(haldata->x_pivot); - double Lz = hal_get_real(haldata->z_pivot); - double Dx = hal_get_real(haldata->x_offset); - double Dy = hal_get_real(haldata->y_offset); - double Drax = hal_get_real(haldata->x_rot_axis) - Lx - Dx; - double Draz = hal_get_real(haldata->z_rot_axis) - Lz; - double nu = hal_get_real(haldata->nut_angle); // degrees - double Dt = hal_get_real(haldata->tool_offset_z); + GEOMETRY(p); + (void)tc; (void)theta_1; (void)theta_2; double Sv = sin(nu*TO_RAD); double Cv = cos(nu*TO_RAD); @@ -542,7 +478,7 @@ static int tcpKinematicsJacobian(const double *j, return 0; } // tcpKinematicsJacobian() -static int toolKinematicsJacobian(const double *j, +static int toolKinematicsJacobian(const kins_params *p, const double *j, const EmcPose * pos, double jac[EMCMOT_MAX_JOINTS][EMCMOT_MAX_AXIS], const KINEMATICS_INVERSE_FLAGS * iflags) @@ -553,10 +489,10 @@ static int toolKinematicsJacobian(const double *j, // the head angles come from pins, so the inverse is linear in the pose // and the rows are its coefficients - double tc = hal_get_real(haldata->pre_rot); - double nu = hal_get_real(haldata->nut_angle); // degrees - double theta_1 = hal_get_real(haldata->prim_angle); // degrees - double theta_2 = hal_get_real(haldata->sec_angle); // degrees + double tc = p->geometry[P_PRE_ROT]; + double nu = p->geometry[P_NUT]; // degrees + double theta_1 = p->geometry[P_PRIM]; // degrees + double theta_2 = p->geometry[P_SEC]; // degrees double Sv = sin(nu*TO_RAD); double Cv = cos(nu*TO_RAD); @@ -595,43 +531,55 @@ static int toolKinematicsJacobian(const double *j, return 0; } // toolKinematicsJacobian() +static const kins_ops tcp_ops = { + .forward = tcpKinematicsForward, + .inverse = tcpKinematicsInverse, + .work = tcpKinematicsWorkFrame, + .tool = tcpKinematicsToolFrame, + .native = &TOOL_FRAME_SPINDLE, + .jacobian = tcpKinematicsJacobian, +}; + +// the tool kinematics report in tool axes, so the tool is square with the +// world by construction and nothing turns the work against it +static const kins_ops tool_ops = { + .forward = toolKinematicsForward, + .inverse = toolKinematicsInverse, + .work = kinsIdentityFrame, + .tool = kinsIdentityFrame, + .native = &TOOL_FRAME_SPINDLE, + .jacobian = toolKinematicsJacobian, +}; + +int switchkinsSetup(kparms* kp, + KS* kset0, KS* kset1, KS* kset2, + KF* kfwd0, KF* kfwd1, KF* kfwd2, + KI* kinv0, KI* kinv1, KI* kinv2 + ) +{ + (void)kset0; (void)kset1; (void)kset2; + (void)kfwd0; (void)kfwd1; (void)kfwd2; + (void)kinv0; (void)kinv1; (void)kinv2; + kp->kinsname = "xyzbca_trsrn"; + kp->halprefix = "xyzbca_trsrn_kins"; + kp->required_coordinates = "xyzabc"; + kp->allow_duplicates = 0; + kp->max_joints = strlen(kp->required_coordinates); + kp->params = trsrn_params; + kp->nparams = sizeof(trsrn_params)/sizeof(trsrn_params[0]); + + switchkinsRegisterOps(0, &KINS_IDENTITY_OPS); + switchkinsRegisterOps(1, &tcp_ops); + switchkinsRegisterOps(2, &tool_ops); + return 0; +} // switchkinsSetup() + // halcompile has done hal_init() and does hal_ready() after this returns, // which is what switchkinsInit() expects EXTRA_SETUP() { - kparms kp = {0}; + kparms kp; (void)__comp_inst; (void)prefix; (void)extra_arg; - kp.kinsname = "xyzbca_trsrn"; - kp.halprefix = "xyzbca_trsrn_kins"; - kp.required_coordinates = "xyzabc"; - kp.allow_duplicates = 0; - kp.fwd_iterates_mask = 0; - kp.gui_kinstype = -1; - kp.sparm = NULL; - kp.max_joints = strlen(kp.required_coordinates); - - if (switchkinsRegister(0, identityKinematicsSetup, - identityKinematicsForward, - identityKinematicsInverse)) { return -1; } - if (switchkinsRegister(1, trsrnKinematicsSetup, - tcpKinematicsForward, - tcpKinematicsInverse)) { return -1; } - if (switchkinsRegister(2, toolKinematicsSetup, - toolKinematicsForward, - toolKinematicsInverse)) { return -1; } - if (switchkinsRegisterFrames(1, tcpKinematicsWorkFrame, - tcpKinematicsToolFrame, - &TOOL_FRAME_SPINDLE)) { return -1; } - if (switchkinsRegisterJacobian(1, tcpKinematicsJacobian)) { return -1; } - // the tool kinematics report in tool axes, so the tool is square with - // the world by construction and nothing turns the work against it - if (switchkinsRegisterFrames(2, identityKinematicsWorkFrame, - identityKinematicsToolFrame, - &TOOL_FRAME_SPINDLE)) { return -1; } - if (switchkinsRegisterJacobian(2, toolKinematicsJacobian)) { return -1; } - - if (switchkinsDeclare(0, KINSTYPE_IDENTITY)) { return -1; } - if (switchkinsDeclare(1, KINSTYPE_PRIMARY)) { return -1; } - + if (switchkinsRunSetup(&kp, NULL)) { return -1; } return switchkinsInit(comp_id, &kp, coordinates); } // EXTRA_SETUP() From 0cfaea93bd2a2592ef01fc647d0904e9a5addd16 Mon Sep 17 00:00:00 2001 From: Luca Toniolo <10792599+grandixximo@users.noreply.github.com> Date: Fri, 4 Sep 2026 13:13:52 +1000 Subject: [PATCH 33/60] genserkins, genhexkins, pentakins: move onto the parameter block The three that build a geometry from many pins build it from the block on each call: genser its link description, genhex its base and platform points, pentakins its base points and effector circles. What they reported through output pins, iteration counts, strut corrections, the hexapod's fwd-kins-fail and gui pose, goes through the scratch, so each caller keeps its own. genhex and pentakins declare their forward as iterating and the switchkins core seeds it as before. genhexkins's six gui pose pins were inputs written by the module and are outputs now; pentakins's HAL parameters become pins of the same names. genserfuncs.c loses its haldata and globals, and ugenserkins builds a block and calls the ops. --- src/Makefile | 2 + src/emc/kinematics/Submakefile | 1 + src/emc/kinematics/genhexkins.c | 529 ++++++++++++++----------------- src/emc/kinematics/genserfuncs.c | 296 +++++++---------- src/emc/kinematics/genserkins.c | 36 +-- src/emc/kinematics/genserkins.h | 36 +-- src/emc/kinematics/pentakins.c | 280 ++++++++-------- src/emc/kinematics/ugenserkins.c | 36 ++- 8 files changed, 515 insertions(+), 701 deletions(-) diff --git a/src/Makefile b/src/Makefile index 2cef5765d25..71a9f7f06b0 100644 --- a/src/Makefile +++ b/src/Makefile @@ -1164,6 +1164,8 @@ lineardeltakins-objs += emc/kinematics/kins_single.o obj-m += pentakins.o pentakins-objs := emc/kinematics/pentakins.o +pentakins-objs += emc/kinematics/kins_util.o +pentakins-objs += emc/kinematics/kins_single.o pentakins-objs += libposemath/_posemath.o pentakins-objs += $(MATHSTUB) diff --git a/src/emc/kinematics/Submakefile b/src/emc/kinematics/Submakefile index c71c18696e2..dbbc783f21b 100644 --- a/src/emc/kinematics/Submakefile +++ b/src/emc/kinematics/Submakefile @@ -2,6 +2,7 @@ GENSERKINSSRCS := emc/kinematics/ugenserkins.c GENSERKINSSRCS += emc/kinematics/genserfuncs.c +GENSERKINSSRCS += emc/kinematics/kins_util.c USERSRCS += $(GENSERKINSSRCS) DELTAMODULESRCS := emc/kinematics/lineardeltakins.cc diff --git a/src/emc/kinematics/genhexkins.c b/src/emc/kinematics/genhexkins.c index b4750e5c540..1efebaba756 100644 --- a/src/emc/kinematics/genhexkins.c +++ b/src/emc/kinematics/genhexkins.c @@ -16,17 +16,17 @@ machines referred to as "Stewart Platforms". The functions are general enough to be configured for any platform - configuration. In the functions "genhexKinematicsForward" and - "genhexKinematicsInverse" are arrays "a[i]" and "b[i]". The values stored - in these arrays correspond to the positions of the ends of the i'th - strut. The value stored in a[i] is the position of the end of the i'th - strut attached to the platform, in platform coordinates. The value - stored in b[i] is the position of the end of the i'th strut attached - to the base, in base (world) coordinates. + configuration. In the functions "genhex_forward" and "genhex_inverse" + are arrays "a[i]" and "b[i]". The values stored in these arrays + correspond to the positions of the ends of the i'th strut. The value + stored in a[i] is the position of the end of the i'th strut attached + to the platform, in platform coordinates. The value stored in b[i] is + the position of the end of the i'th strut attached to the base, in + base (world) coordinates. The default values for base and platform joints positions are defined in the header file genhexkins.h. The actual values for a particular - machine can be adjusted by hal parameters: + machine can be adjusted by hal pins: genhexkins.base.N.x genhexkins.base.N.y @@ -67,18 +67,18 @@ genhexkins.correction.N - pins showing current values of strut length correction. - The genhexKinematicsInverse function solves the inverse kinematics using + The genhex_inverse function solves the inverse kinematics using a closed form algorithm. The inverse kinematics problem is given the pose of the platform and returns the strut lengths. For this problem there is only one solution that is always returned correctly. - The genhexKinematicsForward function solves the forward kinematics using + The genhex_forward function solves the forward kinematics using an iterative algorithm. Due to the iterative nature of this algorithm - the genhexKinematicsForward function requires an initial value to begin the + the genhex_forward function requires an initial value to begin the iterative routine and then converges to the "nearest" solution. The forward kinematics problem is given the strut lengths and returns the pose of the platform. For this problem there arein multiple - solutions. The genhexKinematicsForward function will return only one of + solutions. The genhex_forward function will return only one of these solutions which will be the solution nearest to the initial value given. It is possible that there are no solutions "near" the given initial value and the iteration will not converge and no @@ -103,6 +103,10 @@ genhexkins.max-iterations - maximum number of iterations spent for a converged solution during current session. + The maths is written as pure functions of the parameter block (see + kinematics.h): the pins above are the table below, read into the block + before every call and written from the scratch after it. + ----------------------------------------------------------------------------*/ #include @@ -114,49 +118,98 @@ #include "genhexkins.h" #include -static struct haldata { - hal_real_t basex[NUM_STRUTS]; - hal_real_t basey[NUM_STRUTS]; - hal_real_t basez[NUM_STRUTS]; - hal_real_t platformx[NUM_STRUTS]; - hal_real_t platformy[NUM_STRUTS]; - hal_real_t platformz[NUM_STRUTS]; - hal_real_t basenx[NUM_STRUTS]; - hal_real_t baseny[NUM_STRUTS]; - hal_real_t basenz[NUM_STRUTS]; - hal_real_t platformnx[NUM_STRUTS]; - hal_real_t platformny[NUM_STRUTS]; - hal_real_t platformnz[NUM_STRUTS]; - hal_real_t correction[NUM_STRUTS]; - hal_real_t screw_lead; - hal_uint_t last_iter; - hal_uint_t max_iter; - hal_uint_t iter_limit; - hal_real_t max_error; - hal_real_t conv_criterion; - hal_real_t tool_offset; - hal_real_t spindle_offset; - hal_bool_t fwd_kins_fail; - - hal_real_t gui_x; - hal_real_t gui_y; - hal_real_t gui_z; - hal_real_t gui_a; - hal_real_t gui_b; - hal_real_t gui_c; - -} *haldata; - -static int genhex_gui_forward_kins(EmcPose *pos) -{ - hal_set_real(haldata->gui_x, pos->tran.x); - hal_set_real(haldata->gui_y, pos->tran.y); - hal_set_real(haldata->gui_z, pos->tran.z); - hal_set_real(haldata->gui_a, pos->a); - hal_set_real(haldata->gui_b, pos->b); - hal_set_real(haldata->gui_c, pos->c); - return 0; -} // genhex_gui_forward_kins +// the table: thirteen entries per strut, then the iteration controls, +// the offsets and the reports. The macros index it. +#define STRUT_ENTRIES 13 +#define P_BASE_X(i) (STRUT_ENTRIES*(i) + 0) +#define P_BASE_Y(i) (STRUT_ENTRIES*(i) + 1) +#define P_BASE_Z(i) (STRUT_ENTRIES*(i) + 2) +#define P_PLAT_X(i) (STRUT_ENTRIES*(i) + 3) +#define P_PLAT_Y(i) (STRUT_ENTRIES*(i) + 4) +#define P_PLAT_Z(i) (STRUT_ENTRIES*(i) + 5) +#define P_BASE_NX(i) (STRUT_ENTRIES*(i) + 6) +#define P_BASE_NY(i) (STRUT_ENTRIES*(i) + 7) +#define P_BASE_NZ(i) (STRUT_ENTRIES*(i) + 8) +#define P_PLAT_NX(i) (STRUT_ENTRIES*(i) + 9) +#define P_PLAT_NY(i) (STRUT_ENTRIES*(i) + 10) +#define P_PLAT_NZ(i) (STRUT_ENTRIES*(i) + 11) +#define P_CORR(i) (STRUT_ENTRIES*(i) + 12) +enum { + P_LAST_ITER = STRUT_ENTRIES*NUM_STRUTS, + P_MAX_ITER, + P_MAX_ERROR, + P_CONV_CRITERION, + P_ITER_LIMIT, + P_TOOL_OFFSET, + P_SPINDLE_OFFSET, + P_SCREW_LEAD, + P_GUI_X, P_GUI_Y, P_GUI_Z, P_GUI_A, P_GUI_B, P_GUI_C, + P_FWD_FAIL, + P_COUNT +}; + +#define STRUT_ROWS(i, bx, by, bz, px, py, pz, bnx, bny, bnz, pnx, pny, pnz) \ + { "base." #i ".x", KINS_PARAM_FLOAT, KINS_IN, 0, bx }, \ + { "base." #i ".y", KINS_PARAM_FLOAT, KINS_IN, 0, by }, \ + { "base." #i ".z", KINS_PARAM_FLOAT, KINS_IN, 0, bz }, \ + { "platform." #i ".x", KINS_PARAM_FLOAT, KINS_IN, 0, px }, \ + { "platform." #i ".y", KINS_PARAM_FLOAT, KINS_IN, 0, py }, \ + { "platform." #i ".z", KINS_PARAM_FLOAT, KINS_IN, 0, pz }, \ + { "base-n." #i ".x", KINS_PARAM_FLOAT, KINS_IN, 0, bnx }, \ + { "base-n." #i ".y", KINS_PARAM_FLOAT, KINS_IN, 0, bny }, \ + { "base-n." #i ".z", KINS_PARAM_FLOAT, KINS_IN, 0, bnz }, \ + { "platform-n." #i ".x", KINS_PARAM_FLOAT, KINS_IN, 0, pnx }, \ + { "platform-n." #i ".y", KINS_PARAM_FLOAT, KINS_IN, 0, pny }, \ + { "platform-n." #i ".z", KINS_PARAM_FLOAT, KINS_IN, 0, pnz }, \ + { "correction." #i, KINS_PARAM_FLOAT, KINS_OUT, 0, 0.0 } + +static const kins_param_desc genhex_params[P_COUNT] = { + STRUT_ROWS(0, DEFAULT_BASE_0_X, DEFAULT_BASE_0_Y, DEFAULT_BASE_0_Z, + DEFAULT_PLATFORM_0_X, DEFAULT_PLATFORM_0_Y, DEFAULT_PLATFORM_0_Z, + DEFAULT_BASE_0_NX, DEFAULT_BASE_0_NY, DEFAULT_BASE_0_NZ, + DEFAULT_PLATFORM_0_NX, DEFAULT_PLATFORM_0_NY, DEFAULT_PLATFORM_0_NZ), + STRUT_ROWS(1, DEFAULT_BASE_1_X, DEFAULT_BASE_1_Y, DEFAULT_BASE_1_Z, + DEFAULT_PLATFORM_1_X, DEFAULT_PLATFORM_1_Y, DEFAULT_PLATFORM_1_Z, + DEFAULT_BASE_1_NX, DEFAULT_BASE_1_NY, DEFAULT_BASE_1_NZ, + DEFAULT_PLATFORM_1_NX, DEFAULT_PLATFORM_1_NY, DEFAULT_PLATFORM_1_NZ), + STRUT_ROWS(2, DEFAULT_BASE_2_X, DEFAULT_BASE_2_Y, DEFAULT_BASE_2_Z, + DEFAULT_PLATFORM_2_X, DEFAULT_PLATFORM_2_Y, DEFAULT_PLATFORM_2_Z, + DEFAULT_BASE_2_NX, DEFAULT_BASE_2_NY, DEFAULT_BASE_2_NZ, + DEFAULT_PLATFORM_2_NX, DEFAULT_PLATFORM_2_NY, DEFAULT_PLATFORM_2_NZ), + STRUT_ROWS(3, DEFAULT_BASE_3_X, DEFAULT_BASE_3_Y, DEFAULT_BASE_3_Z, + DEFAULT_PLATFORM_3_X, DEFAULT_PLATFORM_3_Y, DEFAULT_PLATFORM_3_Z, + DEFAULT_BASE_3_NX, DEFAULT_BASE_3_NY, DEFAULT_BASE_3_NZ, + DEFAULT_PLATFORM_3_NX, DEFAULT_PLATFORM_3_NY, DEFAULT_PLATFORM_3_NZ), + STRUT_ROWS(4, DEFAULT_BASE_4_X, DEFAULT_BASE_4_Y, DEFAULT_BASE_4_Z, + DEFAULT_PLATFORM_4_X, DEFAULT_PLATFORM_4_Y, DEFAULT_PLATFORM_4_Z, + DEFAULT_BASE_4_NX, DEFAULT_BASE_4_NY, DEFAULT_BASE_4_NZ, + DEFAULT_PLATFORM_4_NX, DEFAULT_PLATFORM_4_NY, DEFAULT_PLATFORM_4_NZ), + STRUT_ROWS(5, DEFAULT_BASE_5_X, DEFAULT_BASE_5_Y, DEFAULT_BASE_5_Z, + DEFAULT_PLATFORM_5_X, DEFAULT_PLATFORM_5_Y, DEFAULT_PLATFORM_5_Z, + DEFAULT_BASE_5_NX, DEFAULT_BASE_5_NY, DEFAULT_BASE_5_NZ, + DEFAULT_PLATFORM_5_NX, DEFAULT_PLATFORM_5_NY, DEFAULT_PLATFORM_5_NZ), + [P_LAST_ITER] = { "last-iterations", KINS_PARAM_U32, KINS_OUT, 0, 0 }, + [P_MAX_ITER] = { "max-iterations", KINS_PARAM_U32, KINS_OUT, 0, 0 }, + [P_MAX_ERROR] = { "max-error", KINS_PARAM_FLOAT, KINS_IN, 0, 500.0 }, + [P_CONV_CRITERION] = { "convergence-criterion", KINS_PARAM_FLOAT, KINS_IN, 0, 1e-9 }, + [P_ITER_LIMIT] = { "limit-iterations", KINS_PARAM_U32, KINS_IN, 0, 120 }, + [P_TOOL_OFFSET] = { "tool-offset", KINS_PARAM_FLOAT, KINS_IN, 1, 0.0 }, + [P_SPINDLE_OFFSET] = { "spindle-offset", KINS_PARAM_FLOAT, KINS_IN, 0, 0.0 }, + [P_SCREW_LEAD] = { "screw-lead", KINS_PARAM_FLOAT, KINS_IN, 0, DEFAULT_SCREW_LEAD }, + // the pose the forward found, for a vismach gui; switchkins provides + // the skgui.* pins for the same purpose + [P_GUI_X] = { "x", KINS_PARAM_FLOAT, KINS_OUT, 0, 0.0 }, + [P_GUI_Y] = { "y", KINS_PARAM_FLOAT, KINS_OUT, 0, 0.0 }, + [P_GUI_Z] = { "z", KINS_PARAM_FLOAT, KINS_OUT, 0, 0.0 }, + [P_GUI_A] = { "a", KINS_PARAM_FLOAT, KINS_OUT, 0, 0.0 }, + [P_GUI_B] = { "b", KINS_PARAM_FLOAT, KINS_OUT, 0, 0.0 }, + [P_GUI_C] = { "c", KINS_PARAM_FLOAT, KINS_OUT, 0, 0.0 }, + [P_FWD_FAIL] = { "fwd-kins-fail", KINS_PARAM_BIT, KINS_OUT, 0, 0 }, +}; + +// the most iterations a converged solution has taken this session, kept +// in the caller's scratch so each caller reports its own +#define MAX_ITER_SEEN(s) ((s)->aux[0]) /******************************* MatInvert() ***************************/ @@ -259,45 +312,45 @@ static void MatMult(double J[][6], const double x[], double Ans[]) } } // MatMult() -/* declare arrays for base and platform coordinates */ -static PmCartesian b[NUM_STRUTS]; -static PmCartesian a[NUM_STRUTS]; - -/* declare base and platform joint axes vectors */ - -static PmCartesian nb1[NUM_STRUTS]; -static PmCartesian na0[NUM_STRUTS]; - -/************************genhex_read_hal_pins**************************/ - -static int genhex_read_hal_pins(void) { +/* the geometry of one call, taken from the block: base and platform + coordinates, the joint axes vectors and the screw lead */ +typedef struct { + PmCartesian b[NUM_STRUTS]; + PmCartesian a[NUM_STRUTS]; + PmCartesian nb1[NUM_STRUTS]; + PmCartesian na0[NUM_STRUTS]; + double screw_lead; +} genhex_geometry; + +static void geometry_of(const kins_params *p, genhex_geometry *g) { int t; - /* set the base and platform coordinates from hal pin values */ - rtapi_real spindle_offset = hal_get_real(haldata->spindle_offset); - rtapi_real tool_offset = hal_get_real(haldata->tool_offset); + /* set the base and platform coordinates from the block */ + const double spindle_offset = p->geometry[P_SPINDLE_OFFSET]; + const double tool_offset = p->tool.tran.z; for (t = 0; t < NUM_STRUTS; t++) { - b[t].x = hal_get_real(haldata->basex[t]); - b[t].y = hal_get_real(haldata->basey[t]); - b[t].z = hal_get_real(haldata->basez[t]) + spindle_offset + tool_offset; - a[t].x = hal_get_real(haldata->platformx[t]); - a[t].y = hal_get_real(haldata->platformy[t]); - a[t].z = hal_get_real(haldata->platformz[t]) + spindle_offset + tool_offset; - - nb1[t].x = hal_get_real(haldata->basenx[t]); - nb1[t].y = hal_get_real(haldata->baseny[t]); - nb1[t].z = hal_get_real(haldata->basenz[t]); - na0[t].x = hal_get_real(haldata->platformnx[t]); - na0[t].y = hal_get_real(haldata->platformny[t]); - na0[t].z = hal_get_real(haldata->platformnz[t]); + g->b[t].x = p->geometry[P_BASE_X(t)]; + g->b[t].y = p->geometry[P_BASE_Y(t)]; + g->b[t].z = p->geometry[P_BASE_Z(t)] + spindle_offset + tool_offset; + g->a[t].x = p->geometry[P_PLAT_X(t)]; + g->a[t].y = p->geometry[P_PLAT_Y(t)]; + g->a[t].z = p->geometry[P_PLAT_Z(t)] + spindle_offset + tool_offset; + + g->nb1[t].x = p->geometry[P_BASE_NX(t)]; + g->nb1[t].y = p->geometry[P_BASE_NY(t)]; + g->nb1[t].z = p->geometry[P_BASE_NZ(t)]; + g->na0[t].x = p->geometry[P_PLAT_NX(t)]; + g->na0[t].y = p->geometry[P_PLAT_NY(t)]; + g->na0[t].z = p->geometry[P_PLAT_NZ(t)]; } - return 0; -} // genhex_read_hal_pins() + g->screw_lead = p->geometry[P_SCREW_LEAD]; +} // geometry_of() /***************************StrutLengthCorrection***************************/ -static int StrutLengthCorrection(const PmCartesian * StrutVectUnit, +static int StrutLengthCorrection(const genhex_geometry *g, + const PmCartesian * StrutVectUnit, const PmRotationMatrix * RMatrix, const int strut_number, double * correction) @@ -306,32 +359,34 @@ static int StrutLengthCorrection(const PmCartesian * StrutVectUnit, double dotprod; /* define base joints axis vectors */ - pmCartCartCross(&nb1[strut_number], StrutVectUnit, &nb2); + pmCartCartCross(&g->nb1[strut_number], StrutVectUnit, &nb2); pmCartCartCross(StrutVectUnit, &nb2, &nb3); pmCartUnitEq(&nb3); /* define platform joints axis vectors */ - pmMatCartMult(RMatrix, &na0[strut_number], &na1); + pmMatCartMult(RMatrix, &g->na0[strut_number], &na1); pmCartCartCross(&na1, StrutVectUnit, &na2); pmCartUnitEq(&na2); /* define dot product */ pmCartCartDot(&nb3, &na2, &dotprod); - *correction = hal_get_real(haldata->screw_lead) * asin(dotprod) / PM_2_PI; + *correction = g->screw_lead * asin(dotprod) / PM_2_PI; return 0; } // StrutLengthCorrection() -/**************** genhexKinematicsForward() *****************/ -static int genhexKinematicsForward(const double * joints, - EmcPose * pos, - const KINEMATICS_FORWARD_FLAGS * fflags, - KINEMATICS_INVERSE_FLAGS * iflags) +/**************** genhex_forward() *****************/ +static int genhex_forward(const kins_params *p, kins_scratch *s, + const double * joints, + EmcPose * pos, + const KINEMATICS_FORWARD_FLAGS * fflags, + KINEMATICS_INVERSE_FLAGS * iflags) { (void)fflags; (void)iflags; + genhex_geometry g; PmCartesian aw; PmCartesian InvKinStrutVect,InvKinStrutVectUnit; PmCartesian q_trans, RMatrix_a, RMatrix_a_cross_Strut; @@ -350,7 +405,7 @@ static int genhexKinematicsForward(const double * joints, int i; unsigned iteration = 0; - genhex_read_hal_pins(); + geometry_of(p, &g); /* abort on obvious problems, like joints <= 0 */ /* FIXME-- should check against triangle inequality, so that joints @@ -375,13 +430,16 @@ static int genhexKinematicsForward(const double * joints, q_trans.z = pos->tran.z; /* Enter Newton-Raphson iterative method */ - rtapi_real max_error = hal_get_real(haldata->max_error); + const double max_error = p->geometry[P_MAX_ERROR]; + const unsigned iter_limit = (unsigned)p->geometry[P_ITER_LIMIT]; + const double conv_criterion = p->geometry[P_CONV_CRITERION]; while (iterate) { /* check for large error and return error flag if no convergence */ if ((conv_err > +max_error) || (conv_err < -max_error)) { /* we can't converge */ - hal_set_bool(haldata->fwd_kins_fail, 1); + s->failed = 1; + s->out[P_FWD_FAIL] = 1; return -2; }; @@ -389,9 +447,10 @@ static int genhexKinematicsForward(const double * joints, /* check iteration to see if the kinematics can reach the convergence criterion and return error flag if it can't */ - if (iteration > hal_get_ui32(haldata->iter_limit)) { + if (iteration > iter_limit) { /* we can't converge */ - hal_set_bool(haldata->fwd_kins_fail, 1); + s->failed = 1; + s->out[P_FWD_FAIL] = 1; return -5; } @@ -402,18 +461,19 @@ static int genhexKinematicsForward(const double * joints, estimate to get joint estimate, subtract joints to get joint deltas, and compute inv J while we're at it */ for (i = 0; i < NUM_STRUTS; i++) { - pmMatCartMult(&RMatrix, &a[i], &RMatrix_a); + pmMatCartMult(&RMatrix, &g.a[i], &RMatrix_a); pmCartCartAdd(&q_trans, &RMatrix_a, &aw); - pmCartCartSub(&aw, &b[i], &InvKinStrutVect); + pmCartCartSub(&aw, &g.b[i], &InvKinStrutVect); if (0 != pmCartUnit(&InvKinStrutVect, &InvKinStrutVectUnit)) { - hal_set_bool(haldata->fwd_kins_fail, 1); + s->failed = 1; + s->out[P_FWD_FAIL] = 1; return -1; } pmCartMag(&InvKinStrutVect, &InvKinStrutLength); - if (hal_get_real(haldata->screw_lead) != 0.0) { + if (g.screw_lead != 0.0) { /* enable strut length correction */ - StrutLengthCorrection(&InvKinStrutVectUnit, &RMatrix, i, &corr); + StrutLengthCorrection(&g, &InvKinStrutVectUnit, &RMatrix, i, &corr); /* define corrected joint lengths */ InvKinStrutLength += corr; } @@ -454,7 +514,6 @@ static int genhexKinematicsForward(const double * joints, /* enter loop to determine if a strut needs another iteration */ iterate = 0; /*assume iteration is done */ - rtapi_real conv_criterion = hal_get_real(haldata->conv_criterion); for (i = 0; i < NUM_STRUTS; i++) { if (fabs(StrutLengthDiff[i]) > conv_criterion) { iterate = 1; @@ -472,33 +531,42 @@ static int genhexKinematicsForward(const double * joints, pos->tran.y = q_trans.y; pos->tran.z = q_trans.z; - hal_set_ui32(haldata->last_iter, iteration); - - if (iteration > hal_get_ui32(haldata->max_iter)){ - hal_set_ui32(haldata->max_iter, iteration); + s->iterations = iteration; + s->failed = 0; + s->out[P_LAST_ITER] = iteration; + if (iteration > MAX_ITER_SEEN(s)) { + MAX_ITER_SEEN(s) = iteration; } - hal_set_bool(haldata->fwd_kins_fail, 0); + s->out[P_MAX_ITER] = MAX_ITER_SEEN(s); + s->out[P_FWD_FAIL] = 0; - genhex_gui_forward_kins(pos); + s->out[P_GUI_X] = pos->tran.x; + s->out[P_GUI_Y] = pos->tran.y; + s->out[P_GUI_Z] = pos->tran.z; + s->out[P_GUI_A] = pos->a; + s->out[P_GUI_B] = pos->b; + s->out[P_GUI_C] = pos->c; return 0; -} // genhexKinematicsForward() +} // genhex_forward() -/************************ genhexKinematicsInverse() ************************/ +/************************ genhex_inverse() ************************/ /* the inverse kinematics take world coordinates and determine joint values, given the inverse kinematics flags to resolve any ambiguities. The forward flags are set to indicate their value appropriate to the world coordinates passed in. */ -static int genhexKinematicsInverse(const EmcPose * pos, - double * joints, - const KINEMATICS_INVERSE_FLAGS * iflags, - KINEMATICS_FORWARD_FLAGS * fflags) +static int genhex_inverse(const kins_params *p, kins_scratch *s, + const EmcPose * pos, + double * joints, + const KINEMATICS_INVERSE_FLAGS * iflags, + KINEMATICS_FORWARD_FLAGS * fflags) { (void)iflags; (void)fflags; + genhex_geometry g; PmCartesian aw, temp; PmCartesian InvKinStrutVect, InvKinStrutVectUnit; PmRotationMatrix RMatrix; @@ -506,7 +574,7 @@ static int genhexKinematicsInverse(const EmcPose * pos, int i; double InvKinStrutLength, corr; - genhex_read_hal_pins(); + geometry_of(p, &g); /* define Rotation Matrix */ rpy.r = pos->a * PM_PI / 180.0; @@ -518,22 +586,22 @@ static int genhexKinematicsInverse(const EmcPose * pos, for (i = 0; i < NUM_STRUTS; i++) { /* convert location of platform strut end from platform to world coordinates */ - pmMatCartMult(&RMatrix, &a[i], &temp); + pmMatCartMult(&RMatrix, &g.a[i], &temp); pmCartCartAdd(&pos->tran, &temp, &aw); /* define strut lengths */ - pmCartCartSub(&aw, &b[i], &InvKinStrutVect); + pmCartCartSub(&aw, &g.b[i], &InvKinStrutVect); pmCartMag(&InvKinStrutVect, &InvKinStrutLength); - if (hal_get_real(haldata->screw_lead) != 0.0) { + if (g.screw_lead != 0.0) { /* enable strut length correction */ /* define unit strut vector */ if (0 != pmCartUnit(&InvKinStrutVect, &InvKinStrutVectUnit)) { return -1; } /* define correction value and corrected joint lengths */ - StrutLengthCorrection(&InvKinStrutVectUnit, &RMatrix, i, &corr); - hal_set_real(haldata->correction[i], corr); + StrutLengthCorrection(&g, &InvKinStrutVectUnit, &RMatrix, i, &corr); + s->out[P_CORR(i)] = corr; InvKinStrutLength += corr; } @@ -541,9 +609,9 @@ static int genhexKinematicsInverse(const EmcPose * pos, } return 0; -} //genhexKinematicsInverse() +} //genhex_inverse() -/************************ genhexKinematicsJacobian() ***********************/ +/************************ genhex_jacobian() ***********************/ /* A strut length changes by the component of its platform end's motion along the strut. That end moves with the platform, dP + w x (R a), so the row for strut i is [u_i, (R a_i x u_i) . E] with u_i the unit strut @@ -551,11 +619,18 @@ static int genhexKinematicsInverse(const EmcPose * pos, words to the angular velocity w for R = Rz(c) Ry(b) Rx(a). The forward kinematics builds the same rows for its Newton step, in radians. */ -static int genhexKinematicsJacobian(const double * joints, - const EmcPose * pos, - double jac[EMCMOT_MAX_JOINTS][EMCMOT_MAX_AXIS], - const KINEMATICS_INVERSE_FLAGS * iflags) +// the inverse alone, for differencing where the closed form does not apply +static const kins_ops genhex_diff_ops = { + .forward = genhex_forward, + .inverse = genhex_inverse, +}; + +static int genhex_jacobian(const kins_params *p, const double * joints, + const EmcPose * pos, + double jac[EMCMOT_MAX_JOINTS][EMCMOT_MAX_AXIS], + const KINEMATICS_INVERSE_FLAGS * iflags) { + genhex_geometry g; PmCartesian aw, RMatrix_a, strut, u, moment; PmRotationMatrix RMatrix; PmRpy rpy; @@ -563,13 +638,14 @@ static int genhexKinematicsJacobian(const double * joints, double sb, cb, sc, cc; int i, j, col, m; - genhex_read_hal_pins(); + geometry_of(p, &g); /* the screw lead correction is a function of the pose too, and this does not differentiate it; difference the inverse instead */ - if (hal_get_real(haldata->screw_lead) != 0.0) { - return kinsJacobianFromInverse(genhexKinematicsInverse, NUM_STRUTS, - joints, pos, iflags, jac); + if (g.screw_lead != 0.0) { + kins_scratch scratch; + kinsScratchInit(&scratch); + return kinsOpsJacobian(&genhex_diff_ops, p, &scratch, joints, pos, jac, iflags); } for (j = 0; j < EMCMOT_MAX_JOINTS; j++) { @@ -592,9 +668,9 @@ static int genhexKinematicsJacobian(const double * joints, for (i = 0; i < NUM_STRUTS; i++) { double len; - pmMatCartMult(&RMatrix, &a[i], &RMatrix_a); + pmMatCartMult(&RMatrix, &g.a[i], &RMatrix_a); pmCartCartAdd(&pos->tran, &RMatrix_a, &aw); - pmCartCartSub(&aw, &b[i], &strut); + pmCartCartSub(&aw, &g.b[i], &strut); pmCartMag(&strut, &len); if (len <= 0) { return -1; } pmCartScalMult(&strut, 1.0/len, &u); @@ -610,145 +686,16 @@ static int genhexKinematicsJacobian(const double * joints, } } return 0; -} // genhexKinematicsJacobian() - -// HAL pin initializaion values. In small arrays so we can easily -// address them in the pin creation loop. -static const rtapi_real init_basex[NUM_STRUTS] = { - DEFAULT_BASE_0_X, DEFAULT_BASE_1_X, DEFAULT_BASE_2_X, - DEFAULT_BASE_3_X, DEFAULT_BASE_4_X, DEFAULT_BASE_5_X, -}; -static const rtapi_real init_basey[NUM_STRUTS] = { - DEFAULT_BASE_0_Y, DEFAULT_BASE_1_Y, DEFAULT_BASE_2_Y, - DEFAULT_BASE_3_Y, DEFAULT_BASE_4_Y, DEFAULT_BASE_5_Y, -}; -static const rtapi_real init_basez[NUM_STRUTS] = { - DEFAULT_BASE_0_Z, DEFAULT_BASE_1_Z, DEFAULT_BASE_2_Z, - DEFAULT_BASE_3_Z, DEFAULT_BASE_4_Z, DEFAULT_BASE_5_Z, -}; -static const rtapi_real init_platformx[NUM_STRUTS] = { - DEFAULT_PLATFORM_0_X, DEFAULT_PLATFORM_1_X, DEFAULT_PLATFORM_2_X, - DEFAULT_PLATFORM_3_X, DEFAULT_PLATFORM_4_X, DEFAULT_PLATFORM_5_X, -}; -static const rtapi_real init_platformy[NUM_STRUTS] = { - DEFAULT_PLATFORM_0_Y, DEFAULT_PLATFORM_1_Y, DEFAULT_PLATFORM_2_Y, - DEFAULT_PLATFORM_3_Y, DEFAULT_PLATFORM_4_Y, DEFAULT_PLATFORM_5_Y, -}; -static const rtapi_real init_platformz[NUM_STRUTS] = { - DEFAULT_PLATFORM_0_Z, DEFAULT_PLATFORM_1_Z, DEFAULT_PLATFORM_2_Z, - DEFAULT_PLATFORM_3_Z, DEFAULT_PLATFORM_4_Z, DEFAULT_PLATFORM_5_Z, -}; -static const rtapi_real init_basenx[NUM_STRUTS] = { - DEFAULT_BASE_0_NX, DEFAULT_BASE_1_NX, DEFAULT_BASE_2_NX, - DEFAULT_BASE_3_NX, DEFAULT_BASE_4_NX, DEFAULT_BASE_5_NX, -}; -static const rtapi_real init_baseny[NUM_STRUTS] = { - DEFAULT_BASE_0_NY, DEFAULT_BASE_1_NY, DEFAULT_BASE_2_NY, - DEFAULT_BASE_3_NY, DEFAULT_BASE_4_NY, DEFAULT_BASE_5_NY, +} // genhex_jacobian() + +// the forward iterates from the pose it is handed, so it is seeded with +// the last answer after a switch +static const kins_ops genhex_ops = { + .forward = genhex_forward, + .inverse = genhex_inverse, + .jacobian = genhex_jacobian, + .fwd_iterates = 1, }; -static const rtapi_real init_basenz[NUM_STRUTS] = { - DEFAULT_BASE_0_NZ, DEFAULT_BASE_1_NZ, DEFAULT_BASE_2_NZ, - DEFAULT_BASE_3_NZ, DEFAULT_BASE_4_NZ, DEFAULT_BASE_5_NZ, -}; -static const rtapi_real init_platformnx[NUM_STRUTS] = { - DEFAULT_PLATFORM_0_NX, DEFAULT_PLATFORM_1_NX, DEFAULT_PLATFORM_2_NX, - DEFAULT_PLATFORM_3_NX, DEFAULT_PLATFORM_4_NX, DEFAULT_PLATFORM_5_NX, -}; -static const rtapi_real init_platformny[NUM_STRUTS] = { - DEFAULT_PLATFORM_0_NY, DEFAULT_PLATFORM_1_NY, DEFAULT_PLATFORM_2_NY, - DEFAULT_PLATFORM_3_NY, DEFAULT_PLATFORM_4_NY, DEFAULT_PLATFORM_5_NY, -}; -static const rtapi_real init_platformnz[NUM_STRUTS] = { - DEFAULT_PLATFORM_0_NZ, DEFAULT_PLATFORM_1_NZ, DEFAULT_PLATFORM_2_NZ, - DEFAULT_PLATFORM_3_NZ, DEFAULT_PLATFORM_4_NZ, DEFAULT_PLATFORM_5_NZ, -}; - -static -int genhexKinematicsSetup(const int comp_id, - const char* coordinates, - kparms* kp) -{ - (void)coordinates; - int i,res=0; - - if (kp->max_joints < 0 || kp->max_joints > NUM_STRUTS) { - rtapi_print_msg(RTAPI_MSG_ERR, "genhexKinematicsSetup: max_joints %d less than 0 or larger NUM_STRUTS %d\n", - kp->max_joints, NUM_STRUTS); - return -1; - } - - haldata = hal_malloc(sizeof(struct haldata)); - if (!haldata) { - rtapi_print_msg(RTAPI_MSG_ERR,"genhexKinematicsSetup: hal_malloc fail\n"); - return -1; - } - - for (i = 0; i < kp->max_joints; i++) { - res += hal_pin_new_real(comp_id, HAL_IN, &(haldata->basex[i]), - init_basex[i], "%s.base.%d.x", kp->halprefix, i); - res += hal_pin_new_real(comp_id, HAL_IN, &haldata->basey[i], - init_basey[i], "%s.base.%d.y", kp->halprefix, i); - res += hal_pin_new_real(comp_id, HAL_IN, &haldata->basez[i], - init_basez[i], "%s.base.%d.z", kp->halprefix, i); - res += hal_pin_new_real(comp_id, HAL_IN, &haldata->platformx[i], - init_platformx[i], "%s.platform.%d.x", kp->halprefix, i); - res += hal_pin_new_real(comp_id, HAL_IN, &haldata->platformy[i], - init_platformy[i], "%s.platform.%d.y", kp->halprefix, i); - res += hal_pin_new_real(comp_id, HAL_IN, &haldata->platformz[i], - init_platformz[i], "%s.platform.%d.z", kp->halprefix, i); - res += hal_pin_new_real(comp_id, HAL_IN, &haldata->basenx[i], - init_basenx[i], "%s.base-n.%d.x", kp->halprefix, i); - res += hal_pin_new_real(comp_id, HAL_IN, &haldata->baseny[i], - init_baseny[i], "%s.base-n.%d.y", kp->halprefix, i); - res += hal_pin_new_real(comp_id, HAL_IN, &haldata->basenz[i], - init_basenz[i], "%s.base-n.%d.z", kp->halprefix, i); - res += hal_pin_new_real(comp_id, HAL_IN, &haldata->platformnx[i], - init_platformnx[i], "%s.platform-n.%d.x", kp->halprefix, i); - res += hal_pin_new_real(comp_id, HAL_IN, &haldata->platformny[i], - init_platformny[i], "%s.platform-n.%d.y", kp->halprefix, i); - res += hal_pin_new_real(comp_id, HAL_IN, &haldata->platformnz[i], - init_platformnz[i], "%s.platform-n.%d.z", kp->halprefix, i); - res += hal_pin_new_real(comp_id, HAL_OUT, &haldata->correction[i], - 0.0, "%s.correction.%d", kp->halprefix, i); - if (res) {goto error;} - } - - res += hal_pin_new_ui32(comp_id, HAL_OUT, &haldata->last_iter, - 0, "genhexkins.last-iterations"); - res += hal_pin_new_ui32(comp_id, HAL_OUT, &haldata->max_iter, - 0, "genhexkins.max-iterations"); - res += hal_pin_new_real(comp_id, HAL_IN, &haldata->max_error, - 500.0, "genhexkins.max-error"); - res += hal_pin_new_real(comp_id, HAL_IN, &haldata->conv_criterion, - 1e-9, "genhexkins.convergence-criterion"); - res += hal_pin_new_ui32(comp_id, HAL_IN, &haldata->iter_limit, - 120, "genhexkins.limit-iterations"); - res += hal_pin_new_real(comp_id, HAL_IN, &haldata->tool_offset, - 0.0, "genhexkins.tool-offset"); - res += hal_pin_new_real(comp_id, HAL_IN, &haldata->spindle_offset, - 0.0, "genhexkins.spindle-offset"); - res += hal_pin_new_real(comp_id, HAL_IN, &haldata->screw_lead, - DEFAULT_SCREW_LEAD, "genhexkins.screw-lead"); - - if (res) {goto error;} - - //note: switchkins does not uses these as it provides gui.x, gui.y, etc. - res += hal_pin_new_real(comp_id, HAL_IN, &haldata->gui_x, 0.0, "genhexkins.x"); - res += hal_pin_new_real(comp_id, HAL_IN, &haldata->gui_y, 0.0, "genhexkins.y"); - res += hal_pin_new_real(comp_id, HAL_IN, &haldata->gui_z, 0.0, "genhexkins.z"); - res += hal_pin_new_real(comp_id, HAL_IN, &haldata->gui_a, 0.0, "genhexkins.a"); - res += hal_pin_new_real(comp_id, HAL_IN, &haldata->gui_b, 0.0, "genhexkins.b"); - res += hal_pin_new_real(comp_id, HAL_IN, &haldata->gui_c, 0.0, "genhexkins.c"); - - res += hal_pin_new_bool(comp_id, HAL_OUT, &haldata->fwd_kins_fail, - 0, "genhexkins.fwd-kins-fail"); - - if (res) goto error; - return 0; - -error: - return res; -} // genhexKinematicsSetup() int switchkinsSetup(kparms* kp, KS* kset0, KS* kset1, KS* kset2, @@ -756,46 +703,32 @@ int switchkinsSetup(kparms* kp, KI* kinv0, KI* kinv1, KI* kinv2 ) { + (void)kset0; (void)kset1; (void)kset2; + (void)kfwd0; (void)kfwd1; (void)kfwd2; + (void)kinv0; (void)kinv1; (void)kinv2; kp->kinsname = "genhexkins"; // !!! must agree with filename kp->halprefix = "genhexkins"; // hal pin names kp->required_coordinates = "xyzabc"; kp->max_joints = strlen(kp->required_coordinates); kp->allow_duplicates = 0; + kp->params = genhex_params; + kp->nparams = P_COUNT; + if (kp->sparm && strstr(kp->sparm,"identityfirst")) { rtapi_print("\n!!! switchkins-type 0 is IDENTITY\n"); kp->fwd_iterates_mask = 0x2; //genhexkins switchkins_type==1 kp->gui_kinstype = 1; //vismach gui for switchkins_type==1 - - *kset0 = identityKinematicsSetup; - *kfwd0 = identityKinematicsForward; - *kinv0 = identityKinematicsInverse; - - *kset1 = genhexKinematicsSetup; - *kfwd1 = genhexKinematicsForward; - *kinv1 = genhexKinematicsInverse; - switchkinsDeclare(0, KINSTYPE_IDENTITY); - switchkinsDeclare(1, KINSTYPE_PRIMARY); - switchkinsRegisterJacobian(1, genhexKinematicsJacobian); + switchkinsRegisterOps(0, &KINS_IDENTITY_OPS); + switchkinsRegisterOps(1, &genhex_ops); } else { rtapi_print("\n!!! switchkins-type 0 is %s\n",kp->kinsname); kp->fwd_iterates_mask = 0x1; //genhexkins switchkins_type==0 kp->gui_kinstype = 0; //vismach gui for switchkins_type==0 - - *kset0 = genhexKinematicsSetup; - *kfwd0 = genhexKinematicsForward; - *kinv0 = genhexKinematicsInverse; - switchkinsRegisterJacobian(0, genhexKinematicsJacobian); - - *kset1 = identityKinematicsSetup; - *kfwd1 = identityKinematicsForward; - *kinv1 = identityKinematicsInverse; - switchkinsDeclare(0, KINSTYPE_PRIMARY); - switchkinsDeclare(1, KINSTYPE_IDENTITY); + switchkinsRegisterOps(0, &genhex_ops); + switchkinsRegisterOps(1, &KINS_IDENTITY_OPS); } - *kset2 = userkKinematicsSetup; - *kfwd2 = userkKinematicsForward; - *kinv2 = userkKinematicsInverse; + switchkinsRegisterOps(2, &USERK_OPS); return 0; } //switchkinsSetup() diff --git a/src/emc/kinematics/genserfuncs.c b/src/emc/kinematics/genserfuncs.c index d8432cdec3d..2f943d681aa 100644 --- a/src/emc/kinematics/genserfuncs.c +++ b/src/emc/kinematics/genserfuncs.c @@ -28,6 +28,11 @@ Currently the type of the joints is hardcoded to ANGULAR, although the kins support both ANGULAR and LINEAR axes. + The maths is written as pure functions of the parameter block (see + kinematics.h): the pins are the table below, read into the block + before every call, and the link description is built from the block + on each call. + TODO: * make number of joints a loadtime parameter * add HAL pins for all settable parameters, including joint type: ANGULAR / LINEAR @@ -48,44 +53,53 @@ #if __GNUC__ && !defined(__clang__) // The matrix and vector storage is just big. // genser_kin_jac_inv() is 2112 -// genserKinematicsInverse() is 2640 - #pragma GCC diagnostic warning "-Wframe-larger-than=2648" +// genser_inverse() is 2640 plus the link description it builds + #pragma GCC diagnostic warning "-Wframe-larger-than=3400" #endif -static struct haldata { - hal_uint_t max_iterations; - hal_uint_t last_iterations; - hal_real_t a[GENSER_MAX_JOINTS]; - hal_real_t alpha[GENSER_MAX_JOINTS]; - hal_real_t d[GENSER_MAX_JOINTS]; - hal_sint_t unrotate[GENSER_MAX_JOINTS]; - genser_struct *kins; - go_pose *pos; // used in various functions, we malloc it - // only once in genserKinematicsSetup() -} *haldata = NULL; - -static int total_joints; -double j[GENSER_MAX_JOINTS]; +// the table: four entries per joint, then the iteration count in and out +#define P_A(i) (4*(i) + 0) +#define P_ALPHA(i) (4*(i) + 1) +#define P_D(i) (4*(i) + 2) +#define P_UNROT(i) (4*(i) + 3) +enum { + P_LAST_ITER = 4*GENSER_MAX_JOINTS, + P_MAX_ITER, + P_COUNT +}; -#define KINS_PTR (haldata->kins) +#define JOINT_ROWS(i, a, alpha, d) \ + { "A-" #i, KINS_PARAM_FLOAT, KINS_IN, 0, a }, \ + { "ALPHA-" #i, KINS_PARAM_FLOAT, KINS_IN, 0, alpha }, \ + { "D-" #i, KINS_PARAM_FLOAT, KINS_IN, 0, d }, \ + { "unrotate-" #i, KINS_PARAM_S32, KINS_IN, 0, 0 } + +const kins_param_desc GENSER_PARAMS[P_COUNT] = { + JOINT_ROWS(0, DEFAULT_A1, DEFAULT_ALPHA1, DEFAULT_D1), + JOINT_ROWS(1, DEFAULT_A2, DEFAULT_ALPHA2, DEFAULT_D2), + JOINT_ROWS(2, DEFAULT_A3, DEFAULT_ALPHA3, DEFAULT_D3), + JOINT_ROWS(3, DEFAULT_A4, DEFAULT_ALPHA4, DEFAULT_D4), + JOINT_ROWS(4, DEFAULT_A5, DEFAULT_ALPHA5, DEFAULT_D5), + JOINT_ROWS(5, DEFAULT_A6, DEFAULT_ALPHA6, DEFAULT_D6), + [P_LAST_ITER] = { "last-iterations", KINS_PARAM_U32, KINS_OUT, 0, 0 }, + [P_MAX_ITER] = { "max-iterations", KINS_PARAM_U32, KINS_IN, 0, GENSER_DEFAULT_MAX_ITERATIONS }, +}; +const int GENSER_NPARAMS = P_COUNT; #if GENSER_MAX_JOINTS < 6 #error GENSER_MAX_JOINTS must be at least 6; fix genserkins.h #endif -static int genser_hal_inited = 0; - -int genser_kin_init(void) { - genser_struct *genser = KINS_PTR; +void genser_links_of(const kins_params *p, genser_struct *genser) { int t; static volatile double tst=0;tst=sqrt(tst); // ensure -lm used /* init them all and make them revolute joints */ /* FIXME: should allow LINEAR joints based on HAL param too */ for (t = 0; t < GENSER_MAX_JOINTS; t++) { - genser->links[t].u.dh.a = hal_get_real(haldata->a[t]); - genser->links[t].u.dh.alpha = hal_get_real(haldata->alpha[t]); - genser->links[t].u.dh.d = hal_get_real(haldata->d[t]); + genser->links[t].u.dh.a = p->geometry[P_A(t)]; + genser->links[t].u.dh.alpha = p->geometry[P_ALPHA(t)]; + genser->links[t].u.dh.d = p->geometry[P_D(t)]; genser->links[t].u.dh.theta = 0; genser->links[t].type = GO_LINK_DH; genser->links[t].quantity = GO_QUANTITY_ANGLE; @@ -94,8 +108,13 @@ int genser_kin_init(void) { /* set a select few to make it PUMA-like */ // FIXME-AJ: make a hal pin, also set number of joints based on it genser->link_num = 6; + genser->iterations = 0; +} // genser_links_of() - return GO_RESULT_OK; +/* the unrotate coupling of one joint, from the block */ +static rtapi_s32 unrotate_of(const kins_params *p, int link) +{ + return (rtapi_s32)p->geometry[P_UNROT(link)]; } /* compute the forward jacobian function: @@ -314,7 +333,7 @@ int genser_kin_jac_fwd(void *kins, } /* The Jacobian in the terms of kinematics.h: joints in degrees per pose - word in EmcPose units, the derivative of genserKinematicsInverse(). + word in EmcPose units, the derivative of genser_inverse(). compute_jinv() gives the geometric inverse Jacobian, radians of joint per unit of base-frame twist. A pose word rate is not a twist: the roll, @@ -326,13 +345,14 @@ int genser_kin_jac_fwd(void *kins, with the unit conversions and the unrotate coupling applied in the order the inverse applies them. */ -int genserKinematicsJacobian(const double *joint, - const EmcPose *world, - double jac[EMCMOT_MAX_JOINTS][EMCMOT_MAX_AXIS], - const KINEMATICS_INVERSE_FLAGS *iflags) +static int genser_jacobian(const kins_params *p, const double *joint, + const EmcPose *world, + double jac[EMCMOT_MAX_JOINTS][EMCMOT_MAX_AXIS], + const KINEMATICS_INVERSE_FLAGS *iflags) { (void)iflags; - genser_struct *genser = KINS_PTR; + genser_struct genser_stg; + genser_struct *genser = &genser_stg; GO_MATRIX_DECLARE(Jfwd, Jfwd_stg, 6, GENSER_MAX_JOINTS); GO_MATRIX_DECLARE(Jinv, Jinv_stg, GENSER_MAX_JOINTS, 6); go_pose T_L_0; @@ -342,14 +362,7 @@ int genserKinematicsJacobian(const double *joint, double sb, cb, sc, cc; int link, i, j, a, m, retval; -#ifndef ULAPI - genser_kin_init(); - if (!genser_hal_inited) { - rtapi_print_msg(RTAPI_MSG_ERR, - "genserKinematicsJacobian: not initialized\n"); - return -1; - } -#endif + genser_links_of(p, genser); for (j = 0; j < EMCMOT_MAX_JOINTS; j++) { for (a = 0; a < EMCMOT_MAX_AXIS; a++) { jac[j][a] = 0; } @@ -358,7 +371,7 @@ int genserKinematicsJacobian(const double *joint, // the kinematic joint angles, in radians and with the unrotate // coupling removed, exactly as the forward prepares them for (link = 0; link < genser->link_num; link++) { - rtapi_s32 unrotate = hal_get_si32(haldata->unrotate[link]); + rtapi_s32 unrotate = unrotate_of(p, link); jest[link] = joint[link] * (PM_PI / 180); if (link && unrotate) jest[link] -= unrotate * jest[link-1]; @@ -404,7 +417,7 @@ int genserKinematicsJacobian(const double *joint, // the unrotate coupling, in link order as the inverse applies it for (link = 1; link < genser->link_num; link++) { - rtapi_s32 unrotate = hal_get_si32(haldata->unrotate[link]); + rtapi_s32 unrotate = unrotate_of(p, link); if (unrotate) { for (a = 0; a < EMCMOT_MAX_AXIS; a++) { jac[link][a] += unrotate * jac[link-1][a]; @@ -413,86 +426,74 @@ int genserKinematicsJacobian(const double *joint, } // uvw pass through as joints 6, 7, 8 - if (total_joints > 6) jac[6][6] = 1; - if (total_joints > 7) jac[7][7] = 1; - if (total_joints > 8) jac[8][8] = 1; + if (p->max_joints > 6) jac[6][6] = 1; + if (p->max_joints > 7) jac[7][7] = 1; + if (p->max_joints > 8) jac[8][8] = 1; return 0; -} // genserKinematicsJacobian() +} // genser_jacobian() /* main function called by emc2 for forward Kins */ -int genserKinematicsForward(const double *joint, - EmcPose * world, - const KINEMATICS_FORWARD_FLAGS * fflags, - KINEMATICS_INVERSE_FLAGS * iflags) { +static int genser_forward(const kins_params *p, kins_scratch *s, + const double *joint, + EmcPose * world, + const KINEMATICS_FORWARD_FLAGS * fflags, + KINEMATICS_INVERSE_FLAGS * iflags) { + (void)s; (void)fflags; (void)iflags; - go_pose *pos; + genser_struct genser; + go_pose pos; go_rpy rpy; go_real jcopy[GENSER_MAX_JOINTS]; // will hold the radian conversion of joints int ret = 0; - int i, changed=0; - if (!genser_hal_inited) { - rtapi_print_msg(RTAPI_MSG_ERR, - "genserKinematicsForward: not initialized\n"); - return -1; - } + int i; + + genser_links_of(p, &genser); for (i=0; i< 6; i++) { - // FIXME - debug hack - if (!GO_ROT_CLOSE(j[i],joint[i])) changed = 1; // convert to radians to pass to genser_kin_fwd jcopy[i] = joint[i] * PM_PI / 180; - rtapi_s32 unrotate = hal_get_si32(haldata->unrotate[i]); + rtapi_s32 unrotate = unrotate_of(p, i); if ((i) && unrotate) jcopy[i] -= unrotate * jcopy[i-1]; } - if (changed) { - for (i=0; i< 6; i++) - j[i] = joint[i]; - // rtapi_print("genserKinematicsForward(joints: %f %f %f %f %f %f)\n", - //joint[0],joint[1],joint[2],joint[3],joint[4],joint[5]); - } // AJ: convert from emc2 coords (XYZABC - which are actually rpy euler // angles) // to go angles (quaternions) - pos = haldata->pos; rpy.y = world->c * PM_PI / 180; rpy.p = world->b * PM_PI / 180; rpy.r = world->a * PM_PI / 180; - go_rpy_quat_convert(&rpy, &pos->rot); - pos->tran.x = world->tran.x; - pos->tran.y = world->tran.y; - pos->tran.z = world->tran.z; + go_rpy_quat_convert(&rpy, &pos.rot); + pos.tran.x = world->tran.x; + pos.tran.y = world->tran.y; + pos.tran.z = world->tran.z; //pass through unused 678 as uvw - if (total_joints > 6) world->u = joint[6]; - if (total_joints > 7) world->v = joint[7]; - if (total_joints > 8) world->w = joint[8]; + if (p->max_joints > 6) world->u = joint[6]; + if (p->max_joints > 7) world->v = joint[7]; + if (p->max_joints > 8) world->w = joint[8]; // pos will be the world location // jcopy: joitn position in radians - ret = genser_kin_fwd(KINS_PTR, jcopy, pos); + ret = genser_kin_fwd(&genser, jcopy, &pos); if (ret < 0) return ret; // AJ: convert back to emc2 coords - ret = go_quat_rpy_convert(&pos->rot, &rpy); + ret = go_quat_rpy_convert(&pos.rot, &rpy); if (ret < 0) return ret; - world->tran.x = pos->tran.x; - world->tran.y = pos->tran.y; - world->tran.z = pos->tran.z; + world->tran.x = pos.tran.x; + world->tran.y = pos.tran.y; + world->tran.z = pos.tran.z; world->a = rpy.r * 180 / PM_PI; world->b = rpy.p * 180 / PM_PI; world->c = rpy.y * 180 / PM_PI; - if (changed) { -// rtapi_print("genserKinematicsForward(world: %f %f %f %f %f %f)\n", world->tran.x, world->tran.y, world->tran.z, world->a, world->b, world->c); - } return 0; } @@ -504,8 +505,6 @@ int genser_kin_fwd(void *kins, const go_real * joints, go_pose * pos) int link; int retval; - genser_kin_init(); - for (link = 0; link < genser->link_num; link++) { retval = go_link_joint_set(&genser->links[link], joints[link], &linkout[link]); if (GO_RESULT_OK != retval) @@ -519,22 +518,25 @@ int genser_kin_fwd(void *kins, const go_real * joints, go_pose * pos) return GO_RESULT_OK; } -int genserKinematicsInverse(const EmcPose * world, - double *joints, - const KINEMATICS_INVERSE_FLAGS * iflags, - KINEMATICS_FORWARD_FLAGS * fflags) +static int genser_inverse(const kins_params *p, kins_scratch *s, + const EmcPose * world, + double *joints, + const KINEMATICS_INVERSE_FLAGS * iflags, + KINEMATICS_FORWARD_FLAGS * fflags) { (void)iflags; (void)fflags; - genser_struct *genser = KINS_PTR; + genser_struct genser_stg; + genser_struct *genser = &genser_stg; GO_MATRIX_DECLARE(Jfwd, Jfwd_stg, 6, GENSER_MAX_JOINTS); GO_MATRIX_DECLARE(Jinv, Jinv_stg, GENSER_MAX_JOINTS, 6); go_pose T_L_0; go_real dvw[6]; go_real jest[GENSER_MAX_JOINTS]; go_real dj[GENSER_MAX_JOINTS]; - go_pose pest, pestinv, Tdelta; // pos = converted pose from EmcPose + go_pose pos; // converted pose from EmcPose + go_pose pest, pestinv, Tdelta; go_rpy rpy; go_rvec rvec; go_cart cart; @@ -542,30 +544,19 @@ int genserKinematicsInverse(const EmcPose * world, int link; int smalls; int retval; + const unsigned max_iterations = (unsigned)p->geometry[P_MAX_ITER]; - // rtapi_print("kineInverse(joints: %f %f %f %f %f %f)\n", - // joints[0],joints[1],joints[2],joints[3],joints[4],joints[5]); - // rtapi_print("kineInverse(world: %f %f %f %f %f %f)\n", - // world->tran.x, world->tran.y, world->tran.z, world->a, world->b, world->c); - -#ifndef ULAPI - genser_kin_init(); - if (!genser_hal_inited) { - rtapi_print_msg(RTAPI_MSG_ERR, - "genserKinematicsInverse: not initialized\n"); - return -1; - } -#endif + genser_links_of(p, genser); // FIXME-AJ: rpy or zyx ? rpy.y = world->c * PM_PI / 180; rpy.p = world->b * PM_PI / 180; rpy.r = world->a * PM_PI / 180; - go_rpy_quat_convert(&rpy, &haldata->pos->rot); - haldata->pos->tran.x = world->tran.x; - haldata->pos->tran.y = world->tran.y; - haldata->pos->tran.z = world->tran.z; + go_rpy_quat_convert(&rpy, &pos.rot); + pos.tran.x = world->tran.x; + pos.tran.y = world->tran.y; + pos.tran.z = world->tran.z; go_matrix_init(Jfwd, Jfwd_stg, 6, genser->link_num); go_matrix_init(Jinv, Jinv_stg, genser->link_num, 6); @@ -577,9 +568,10 @@ int genserKinematicsInverse(const EmcPose * world, } for (genser->iterations = 0; - genser->iterations < hal_get_ui32(haldata->max_iterations); + genser->iterations < max_iterations; genser->iterations++) { - hal_set_ui32(haldata->last_iterations, genser->iterations); + s->iterations = genser->iterations; + s->out[P_LAST_ITER] = genser->iterations; /* update the Jacobians */ for (link = 0; link < genser->link_num; link++) { go_link_joint_set(&genser->links[link], jest[link], &linkout[link]); @@ -598,8 +590,7 @@ int genserKinematicsInverse(const EmcPose * world, } /* pest is the resulting pose estimate given joint estimate */ - genser_kin_fwd(KINS_PTR, jest, &pest); - //printf("jest: %f %f %f %f %f %f\n",jest[0],jest[1],jest[2],jest[3],jest[4],jest[5]); + genser_kin_fwd(genser, jest, &pest); /* pestinv is its inverse */ go_pose_inv(&pest, &pestinv); /* @@ -613,7 +604,7 @@ int genserKinematicsInverse(const EmcPose * world, .Tdelta = pestinv * pos L 0 L */ - go_pose_pose_mult(&pestinv, haldata->pos, &Tdelta); + go_pose_pose_mult(&pestinv, &pos, &Tdelta); /* We need Tdelta in 0 frame, not pest frame, so rotate it @@ -642,9 +633,9 @@ int genserKinematicsInverse(const EmcPose * world, go_matrix_vector_mult(&Jinv, dvw, dj); //pass through 678 as uvw - if (total_joints > 6) joints[6] = world->u; - if (total_joints > 7) joints[7] = world->v; - if (total_joints > 8) joints[8] = world->w; + if (p->max_joints > 6) joints[6] = world->u; + if (p->max_joints > 7) joints[7] = world->v; + if (p->max_joints > 8) joints[8] = world->w; /* check for small joint increments, if so we're done */ for (link = 0, smalls = 0; link < genser->link_num; link++) { @@ -661,14 +652,10 @@ int genserKinematicsInverse(const EmcPose * world, for (link = 0; link < genser->link_num; link++) { // convert from radians back to angles joints[link] = jest[link] * 180 / PM_PI; - rtapi_s32 unrotate = hal_get_si32(haldata->unrotate[link]); + rtapi_s32 unrotate = unrotate_of(p, link); if ((link) && unrotate) joints[link] += unrotate * joints[link-1]; } - //rtapi_print("DONEkineInverse(joints: %f %f %f %f %f %f), (iterations=%d)\n", - // joints[0],joints[1],joints[2],joints[3],joints[4],joints[5], genser->iterations); - //rtapi_print("OKkineInverse: %.2f %.2f %.2f %.2f %.2f %.2f)\n", - // world->tran.x, world->tran.y, world->tran.z, world->a, world->b, world->c); return GO_RESULT_OK; } /* else keep iterating */ @@ -682,6 +669,12 @@ int genserKinematicsInverse(const EmcPose * world, return GO_RESULT_ERROR; } +const kins_ops GENSER_OPS = { + .forward = genser_forward, + .inverse = genser_inverse, + .jacobian = genser_jacobian, +}; + /* Extras, not callable using go_kin_ wrapper but if you know you have linked in these kinematics, go ahead and call these for your ad hoc @@ -692,68 +685,3 @@ int genser_kin_inv_iterations(genser_struct * genser) { return genser->iterations; } - -int genser_kin_inv_set_max_iterations(int i) -{ - if (i <= 0) return GO_RESULT_ERROR; - hal_set_ui32(haldata->max_iterations, i); - return GO_RESULT_OK; -} - -int genser_kin_inv_get_max_iterations() -{ - return hal_get_ui32(haldata->max_iterations); -} - -static const rtapi_real init_a[GENSER_MAX_JOINTS] = { - DEFAULT_A1, DEFAULT_A2, DEFAULT_A3, DEFAULT_A4, DEFAULT_A5, DEFAULT_A6 -}; -static const rtapi_real init_alpha[GENSER_MAX_JOINTS] = { - DEFAULT_ALPHA1, DEFAULT_ALPHA2, DEFAULT_ALPHA3, DEFAULT_ALPHA4, DEFAULT_ALPHA5, DEFAULT_ALPHA6 -}; -static const rtapi_real init_d[GENSER_MAX_JOINTS] = { - DEFAULT_D1, DEFAULT_D2, DEFAULT_D3, DEFAULT_D4, DEFAULT_D5, DEFAULT_D6 -}; - - -int genserKinematicsSetup(const int comp_id, - const char* coordinates, - kparms* kp) -{ - (void)coordinates; - int i,res=0; - haldata = hal_malloc(sizeof(struct haldata)); - if (!haldata) {goto error;} - - // allow for pass through joints 6,7,8 u,v,w - total_joints = kp->max_joints; - - // only the first 6 joints have A,ALPHA,D,unrotate pins - for (i = 0; i < GENSER_MAX_JOINTS; i++) { - res += hal_pin_new_real(comp_id, HAL_IN, &(haldata->a[i]), - init_a[i], "%s.A-%d", kp->halprefix, i); - res += hal_pin_new_real(comp_id, HAL_IN, &(haldata->alpha[i]), - init_alpha[i], "%s.ALPHA-%d", kp->halprefix, i); - res += hal_pin_new_real(comp_id, HAL_IN, &(haldata->d[i]), - init_d[i], "%s.D-%d", kp->halprefix, i); - res += hal_pin_new_si32(comp_id, HAL_IN, &(haldata->unrotate[i]), - 0, "%s.unrotate-%d", kp->halprefix, i); - } - res += hal_pin_new_ui32(comp_id, HAL_OUT, &(haldata->last_iterations), - 0, "%s.last-iterations",kp->halprefix); - - KINS_PTR = hal_malloc(sizeof(genser_struct)); - haldata->pos = (go_pose *) hal_malloc(sizeof(go_pose)); - if (KINS_PTR == NULL) {goto error;} - if (haldata->pos == NULL) {goto error;} - res += hal_pin_new_ui32(comp_id, HAL_IN, &haldata->max_iterations, - GENSER_DEFAULT_MAX_ITERATIONS, "%s.max-iterations",kp->halprefix); - - if (res) {goto error;} - - genser_hal_inited = 1; - return 0; - -error: - return -1; -} // genserKinematicsSetup() diff --git a/src/emc/kinematics/genserkins.c b/src/emc/kinematics/genserkins.c index be71f33ffc5..09c72a9fcbe 100644 --- a/src/emc/kinematics/genserkins.c +++ b/src/emc/kinematics/genserkins.c @@ -4,7 +4,8 @@ * * NOTEs: * 1) specify all kparms items -* 2) specify 3 KS,KF,KI functions (setup,forward,inverse) +* 2) the maths and the geometry table are in genserfuncs.c, written as +* pure functions of the parameter block (see kinematics.h) */ /******************************************************************** @@ -57,41 +58,28 @@ int switchkinsSetup(kparms* kp, KI* kinv0, KI* kinv1, KI* kinv2 ) { + (void)kset0; (void)kset1; (void)kset2; + (void)kfwd0; (void)kfwd1; (void)kfwd2; + (void)kinv0; (void)kinv1; (void)kinv2; kp->kinsname = "genserkins"; // !!! must agree with filename kp->halprefix = "genserkins"; // hal pin names kp->required_coordinates = "xyzabcuvw"; // u,v,w are joints 6,7,8 kp->max_joints = strlen(kp->required_coordinates); kp->allow_duplicates = 0; + kp->params = GENSER_PARAMS; + kp->nparams = GENSER_NPARAMS; if (kp->sparm && strstr(kp->sparm,"identityfirst")) { rtapi_print("\n!!! switchkins-type 0 is IDENTITY\n"); - *kset0 = identityKinematicsSetup; - *kfwd0 = identityKinematicsForward; - *kinv0 = identityKinematicsInverse; - - *kset1 = genserKinematicsSetup; - *kfwd1 = genserKinematicsForward; - *kinv1 = genserKinematicsInverse; - switchkinsDeclare(0, KINSTYPE_IDENTITY); - switchkinsDeclare(1, KINSTYPE_PRIMARY); - switchkinsRegisterJacobian(1, genserKinematicsJacobian); + switchkinsRegisterOps(0, &KINS_IDENTITY_OPS); + switchkinsRegisterOps(1, &GENSER_OPS); } else { rtapi_print("\n!!! switchkins-type 0 is %s\n",kp->kinsname); - *kset0 = genserKinematicsSetup; - *kfwd0 = genserKinematicsForward; - *kinv0 = genserKinematicsInverse; - switchkinsRegisterJacobian(0, genserKinematicsJacobian); - - *kset1 = identityKinematicsSetup; - *kfwd1 = identityKinematicsForward; - *kinv1 = identityKinematicsInverse; - switchkinsDeclare(0, KINSTYPE_PRIMARY); - switchkinsDeclare(1, KINSTYPE_IDENTITY); + switchkinsRegisterOps(0, &GENSER_OPS); + switchkinsRegisterOps(1, &KINS_IDENTITY_OPS); } - *kset2 = userkKinematicsSetup; - *kfwd2 = userkKinematicsForward; - *kinv2 = userkKinematicsInverse; + switchkinsRegisterOps(2, &USERK_OPS); return 0; } diff --git a/src/emc/kinematics/genserkins.h b/src/emc/kinematics/genserkins.h index b74b826d2ec..c5a2d9526f8 100644 --- a/src/emc/kinematics/genserkins.h +++ b/src/emc/kinematics/genserkins.h @@ -81,8 +81,6 @@ typedef struct { extern int genser_kin_size(void); -extern int genser_kin_init(void); - extern const char * genser_kin_get_name(void); extern int genser_kin_num_joints(void * kins); @@ -125,15 +123,6 @@ extern int genser_kin_fwd_interations(genser_struct * genser); inverse kinematics functions */ extern int genser_kin_inv_iterations(genser_struct * genser); -/*! Sets the maximum number of iterations to use in future calls to - the inverse kinematics functions, after which an error will be - reported */ -extern int genser_kin_inv_set_max_iterations(int i); - -/*! Returns the maximum number of iterations that will be used to - compute inverse kinematics functions */ -extern int genser_kin_inv_get_max_iterations(void); - extern int compute_jfwd(go_link * link_params, int link_number, go_matrix * Jfwd, @@ -142,23 +131,14 @@ extern int compute_jfwd(go_link * link_params, extern int compute_jinv(go_matrix * Jfwd, go_matrix * Jinv); -extern int genserKinematicsJacobian(const double *joint, - const EmcPose *world, - double jac[EMCMOT_MAX_JOINTS][EMCMOT_MAX_AXIS], - const KINEMATICS_INVERSE_FLAGS *iflags); - -extern int genserKinematicsForward(const double *joint, - EmcPose * world, - const KINEMATICS_FORWARD_FLAGS * fflags, - KINEMATICS_INVERSE_FLAGS * iflags); - -extern int genserKinematicsInverse(const EmcPose * world, - double *joints, - const KINEMATICS_INVERSE_FLAGS * iflags, - KINEMATICS_FORWARD_FLAGS * fflags); +/* The kinematics as functions of the parameter block (see kinematics.h): + the DH parameters and the unrotate couplings are the table, the maths + is the ops. genser_links_of() fills a link description from a block, + for a caller that wants the go_ routines directly. */ +extern const kins_param_desc GENSER_PARAMS[]; +extern const int GENSER_NPARAMS; +extern const kins_ops GENSER_OPS; -extern int genserKinematicsSetup(const int comp_id, - const char* coordinates, - kparms* ksetup_parms); +extern void genser_links_of(const kins_params *p, genser_struct *genser); #endif diff --git a/src/emc/kinematics/pentakins.c b/src/emc/kinematics/pentakins.c index f8415b4112c..42047ae8ac1 100644 --- a/src/emc/kinematics/pentakins.c +++ b/src/emc/kinematics/pentakins.c @@ -17,7 +17,7 @@ The default values for base and effector joints positions are defined in the header file pentakins.h. The actual values for a particular - machine can be adjusted by hal parameters: + machine can be adjusted by hal pins: pentakins.base.N.x pentakins.base.N.y @@ -45,6 +45,10 @@ pentakins.tool-offset - tool length from the origin along z axis, changes the effector pivot point. + The maths is written as pure functions of the parameter block (see + kinematics.h): the pins above are the table below, read into the block + before every call, and the entry points come from kins_single.c. + ----------------------------------------------------------------------------*/ #include @@ -52,23 +56,51 @@ #include #include #include /* these decls, KINEMATICS_FORWARD_FLAGS */ +#include #include "pentakins.h" -struct haldata { - hal_real_t basex[NUM_STRUTS]; - hal_real_t basey[NUM_STRUTS]; - hal_real_t basez[NUM_STRUTS]; - hal_real_t effectorr[NUM_STRUTS]; - hal_real_t effectorz[NUM_STRUTS]; - hal_uint_t last_iter; - hal_uint_t max_iter; - hal_uint_t iter_limit; - hal_real_t max_error; - hal_real_t conv_criterion; - hal_real_t tool_offset; -} *haldata; +// the table: five struts' worth of geometry, then the iteration controls +// and reports. P_BASE_X(i) and the rest index it. +#define P_BASE_X(i) (5*(i) + 0) +#define P_BASE_Y(i) (5*(i) + 1) +#define P_BASE_Z(i) (5*(i) + 2) +#define P_EFF_R(i) (5*(i) + 3) +#define P_EFF_Z(i) (5*(i) + 4) +enum { + P_LAST_ITER = 5*NUM_STRUTS, + P_MAX_ITER, + P_MAX_ERROR, + P_CONV_CRITERION, + P_ITER_LIMIT, + P_TOOL_OFFSET, + P_COUNT +}; +#define STRUT_ROWS(i, bx, by, bz, er, ez) \ + { "base." #i ".x", KINS_PARAM_FLOAT, KINS_IN, 0, bx }, \ + { "base." #i ".y", KINS_PARAM_FLOAT, KINS_IN, 0, by }, \ + { "base." #i ".z", KINS_PARAM_FLOAT, KINS_IN, 0, bz }, \ + { "effector." #i ".r", KINS_PARAM_FLOAT, KINS_IN, 0, er }, \ + { "effector." #i ".z", KINS_PARAM_FLOAT, KINS_IN, 0, ez } + +static const kins_param_desc penta_params[P_COUNT] = { + STRUT_ROWS(0, DEFAULT_BASE_0_X, DEFAULT_BASE_0_Y, DEFAULT_BASE_0_Z, DEFAULT_EFFECTOR_0_R, DEFAULT_EFFECTOR_0_Z), + STRUT_ROWS(1, DEFAULT_BASE_1_X, DEFAULT_BASE_1_Y, DEFAULT_BASE_1_Z, DEFAULT_EFFECTOR_1_R, DEFAULT_EFFECTOR_1_Z), + STRUT_ROWS(2, DEFAULT_BASE_2_X, DEFAULT_BASE_2_Y, DEFAULT_BASE_2_Z, DEFAULT_EFFECTOR_2_R, DEFAULT_EFFECTOR_2_Z), + STRUT_ROWS(3, DEFAULT_BASE_3_X, DEFAULT_BASE_3_Y, DEFAULT_BASE_3_Z, DEFAULT_EFFECTOR_3_R, DEFAULT_EFFECTOR_3_Z), + STRUT_ROWS(4, DEFAULT_BASE_4_X, DEFAULT_BASE_4_Y, DEFAULT_BASE_4_Z, DEFAULT_EFFECTOR_4_R, DEFAULT_EFFECTOR_4_Z), + [P_LAST_ITER] = { "last-iterations", KINS_PARAM_U32, KINS_OUT, 0, 0 }, + [P_MAX_ITER] = { "max-iterations", KINS_PARAM_U32, KINS_OUT, 0, 0 }, + [P_MAX_ERROR] = { "max-error", KINS_PARAM_FLOAT, KINS_IO, 0, 100.0 }, + [P_CONV_CRITERION] = { "convergence-criterion", KINS_PARAM_FLOAT, KINS_IO, 0, 1e-9 }, + [P_ITER_LIMIT] = { "limit-iterations", KINS_PARAM_U32, KINS_IO, 0, 120 }, + [P_TOOL_OFFSET] = { "tool-offset", KINS_PARAM_FLOAT, KINS_IN, 1, 0.0 }, +}; + +// the most iterations a converged solution has taken this session, kept +// in the caller's scratch so each caller reports its own +#define MAX_ITER_SEEN(s) ((s)->aux[0]) /******************************* MatInvert5() ***************************/ @@ -179,31 +211,29 @@ static double sqr(double x) return (x)*(x); } -/* declare arrays for base and effector coordinates */ -static PmCartesian b[NUM_STRUTS]; -static double za[NUM_STRUTS], ra[NUM_STRUTS]; - -/************************pentakins_read_hal_pins**************************/ +/* the base and effector geometry of one call, taken from the block */ +typedef struct { + PmCartesian b[NUM_STRUTS]; + double za[NUM_STRUTS], ra[NUM_STRUTS]; +} penta_geometry; -int pentakins_read_hal_pins(void) { +static void geometry_of(const kins_params *p, penta_geometry *g) { int t; - - /* set the base and effector coordinates from hal pin values */ - rtapi_real tool_offset = hal_get_real(haldata->tool_offset); + const double tool_offset = p->tool.tran.z; for (t = 0; t < NUM_STRUTS; t++) { - b[t].x = hal_get_real(haldata->basex[t]); - b[t].y = hal_get_real(haldata->basey[t]); - b[t].z = hal_get_real(haldata->basez[t]) + tool_offset; - ra[t] = hal_get_real(haldata->effectorr[t]); - za[t] = hal_get_real(haldata->effectorz[t]) + tool_offset; + g->b[t].x = p->geometry[P_BASE_X(t)]; + g->b[t].y = p->geometry[P_BASE_Y(t)]; + g->b[t].z = p->geometry[P_BASE_Z(t)] + tool_offset; + g->ra[t] = p->geometry[P_EFF_R(t)]; + g->za[t] = p->geometry[P_EFF_Z(t)] + tool_offset; } - return 0; } /************************ InvKins() ********************************/ -int InvKins(const double * coord, - double * struts) +static int InvKins(const penta_geometry *g, + const double * coord, + double * struts) { PmCartesian xyz, pmcoord, temp; @@ -211,8 +241,6 @@ int InvKins(const double * coord, PmRpy rpy; int i; -// pentakins_read_hal_pins(); - /* define Rotation Matrix */ pmcoord.x = coord[0]; pmcoord.y = coord[1]; @@ -226,32 +254,30 @@ int InvKins(const double * coord, for (i = 0; i < NUM_STRUTS; i++) { /* convert location of effector strut end from effector to world coordinates */ - pmCartCartSub(&b[i], &pmcoord, &temp); + pmCartCartSub(&g->b[i], &pmcoord, &temp); pmMatInv(&RMatrix, &InvRMatrix); pmMatCartMult(&InvRMatrix, &temp, &xyz); /* define strut lengths */ - struts[i] = sqrt( sqr(xyz.z - za[i]) + sqr( sqrt(sqr(xyz.x) + sqr(xyz.y)) - ra[i]) ); + struts[i] = sqrt( sqr(xyz.z - g->za[i]) + sqr( sqrt(sqr(xyz.x) + sqr(xyz.y)) - g->ra[i]) ); } return 0; } -/**************************** kinematicsForward() ***************************/ +/**************************** penta_forward() ***************************/ -int kinematicsForward(const double * joints, - EmcPose * pos, - const KINEMATICS_FORWARD_FLAGS * fflags, - KINEMATICS_INVERSE_FLAGS * iflags) +static int penta_forward(const kins_params *p, kins_scratch *s, + const double * joints, + EmcPose * pos, + const KINEMATICS_FORWARD_FLAGS * fflags, + KINEMATICS_INVERSE_FLAGS * iflags) { (void)fflags; (void)iflags; -// PmCartesian aw; -// PmCartesian InvKinStrutVect,InvKinStrutVectUnit; -// PmCartesian q_trans, RMatrix_a, RMatrix_a_cross_Strut; - + penta_geometry g; double Jacobian[NUM_STRUTS][NUM_STRUTS]; double InverseJacobian[NUM_STRUTS][NUM_STRUTS]; double InvKinStrutLength[NUM_STRUTS], StrutLengthDiff[NUM_STRUTS]; @@ -260,14 +286,11 @@ int kinematicsForward(const double * joints, double coord[NUM_STRUTS]; double conv_err = 1.0; -// PmRotationMatrix RMatrix; -// PmRpy q_RPY; - int iterate = 1; int i, j; unsigned iteration = 0; - pentakins_read_hal_pins(); + geometry_of(p, &g); /* abort on obvious problems, like joints <= 0 */ if (joints[0] <= 0.0 || @@ -286,12 +309,15 @@ int kinematicsForward(const double * joints, coord[4] = pos->b * PM_PI / 180.0; /* Enter Newton-Raphson iterative method */ - rtapi_real max_error = hal_get_real(haldata->max_error); + const double max_error = p->geometry[P_MAX_ERROR]; + const unsigned iter_limit = (unsigned)p->geometry[P_ITER_LIMIT]; + const double conv_criterion = p->geometry[P_CONV_CRITERION]; while (iterate) { /* check for large error and return error flag if no convergence */ if ((conv_err > +(max_error)) || (conv_err < -(max_error))) { /* we can't converge */ + s->failed = 1; return -2; }; @@ -299,22 +325,23 @@ int kinematicsForward(const double * joints, /* check iteration to see if the kinematics can reach the convergence criterion and return error flag if it can't */ - if (iteration > hal_get_ui32(haldata->iter_limit)) { + if (iteration > iter_limit) { /* we can't converge */ + s->failed = 1; return -5; } /* compute StrutLengthDiff[] by running inverse kins on Cartesian estimate to get joint estimate, subtract joints to get joint deltas, and compute inv J while we're at it */ - InvKins(coord, InvKinStrutLength); + InvKins(&g, coord, InvKinStrutLength); for (i = 0; i < NUM_STRUTS; i++) { StrutLengthDiff[i] = InvKinStrutLength[i] - joints[i]; /* Build Inverse Jacobian Matrix */ coord[i] += 1e-4; - InvKins(coord, jointdelta); + InvKins(&g, coord, jointdelta); coord[i] -= 1e-4; for (j = 0; j < NUM_STRUTS; j++) { InverseJacobian[j][i] = (jointdelta[j] - InvKinStrutLength[j]) * 1e4; @@ -342,7 +369,6 @@ int kinematicsForward(const double * joints, /* enter loop to determine if a strut needs another iteration */ iterate = 0; /*assume iteration is done */ - rtapi_real conv_criterion = hal_get_real(haldata->conv_criterion); for (i = 0; i < NUM_STRUTS; i++) { if (fabs(StrutLengthDiff[i]) > conv_criterion) { iterate = 1; @@ -357,34 +383,37 @@ int kinematicsForward(const double * joints, pos->a = coord[3] * 180.0 / PM_PI; pos->b = coord[4] * 180.0 / PM_PI; - hal_set_ui32(haldata->last_iter, iteration); - - if (iteration > hal_get_ui32(haldata->max_iter)){ - hal_set_ui32(haldata->max_iter, iteration); + s->iterations = iteration; + s->failed = 0; + s->out[P_LAST_ITER] = iteration; + if (iteration > MAX_ITER_SEEN(s)) { + MAX_ITER_SEEN(s) = iteration; } + s->out[P_MAX_ITER] = MAX_ITER_SEEN(s); return 0; } -/************************ kinematicsInverse() ********************************/ +/************************ penta_inverse() ********************************/ /* the inverse kinematics take world coordinates and determine joint values, given the inverse kinematics flags to resolve any ambiguities. The forward flags are set to indicate their value appropriate to the world coordinates passed in. */ -/************************ kinematicsInverse() ********************************/ - -int kinematicsInverse(const EmcPose * pos, - double * joints, - const KINEMATICS_INVERSE_FLAGS * iflags, - KINEMATICS_FORWARD_FLAGS * fflags) +static int penta_inverse(const kins_params *p, kins_scratch *s, + const EmcPose * pos, + double * joints, + const KINEMATICS_INVERSE_FLAGS * iflags, + KINEMATICS_FORWARD_FLAGS * fflags) { + (void)s; (void)iflags; (void)fflags; + penta_geometry g; double coord[NUM_STRUTS]; - pentakins_read_hal_pins(); + geometry_of(p, &g); coord[0] = pos->tran.x; coord[1] = pos->tran.y; @@ -392,18 +421,19 @@ int kinematicsInverse(const EmcPose * pos, coord[3] = pos->a * PM_PI / 180.0; coord[4] = pos->b * PM_PI / 180.0; - if (0 != InvKins(coord,joints)) { + if (0 != InvKins(&g, coord, joints)) { return -1; } return 0; } -int kinematicsJacobian(const double * joints, - const EmcPose * pos, - double jac[EMCMOT_MAX_JOINTS][EMCMOT_MAX_AXIS], - const KINEMATICS_INVERSE_FLAGS * iflags) +static int penta_jacobian(const kins_params *p, const double * joints, + const EmcPose * pos, + double jac[EMCMOT_MAX_JOINTS][EMCMOT_MAX_AXIS], + const KINEMATICS_INVERSE_FLAGS * iflags) { + penta_geometry g; PmRotationMatrix R; PmRpy rpy; PmCartesian P, d, xyz, wa, wb, dxyz[5]; @@ -411,7 +441,7 @@ int kinematicsJacobian(const double * joints, (void)joints; (void)iflags; - pentakins_read_hal_pins(); + geometry_of(p, &g); for (j = 0; j < EMCMOT_MAX_JOINTS; j++) { for (a = 0; a < EMCMOT_MAX_AXIS; a++) { jac[j][a] = 0; } } @@ -434,7 +464,7 @@ int kinematicsJacobian(const double * joints, for (i = 0; i < NUM_STRUTS; i++) { double rho, A, B, len; - pmCartCartSub(&b[i], &P, &d); + pmCartCartSub(&g.b[i], &P, &d); /* R^T d, written out since pmMatCartMult applies R */ xyz.x = R.x.x*d.x + R.x.y*d.y + R.x.z*d.z; xyz.y = R.y.x*d.x + R.y.y*d.y + R.y.z*d.z; @@ -461,8 +491,8 @@ int kinematicsJacobian(const double * joints, } rho = sqrt(sqr(xyz.x) + sqr(xyz.y)); - A = xyz.z - za[i]; - B = rho - ra[i]; + A = xyz.z - g.za[i]; + B = rho - g.ra[i]; len = sqrt(sqr(A) + sqr(B)); if (len <= 0 || rho <= 0) { return -1; } for (col = 0; col < 5; col++) { @@ -473,103 +503,43 @@ int kinematicsJacobian(const double * joints, return 0; } -KINEMATICS_TYPE kinematicsType() -{ - return KINEMATICS_BOTH; -} +// the forward iterates from the pose it is handed +static const kins_ops penta_ops = { + .forward = penta_forward, + .inverse = penta_inverse, + .jacobian = penta_jacobian, + .fwd_iterates = 1, +}; -KINS_NOT_SWITCHABLE -EXPORT_SYMBOL(kinematicsType); -EXPORT_SYMBOL(kinematicsForward); -EXPORT_SYMBOL(kinematicsInverse); -EXPORT_SYMBOL(kinematicsJacobian); +const kins_module_info kins_module = { + .name = "pentakins", + .halprefix = "pentakins", + .params = penta_params, + .nparams = P_COUNT, + .required_coordinates = "XYZAB", + .max_joints = NUM_STRUTS, + .allow_duplicates = 0, + .ntypes = 1, + .ops = { &penta_ops }, +}; MODULE_LICENSE("GPL"); int comp_id; -static const rtapi_real init_basex[NUM_STRUTS] = { - DEFAULT_BASE_0_X, DEFAULT_BASE_1_X, DEFAULT_BASE_2_X, DEFAULT_BASE_3_X, DEFAULT_BASE_4_X -}; -static const rtapi_real init_basey[NUM_STRUTS] = { - DEFAULT_BASE_0_Y, DEFAULT_BASE_1_Y, DEFAULT_BASE_2_Y, DEFAULT_BASE_3_Y, DEFAULT_BASE_4_Y -}; -static const rtapi_real init_basez[NUM_STRUTS] = { - DEFAULT_BASE_0_Z, DEFAULT_BASE_1_Z, DEFAULT_BASE_2_Z, DEFAULT_BASE_3_Z, DEFAULT_BASE_4_Z -}; -static const rtapi_real init_effectorr[NUM_STRUTS] = { - DEFAULT_EFFECTOR_0_R, DEFAULT_EFFECTOR_1_R, DEFAULT_EFFECTOR_2_R, DEFAULT_EFFECTOR_3_R, DEFAULT_EFFECTOR_4_R -}; -static const rtapi_real init_effectorz[NUM_STRUTS] = { - DEFAULT_EFFECTOR_0_Z, DEFAULT_EFFECTOR_1_Z, DEFAULT_EFFECTOR_2_Z, DEFAULT_EFFECTOR_3_Z, DEFAULT_EFFECTOR_4_Z -}; - int rtapi_app_main(void) { - int res = 0, i; - comp_id = hal_init("pentakins"); if (comp_id < 0) return comp_id; - haldata = hal_malloc(sizeof(struct haldata)); - if (!haldata) - goto error; - - - for (i = 0; i < NUM_STRUTS; i++) { - - if ((res = hal_param_new_real(comp_id, HAL_RW, &(haldata->basex[i]), - init_basex[i], "pentakins.base.%d.x", i)) < 0) - goto error; - - if ((res = hal_param_new_real(comp_id, HAL_RW, &haldata->basey[i], - init_basey[i], "pentakins.base.%d.y", i)) < 0) - goto error; - - if ((res = hal_param_new_real(comp_id, HAL_RW, &haldata->basez[i], - init_basez[i], "pentakins.base.%d.z", i)) < 0) - goto error; - - if ((res = hal_param_new_real(comp_id, HAL_RW, &haldata->effectorr[i], - init_effectorr[i], "pentakins.effector.%d.r", i)) < 0) - goto error; - - if ((res = hal_param_new_real(comp_id, HAL_RW, &haldata->effectorz[i], - init_effectorz[i], "pentakins.effector.%d.z", i)) < 0) - goto error; + if (kinsSingleInit(comp_id, "XYZAB", KINEMATICS_BOTH)) { + hal_exit(comp_id); + return -1; } - if ((res = hal_pin_new_ui32(comp_id, HAL_OUT, &haldata->last_iter, - 0, "pentakins.last-iterations")) < 0) - goto error; - - if ((res = hal_pin_new_ui32(comp_id, HAL_OUT, &haldata->max_iter, - 0, "pentakins.max-iterations")) < 0) - goto error; - - if ((res = hal_pin_new_real(comp_id, HAL_IO, &haldata->max_error, - 100.0, "pentakins.max-error")) < 0) - goto error; - - if ((res = hal_pin_new_real(comp_id, HAL_IO, &haldata->conv_criterion, - 1e-9, "pentakins.convergence-criterion")) < 0) - goto error; - - if ((res = hal_pin_new_ui32(comp_id, HAL_IO, &haldata->iter_limit, - 120, "pentakins.limit-iterations")) < 0) - goto error; - - if ((res = hal_pin_new_real(comp_id, HAL_IN, &haldata->tool_offset, - 0.0, "pentakins.tool-offset")) < 0) - goto error; - hal_ready(comp_id); return 0; - -error: - hal_exit(comp_id); - return res; } diff --git a/src/emc/kinematics/ugenserkins.c b/src/emc/kinematics/ugenserkins.c index 1d80e5ae70c..0d2c05b5a41 100644 --- a/src/emc/kinematics/ugenserkins.c +++ b/src/emc/kinematics/ugenserkins.c @@ -13,6 +13,7 @@ #include /* ulapi */ +#include #include /* struct timeval */ #include "genserkins.h" @@ -43,14 +44,25 @@ int main(int argc, char *argv[]) int retval = 0; double start, end; int comp_id; - kparms kp; - kp.max_joints = GENSER_MAX_JOINTS; - kp.allow_duplicates = 0; + kins_module_info info; + kins_params params; + kins_scratch scratch; - comp_id = hal_init("usergenserkins"); - if (genserKinematicsSetup(comp_id,"XYZABC",&kp)) printf("unexpected\n"); + /* the module described the way kinsDescribe() would, then a block at + the table defaults; setp has no say here */ + memset(&info, 0, sizeof(info)); + info.name = "genserkins"; + info.halprefix = "genserkins"; + info.params = GENSER_PARAMS; + info.nparams = GENSER_NPARAMS; + info.required_coordinates = "XYZABC"; + info.max_joints = GENSER_MAX_JOINTS; + info.ntypes = 1; + info.ops[0] = &GENSER_OPS; - genser_kin_init(); + comp_id = hal_init("usergenserkins"); + if (kinsParamsInit(¶ms, &info, "XYZABC")) printf("unexpected\n"); + kinsScratchInit(&scratch); /* syntax is a.out {i|f # # # # # #} */ if (argc == 8) { @@ -123,14 +135,14 @@ fprintf(stderr,"gki0:P %6.2f %6.2f %6.2f %6.2f %6.2f %6.2f\n", pos.tran.x,pos.tran.y,pos.tran.z,pos.a,pos.b,pos.c); fprintf(stderr,"gki1:J %6.2f %6.2f %6.2f %6.2f %6.2f %6.2f\n", joints[0],joints[1],joints[2],joints[3],joints[4],joints[5]); - retval = genserKinematicsInverse(&pos, joints, &iflags, &fflags); + retval = GENSER_OPS.inverse(¶ms, &scratch, &pos, joints, &iflags, &fflags); fprintf(stderr,"gki2:J %6.2f %6.2f %6.2f %6.2f %6.2f %6.2f\n", joints[0],joints[1],joints[2],joints[3],joints[4],joints[5]); if (0 != retval) { printf("inv kins error %d <%s>\n", retval,go_result_to_string(retval)); } } else { - retval = genserKinematicsForward(joints, &pos, &fflags, &iflags); + retval = GENSER_OPS.forward(¶ms, &scratch, joints, &pos, &fflags, &iflags); if (0 != retval) { printf("fwd kins error %d\n", retval); } @@ -220,14 +232,14 @@ joints[0],joints[1],joints[2],joints[3],joints[4],joints[5]); } else { fprintf(stderr,"gki1:\n"); retval = - genserKinematicsInverse(&pos, joints, &iflags, &fflags); + GENSER_OPS.inverse(¶ms, &scratch, &pos, joints, &iflags, &fflags); printf("%f %f %f %f %f %f\n", joints[0], joints[1], joints[2], joints[3], joints[4], joints[5]); if (0 != retval) { printf("inv kins error %d <%s>\n", retval,go_result_to_string(retval)); } else { retval = - genserKinematicsForward(joints, &pos, &fflags, &iflags); + GENSER_OPS.forward(¶ms, &scratch, joints, &pos, &fflags, &iflags); printf("%f %f %f %f %f %f\n", pos.tran.x, pos.tran.y, pos.tran.z, pos.a, pos.b, pos.c); if (0 != retval) { @@ -271,13 +283,13 @@ fprintf(stderr,"gki1:\n"); &joints[0], &joints[1], &joints[2], &joints[3], &joints[4], &joints[5])) { printf("?\n"); } else { - retval = genserKinematicsForward(joints, &pos, &fflags, &iflags); + retval = GENSER_OPS.forward(¶ms, &scratch, joints, &pos, &fflags, &iflags); printf("xyzabc: %f %f %f %f %f %f\n", pos.tran.x, pos.tran.y, pos.tran.z, pos.a, pos.b, pos.c); if (0 != retval) { printf("fwd kins error %d\n", retval); } else { - retval = genserKinematicsInverse(&pos, joints, &iflags, &fflags); + retval = GENSER_OPS.inverse(¶ms, &scratch, &pos, joints, &iflags, &fflags); printf("j0--j5: %f %f %f %f %f %f\n", joints[0], joints[1], joints[2], joints[3], joints[4], joints[5]); if (0 != retval) { From 13bac4b3fb96c5d9a37997a90a6142909cda90b9 Mon Sep 17 00:00:00 2001 From: Luca Toniolo <10792599+grandixximo@users.noreply.github.com> Date: Fri, 4 Sep 2026 13:24:57 +1000 Subject: [PATCH 34/60] switchkinscomp: write the template on the parameter block 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. --- .gitignore | 1 + debian/linuxcnc-uspace-dev.install | 1 + src/Makefile | 2 +- src/emc/kinematics/Submakefile | 1 + src/hal/components/switchkinscomp.comp | 134 +++++++++++++++---------- 5 files changed, 84 insertions(+), 55 deletions(-) diff --git a/.gitignore b/.gitignore index 19647ebbccd..e3f96f3a77a 100644 --- a/.gitignore +++ b/.gitignore @@ -22,6 +22,7 @@ share/desktop-directories/linuxcnc-ref.directory share/desktop-directories/linuxcnc-doc.directory share/linuxcnc/mesa_modbus.c.tmpl share/linuxcnc/switchkins.c +share/linuxcnc/switchkins_setup.c share/linuxcnc/kins_util.c share/linuxcnc/kins_single.c src/modules.order diff --git a/debian/linuxcnc-uspace-dev.install b/debian/linuxcnc-uspace-dev.install index 251c401a9e9..e13585d482d 100644 --- a/debian/linuxcnc-uspace-dev.install +++ b/debian/linuxcnc-uspace-dev.install @@ -6,5 +6,6 @@ usr/lib/*.so usr/share/linuxcnc/Makefile.modinc usr/share/linuxcnc/mesa_modbus.c.tmpl usr/share/linuxcnc/switchkins.c +usr/share/linuxcnc/switchkins_setup.c usr/share/linuxcnc/kins_util.c usr/share/linuxcnc/kins_single.c diff --git a/src/Makefile b/src/Makefile index 71a9f7f06b0..fabf8b2c041 100644 --- a/src/Makefile +++ b/src/Makefile @@ -788,7 +788,7 @@ ifeq ($(BUILD_GUI),yes) endif $(FILE) ../src/hal/drivers/mesa-hostmot2/modbus/*.tmpl $(DESTDIR)$(prefix)/share/linuxcnc/ - $(FILE) ../src/emc/kinematics/switchkins.c ../src/emc/kinematics/kins_util.c ../src/emc/kinematics/kins_single.c $(DESTDIR)$(prefix)/share/linuxcnc/ + $(FILE) ../src/emc/kinematics/switchkins.c ../src/emc/kinematics/switchkins_setup.c ../src/emc/kinematics/kins_util.c ../src/emc/kinematics/kins_single.c $(DESTDIR)$(prefix)/share/linuxcnc/ install-kernel-indep: install-python install-python: install-dirs diff --git a/src/emc/kinematics/Submakefile b/src/emc/kinematics/Submakefile index dbbc783f21b..89c6173d5be 100644 --- a/src/emc/kinematics/Submakefile +++ b/src/emc/kinematics/Submakefile @@ -40,6 +40,7 @@ PYTARGETS += $(RDELTAMODULE) # in-tree ones link it. EMCKINEMATICSSRCS = \ ../share/linuxcnc/switchkins.c \ + ../share/linuxcnc/switchkins_setup.c \ ../share/linuxcnc/kins_util.c \ ../share/linuxcnc/kins_single.c diff --git a/src/hal/components/switchkinscomp.comp b/src/hal/components/switchkinscomp.comp index 7e90edc380f..e5ca4034b9c 100644 --- a/src/hal/components/switchkinscomp.comp +++ b/src/hal/components/switchkinscomp.comp @@ -17,6 +17,12 @@ replace with the kinematics wanted. The switchkins implementation is installed as source alongside the headers, so nothing needs a path to a LinuxCNC source tree. +The kinematics are written as functions of a parameter block, see +kinematics.h and the Kinematics Conventions chapter: the geometry is +declared once in a table, one HAL pin is made per entry, and the maths +reads the block where it would have read a pin. The same maths can +then be evaluated outside realtime. + To avoid updates that overwrite switchkinscomp.comp, best practice is to rename the file and its component name (example: *user_switchkins.comp* creates module: *user_switchkins*). @@ -53,11 +59,15 @@ option extra_setup; // switchkins.c provides kinematicsForward(), kinematicsInverse(), // kinematicsSwitch() and the rest of the kinematics interface, and // dispatches each call to the currently selected switchkins-type. -// kins_util.c provides the identity kinematics and the coordinates -// letters-to-joints mapping they use. Both are installed with the -// headers, so halcompile finds them with no path of your own. +// switchkins_setup.c runs the switchkinsSetup() below and provides +// kinsDescribe() for a copy of the module loaded outside realtime. +// kins_util.c provides the identity kinematics, the parameter block +// helpers and the coordinates letters-to-joints mapping. All are +// installed with the headers, so halcompile finds them with no path of +// your own. #include +#include #include //===================================================================== @@ -66,36 +76,31 @@ static char *coordinates; RTAPI_MP_STRING(coordinates, "Axes-to-joints-ordering"); //--------------------------------------------------------------------- -// Example switchkins-type. A setup routine creating whatever hal pins -// the kinematics need, plus a forward and an inverse routine. Replace -// the arithmetic with the real kinematics. - -static struct { - hal_real_t x_offset; -} *mydata; - -static int myKinematicsSetup(const int comp_id, - const char* coords, - kparms* kp) -{ - (void)coords; // this type does not use the coordinates mapping - - mydata = hal_malloc(sizeof(*mydata)); - if (!mydata) return -1; +// The geometry: one HAL pin per entry, named ., read +// into the block before every call. Add whatever the real kinematics +// need; an entry flagged as the tool arrives in p->tool.tran.z as well. - return hal_pin_new_real(comp_id, HAL_IN, &mydata->x_offset, 0.0, - "%s.x-offset", kp->halprefix); -} // myKinematicsSetup() +static const kins_param_desc my_params[] = { + { "x-offset", KINS_PARAM_FLOAT, KINS_IN, 0, 0.0 }, +}; +enum { P_X_OFFSET }; -static int myKinematicsForward(const double *j, - EmcPose * pos, - const KINEMATICS_FORWARD_FLAGS * fflags, - KINEMATICS_INVERSE_FLAGS * iflags) +//--------------------------------------------------------------------- +// Example switchkins-type: a forward and an inverse over the block. +// Replace the arithmetic with the real kinematics. The frames and the +// Jacobian are optional, see kinematics.h. + +static int myForward(const kins_params *p, kins_scratch *s, + const double *j, + EmcPose * pos, + const KINEMATICS_FORWARD_FLAGS * fflags, + KINEMATICS_INVERSE_FLAGS * iflags) { + (void)s; (void)fflags; (void)iflags; - pos->tran.x = j[0] + hal_get_real(mydata->x_offset); + pos->tran.x = j[0] + p->geometry[P_X_OFFSET]; pos->tran.y = j[1]; pos->tran.z = j[2]; @@ -104,50 +109,71 @@ static int myKinematicsForward(const double *j, pos->u = pos->v = pos->w = 0; return 0; -} // myKinematicsForward() +} // myForward() -static int myKinematicsInverse(const EmcPose * pos, - double *j, - const KINEMATICS_INVERSE_FLAGS * iflags, - KINEMATICS_FORWARD_FLAGS * fflags) +static int myInverse(const kins_params *p, kins_scratch *s, + const EmcPose * pos, + double *j, + const KINEMATICS_INVERSE_FLAGS * iflags, + KINEMATICS_FORWARD_FLAGS * fflags) { + (void)s; (void)iflags; (void)fflags; - j[0] = pos->tran.x - hal_get_real(mydata->x_offset); + j[0] = pos->tran.x - p->geometry[P_X_OFFSET]; j[1] = pos->tran.y; j[2] = pos->tran.z; return 0; -} // myKinematicsInverse() +} // myInverse() + +static const kins_ops my_ops = { + .forward = myForward, + .inverse = myInverse, +}; + +//--------------------------------------------------------------------- +// The module's configuration and its switchkins-types. Type 0 is the +// startup default. Types run from 0 to SWITCHKINS_MAX_TYPES-1 with no +// gaps. + +int switchkinsSetup(kparms* kp, + KS* kset0, KS* kset1, KS* kset2, + KF* kfwd0, KF* kfwd1, KF* kfwd2, + KI* kinv0, KI* kinv1, KI* kinv2 + ) +{ + // the pointer arguments are the older way of providing types 0 to 2 + (void)kset0; (void)kset1; (void)kset2; + (void)kfwd0; (void)kfwd1; (void)kfwd2; + (void)kinv0; (void)kinv1; (void)kinv2; + + kp->kinsname = "switchkinscomp"; // must agree with the module name + kp->halprefix = "switchkinscomp"; // hal pin names + kp->required_coordinates = "xyz"; + kp->allow_duplicates = 0; + kp->fwd_iterates_mask = 0; // set bit N if type N iterates + kp->gui_kinstype = -1; // negative means: not used + kp->max_joints = strlen(kp->required_coordinates); + kp->params = my_params; + kp->nparams = sizeof(my_params)/sizeof(my_params[0]); + + if (switchkinsRegisterOps(0, &KINS_IDENTITY_OPS)) { return -1; } + if (switchkinsRegisterOps(1, &my_ops)) { return -1; } + return 0; +} // switchkinsSetup() //--------------------------------------------------------------------- // rtapi_app_main() is supplied by halcompile, which calls hal_init() // before EXTRA_SETUP() and hal_ready() after it. That is what -// switchkinsInit() expects, so the switchkins-types are registered and -// the implementation started from here. +// switchkinsInit() expects, so setup is run and the implementation +// started from here. EXTRA_SETUP() { kparms kp; (void)__comp_inst; (void)prefix; (void)extra_arg; - kp.kinsname = "switchkinscomp"; // must agree with the module name - kp.halprefix = "switchkinscomp"; // hal pin names - kp.required_coordinates = "xyz"; - kp.allow_duplicates = 0; - kp.fwd_iterates_mask = 0; // set bit N if type N iterates - kp.gui_kinstype = -1; // negative means: not used - kp.sparm = NULL; - kp.max_joints = strlen(kp.required_coordinates); - - // switchkins-type 0 is the startup default. Types run from 0 to - // SWITCHKINS_MAX_TYPES-1 with no gaps. - if (switchkinsRegister(0, identityKinematicsSetup, - identityKinematicsForward, - identityKinematicsInverse)) { return -1; } - if (switchkinsRegister(1, myKinematicsSetup, - myKinematicsForward, - myKinematicsInverse)) { return -1; } - + if (switchkinsRunSetup(&kp, NULL)) { return -1; } return switchkinsInit(comp_id, &kp, coordinates); } // EXTRA_SETUP() From abab22c03e0c685fe94cee3cd4e81d46da50d79e Mon Sep 17 00:00:00 2001 From: Luca Toniolo <10792599+grandixximo@users.noreply.github.com> Date: Fri, 11 Sep 2026 19:55:34 +0800 Subject: [PATCH 35/60] switchkins: declare the primary kinstype from the ops table A kins_ops table now says what its type IS the same way it says the maths: .primary marks the module's working transform, and switchkinsRegisterOps() bridges it into the declared flags exactly like .identity. G43.4 resolves the switch target from the flag, so the number of the working kinematics stops being a guess. Like identity, primary may be declared for at most one kinstype; two answers fail the module load. Every in-tree working transform declares it: fiveaxis, the two trt tables, scara, puma, genser, genhex, three21, millturn, tdr and the trsrn tcp tables. The trsrn tool kinematics stays undeclared: it is a reporting mode, not the working transform. --- src/emc/kinematics/5axiskins.c | 1 + src/emc/kinematics/genhexkins.c | 1 + src/emc/kinematics/genserfuncs.c | 1 + src/emc/kinematics/kinematics.h | 4 +++- src/emc/kinematics/kins_single.c | 8 ++++++++ src/emc/kinematics/pumakins.c | 1 + src/emc/kinematics/scarakins.c | 1 + src/emc/kinematics/switchkins.c | 11 +++++++++-- src/emc/kinematics/three21kins.c | 1 + src/emc/kinematics/trtfuncs.c | 2 ++ src/hal/components/millturn.comp | 1 + src/hal/components/xyzab_tdr_kins.comp | 1 + src/hal/components/xyzacb_trsrn.comp | 1 + src/hal/components/xyzbca_trsrn.comp | 1 + 14 files changed, 32 insertions(+), 3 deletions(-) diff --git a/src/emc/kinematics/5axiskins.c b/src/emc/kinematics/5axiskins.c index 267025f910f..7b95300f4cf 100644 --- a/src/emc/kinematics/5axiskins.c +++ b/src/emc/kinematics/5axiskins.c @@ -206,6 +206,7 @@ static const kins_ops fiveaxis_ops = { .forward = fiveaxis_forward, .inverse = fiveaxis_inverse, .jacobian = fiveaxis_jacobian, + .primary = 1, }; int switchkinsSetup(kparms* kp, diff --git a/src/emc/kinematics/genhexkins.c b/src/emc/kinematics/genhexkins.c index 1efebaba756..418dfc8f5dd 100644 --- a/src/emc/kinematics/genhexkins.c +++ b/src/emc/kinematics/genhexkins.c @@ -695,6 +695,7 @@ static const kins_ops genhex_ops = { .inverse = genhex_inverse, .jacobian = genhex_jacobian, .fwd_iterates = 1, + .primary = 1, }; int switchkinsSetup(kparms* kp, diff --git a/src/emc/kinematics/genserfuncs.c b/src/emc/kinematics/genserfuncs.c index 2f943d681aa..a25ebcc3c71 100644 --- a/src/emc/kinematics/genserfuncs.c +++ b/src/emc/kinematics/genserfuncs.c @@ -673,6 +673,7 @@ const kins_ops GENSER_OPS = { .forward = genser_forward, .inverse = genser_inverse, .jacobian = genser_jacobian, + .primary = 1, }; /* diff --git a/src/emc/kinematics/kinematics.h b/src/emc/kinematics/kinematics.h index ec31358075f..3846170f204 100644 --- a/src/emc/kinematics/kinematics.h +++ b/src/emc/kinematics/kinematics.h @@ -557,7 +557,8 @@ typedef int (*kins_jacobian_fn)(const kins_params *p, const double *joint, a missing Jacobian is differenced from the inverse. fwd_iterates says the forward starts from the pose it is handed, so the shared code seeds it with the last answer after a switch. identity says joints are axes, which - a consumer may use to skip the maths altogether. */ + a consumer may use to skip the maths altogether. primary says this is + the module's working transform, the type G43.4 switches to. */ typedef struct kins_ops { kins_forward_fn forward; kins_inverse_fn inverse; @@ -567,6 +568,7 @@ typedef struct kins_ops { kins_jacobian_fn jacobian; int fwd_iterates; int identity; /* joints are axes */ + int primary; /* the working transform */ } kins_ops; /* A module described for a caller outside RT: its table, its joint diff --git a/src/emc/kinematics/kins_single.c b/src/emc/kinematics/kins_single.c index 58f914076d5..16461c49375 100644 --- a/src/emc/kinematics/kins_single.c +++ b/src/emc/kinematics/kins_single.c @@ -130,6 +130,13 @@ int kinematicsSwitch(int switchkins_type) return 0; } +// one type and nothing declared about it: -1, no information +int kinematicsTypeFlags(int ktype) +{ + (void)ktype; + return -1; +} + // The module's description, for a copy of it loaded outside RT. A module // with one type does not depend on its parameters for its shape, so this // is the table as declared. @@ -152,4 +159,5 @@ EXPORT_SYMBOL(kinematicsToolFrame); EXPORT_SYMBOL(kinematicsJacobian); EXPORT_SYMBOL(kinematicsSwitchable); EXPORT_SYMBOL(kinematicsSwitch); +EXPORT_SYMBOL(kinematicsTypeFlags); EXPORT_SYMBOL(kinsDescribe); diff --git a/src/emc/kinematics/pumakins.c b/src/emc/kinematics/pumakins.c index 09dfb8193ab..8985ec4b5ac 100644 --- a/src/emc/kinematics/pumakins.c +++ b/src/emc/kinematics/pumakins.c @@ -400,6 +400,7 @@ static const kins_ops puma_ops = { .work = kinsIdentityFrame, .tool = puma_tool_frame, .native = &TOOL_FRAME_FLANGE, + .primary = 1, }; int switchkinsSetup(kparms* kp, diff --git a/src/emc/kinematics/scarakins.c b/src/emc/kinematics/scarakins.c index c6137331d7f..3092f33e189 100644 --- a/src/emc/kinematics/scarakins.c +++ b/src/emc/kinematics/scarakins.c @@ -251,6 +251,7 @@ static const kins_ops scara_ops = { .forward = scara_forward, .inverse = scara_inverse, .jacobian = scara_jacobian, + .primary = 1, }; int switchkinsSetup(kparms* kp, diff --git a/src/emc/kinematics/switchkins.c b/src/emc/kinematics/switchkins.c index 24b432262fb..a1a2c2d4066 100644 --- a/src/emc/kinematics/switchkins.c +++ b/src/emc/kinematics/switchkins.c @@ -482,6 +482,7 @@ int switchkinsRegisterOps(int ktype, const kins_ops *ops) } kops[ktype] = ops; if (ops->identity) { ktype_flags[ktype] |= KINSTYPE_IDENTITY; } + if (ops->primary) { ktype_flags[ktype] |= KINSTYPE_PRIMARY; } return 0; } // switchkinsRegisterOps() @@ -604,7 +605,7 @@ int switchkinsInit(const int comp_id, const char* coordinates) { int i; - int identities; + int identities, primaries; int res = 0; char* emsg = "other"; @@ -633,8 +634,10 @@ int switchkinsInit(const int comp_id, if (!kins_count) { emsg = "no switchkins-types provided"; goto error; } // declarations must name provided types, and identity is unique: - // G13.1 resolves it from the flags, so two answers is a load error + // G13.1 resolves it from the flags, so two answers is a load error. + // Primary is unique the same way: G43.4 switches to it. identities = 0; + primaries = 0; for (i=0; i < SWITCHKINS_MAX_TYPES; i++) { if (!ktype_flags[i]) { continue; } if (i >= kins_count) { @@ -644,6 +647,7 @@ int switchkinsInit(const int comp_id, emsg = "declared switchkins-type not provided"; goto error; } if (ktype_flags[i] & KINSTYPE_IDENTITY) { identities++; } + if (ktype_flags[i] & KINSTYPE_PRIMARY) { primaries++; } rtapi_print("switchkins-type %d declared:%s%s\n", i, (ktype_flags[i] & KINSTYPE_IDENTITY) ? " identity" : "", (ktype_flags[i] & KINSTYPE_PRIMARY) ? " primary" : ""); @@ -651,6 +655,9 @@ int switchkinsInit(const int comp_id, if (identities > 1) { emsg = "more than one identity switchkins-type declared"; goto error; } + if (primaries > 1) { + emsg = "more than one primary switchkins-type declared"; goto error; + } for (i=0; i < SWITCHKINS_MAX_TYPES; i++) { if (kp.fwd_iterates_mask & (1< Date: Fri, 4 Sep 2026 13:24:58 +1000 Subject: [PATCH 36/60] docs: describe the parameter block form of a kinematics module 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. --- docs/src/motion/kinematics-conventions.adoc | 97 +++++++++++++- docs/src/motion/switchkins.adoc | 137 ++++++++++++-------- 2 files changed, 176 insertions(+), 58 deletions(-) diff --git a/docs/src/motion/kinematics-conventions.adoc b/docs/src/motion/kinematics-conventions.adoc index 1825d8af4d4..2631163c357 100644 --- a/docs/src/motion/kinematics-conventions.adoc +++ b/docs/src/motion/kinematics-conventions.adoc @@ -178,7 +178,7 @@ half turn about one of the two transverse axes, and which one is chosen decides where tool X lands. Because it is a rotation in its own right, a module declares it rather than -applying it by hand, as the last argument of `switchkinsRegisterFrames()`. +applying it by hand, in the `native` field of its ops table. Shared code applies it and checks once, at load, that it is orthonormal with determinant +1. `TOOL_FRAME_SPINDLE` is the identity, for a module whose maths is already in the convention; `TOOL_FRAME_FLANGE` is the half turn a @@ -289,6 +289,8 @@ All of these are functions of the joint values and the module's own geometry. None needs state carried between calls, and none needs the module to be running in a realtime thread to be useful: the interesting callers, a limit check before a move and a preview before a program runs, are not in the servo loop. +<> is how a module is written so that they +can call it. [[sec:orientation-inverse]] == The Orientation Inverse @@ -444,9 +446,9 @@ flags select. That costs a few microseconds on a closed form inverse and milliseconds on one that iterates, and it answers to the inverse's own precision, which for an iterating inverse is its convergence tolerance divided by the step. Modules built on `switchkins.c` answer this way for every type -that registers nothing; an identity type answers exactly. +whose ops table has no Jacobian; an identity type answers exactly. -A module with a closed form registers it with `switchkinsRegisterJacobian()`. +A module with a closed form puts it in the `jacobian` field of its ops table. It is exact, it costs what the inverse costs, and it knows its own singular poses rather than discovering them as an inverse that fails a step away from the pose. Every module in the tree whose inverse is written out supplies one. @@ -458,6 +460,88 @@ rather than from the pose, which the nutating heads do, has an inverse whose derivative about the pose is not the coupling the machine has. Such a module supplies the closed form, taken against the pose. +[[sec:parameters]] +== The Parameter Block + +Everything above is a function of the joint values, the tool and the machine's +geometry. A module written the old way reads its geometry from HAL pins it +created, keeps its kinematics type and its iteration scratch in statics, and so +can only answer for the machine as it is now, from inside the realtime thread. +Anything else that needs the same maths, a planner evaluating poses the machine +has not reached, task checking a program at load, a tool asking what if, had to +carry a second copy of it, and the two copies drift. + +A module is written instead as functions of two blocks the caller supplies. +`kins_params` describes the machine: the kinematics type, the joint map from +`coordinates=`, the tool offset, and the geometry as an array of doubles. One +copy may be shared by any number of callers, since nothing writes it during a +call. `kins_scratch` is what one caller carries between its own calls: the pose +an iterating forward last found, which seeds the next, and what the module +reports about the call it just made. It is never shared between callers, so +motion and a planner evaluating the same module cannot disturb each other. + +=== The table + +A module declares its geometry as a table of named entries, one per value it +reads. The name is the pin name the config already uses, less the module +prefix, so nothing in a config changes. + +[source,c] +---- +static const kins_param_desc fiveaxis_params[] = { + { "pivot-length", KINS_PARAM_FLOAT, KINS_IN, 0, DEFAULT_PIVOT_LENGTH }, +}; +enum { P_PIVOT_LENGTH }; +---- + +An entry is an input, an output, or an input that can be poked (`KINS_IO`, a +`HAL_IO` pin). The maths reads `p->geometry[P_PIVOT_LENGTH]` where it read a +pin, and writes an output into `s->out[]` at the same index. An entry flagged +as the tool is the tool length along the tool axis; the shared code puts its +value in `p->tool.tran.z` as well, which is what the maths reads, so that a +caller outside realtime can supply the tool from the tool table without there +being a pin. + +=== The ops table + +The maths of one kinematics type is a `kins_ops` table: the forward and inverse, +the optional work and tool frames with the native rotation that relates the +tool frame to the convention, and the optional Jacobian. A type whose forward +iterates from the pose it is handed says so, and the shared code seeds it with +the last answer after a switch. A type also says what it IS: `identity` marks +the no-transform type `G13.1` cancels to, `primary` the working transform +`G43.4` switches to (see the Switchable Kinematics chapter). A module with +several types has one geometry table and one ops table per type, registered +with `switchkinsRegisterOps()`; a module with one type describes itself in a +`kins_module` and links `kins_single.c`. + +=== What the shared code does + +In realtime it makes one HAL pin per table entry, copies the pins into the +block before every call and the outputs back after it, and supplies the classic +entry points, `kinematicsForward()` and the rest, so that motion sees no +difference. Outside realtime a module exports `kinsDescribe()`, which hands a +caller its table and the ops of each type; the caller fills a block from +wherever it likes and asks the same functions through `kinsOpsForward()`, +`kinsOpsInverse()`, `kinsOpsJacobian()` and the frame calls, with the same +defaults applied, so both sides get the same answers. The non-realtime loader +in `kinematics_userspace/` binds the pins of the running module by the table's +names and takes the tool from motion's own offset pins, and says once when the +module's tool pin disagrees with them, which is a config that lost the tool on +the way. `kinslimits` is built on it. + +A module that does not provide the form keeps working as it did. It just cannot +be evaluated outside realtime, which the loader reports. + +=== What stays outside the block + +The kinematics type is in the block, so a caller evaluating a program that +switches type puts the type each block will run under in its own block, and +nothing is switched globally. The tool is in the block, from motion. The joint +map is in the block, from `coordinates=`. Nothing else the maths needs exists, +and a module that finds it needs something else has found a parameter it should +declare. + [[sec:writing-a-module]] == Writing a Module @@ -498,6 +582,13 @@ Geometry stays in the module:: the module. A consumer that restates it has taken a copy that nothing keeps in step, which is the situation this chapter exists to end. +No state:: + Write the maths as functions of the parameter block and the scratch, as + <> describes: geometry in the table, + the kinematics type and the tool from the block, and anything carried + between calls in the scratch. A static in a module is a second machine + that only the realtime thread can see. + Mount orientation is not this:: A tool or holder mount orientation is a different quantity: a right-angle head, a tool held at an angle, an end effector clocked on its flange. Those diff --git a/docs/src/motion/switchkins.adoc b/docs/src/motion/switchkins.adoc index 14fb1b7aabb..de3afc3fc40 100644 --- a/docs/src/motion/switchkins.adoc +++ b/docs/src/motion/switchkins.adoc @@ -440,7 +440,8 @@ Custom kinematics can be coded and tested on Run-In-Place ('RIP') builds. A template file src/emc/kinematics/userkfuncs.c is provided in the distribution. This file can be copied/renamed to a user directory and edited to supply custom kinematics with -kinstype==2. +kinstype==2: the in-tree modules register its USERK_OPS as that +kinstype, so the forward and inverse in the copy are what runs. The user custom kinematics file can be compiled from out-of-tree source locations for rt-preempt implementations or by replacing @@ -469,18 +470,20 @@ is included: [source,c] ---- #include +#include #include ---- A realtime module cannot link a library, so the implementation arrives -as source: switchkins.c and kins_util.c are installed beside the -headers, in share/linuxcnc, and halcompile already looks there. With +as source: switchkins.c, switchkins_setup.c and kins_util.c are +installed beside the headers, in share/linuxcnc, and halcompile already +looks there. With a deb install they come from the linuxcnc-dev package. -The module registers each of its kinstypes and calls switchkinsInit() -from EXTRA_SETUP(), which halcompile runs after hal_init() and before -hal_ready(). See <> for both -calls. +The module supplies switchkinsSetup() and calls switchkinsRunSetup() +and switchkinsInit() from EXTRA_SETUP(), which halcompile runs after +hal_init() and before hal_ready(). See <> for the calls. ---- $ halcompile --install user_switchkins.comp @@ -532,17 +535,26 @@ kinstype currently selected, and it creates the HAL pins common to all switchkins modules. It does not provide the module 'main' program, so a module can get that from wherever suits it. -A kinstype is supplied by calling switchkinsRegister(), once per -kinstype: +A kinstype is supplied by calling switchkinsRegisterOps(), once per +kinstype, with the maths of that type written as functions of the +parameter block (see the Kinematics Conventions chapter): + +---- +int switchkinsRegisterOps(int ktype, const kins_ops *ops); +---- + +The geometry of the whole module is one table, named in the kparms +fields 'params' and 'nparams'; every kinstype reads it from the block. +The older form, switchkinsRegister() with a setup, forward and inverse +routine per kinstype that read pins of their own, is still accepted: ---- int switchkinsRegister(int ktype, KS kset, KF kfwd, KI kinv); ---- 'ktype' runs from 0 to SWITCHKINS_MAX_TYPES-1 (defined in -kinematics.h). A kinstype has to come from one route or the -other, so registering one that switchkinsSetup() has already -filled in is an error, and so is leaving a gap below the highest +kinematics.h as KINS_MAX_TYPES). Registering a kinstype twice, by +either route, is an error, and so is leaving a gap below the highest kinstype provided. Either mistake fails the module load and says which kinstype is at fault. @@ -581,15 +593,15 @@ When every kinstype is registered, the module calls: int switchkinsInit(const int comp_id, kparms* kp, const char* coordinates); ---- -which checks the supplied parameters, creates the HAL pins, selects -kinstype 0, and then invokes the setup routine registered for each -kinstype. The caller owns the HAL component: it does hal_init() -before switchkinsInit() and hal_ready() after it. +which checks the supplied parameters, creates the HAL pins, the +table's among them, selects kinstype 0, and then invokes the setup +routine of each kinstype registered the older way. The caller owns +the HAL component: it does hal_init() before switchkinsInit() and +hal_ready() after it. -Each kinstype setup routine can (optionally) create HAL -pins and set them to default values. A setup routine is called -once per kinstype it is registered for, so a routine used for two -kinstypes must not create the same pin twice. +A module built this way also exports kinsDescribe(), through which a +copy of it loaded outside realtime learns its table and the maths of +each kinstype; the non-realtime loader and kinslimits use it. === Module main program @@ -606,31 +618,59 @@ int switchkinsSetup(kparms* kp, KI* kinv0, KI* kinv1, KI* kinv2); ---- -which identifies the setup, forward and inverse routines for -kinstypes 0,1,2 and sets a number of configuration settings. Those -three are registered for the module, so it can supply further -kinstypes by calling switchkinsRegister() itself, and registering -one that switchkinsSetup() has already filled in is the same error -as any other duplicate. +which sets the configuration settings, names the geometry table and +registers the kinstypes with switchkinsRegisterOps(). The pointer +arguments are the older route for kinstypes 0,1,2; a module using +them leaves the rest alone. switchkinsRunSetup() in +switchkins_setup.c is what runs switchkinsSetup() and registers what +it returned, for the 'main' program and for kinsDescribe() alike. A module written as a halcompile component gets rtapi_app_main() -from halcompile instead. It registers its kinstypes and calls -switchkinsInit() from its EXTRA_SETUP() routine, which halcompile -runs after hal_init() and before hal_ready(). The component names -the objects it needs in hal/components/Submakefile: +from halcompile instead. It supplies the same switchkinsSetup(), and +from its EXTRA_SETUP() routine, which halcompile runs after +hal_init() and before hal_ready(), calls switchkinsRunSetup() and +then switchkinsInit(). The component names the objects it needs in +hal/components/Submakefile: ---- -millturn-extra-objs := emc/kinematics/switchkins.o emc/kinematics/kins_util.o +millturn-extra-objs := emc/kinematics/switchkins.o emc/kinematics/switchkins_setup.o emc/kinematics/kins_util.o ---- === Outline -The two routes in one switchkinsSetup(), with the kinematics itself -left out. Types 0 to 2 are filled in through the pointer arguments as -they always were, and a fourth is registered: +A switchkinsSetup() with the kinematics itself left out: the table, +the ops table of the machine's own kinstype, and the shared identity +and userk ops for the other two: [source,c] ---- +static const kins_param_desc my_params[] = { + { "pivot-length", KINS_PARAM_FLOAT, KINS_IN, 0, 100.0 }, + { "tool-offset", KINS_PARAM_FLOAT, KINS_IN, 1, 0.0 }, // the tool length +}; +enum { P_PIVOT_LENGTH, P_TOOL_OFFSET }; + +static int my_forward(const kins_params *p, kins_scratch *s, + const double *joint, EmcPose *pos, + const KINEMATICS_FORWARD_FLAGS *fflags, + KINEMATICS_INVERSE_FLAGS *iflags) +{ + double pivot = p->geometry[P_PIVOT_LENGTH]; // where a pin was read + double tool = p->tool.tran.z; // the tool, from wherever the caller has it + // ... +} + +static int my_inverse(const kins_params *p, kins_scratch *s, + const EmcPose *pos, double *joint, + const KINEMATICS_INVERSE_FLAGS *iflags, + KINEMATICS_FORWARD_FLAGS *fflags); + +static const kins_ops my_ops = { + .forward = my_forward, + .inverse = my_inverse, + // .work, .tool, .native and .jacobian are optional, see kinematics.h +}; + int switchkinsSetup(kparms* kp, KS* kset0, KS* kset1, KS* kset2, KF* kfwd0, KF* kfwd1, KF* kfwd2, @@ -641,37 +681,24 @@ int switchkinsSetup(kparms* kp, kp->halprefix = "mykins"; // hal pin names kp->required_coordinates = "xyzab"; kp->max_joints = strlen(kp->required_coordinates); + kp->params = my_params; + kp->nparams = sizeof(my_params)/sizeof(my_params[0]); // remaining kparms fields - *kset0 = identityKinematicsSetup; // kinstype 0 is the startup default - *kfwd0 = identityKinematicsForward; - *kinv0 = identityKinematicsInverse; - - *kset1 = myKinematicsSetup; - *kfwd1 = myKinematicsForward; - *kinv1 = myKinematicsInverse; - - *kset2 = userkKinematicsSetup; - *kfwd2 = userkKinematicsForward; - *kinv2 = userkKinematicsInverse; - - // any further kinstype comes from switchkinsRegister(), and the - // numbering carries on from the three above with no gaps - if (switchkinsRegister(3, myOtherKinematicsSetup, - myOtherKinematicsForward, - myOtherKinematicsInverse)) { return -1; } + switchkinsRegisterOps(0, &my_ops); // kinstype 0 is the startup default + switchkinsRegisterOps(1, &KINS_IDENTITY_OPS); + switchkinsRegisterOps(2, &USERK_OPS); + // any further kinstype is registered the same way, and the + // numbering carries on with no gaps return 0; } // switchkinsSetup() ---- -A module wanting fewer than three kinstypes leaves the unused pointer -arguments alone and starts registering at the first free number. - For the surrounding shape, the in-tree switchkinsSetup() routines are in src/emc/kinematics: 5axiskins.c, xyzac-trt-kins.c, genserkins.c, scarakins.c and the others listed at the top of this document. None of -them registers a fourth kinstype yet, so the call above has no in-tree +them registers a fourth kinstype yet, so a call for one has no in-tree example to copy. // vim: set syntax=asciidoc: From 9e4fb5851054c61a75b92e0663e34fea464b5108 Mon Sep 17 00:00:00 2001 From: Luca Toniolo <10792599+grandixximo@users.noreply.github.com> Date: Fri, 4 Sep 2026 13:24:57 +1000 Subject: [PATCH 37/60] kinematics_user: take the joints in as well as out, and bind modules lazily The loader zeroed the joints before 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 its rotary angles, the hexapod its starting pose, so zeros put the loader on a different branch from realtime for xyzacb_trsrn. The caller's joints are the seed now, an iterating forward keeps the caller's pose, 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, so bind lazily. The module is looked up in the directory it is installed in, with the HAL library within reach. --- .../kinematics_userspace/kinematics_user.c | 44 +++++++++++++++---- .../kinematics_userspace/kinematics_user.h | 17 +++++-- 2 files changed, 49 insertions(+), 12 deletions(-) diff --git a/src/emc/kinematics_userspace/kinematics_user.c b/src/emc/kinematics_userspace/kinematics_user.c index b1ccec60eca..c7fe4f25bf5 100644 --- a/src/emc/kinematics_userspace/kinematics_user.c +++ b/src/emc/kinematics_userspace/kinematics_user.c @@ -28,7 +28,7 @@ #include #include -#include "config.h" /* EMC2_HOME */ +#include "config.h" /* EMC2_RTLIB_DIR, MODULE_EXT */ typedef int (*kins_describe_fn)(const char *coordinates, const char *sparm, kins_module_info *info); @@ -57,6 +57,7 @@ struct KinematicsUserContext { int cell_of_tool[AXIS_COUNT]; /* motion.tooloffset.*, -1 if absent */ int tool_param; /* the table's tool entry, -1 if none */ int warned_tool; + double last_joints[EMCMOT_MAX_JOINTS]; /* what the last inverse found */ }; /* ======================================================================== @@ -284,13 +285,29 @@ static int load_module(KinematicsUserContext *ctx, const char *sparm) { char module_path[512]; - void *handle; + void *handle, *hal_lib; kins_describe_fn describe; snprintf(module_path, sizeof(module_path), - "%s/rtlib/%s.so", EMC2_HOME, module_name); - - handle = dlopen(module_path, RTLD_NOW | RTLD_LOCAL); + "%s/%s%s", EMC2_RTLIB_DIR, module_name, MODULE_EXT); + + /* A module calls rtapi_print() and the rest of the HAL library, and a + program holding that library under a shared object of its own (a GUI + holds it under the interpreter it loaded) keeps those symbols out of + the scope a module resolves against: the module loads and then dies + at the first call it cannot bind. Failing here is not itself an + error, since a program that links the library has them in reach. */ + hal_lib = dlopen("liblinuxcnchal.so.0", RTLD_LAZY | RTLD_GLOBAL); + if (!hal_lib) { + fprintf(stderr, "kinematicsUserInit: dlopen 'liblinuxcnchal.so.0':" + " %s\n", dlerror()); + } + + /* lazily: a halcompile component references hal_export_funct() and + the rest of what its rtapi_app_main() needs, which only the realtime + HAL library provides, and nothing here calls that main. What is + called, kinsDescribe() and the ops, resolves when it is called. */ + handle = dlopen(module_path, RTLD_LAZY | RTLD_LOCAL); if (!handle) { fprintf(stderr, "kinematicsUserInit: dlopen '%s': %s\n", module_path, dlerror()); @@ -431,12 +448,18 @@ int kinematicsUserInverse(KinematicsUserContext* ctx, if (ctx->rt_only) return -1; refresh(ctx); - for (i = 0; i < EMCMOT_MAX_JOINTS; i++) j[i] = 0.0; + /* the joints go in as well as out: motion hands a module where the + machine is, and some read that (a nutating head takes its rotary + angles from it), so the caller's array is the seed */ + for (i = 0; i < EMCMOT_MAX_JOINTS; i++) { + j[i] = (i < ctx->num_joints) ? joints[i] : 0.0; + } if (kinsOpsInverse(ctx->info.ops[ctx->ktype], &ctx->params, &ctx->scratch, world, j, &iflags, &fflags) != 0) { return -1; } for (i = 0; i < ctx->num_joints; i++) joints[i] = j[i]; + memcpy(ctx->last_joints, j, sizeof(ctx->last_joints)); return 0; } @@ -456,7 +479,11 @@ int kinematicsUserForward(KinematicsUserContext* ctx, for (i = 0; i < EMCMOT_MAX_JOINTS; i++) { j[i] = (i < ctx->num_joints) ? joints[i] : 0.0; } - memset(world, 0, sizeof(*world)); + /* a forward that iterates starts from the pose it is handed, so the + caller's world is the seed; any other gets a clean one */ + if (!ctx->info.ops[ctx->ktype]->fwd_iterates) { + memset(world, 0, sizeof(*world)); + } return kinsOpsForward(ctx->info.ops[ctx->ktype], &ctx->params, &ctx->scratch, j, world, &fflags, &iflags); } @@ -475,7 +502,8 @@ int kinematicsUserJacobian(KinematicsUserContext* ctx, if (ctx->rt_only) return -1; refresh(ctx); - for (r = 0; r < EMCMOT_MAX_JOINTS; r++) j[r] = 0.0; + /* the joints at this pose, on the branch the last inverse was on */ + memcpy(j, ctx->last_joints, sizeof(j)); if (kinsOpsInverse(ctx->info.ops[ctx->ktype], &ctx->params, &ctx->scratch, world, j, &iflags, &fflags) != 0) { return -1; diff --git a/src/emc/kinematics_userspace/kinematics_user.h b/src/emc/kinematics_userspace/kinematics_user.h index 0a7187537f9..3d1e8c2bf8f 100644 --- a/src/emc/kinematics_userspace/kinematics_user.h +++ b/src/emc/kinematics_userspace/kinematics_user.h @@ -90,9 +90,14 @@ int kinematicsUserGetNumTypes(KinematicsUserContext* ctx); /** * Perform inverse kinematics (world coords -> joint positions) * + * The joint array goes in as well as out: motion hands a module the + * joints the machine is at, and a module may read them (a nutating + * head takes its rotary angles from there, an iterating inverse starts + * there), so pass the current joints, not zeros. + * * @param ctx Kinematics context from kinematicsUserInit * @param world World coordinates (X, Y, Z, A, B, C, U, V, W) - * @param joints Output array of joint positions [KINEMATICS_USER_MAX_JOINTS] + * @param joints Joint positions in and out [KINEMATICS_USER_MAX_JOINTS] * @return 0 on success, -1 on failure */ int kinematicsUserInverse(KinematicsUserContext* ctx, @@ -102,9 +107,12 @@ int kinematicsUserInverse(KinematicsUserContext* ctx, /** * Perform forward kinematics (joint positions -> world coords) * + * A module whose forward iterates (the hexapod, the pentapod) starts + * from the pose in *world, so hand it one near the answer. + * * @param ctx Kinematics context from kinematicsUserInit * @param joints Array of joint positions [KINEMATICS_USER_MAX_JOINTS] - * @param world Output world coordinates + * @param world Output world coordinates, and the seed on input * @return 0 on success, -1 on failure */ int kinematicsUserForward(KinematicsUserContext* ctx, @@ -114,8 +122,9 @@ int kinematicsUserForward(KinematicsUserContext* ctx, /** * The Jacobian at a pose, J[joint][axis] = d joint / d axis, from the * module's closed form where it has one and by differencing its inverse - * where it does not. The inverse is run at the pose first, so the - * derivative is taken on the solution branch the module picks there. + * where it does not. The inverse is run at the pose first, seeded with + * what the last kinematicsUserInverse() found, so the derivative is + * taken on the solution branch the caller is on. * * @return 0 on success, -1 on failure */ From de01dde86866031d8ebe1158682e4391c86fe8bb Mon Sep 17 00:00:00 2001 From: Luca Toniolo <10792599+grandixximo@users.noreply.github.com> Date: Fri, 4 Sep 2026 13:24:58 +1000 Subject: [PATCH 38/60] tests: check that a module answers the same outside realtime 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. --- tests/kins-params/check.py | 136 +++++++++++++++++++++++++++ tests/kins-params/checkresult | 4 + tests/kins-params/paritycheck.c | 148 +++++++++++++++++++++++++++++ tests/kins-params/skip | 4 + tests/kins-params/test.sh | 161 ++++++++++++++++++++++++++++++++ 5 files changed, 453 insertions(+) create mode 100755 tests/kins-params/check.py create mode 100755 tests/kins-params/checkresult create mode 100644 tests/kins-params/paritycheck.c create mode 100755 tests/kins-params/skip create mode 100755 tests/kins-params/test.sh diff --git a/tests/kins-params/check.py b/tests/kins-params/check.py new file mode 100755 index 00000000000..b6f7758f892 --- /dev/null +++ b/tests/kins-params/check.py @@ -0,0 +1,136 @@ +#!/usr/bin/env python3 +# The non-realtime half of the parameter block parity test. +# +# Evaluates the module paritycheck was loaded after through the +# non-realtime loader (libkinslimits, kinematicsUserInit and friends), +# which dlopens the module, asks it to describe itself through +# kinsDescribe(), fills a parameter block from the module's own pins and +# calls the same ops the realtime wrapper calls. The answers have to +# match what paritycheck published, to rounding, or the module is not +# the pure function of its parameters it claims to be. +# +# Usage: check.py MODULE JOINTS COORDS KTYPE FROMPOSE POSE JNT SPARM +# POSE and JNT are comma separated numbers, as given to paritycheck; +# COORDS and SPARM are a dash when the module was loaded without them. + +import ctypes +import os +import sys + +import hal + +EMC2_HOME = os.environ.get("EMC2_HOME") +# global, so the module the loader dlopens resolves its HAL and RTAPI +# symbols against the same library +def lib(name): + if EMC2_HOME: + return ctypes.CDLL(os.path.join(EMC2_HOME, "lib", name), mode=ctypes.RTLD_GLOBAL) + return ctypes.CDLL(name, mode=ctypes.RTLD_GLOBAL) + +class EmcPose(ctypes.Structure): + _fields_ = [(n, ctypes.c_double) for n in "xyzabcuvw"] + +MAX_JOINTS = 9 +AXES = 9 +Joints = ctypes.c_double * MAX_JOINTS +Jac = (ctypes.c_double * AXES) * MAX_JOINTS + +module, joints, coords, ktype, frompose = sys.argv[1], int(sys.argv[2]), sys.argv[3], int(sys.argv[4]), int(sys.argv[5]) +pose_in = [float(v) for v in sys.argv[6].split(",")] +jnt_in = [float(v) for v in sys.argv[7].split(",")] +# a dash stands for an absent value, since halcmd hands quotes through +if coords == "-": + coords = "" +sparm = sys.argv[8].encode() if len(sys.argv) > 8 and sys.argv[8] not in ("", "-") else None +pose_in += [0.0] * (AXES - len(pose_in)) +jnt_in += [0.0] * (MAX_JOINTS - len(jnt_in)) + +halc = lib("liblinuxcnchal.so.0") +kins = lib("libkinslimits.so.0") + +kins.kinematicsUserInitSparm.restype = ctypes.c_void_p +kins.kinematicsUserInitSparm.argtypes = [ctypes.c_char_p, ctypes.c_int, ctypes.c_char_p, + ctypes.c_char_p, ctypes.c_int, ctypes.c_char_p] +for fn in ("kinematicsUserIsRtOnly", "kinematicsUserGetNumTypes"): + getattr(kins, fn).argtypes = [ctypes.c_void_p] +kins.kinematicsUserSetType.argtypes = [ctypes.c_void_p, ctypes.c_int] +kins.kinematicsUserInverse.argtypes = [ctypes.c_void_p, ctypes.POINTER(EmcPose), Joints] +kins.kinematicsUserForward.argtypes = [ctypes.c_void_p, Joints, ctypes.POINTER(EmcPose)] +kins.kinematicsUserJacobian.argtypes = [ctypes.c_void_p, ctypes.POINTER(EmcPose), Jac] +kins.kinematicsUserFree.argtypes = [ctypes.c_void_p] + +comp_id = halc.hal_init(b"kpcheck") +if comp_id < 0: + print("kins-params: FAIL hal_init") + sys.exit(1) +ctx = kins.kinematicsUserInitSparm(module.encode(), joints, coords.encode() if coords else None, sparm, + comp_id, b"kpcheck") +halc.hal_ready(comp_id) +failures = 0 + +def fail(what): + global failures + failures += 1 + print("kins-params: FAIL %s" % what) + +if not ctx or kins.kinematicsUserIsRtOnly(ctx): + fail("%s cannot be evaluated outside realtime" % module) + sys.exit(1) +if ktype and kins.kinematicsUserSetType(ctx, ktype): + fail("%s has no type %d in the block form" % (module, ktype)) + sys.exit(1) + +def pose_of(values): + p = EmcPose() + for n, v in zip("xyzabcuvw", values): + setattr(p, n, v) + return p + +def close(a, b): + return abs(a - b) <= 1e-9 * max(1.0, abs(a), abs(b)) + +def compare(what, ours, theirs): + if not close(ours, theirs): + fail("%s: loader %.12g, realtime %.12g" % (what, ours, theirs)) + +rc_fwd = hal.get_value("paritycheck.rc-fwd") +rc_inv = hal.get_value("paritycheck.rc-inv") +rc_jac = hal.get_value("paritycheck.rc-jac") + +q = Joints(*jnt_in) +qi = Joints(*jnt_in) +J = Jac() +if frompose: + P = pose_of(pose_in) + r_inv = kins.kinematicsUserInverse(ctx, ctypes.byref(P), qi) + F = pose_of(pose_in) + r_fwd = kins.kinematicsUserForward(ctx, qi, ctypes.byref(F)) + r_jac = kins.kinematicsUserJacobian(ctx, ctypes.byref(P), J) +else: + F = pose_of(pose_in) + r_fwd = kins.kinematicsUserForward(ctx, q, ctypes.byref(F)) + r_inv = kins.kinematicsUserInverse(ctx, ctypes.byref(F), qi) + r_jac = kins.kinematicsUserJacobian(ctx, ctypes.byref(F), J) + +# the same success or failure on both sides, then the same numbers +for what, ours, theirs in (("forward", r_fwd, rc_fwd), ("inverse", r_inv, rc_inv), ("jacobian", r_jac, rc_jac)): + if (ours != 0) != (theirs != 0): + fail("%s returned %d in the loader and %d in realtime" % (what, ours, theirs)) + +if r_fwd == 0 and rc_fwd == 0: + for n in "xyzabcuvw": + compare("forward %s" % n, getattr(F, n), hal.get_value("paritycheck.fwd-%s" % n)) +if r_inv == 0 and rc_inv == 0: + for j in range(joints): + compare("inverse joint %d" % j, qi[j], hal.get_value("paritycheck.inv-%d" % j)) +if r_jac == 0 and rc_jac == 0: + for j in range(joints): + for a, n in enumerate("xyzabcuvw"): + compare("jacobian [%d][%s]" % (j, n), J[j][a], hal.get_value("paritycheck.jac-%d-%s" % (j, n))) + +kins.kinematicsUserFree(ctx) +halc.hal_exit(comp_id) + +if failures: + sys.exit(1) +print("kins-params: %s type %d agrees" % (module, ktype)) diff --git a/tests/kins-params/checkresult b/tests/kins-params/checkresult new file mode 100755 index 00000000000..d3eba1a4da0 --- /dev/null +++ b/tests/kins-params/checkresult @@ -0,0 +1,4 @@ +#!/bin/sh +[ "$(grep -c 'agrees' "$1")" = "$(grep -c '^=== ' "$1")" ] \ + && [ "$(grep -c '^=== ' "$1")" -ge 20 ] \ + && ! grep -q "FAIL" "$1" diff --git a/tests/kins-params/paritycheck.c b/tests/kins-params/paritycheck.c new file mode 100644 index 00000000000..9043df69dac --- /dev/null +++ b/tests/kins-params/paritycheck.c @@ -0,0 +1,148 @@ +/* + * paritycheck: the realtime half of the parameter block parity test. + * + * Loaded after a kinematics module, it evaluates the module through the + * classic entry points once, at load, and publishes the answers on HAL + * pins: the forward pose, the inverse joints and the Jacobian. check.py + * then evaluates the same module through the non-realtime loader, which + * goes through kinsDescribe() and the parameter block, and compares. + * + * Two flows. With frompose=0 the input is a joint set: the pose is the + * forward of it, the joints published are the inverse of that pose, and + * the Jacobian is taken there. With frompose=1 the input is a pose, for + * the parallel machines whose forward wants a seed: the joints are its + * inverse, the forward is run from the pose as seed, and the Jacobian is + * taken there. + * + * Module parameters + * joints joint count the module was loaded for + * ktype switchkins type to select first, 0 for none + * frompose 0 or 1, as above + * pose up to nine integers, the pose (frompose=1) or the forward + * seed (frompose=0) + * jnt up to sixteen integers, the joint set (frompose=0) or the + * inverse seed (frompose=1) + */ +#include +#include +#include +#include +#include + +MODULE_LICENSE("GPL"); + +static int joints = 3; +RTAPI_MP_INT(joints, "joint count the module under test was loaded for"); +static int ktype = 0; +RTAPI_MP_INT(ktype, "switchkins type to select first"); +static int frompose = 0; +RTAPI_MP_INT(frompose, "1 to take the pose as the input"); +static int pose[EMCMOT_MAX_AXIS] = { 0 }; +RTAPI_MP_ARRAY_INT(pose, EMCMOT_MAX_AXIS, "pose, x y z a b c u v w"); +static int jnt[EMCMOT_MAX_JOINTS] = { 10, 20, 30, 40, 50, 60, 70, 80, 90 }; +RTAPI_MP_ARRAY_INT(jnt, EMCMOT_MAX_JOINTS, "joint values, from joint 0"); + +static int comp_id = -1; + +static struct { + hal_real_t fwd[EMCMOT_MAX_AXIS]; + hal_real_t inv[EMCMOT_MAX_JOINTS]; + hal_real_t jac[EMCMOT_MAX_JOINTS][EMCMOT_MAX_AXIS]; + hal_sint_t rc_fwd; + hal_sint_t rc_inv; + hal_sint_t rc_jac; +} *pins; + +static const char letter[EMCMOT_MAX_AXIS] = { 'x','y','z','a','b','c','u','v','w' }; + +static double *coord(EmcPose *p, int a) +{ + switch (a) { + case 0: return &p->tran.x; + case 1: return &p->tran.y; + case 2: return &p->tran.z; + case 3: return &p->a; + case 4: return &p->b; + case 5: return &p->c; + case 6: return &p->u; + case 7: return &p->v; + default: return &p->w; + } +} + +int rtapi_app_main(void) +{ + KINEMATICS_FORWARD_FLAGS fflags = 0; + KINEMATICS_INVERSE_FLAGS iflags = 0; + double q[EMCMOT_MAX_JOINTS], qi[EMCMOT_MAX_JOINTS]; + double jac[EMCMOT_MAX_JOINTS][EMCMOT_MAX_AXIS]; + EmcPose P, F, seed; + int a, j, res = 0; + + if (joints < 1 || joints > EMCMOT_MAX_JOINTS) { return -1; } + + comp_id = hal_init("paritycheck"); + if (comp_id < 0) { return comp_id; } + + pins = hal_malloc(sizeof(*pins)); + if (!pins) { hal_exit(comp_id); return -1; } + + for (a = 0; a < EMCMOT_MAX_AXIS; a++) { + res += hal_pin_new_real(comp_id, HAL_OUT, &pins->fwd[a], 0.0, + "paritycheck.fwd-%c", letter[a]); + } + for (j = 0; j < joints; j++) { + res += hal_pin_new_real(comp_id, HAL_OUT, &pins->inv[j], 0.0, + "paritycheck.inv-%d", j); + for (a = 0; a < EMCMOT_MAX_AXIS; a++) { + res += hal_pin_new_real(comp_id, HAL_OUT, &pins->jac[j][a], 0.0, + "paritycheck.jac-%d-%c", j, letter[a]); + } + } + res += hal_pin_new_si32(comp_id, HAL_OUT, &pins->rc_fwd, 0, "paritycheck.rc-fwd"); + res += hal_pin_new_si32(comp_id, HAL_OUT, &pins->rc_inv, 0, "paritycheck.rc-inv"); + res += hal_pin_new_si32(comp_id, HAL_OUT, &pins->rc_jac, 0, "paritycheck.rc-jac"); + if (res) { hal_exit(comp_id); return -1; } + + if (ktype > 0 && kinematicsSwitchable()) { + if (kinematicsSwitch(ktype)) { hal_exit(comp_id); return -1; } + } + + for (a = 0; a < EMCMOT_MAX_AXIS; a++) { *coord(&seed, a) = pose[a]; } + for (j = 0; j < EMCMOT_MAX_JOINTS; j++) { q[j] = jnt[j]; qi[j] = jnt[j]; } + + // a switchable module's first forward after load restarts from the + // pose it saved, which is nothing yet; take that call here so the one + // measured starts from the seed like the loader's does + F = seed; + kinematicsForward(q, &F, &fflags, &iflags); + fflags = 0; iflags = 0; + + if (frompose) { + P = seed; + hal_set_si32(pins->rc_inv, kinematicsInverse(&P, qi, &iflags, &fflags)); + F = seed; + fflags = 0; iflags = 0; + hal_set_si32(pins->rc_fwd, kinematicsForward(qi, &F, &fflags, &iflags)); + iflags = 0; + hal_set_si32(pins->rc_jac, kinematicsJacobian(qi, &P, jac, &iflags)); + } else { + F = seed; + hal_set_si32(pins->rc_fwd, kinematicsForward(q, &F, &fflags, &iflags)); + iflags = 0; fflags = 0; + hal_set_si32(pins->rc_inv, kinematicsInverse(&F, qi, &iflags, &fflags)); + iflags = 0; + hal_set_si32(pins->rc_jac, kinematicsJacobian(qi, &F, jac, &iflags)); + } + + for (a = 0; a < EMCMOT_MAX_AXIS; a++) { hal_set_real(pins->fwd[a], *coord(&F, a)); } + for (j = 0; j < joints; j++) { + hal_set_real(pins->inv[j], qi[j]); + for (a = 0; a < EMCMOT_MAX_AXIS; a++) { hal_set_real(pins->jac[j][a], jac[j][a]); } + } + + hal_ready(comp_id); + return 0; +} + +void rtapi_app_exit(void) { hal_exit(comp_id); } diff --git a/tests/kins-params/skip b/tests/kins-params/skip new file mode 100755 index 00000000000..a12f31a77c2 --- /dev/null +++ b/tests/kins-params/skip @@ -0,0 +1,4 @@ +#!/bin/sh +# Builds a realtime component with halcompile, which needs the build +# tools present. Skip when testing installed packages. +[ -z "$SYSTEM_BUILD" ] diff --git a/tests/kins-params/test.sh b/tests/kins-params/test.sh new file mode 100755 index 00000000000..82f5d294e23 --- /dev/null +++ b/tests/kins-params/test.sh @@ -0,0 +1,161 @@ +#!/bin/bash +set -e + +${SUDO} halcompile --install paritycheck.c >/dev/null + +# One hal file per module. paritycheck evaluates the module in realtime +# through the classic entry points and publishes the answers; check.py +# evaluates it through the non-realtime loader, kinsDescribe() and the +# parameter block, and compares. Where they disagree the module keeps +# state its table does not declare. +# ONLY= in the environment runs the entries for that module alone +run() { + local loadrt="$1" setp="$2" parms="$3" ktype="$4" + local module coords sparm joints frompose pose jnt hal tok + case "$loadrt" in "${ONLY:-}"*) ;; *) return 0 ;; esac + module=${loadrt%% *} + coords=""; sparm="" + for tok in $loadrt; do + case "$tok" in + coordinates=*) coords=${tok#coordinates=} ;; + sparm=*) sparm=${tok#sparm=} ;; + esac + done + joints=3; frompose=0; pose="0,0,0,0,0,0,0,0,0"; jnt="10,20,30,40,50,60,70,80,90" + for tok in $parms; do + case "$tok" in + joints=*) joints=${tok#joints=} ;; + frompose=*) frompose=${tok#frompose=} ;; + pose=*) pose=${tok#pose=} ;; + jnt=*) jnt=${tok#jnt=} ;; + esac + done + hal=$(mktemp --suffix=.hal) + { printf 'loadrt %s\n' "$loadrt" + printf '%s\n' "$setp" + printf 'loadrt paritycheck %s ktype=%s\n' "$parms" "${ktype:-0}" + # halcmd keeps quotes, so an absent value travels as a dash + printf 'loadusr -w python3 check.py %s %s %s %s %s %s %s %s\n' \ + "$module" "$joints" "${coords:--}" "${ktype:-0}" "$frompose" "$pose" "$jnt" "${sparm:--}" + } > "$hal" + echo "=== $loadrt type ${ktype:-0}" + halrun -f "$hal" + rm -f "$hal" +} + +# identity, a gantry included +run "trivkins coordinates=XYZ" "" "joints=3 jnt=10,20,30" +run "trivkins coordinates=XYZY kinstype=BOTH" "" "joints=4 jnt=10,20,30,20" +run "trivkins coordinates=XYZABCUVW" "" "joints=9" +run "userkins" "" "joints=3 jnt=10,20,30" +run "millturn" "" "joints=4 jnt=10,20,30,40" +run "millturn" "" "joints=4 jnt=10,20,30,40" 1 + +# linear maps and one rotation +run "corexykins" "" "joints=9" +run "rotatekins" "" "joints=9" +run "matrixkins" \ + "setp matrixkins.C_xy 0.02 +setp matrixkins.C_xz -0.01 +setp matrixkins.C_yx 0.03 +setp matrixkins.C_yz 0.015 +setp matrixkins.C_zx -0.02 +setp matrixkins.C_zy 0.01 +setp matrixkins.C_zz 1.001" \ + "joints=9" + +# tables and heads, offsets set so no term drops out +run "maxkins" \ + "setp maxkins.pivot-length 100" \ + "joints=9 jnt=10,20,30,0,15,25,7,0,3" + +run "5axiskins coordinates=XYZBCW" "" "joints=6 jnt=10,20,30,15,25,5" +run "5axiskins coordinates=XYZBCW sparm=identityfirst" "" "joints=6 jnt=10,20,30,15,25,5" 1 + +run "xyzac-trt-kins coordinates=XYZAC" \ + "setp xyzac-trt-kins.y-offset 3 +setp xyzac-trt-kins.z-offset 11 +setp xyzac-trt-kins.tool-offset 7 +setp xyzac-trt-kins.x-rot-point 1 +setp xyzac-trt-kins.y-rot-point 2 +setp xyzac-trt-kins.z-rot-point 5" \ + "joints=5 jnt=10,20,30,15,25" + +run "xyzbc-trt-kins coordinates=XYZBC" \ + "setp xyzbc-trt-kins.conventional-directions 1 +setp xyzbc-trt-kins.x-offset 3 +setp xyzbc-trt-kins.z-offset 11 +setp xyzbc-trt-kins.tool-offset 7 +setp xyzbc-trt-kins.x-rot-point 1 +setp xyzbc-trt-kins.y-rot-point 2 +setp xyzbc-trt-kins.z-rot-point 5" \ + "joints=5 jnt=10,20,30,15,25" + +run "xyzab_tdr_kins" \ + "setp xyzab_tdr_kins.x-offset 3 +setp xyzab_tdr_kins.z-offset 11 +setp xyzab_tdr_kins.tool-offset-z 7 +setp xyzab_tdr_kins.x-rot-point 1 +setp xyzab_tdr_kins.y-rot-point 2 +setp xyzab_tdr_kins.z-rot-point 5" \ + "joints=5 jnt=10,20,30,15,25" 1 + +run "xyzacb_trsrn" \ + "setp xyzacb_trsrn_kins.nut-angle 45 +setp xyzacb_trsrn_kins.y-pivot 100 +setp xyzacb_trsrn_kins.z-pivot 200 +setp xyzacb_trsrn_kins.x-offset 5 +setp xyzacb_trsrn_kins.y-offset 7 +setp xyzacb_trsrn_kins.y-rot-axis 300 +setp xyzacb_trsrn_kins.z-rot-axis 400 +setp xyzacb_trsrn_kins.tool-offset-z 50 +setp xyzacb_trsrn_kins.pre-rot 0.3 +setp xyzacb_trsrn_kins.primary-angle 20 +setp xyzacb_trsrn_kins.secondary-angle 35" \ + "joints=6 jnt=10,20,30,15,25,35" 1 + +run "xyzacb_trsrn" \ + "setp xyzacb_trsrn_kins.nut-angle 45 +setp xyzacb_trsrn_kins.y-pivot 100 +setp xyzacb_trsrn_kins.z-pivot 200 +setp xyzacb_trsrn_kins.pre-rot 0.3 +setp xyzacb_trsrn_kins.primary-angle 20 +setp xyzacb_trsrn_kins.secondary-angle 35" \ + "joints=6 jnt=10,20,30,15,25,35" 2 + +run "xyzbca_trsrn" \ + "setp xyzbca_trsrn_kins.nut-angle 45 +setp xyzbca_trsrn_kins.x-pivot 100 +setp xyzbca_trsrn_kins.z-pivot 200 +setp xyzbca_trsrn_kins.x-offset 5 +setp xyzbca_trsrn_kins.y-offset 7 +setp xyzbca_trsrn_kins.x-rot-axis 300 +setp xyzbca_trsrn_kins.z-rot-axis 400 +setp xyzbca_trsrn_kins.tool-offset-z 50 +setp xyzbca_trsrn_kins.pre-rot 0.3 +setp xyzbca_trsrn_kins.primary-angle 20 +setp xyzbca_trsrn_kins.secondary-angle 35" \ + "joints=6 jnt=10,20,30,15,25,35" 1 + +# polar +run "rosekins" "" "joints=3 jnt=10,5,30" + +# arms +run "scarakins" "" "joints=6 jnt=30,40,20,10,0,0" +run "scorbot-kins" "" "joints=5 jnt=40,60,-20,0,0" +run "pumakins" "setp pumakins.D6 50" "joints=6 jnt=15,20,-35,10,70,20" +run "three21kins" "" "joints=6 jnt=15,20,-35,10,70,20" +run "genserkins" "" "joints=9 jnt=15,20,-35,10,70,20,0,0,0" +run "genserkins" "setp genserkins.unrotate-3 1" "joints=9 jnt=15,20,-35,10,70,20,0,0,0" + +# parallel machines, from a pose the forward can be seeded with +run "tripodkins" \ + "setp tripodkins.Bx 2 +setp tripodkins.Cx 1 +setp tripodkins.Cy 2" \ + "joints=3 frompose=1 pose=1,1,2" +run "lineardeltakins" "" "joints=9 frompose=1 pose=20,30,-200" +run "rotarydeltakins" "" "joints=9 frompose=1 pose=0,0,-12" +run "genhexkins" "setp genhexkins.screw-lead 0" "joints=6 frompose=1 pose=2,3,20,0,5,-7" +run "genhexkins" "setp genhexkins.screw-lead 5" "joints=6 frompose=1 pose=2,3,20,0,5,-7" +run "pentakins" "" "joints=5 frompose=1 pose=10,20,0,5,-7" From 7bf796458797acbf2a6e465ab5f71aa76b8ddc41 Mon Sep 17 00:00:00 2001 From: Luca Toniolo <10792599+grandixximo@users.noreply.github.com> Date: Fri, 4 Sep 2026 16:33:19 +1000 Subject: [PATCH 39/60] kinematics: take the tool offset from motion, not from a net A module whose maths needs the tool length read it from a pin the config had to net from motion.tooloffset.z: a copy of controller state motion already holds, one cycle late, and a missing net gave wrong joints with no error. The block carries the tool, so motion hands it over: kinematicsSetTool() is called whenever the offset changes, referenced weakly so an older module still loads. kins_single.c and switchkins.c export it for every module on the block; the pin is read only until motion has sent anything, and a pin left disagreeing is reported once, since it is a config setting a tool length where the tool table should. A pin nobody set is not a disagreement. The non-RT loader takes the tool from the caller through kinematicsUserSetTool() where one is given, a planner knowing what a segment runs under better than the machine does. tests/kins-tool-offset runs xyzac-trt-kins with nothing on its tool pin and checks that G43 reaches the joints, that netting the pin changes nothing and that G49 takes the length out; tests/kins-params checks the caller's tool on the loader. --- docs/src/code/code-notes.adoc | 3 +- docs/src/motion/5-axis-kinematics.adoc | 21 ++-- docs/src/motion/kinematics-conventions.adoc | 34 ++++-- docs/src/motion/switchkins.adoc | 8 +- src/emc/kinematics/kinematics.h | 9 ++ src/emc/kinematics/kins_rt.h | 20 ++++ src/emc/kinematics/kins_single.c | 13 +- src/emc/kinematics/kins_util.c | 37 ++++++ src/emc/kinematics/switchkins.c | 16 ++- .../kinematics_userspace/kinematics_user.c | 33 ++++- .../kinematics_userspace/kinematics_user.h | 15 ++- src/emc/motion/command.c | 8 ++ tests/kins-params/check.py | 29 +++++ tests/kins-tool-offset/README | 7 ++ tests/kins-tool-offset/checkresult | 2 + tests/kins-tool-offset/sim.hal | 20 ++++ tests/kins-tool-offset/test-ui.py | 113 ++++++++++++++++++ tests/kins-tool-offset/test.ini | 112 +++++++++++++++++ tests/kins-tool-offset/test.sh | 2 + tests/kins-tool-offset/tool.tbl | 1 + 20 files changed, 469 insertions(+), 34 deletions(-) create mode 100644 tests/kins-tool-offset/README create mode 100755 tests/kins-tool-offset/checkresult create mode 100644 tests/kins-tool-offset/sim.hal create mode 100755 tests/kins-tool-offset/test-ui.py create mode 100644 tests/kins-tool-offset/test.ini create mode 100755 tests/kins-tool-offset/test.sh create mode 100644 tests/kins-tool-offset/tool.tbl diff --git a/docs/src/code/code-notes.adoc b/docs/src/code/code-notes.adoc index 3874fd37002..064db167757 100644 --- a/docs/src/code/code-notes.adoc +++ b/docs/src/code/code-notes.adoc @@ -1312,8 +1312,7 @@ settings.tool_offset:: + * Used to compute position in various places. * Sent to Motion via the +EMCMOT_SET_OFFSET+ message. - All motion does with the offsets is export them to the HAL pins +motion.0.tooloffset.[xyzabcuvw]+. - FIXME: export these from someplace closer to the tool table (io or interp, probably) and remove the EMCMOT_SET_OFFSET message. + Motion exports the offsets to the HAL pins +motion.0.tooloffset.[xyzabcuvw]+ and hands them to the kinematics module through +kinematicsSetTool()+, for a module whose maths needs the tool length. settings.pockets_max:: Used interchangeably with +CANON_POCKETS_MAX+ (a #defined constant, set to 1000 as of April 2020). diff --git a/docs/src/motion/5-axis-kinematics.adoc b/docs/src/motion/5-axis-kinematics.adoc index 9391f8611a9..1c6e0fe406c 100644 --- a/docs/src/motion/5-axis-kinematics.adoc +++ b/docs/src/motion/5-axis-kinematics.adoc @@ -317,23 +317,17 @@ See the simulation INI files for details of the HAL connections used for the vis === Tool-Length Compensation -In order to use tools from a tool table sequentially with tool-length compensation applied automatically, a further Z-offset is required. For a tool that is longer than the "master" tool, which typically has a tool length of zero, LinuxCNC has a variable called "motion.tooloffset.z". If this variable is passed on to the kinematic component (and vismach python script), then the necessary additional Z-offset for a new tool can be accounted for by adding the component statement, for example: +In order to use tools from a tool table sequentially with tool-length compensation applied automatically, a further Z-offset is required. For a tool that is longer than the "master" tool, which typically has a tool length of zero, the kinematics accounts for the tool length in effect, for example: image::5-axis-figures/equation__38.png[align="center"] -The required HAL connection (for xyzac-trt) is: +Motion hands the tool offset in effect (G43, G49) to the kinematics module directly, so the module sees the tool from the tool table with no HAL connection. The module's tool-offset pin (xyzac-trt-kins.tool-offset) remains for a configuration that connects it, and is read only until motion has sent an offset; a value set on it that disagrees with the tool table is reported once and not used. -[source,hal] ----- -net :tool-offset motion.tooloffset.z xyzac-trt-kins.tool-offset ----- - -where: +Motion also publishes the offset on the HAL pin "motion.tooloffset.z", which is what a vismach python script reads to draw the tool: +[source,hal] ---- -:tool-offset ---------------- signal name -motion.tooloffset.z --------- output HAL pin from LinuxCNC motion module -xyzac-trt-kins.tool-offset -- input HAL pin to xyzac-trt-kins +net :tool-offset motion.tooloffset.z xyzac-trt-gui.tool-offset ---- == Custom Kinematics Components @@ -383,17 +377,18 @@ KINEMATICS = kinsname where "kinsname" is the name of your kins program. Additional HAL pins may be created by the module for variable configuration items -such as the D~x~, D~y~, D~z~, tool-offset used in the xyzac-trt kinematics module. +such as the D~x~, D~y~, D~z~ used in the xyzac-trt kinematics module. These pins can be connected to a signal for dynamic control or set once with HAL connections like: [source,hal] ---- # set offset parameters -net :tool-offset motion.tooloffset.z xyzac-trt-kins.tool-offset setp xyzac-trt-kins.y-offset 0 setp xyzac-trt-kins.z-offset 20 ---- +The tool length is not among them: motion hands it to the module from the tool table. + == Figures .Table tilting/rotating configuration diff --git a/docs/src/motion/kinematics-conventions.adoc b/docs/src/motion/kinematics-conventions.adoc index 2631163c357..f0a4846ae4c 100644 --- a/docs/src/motion/kinematics-conventions.adoc +++ b/docs/src/motion/kinematics-conventions.adoc @@ -520,15 +520,26 @@ with `switchkinsRegisterOps()`; a module with one type describes itself in a In realtime it makes one HAL pin per table entry, copies the pins into the block before every call and the outputs back after it, and supplies the classic entry points, `kinematicsForward()` and the rest, so that motion sees no -difference. Outside realtime a module exports `kinsDescribe()`, which hands a -caller its table and the ops of each type; the caller fills a block from -wherever it likes and asks the same functions through `kinsOpsForward()`, -`kinsOpsInverse()`, `kinsOpsJacobian()` and the frame calls, with the same -defaults applied, so both sides get the same answers. The non-realtime loader -in `kinematics_userspace/` binds the pins of the running module by the table's -names and takes the tool from motion's own offset pins, and says once when the -module's tool pin disagrees with them, which is a config that lost the tool on -the way. `kinslimits` is built on it. +difference. It also exports `kinematicsSetTool()`, through which motion hands +the module the tool offset in effect whenever that changes, so the tool comes +from the tool table and not from a net the config had to remember. A table +entry flagged as the tool is read from its pin only until motion has sent +anything; after that the pin is overwritten with motion's value, and the shared +code says once if the pin is left disagreeing with it, which is a config +setting a tool length where the tool table should. Motion references the call +weakly, so a module written before it still loads and keeps its pin. + +Outside realtime a module exports `kinsDescribe()`, which hands a caller its +table and the ops of each type; the caller fills a block from wherever it likes +and asks the same functions through `kinsOpsForward()`, `kinsOpsInverse()`, +`kinsOpsJacobian()` and the frame calls, with the same defaults applied, so +both sides get the same answers. The non-realtime loader in +`kinematics_userspace/` binds the pins of the running module by the table's +names. Its tool is the caller's where the caller gives one through +`kinematicsUserSetTool()`, since a planner knows what a segment runs under +better than the machine does; otherwise it is motion's, from motion's own +offset pins, and the loader says once when the module's tool pin disagrees with +them. `kinslimits` is built on it. A module that does not provide the form keeps working as it did. It just cannot be evaluated outside realtime, which the loader reports. @@ -537,8 +548,9 @@ be evaluated outside realtime, which the loader reports. The kinematics type is in the block, so a caller evaluating a program that switches type puts the type each block will run under in its own block, and -nothing is switched globally. The tool is in the block, from motion. The joint -map is in the block, from `coordinates=`. Nothing else the maths needs exists, +nothing is switched globally. The tool is in the block, from motion in realtime +and from the caller outside it. The joint map is in the block, from +`coordinates=`. Nothing else the maths needs exists, and a module that finds it needs something else has found a parameter it should declare. diff --git a/docs/src/motion/switchkins.adoc b/docs/src/motion/switchkins.adoc index de3afc3fc40..ed4d911f6f0 100644 --- a/docs/src/motion/switchkins.adoc +++ b/docs/src/motion/switchkins.adoc @@ -601,7 +601,13 @@ hal_ready() after it. A module built this way also exports kinsDescribe(), through which a copy of it loaded outside realtime learns its table and the maths of -each kinstype; the non-realtime loader and kinslimits use it. +each kinstype; the non-realtime loader and kinslimits use it. It +exports kinematicsSetTool() as well, through which motion hands it +the tool offset in effect whenever that changes. A table entry +flagged as the tool is overwritten with it, and the entry's pin only +matters until motion has sent anything, so a config need not net +motion.tooloffset.z to the module. A kinstype registered the older +way reads its own pins and is not affected. === Module main program diff --git a/src/emc/kinematics/kinematics.h b/src/emc/kinematics/kinematics.h index 3846170f204..a87266e5067 100644 --- a/src/emc/kinematics/kinematics.h +++ b/src/emc/kinematics/kinematics.h @@ -667,6 +667,15 @@ extern int kinsOpsJacobian(const kins_ops *ops, const kins_params *p, extern int kinematicsSwitchable(void); extern int kinematicsSwitch(int switchkins_type); + +/* The tool offset motion applies, handed to the module. Motion calls this + whenever the offset changes (G43, G49) and references it weakly, so a + module that does not export it still loads and keeps reading whatever + tool pin it has. kins_single.c and switchkins.c export it for every + module written on the parameter block: the tool then comes from the tool + table through motion, and the module's tool pin, where it has one, is + read only until motion has spoken. */ +extern int kinematicsSetTool(const EmcPose *tool); //NOTE: switchable kinematics may require Interp::Synch // before/after invoking kinematicsSwitch() // A convenient command to synch is: M66 E0 L0 diff --git a/src/emc/kinematics/kins_rt.h b/src/emc/kinematics/kins_rt.h index 96d7309c739..78d0a61ef7d 100644 --- a/src/emc/kinematics/kins_rt.h +++ b/src/emc/kinematics/kins_rt.h @@ -42,6 +42,26 @@ extern void kinsParamsPinsWrite(const kins_pin_ref *pins, const kins_param_desc *params, int nparams, const kins_scratch *s); +/* Where the RT block's tool comes from. kinematicsSetTool() records what + motion sends in one of these; kinsToolSourceApply() writes it into a + block after the pins have been read, over the tool entry, once motion + has sent anything. Until then the tool entry's pin is all there is, as + under halrun with the module alone. A config that still nets the tool + to the module's pin loses nothing; one that sets that pin to something + else is told, once, after the two have disagreed for a thousand calls, + since the pin lags the send by a cycle. */ +typedef struct { + EmcPose tool; + int have; /* motion has sent a tool */ + int disagreeing; /* consecutive calls with the pin elsewhere */ + int warned; +} kins_tool_source; + +extern void kinsToolSourceSet(kins_tool_source *src, const EmcPose *tool); +extern void kinsToolSourceApply(kins_tool_source *src, const char *prefix, + const kins_param_desc *params, int nparams, + kins_params *p); + /* A module with one kinematics type defines this, describing itself, and links kins_single.c, which supplies kinematicsForward() and the rest from it. ops[0] is the maths; the other entries are ignored. */ diff --git a/src/emc/kinematics/kins_single.c b/src/emc/kinematics/kins_single.c index 16461c49375..2ca2f057e2a 100644 --- a/src/emc/kinematics/kins_single.c +++ b/src/emc/kinematics/kins_single.c @@ -20,6 +20,7 @@ static kins_params rt_params; static kins_scratch rt_scratch; static kins_pin_ref *pins; +static kins_tool_source tool_source; static int inited; static KINEMATICS_TYPE reported_type = KINEMATICS_BOTH; @@ -28,11 +29,13 @@ static const kins_ops *ops(void) return inited ? kins_module.ops[0] : NULL; } -// the block sees the pins as they are now +// the block sees the pins as they are now, and the tool motion sent static void read_pins(void) { kinsParamsPinsRead(pins, kins_module.params, kins_module.nparams, &rt_params); + kinsToolSourceApply(&tool_source, kins_module.halprefix, + kins_module.params, kins_module.nparams, &rt_params); } static void write_pins(void) @@ -117,6 +120,13 @@ int kinematicsJacobian(const double *joint, return kinsOpsJacobian(ops(), &rt_params, &rt_scratch, joint, pos, jac, iflags); } +int kinematicsSetTool(const EmcPose *tool) +{ + if (!tool) { return -1; } + kinsToolSourceSet(&tool_source, tool); + return 0; +} + KINEMATICS_TYPE kinematicsType(void) { return reported_type; @@ -157,6 +167,7 @@ EXPORT_SYMBOL(kinematicsInverse); EXPORT_SYMBOL(kinematicsWorkFrame); EXPORT_SYMBOL(kinematicsToolFrame); EXPORT_SYMBOL(kinematicsJacobian); +EXPORT_SYMBOL(kinematicsSetTool); EXPORT_SYMBOL(kinematicsSwitchable); EXPORT_SYMBOL(kinematicsSwitch); EXPORT_SYMBOL(kinematicsTypeFlags); diff --git a/src/emc/kinematics/kins_util.c b/src/emc/kinematics/kins_util.c index efee26e2f48..656c6ebb6c7 100644 --- a/src/emc/kinematics/kins_util.c +++ b/src/emc/kinematics/kins_util.c @@ -1593,3 +1593,40 @@ void kinsParamsPinsWrite(const kins_pin_ref *pins, } } } // kinsParamsPinsWrite() + +void kinsToolSourceSet(kins_tool_source *src, const EmcPose *tool) +{ + if (!src || !tool) { return; } + src->tool = *tool; + src->have = 1; +} // kinsToolSourceSet() + +void kinsToolSourceApply(kins_tool_source *src, const char *prefix, + const kins_param_desc *params, int nparams, + kins_params *p) +{ + int i; + if (!src || !p || !src->have) { return; } + for (i = 0; i < nparams && i < KINS_MAX_PARAMS; i++) { + const kins_param_desc *d = ¶ms[i]; + double diff; + if (!d->tool || d->dir == KINS_OUT) { continue; } + /* a pin nobody set reads zero, which is not a disagreement */ + diff = p->geometry[i] - src->tool.tran.z; + if (p->geometry[i] != 0.0 && (diff > 1e-9 || diff < -1e-9)) { + if (src->disagreeing < 1000) { + src->disagreeing++; + } else if (!src->warned) { + rtapi_print_msg(RTAPI_MSG_ERR, + "%s.%s disagrees with the tool offset motion applies;" + " motion's is used, the pin is not needed\n", + prefix ? prefix : "kins", d->name); + src->warned = 1; + } + } else { + src->disagreeing = 0; + } + p->geometry[i] = src->tool.tran.z; + } + p->tool = src->tool; +} // kinsToolSourceApply() diff --git a/src/emc/kinematics/switchkins.c b/src/emc/kinematics/switchkins.c index a1a2c2d4066..4c32dfd2cb5 100644 --- a/src/emc/kinematics/switchkins.c +++ b/src/emc/kinematics/switchkins.c @@ -55,6 +55,7 @@ static const kins_ops *kops[SWITCHKINS_MAX_TYPES] = {NULL}; static kins_params rt_params; static kins_scratch rt_scratch[SWITCHKINS_MAX_TYPES]; static kins_pin_ref *pins; +static kins_tool_source tool_source; static int inited; // types provided, counted in rtapi_app_main() once they are all in @@ -113,13 +114,25 @@ static void get_lastpose(int ktype, EmcPose* pos) pos->w = lastpose[ktype].w; } // get_lastpose() -// the block sees the pins as they are now, and the type asked for +// the block sees the pins as they are now, the tool motion sent, and +// the type asked for static void read_block(int ktype) { rt_params.ktype = ktype; kinsParamsPinsRead(pins, kp.params, kp.nparams, &rt_params); + kinsToolSourceApply(&tool_source, kp.halprefix, kp.params, kp.nparams, + &rt_params); } +// the tool from motion, for the types written on the block; a type +// provided the older way reads its own pins and does not see it +int kinematicsSetTool(const EmcPose *tool) +{ + if (!tool) { return -1; } + kinsToolSourceSet(&tool_source, tool); + return 0; +} // kinematicsSetTool() + static void write_block(int ktype) { kinsParamsPinsWrite(pins, kp.params, kp.nparams, &rt_scratch[ktype]); @@ -549,6 +562,7 @@ EXPORT_SYMBOL(kinematicsToolFrame); EXPORT_SYMBOL(kinematicsWorkFrame); EXPORT_SYMBOL(kinematicsToolFrameInverse); EXPORT_SYMBOL(kinematicsJacobian); +EXPORT_SYMBOL(kinematicsSetTool); EXPORT_SYMBOL(switchkinsRegister); EXPORT_SYMBOL(switchkinsRegisterFrames); EXPORT_SYMBOL(switchkinsRegisterToolFrameInverse); diff --git a/src/emc/kinematics_userspace/kinematics_user.c b/src/emc/kinematics_userspace/kinematics_user.c index c7fe4f25bf5..b8b339f564e 100644 --- a/src/emc/kinematics_userspace/kinematics_user.c +++ b/src/emc/kinematics_userspace/kinematics_user.c @@ -6,7 +6,9 @@ * kinsDescribe(), and evaluates its kinematics through the parameter * block (see kinematics.h). The block is filled from HAL: one input pin * of the caller's component per table entry, connected to the signal the - * RT instance's pin reads, so the values are the live ones; and the tool + * RT instance's pin reads, so the values are the live ones. The tool is + * the caller's where it has given one, since a planner knows what a + * segment runs under better than the machine does; otherwise it comes * from motion's own tooloffset pins where motion is loaded, so that the * tool the module sees is the one motion has, whether or not the config * netted it to the module's pin. @@ -57,6 +59,8 @@ struct KinematicsUserContext { int cell_of_tool[AXIS_COUNT]; /* motion.tooloffset.*, -1 if absent */ int tool_param; /* the table's tool entry, -1 if none */ int warned_tool; + EmcPose caller_tool; /* from kinematicsUserSetTool() */ + int have_caller_tool; double last_joints[EMCMOT_MAX_JOINTS]; /* what the last inverse found */ }; @@ -232,7 +236,8 @@ static int bind_all(KinematicsUserContext *ctx) return 0; } -/* The block sees the pins as they are now. */ +/* The block sees the pins as they are now, and the tool of whoever + knows it best: the caller, then motion, then the module's own pin. */ static void refresh(KinematicsUserContext *ctx) { int i; @@ -248,6 +253,14 @@ static void refresh(KinematicsUserContext *ctx) ctx->params.tool.tran.z = ctx->params.geometry[ctx->tool_param]; } + if (ctx->have_caller_tool) { + ctx->params.tool = ctx->caller_tool; + if (ctx->tool_param >= 0) { + ctx->params.geometry[ctx->tool_param] = ctx->caller_tool.tran.z; + } + return; + } + for (i = 0; i < AXIS_COUNT; i++) { int c = ctx->cell_of_tool[i]; tool[i] = 0.0; @@ -259,8 +272,10 @@ static void refresh(KinematicsUserContext *ctx) /* the module's pin and motion disagree: the config lost the tool somewhere between them. Say so once; motion's value is the one - being cut with. */ + being cut with. A pin nobody set reads zero, which is not a + disagreement. */ if (ctx->tool_param >= 0 && !ctx->warned_tool + && ctx->params.geometry[ctx->tool_param] != 0.0 && fabs(tool[AXIS_Z] - ctx->params.geometry[ctx->tool_param]) > 1e-9) { fprintf(stderr, "kinematics_user: %s.%s is %.6g but motion.tooloffset.z is %.6g;" @@ -435,6 +450,18 @@ int kinematicsUserGetNumTypes(KinematicsUserContext* ctx) return ctx->info.ntypes; } +int kinematicsUserSetTool(KinematicsUserContext* ctx, const EmcPose* tool) +{ + if (!ctx || !ctx->initialized || ctx->rt_only) return -1; + if (tool) { + ctx->caller_tool = *tool; + ctx->have_caller_tool = 1; + } else { + ctx->have_caller_tool = 0; + } + return 0; +} + int kinematicsUserInverse(KinematicsUserContext* ctx, const EmcPose* world, double* joints) diff --git a/src/emc/kinematics_userspace/kinematics_user.h b/src/emc/kinematics_userspace/kinematics_user.h index 3d1e8c2bf8f..e3090a34bd5 100644 --- a/src/emc/kinematics_userspace/kinematics_user.h +++ b/src/emc/kinematics_userspace/kinematics_user.h @@ -9,8 +9,9 @@ * The kinematics module is loaded into this process and evaluated through * its parameter block form (see kinematics.h). The block is filled from * input pins belonging to the caller's HAL component, connected to the - * same signals the running RT instance reads, and from motion's tool - * offset pins where motion is loaded, so the maths runs on live values. + * same signals the running RT instance reads, so the maths runs on live + * values; the tool is the caller's where it gives one, and motion's + * otherwise, from motion's tool offset pins where motion is loaded. * * Author: LinuxCNC * License: GPL Version 2 @@ -87,6 +88,16 @@ int kinematicsUserSetType(KinematicsUserContext* ctx, int ktype); */ int kinematicsUserGetNumTypes(KinematicsUserContext* ctx); +/** + * The tool offset to evaluate with: what the caller knows the segment + * runs under, from canon or the tool table, rather than the offset the + * machine happens to have now. It stands until replaced, or until NULL + * puts the context back to taking the tool from motion. + * + * @return 0, or -1 for an RT-only context + */ +int kinematicsUserSetTool(KinematicsUserContext* ctx, const EmcPose* tool); + /** * Perform inverse kinematics (world coords -> joint positions) * diff --git a/src/emc/motion/command.c b/src/emc/motion/command.c index a938fa5d613..9cedc31f759 100644 --- a/src/emc/motion/command.c +++ b/src/emc/motion/command.c @@ -70,6 +70,11 @@ #include "homing.h" #include "axis.h" +// the kinematics module takes the tool offset from here when it can; a +// module written before the call exports no such symbol, and the weak +// reference leaves it NULL rather than refusing to load motion +#pragma weak kinematicsSetTool + #define ABS(x) (((x) < 0) ? -(x) : (x)) @@ -2002,6 +2007,9 @@ void emcmotCommandHandler_locked(void *arg, long servo_period) case EMCMOT_SET_OFFSET: rtapi_print_msg(RTAPI_MSG_DBG, "SET_OFFSET"); emcmotStatus->tool_offset = emcmotCommand->tool_offset; + if (kinematicsSetTool) { + kinematicsSetTool(&emcmotStatus->tool_offset); + } break; case EMCMOT_SET_AXIS_POSITION_LIMITS: diff --git a/tests/kins-params/check.py b/tests/kins-params/check.py index b6f7758f892..265fb606e8c 100755 --- a/tests/kins-params/check.py +++ b/tests/kins-params/check.py @@ -128,6 +128,35 @@ def compare(what, ours, theirs): for a, n in enumerate("xyzabcuvw"): compare("jacobian [%d][%s]" % (j, n), J[j][a], hal.get_value("paritycheck.jac-%d-%s" % (j, n))) +# the caller's tool wins over the module's pin: for a module with a tool +# entry, a length of the caller's must move the inverse, and handing the +# tool back to HAL must return it to what realtime found +kins.kinematicsUserSetTool.argtypes = [ctypes.c_void_p, ctypes.POINTER(EmcPose)] +tool_pin = None +for name in ("tool-offset", "tool-offset-z"): + try: + hal.get_value("%s.%s" % (module, name)) + tool_pin = name + except RuntimeError: + pass +if r_inv == 0 and rc_inv == 0 and tool_pin: + P = pose_of(pose_in) if frompose else F + T = pose_of([0.0] * AXES) + T.z = hal.get_value("%s.%s" % (module, tool_pin)) + 10.0 + kins.kinematicsUserSetTool(ctx, ctypes.byref(T)) + qt = Joints(*jnt_in) + if kins.kinematicsUserInverse(ctx, ctypes.byref(P), qt) != 0: + fail("inverse with the caller's tool") + elif all(close(qt[j], qi[j]) for j in range(joints)): + fail("the caller's tool did not move the inverse") + kins.kinematicsUserSetTool(ctx, None) + qt = Joints(*jnt_in) + if kins.kinematicsUserInverse(ctx, ctypes.byref(P), qt) != 0: + fail("inverse with the tool handed back") + else: + for j in range(joints): + compare("inverse joint %d after the tool is handed back" % j, qt[j], qi[j]) + kins.kinematicsUserFree(ctx) halc.hal_exit(comp_id) diff --git a/tests/kins-tool-offset/README b/tests/kins-tool-offset/README new file mode 100644 index 00000000000..db4bd42a726 --- /dev/null +++ b/tests/kins-tool-offset/README @@ -0,0 +1,7 @@ +The kinematics module takes the tool offset from motion, not from a net. + +Runs xyzac-trt-kins under motion with nothing connected to its tool-offset +pin, applies a tool length through G43, and checks that the joints move as +the tool length requires. Then connects motion.tooloffset.z to the pin the +old way and checks that nothing changes, and that G49 takes the length back +out through motion alone. diff --git a/tests/kins-tool-offset/checkresult b/tests/kins-tool-offset/checkresult new file mode 100755 index 00000000000..24dc9aa53e3 --- /dev/null +++ b/tests/kins-tool-offset/checkresult @@ -0,0 +1,2 @@ +#!/bin/sh +exit 0 # test failure is indicated by test.sh exit value diff --git a/tests/kins-tool-offset/sim.hal b/tests/kins-tool-offset/sim.hal new file mode 100644 index 00000000000..81a0df64444 --- /dev/null +++ b/tests/kins-tool-offset/sim.hal @@ -0,0 +1,20 @@ +# the module under test, with nothing on its tool-offset pin +loadrt [KINS]KINEMATICS +loadrt [EMCMOT]EMCMOT servo_period_nsec=[EMCMOT]SERVO_PERIOD num_joints=[KINS]JOINTS + +addf motion-command-handler servo-thread +addf motion-controller servo-thread + +# offsets, so that the tool length reaches the joints through a rotation +setp xyzac-trt-kins.y-offset 20 +setp xyzac-trt-kins.z-offset 10 + +net J0 joint.0.motor-pos-cmd => joint.0.motor-pos-fb +net J1 joint.1.motor-pos-cmd => joint.1.motor-pos-fb +net J2 joint.2.motor-pos-cmd => joint.2.motor-pos-fb +net J3 joint.3.motor-pos-cmd => joint.3.motor-pos-fb +net J4 joint.4.motor-pos-cmd => joint.4.motor-pos-fb + +net estop-loop iocontrol.0.user-enable-out iocontrol.0.emc-enable-in +net tool-prep-loop iocontrol.0.tool-prepare iocontrol.0.tool-prepared +net tool-change-loop iocontrol.0.tool-change iocontrol.0.tool-changed diff --git a/tests/kins-tool-offset/test-ui.py b/tests/kins-tool-offset/test-ui.py new file mode 100755 index 00000000000..dbd174caa66 --- /dev/null +++ b/tests/kins-tool-offset/test-ui.py @@ -0,0 +1,113 @@ +#!/usr/bin/env python3 +# The kinematics module takes the tool offset from motion. +# +# xyzac-trt-kins runs with nothing connected to its tool-offset pin. A +# tool length applied with G43 must still reach the joints, since motion +# hands the offset to the module; connecting motion.tooloffset.z to the +# pin afterwards, the old way, must change nothing; and G49 must take the +# length back out again through motion alone. + +import linuxcnc +import hal +import subprocess +import sys +import os +import time + +TOOL_LENGTH = 25.0 +POSE = "G0 X10 Y20 Z30 A30 C45" +AWAY = "G0 X0 Y0 Z0 A0 C0" + +c = linuxcnc.command() +s = linuxcnc.stat() + +c.state(linuxcnc.STATE_ESTOP_RESET) +c.state(linuxcnc.STATE_ON) +c.home(-1) +c.wait_complete() +c.mode(linuxcnc.MODE_MDI) + +errors = 0 + +def error(msg): + global errors + errors += 1 + print("*** ERROR " + msg) + +def mdi(*cmds): + for cmd in cmds: + c.mdi(cmd) + c.wait_complete(30) + +def joints(): + # the commanded joint positions once the move has settled: in position, + # nothing queued, and the same answer twice in a row, since the in + # position flag can go up a cycle before the last increment lands + deadline = time.time() + 30 + last = None + while time.time() < deadline: + s.poll() + now = [s.joint_position[i] for i in range(5)] + if s.inpos and not s.queue and now == last: + return now + last = now + time.sleep(0.1) + error("timed out waiting for the move") + return last + +def same(a, b, tol=1e-6): + return all(abs(x - y) <= tol for x, y in zip(a, b)) + +def show(what, j): + print("%-28s %s" % (what, " ".join("%.6f" % v for v in j))) + +# no tool: the pose with nothing applied +mdi("G49", POSE) +base = joints() +show("G49", base) + +# tool applied through motion, the pin still at its default +mdi("G43 H1", AWAY, POSE) +with_tool = joints() +show("G43 H1, pin unconnected", with_tool) +pin = hal.get_value("xyzac-trt-kins.tool-offset") +if pin != 0.0: + error("the tool-offset pin reads %g with nothing connected" % pin) +if same(base, with_tool): + error("the tool length did not reach the joints") + +# the table on rotaries at A30 C45: the tool length moves Y and Z joints, +# by a known amount, since the pivot geometry is the module's alone +tool_z = hal.get_value("motion.tooloffset.z") +if abs(tool_z - TOOL_LENGTH) > 1e-9: + error("motion.tooloffset.z is %g, expected %g" % (tool_z, TOOL_LENGTH)) +if abs(with_tool[0] - base[0]) > 1e-6: + error("the tool length moved joint 0, which the A rotation does not touch") + +# the old connection: nothing may change +subprocess.check_call(["halcmd", "net", ":tool-offset", + "motion.tooloffset.z", "xyzac-trt-kins.tool-offset"]) +mdi(AWAY, POSE) +with_net = joints() +show("G43 H1, pin connected", with_net) +pin = hal.get_value("xyzac-trt-kins.tool-offset") +if abs(pin - TOOL_LENGTH) > 1e-9: + error("the connected tool-offset pin reads %g" % pin) +if not same(with_tool, with_net): + error("connecting the pin changed the joints") + +# and back out, through motion, with the pin connected +mdi("G49", AWAY, POSE) +without = joints() +show("G49, pin connected", without) +if not same(base, without): + error("G49 did not take the tool length back out") + +for f in ("sim.var", "sim.var.bak"): + try: + os.unlink(f) + except OSError: + pass + +print("Exiting with %d errors" % errors) +sys.exit(1 if errors else 0) diff --git a/tests/kins-tool-offset/test.ini b/tests/kins-tool-offset/test.ini new file mode 100644 index 00000000000..bb7839671ed --- /dev/null +++ b/tests/kins-tool-offset/test.ini @@ -0,0 +1,112 @@ +[EMC] +VERSION = 1.1 +DEBUG = 0x0 + +[DISPLAY] +DISPLAY = ./test-ui.py + +[RS274NGC] +PARAMETER_FILE = sim.var + +[EMCMOT] +EMCMOT = motmod +COMM_TIMEOUT = 4.0 +SERVO_PERIOD = 1000000 + +[TASK] +TASK = milltask +CYCLE_TIME = 0.001 + +[HAL] +HALFILE = sim.hal + +[TRAJ] +COORDINATES = XYZAC +LINEAR_UNITS = mm +ANGULAR_UNITS = deg +DEFAULT_LINEAR_VELOCITY = 20 +MAX_LINEAR_VELOCITY = 200 +MAX_LINEAR_ACCELERATION = 2000 +NO_FORCE_HOMING = 1 + +[EMCIO] +TOOL_TABLE = tool.tbl + +[KINS] +KINEMATICS = xyzac-trt-kins +JOINTS = 5 + +[AXIS_X] +MIN_LIMIT = -200 +MAX_LIMIT = 200 +MAX_VELOCITY = 200 +MAX_ACCELERATION = 2000 + +[AXIS_Y] +MIN_LIMIT = -200 +MAX_LIMIT = 200 +MAX_VELOCITY = 200 +MAX_ACCELERATION = 2000 + +[AXIS_Z] +MIN_LIMIT = -200 +MAX_LIMIT = 200 +MAX_VELOCITY = 200 +MAX_ACCELERATION = 2000 + +[AXIS_A] +MIN_LIMIT = -100 +MAX_LIMIT = 100 +MAX_VELOCITY = 200 +MAX_ACCELERATION = 2000 + +[AXIS_C] +MIN_LIMIT = -36000 +MAX_LIMIT = 36000 +MAX_VELOCITY = 200 +MAX_ACCELERATION = 2000 + +[JOINT_0] +TYPE = LINEAR +HOME = 0 +MAX_VELOCITY = 200 +MAX_ACCELERATION = 2000 +MIN_LIMIT = -200 +MAX_LIMIT = 200 +HOME_SEQUENCE = 0 + +[JOINT_1] +TYPE = LINEAR +HOME = 0 +MAX_VELOCITY = 200 +MAX_ACCELERATION = 2000 +MIN_LIMIT = -200 +MAX_LIMIT = 200 +HOME_SEQUENCE = 0 + +[JOINT_2] +TYPE = LINEAR +HOME = 0 +MAX_VELOCITY = 200 +MAX_ACCELERATION = 2000 +MIN_LIMIT = -200 +MAX_LIMIT = 200 +HOME_SEQUENCE = 0 + +[JOINT_3] +TYPE = ANGULAR +HOME = 0 +MAX_VELOCITY = 200 +MAX_ACCELERATION = 2000 +MIN_LIMIT = -100 +MAX_LIMIT = 100 +HOME_SEQUENCE = 0 + +[JOINT_4] +TYPE = ANGULAR +HOME = 0 +MAX_VELOCITY = 200 +MAX_ACCELERATION = 2000 +MIN_LIMIT = -36000 +MAX_LIMIT = 36000 +HOME_SEQUENCE = 0 diff --git a/tests/kins-tool-offset/test.sh b/tests/kins-tool-offset/test.sh new file mode 100755 index 00000000000..a31b772a81c --- /dev/null +++ b/tests/kins-tool-offset/test.sh @@ -0,0 +1,2 @@ +#!/bin/bash -e +linuxcnc -r test.ini diff --git a/tests/kins-tool-offset/tool.tbl b/tests/kins-tool-offset/tool.tbl new file mode 100644 index 00000000000..acb961918d9 --- /dev/null +++ b/tests/kins-tool-offset/tool.tbl @@ -0,0 +1 @@ +T1 P1 Z25 D6 ;the tool with a length From f92dc802774ba91510eddf2bbc91d3b7733327a1 Mon Sep 17 00:00:00 2001 From: Luca Toniolo <10792599+grandixximo@users.noreply.github.com> Date: Mon, 14 Sep 2026 13:41:31 +1000 Subject: [PATCH 40/60] 5axiskins, maxkins: take the tool length from motion The trt modules, genhexkins and pentakins already have motion hand them the offset in effect; these two still read a pin the config had to net from motion.tooloffset.z, and a missing net gave wrong joints with no error. Flag the entry as the tool, drop the nets from the sims, and have the kins-switch test check that G43.4 reaches the joints with nothing netted: in the tilted pose the length moves them by its tilt term, the same length along the tool instead of along Z. --- .../vismach/5axis/bridgemill/5axisgui.hal | 1 - .../sim/axis/vismach/5axis/max5/max5kins.hal | 4 +- docs/src/man/man9/kins.9.adoc | 20 +++++---- src/emc/kinematics/5axiskins.c | 6 +-- src/emc/kinematics/maxkins.c | 2 +- tests/kins-switch/README | 5 ++- tests/kins-switch/test-ui.py | 42 ++++++++++++++++++- 7 files changed, 59 insertions(+), 21 deletions(-) diff --git a/configs/sim/axis/vismach/5axis/bridgemill/5axisgui.hal b/configs/sim/axis/vismach/5axis/bridgemill/5axisgui.hal index c45e7c5765b..3780f9ce3d0 100644 --- a/configs/sim/axis/vismach/5axis/bridgemill/5axisgui.hal +++ b/configs/sim/axis/vismach/5axis/bridgemill/5axisgui.hal @@ -17,7 +17,6 @@ net :gui-pivot-len <= 5axisgui.pivot_len net :gui-pivot-len => 5axiskins.pivot-length net :tool-len <= motion.tooloffset.z -net :tool-len => 5axiskins.tool-length net :tool-len => 5axisgui.tool_length net :tool-diam <= halui.tool.diameter diff --git a/configs/sim/axis/vismach/5axis/max5/max5kins.hal b/configs/sim/axis/vismach/5axis/max5/max5kins.hal index 85b9d37c2be..04b5dc47843 100644 --- a/configs/sim/axis/vismach/5axis/max5/max5kins.hal +++ b/configs/sim/axis/vismach/5axis/max5/max5kins.hal @@ -16,9 +16,7 @@ loadusr -W ./max5gui.py # set a visible tool setp max5gui.tool-radius 3 -# the tool length is applied along the tool, not along Z, so the tip stays -# on the programmed point as B tilts -net tool-len motion.tooloffset.z max5gui.tool-length maxkins.tool-length +net tool-len motion.tooloffset.z max5gui.tool-length # add motion controller functions to servo thread addf motion-command-handler servo-thread diff --git a/docs/src/man/man9/kins.9.adoc b/docs/src/man/man9/kins.9.adoc index f2bd1051d4c..79a3a116e21 100644 --- a/docs/src/man/man9/kins.9.adoc +++ b/docs/src/man/man9/kins.9.adoc @@ -252,9 +252,10 @@ replacing it. Put a given length in one column or the other, not both. *maxkins.tool-length*:: Tool length, applied along the tool rather than along Z, so that the tip - stays on the programmed point as B tilts. Net it from *motion.tooloffset.z*. - Left unconnected, the tool length stays where canon put it, along machine - Z, which is only correct at B0. + stays on the programmed point as B tilts. Motion hands the module the + offset in effect (G43, G49), so the pin needs no connection; it is read + only until motion has sent anything. To avoid a joint jump, change the + tool offset only when B is 0. === pentakins - Pentapod Kinematics @@ -413,12 +414,13 @@ expected by it (XYZBCW `->` joints 0..5) *5axiskins.tool-length*:: Tool length, applied along the tool rather than along Z, so that the tip - stays on the programmed point as B and C move. Net it from - *motion.tooloffset.z*. Left unconnected, the tool length stays where canon - put it, along machine Z, which is only correct at B0. A tool length in the - W column of the tool table reaches the same place, once a block commands W, - and adds to this pin rather than replacing it. Put a given length in one - column or the other, not both. + stays on the programmed point as B and C move. Motion hands the module the + offset in effect (G43, G49), so the pin needs no connection; it is read + only until motion has sent anything. To avoid a joint jump, change the + tool offset only when B is 0. A tool length in the W column of the tool + table reaches the same place, once a block commands W, and adds to this + one rather than replacing it. Put a given length in one column or the + other, not both. == SEE ALSO diff --git a/src/emc/kinematics/5axiskins.c b/src/emc/kinematics/5axiskins.c index 7b95300f4cf..cd8b4db7b75 100644 --- a/src/emc/kinematics/5axiskins.c +++ b/src/emc/kinematics/5axiskins.c @@ -12,8 +12,8 @@ * * Notes: * 1) pivot-length must agree with the mechanical design -* (including vismach simulation); the tool length comes -* in on the tool-length pin of its own +* (including vismach simulation); the tool length is +* the offset motion applies, handed over by motion * 2) C axis: spherical coordinates aziumthal angle (t or theta) * projection of radius to xy plane * 3) B axis: spherical coordinates polar angle (p or phi) @@ -63,7 +63,7 @@ // the geometry, one pin each; the maths reads it from the block static const kins_param_desc fiveaxis_params[] = { { "pivot-length", KINS_PARAM_FLOAT, KINS_IN, 0, DEFAULT_PIVOT_LENGTH }, - { "tool-length", KINS_PARAM_FLOAT, KINS_IN, 0, 0.0 }, + { "tool-length", KINS_PARAM_FLOAT, KINS_IN, 1, 0.0 }, }; enum { P_PIVOT_LENGTH, P_TOOL_LENGTH }; diff --git a/src/emc/kinematics/maxkins.c b/src/emc/kinematics/maxkins.c index 38862871d74..1d72d46b54b 100644 --- a/src/emc/kinematics/maxkins.c +++ b/src/emc/kinematics/maxkins.c @@ -34,7 +34,7 @@ static const kins_param_desc max_params[] = { { "pivot-length", KINS_PARAM_FLOAT, KINS_IO, 0, 0.666 }, { "conventional-directions", KINS_PARAM_BIT, KINS_IN, 0, 0 }, // default is unconventional - { "tool-length", KINS_PARAM_FLOAT, KINS_IN, 0, 0 }, + { "tool-length", KINS_PARAM_FLOAT, KINS_IN, 1, 0 }, }; enum { P_PIVOT_LENGTH, P_CON, P_TOOL_LENGTH }; diff --git a/tests/kins-switch/README b/tests/kins-switch/README index e031f2cd0ff..e426b4d7554 100644 --- a/tests/kins-switch/README +++ b/tests/kins-switch/README @@ -7,5 +7,6 @@ block after the switch is planned in the kinematics that runs it, that the selection reaches the motion controller and the interpreter, that a negative P word and a kinematics the module does not provide are refused, that G13.1 cancels to the identity kinematics the module declares (type 1 -here, not 0), and that G13.1 in an ON_ABORT_COMMAND routine does not -swallow the rest of the routine. +here, not 0), that the tool length G43.4 puts in effect reaches the +kinematics with nothing netted to its pin, and that G13.1 in an +ON_ABORT_COMMAND routine does not swallow the rest of the routine. diff --git a/tests/kins-switch/test-ui.py b/tests/kins-switch/test-ui.py index 1f0aeb0f592..09d3b72cd54 100755 --- a/tests/kins-switch/test-ui.py +++ b/tests/kins-switch/test-ui.py @@ -72,6 +72,7 @@ def mdi(cmd): said = [] seen = [kins_type()] at_switch = None +holding = False strayed = [0.0] * JOINTS w_reached = 0.0 deadline = time.time() + 60 @@ -83,7 +84,13 @@ def mdi(cmd): seen.append(k) if k == 1 and at_switch is None: at_switch = now - if k == 1 and at_switch is not None: + holding = True + else: + holding = False + # the joints are held over the first stretch in identity, the one the + # W stroke runs in; later on the program applies a tool length in the + # tilted pose, which moves the joints on purpose + if holding: for j in CARRIED: strayed[j] = max(strayed[j], abs(now[j] - at_switch[j])) w_reached = max(w_reached, now[5]) @@ -144,10 +151,41 @@ def mdi(cmd): else: print("G43.4 switched to primary with the offset, G49 cancelled both") -# ---- a negative kinematics number is refused ----------------------------- +# ---- the tool length reaches the kinematics with nothing netted ---------- +# +# Motion hands the module the offset G43 puts in effect. The head is +# tilted, so the offset moves the joints, by the tilt term of the length: +# the same length along the tool axis instead of along Z. +import math c.mode(linuxcnc.MODE_MDI) c.wait_complete(30) +drain() +mdi("G12.1 P0") +mdi("G0 X10 Y10 Z-5 B-22.5 C45") +mdi("G49") +before = mdi("G0 X10 Y10 Z-5") +after = mdi("G43.4 H1") +L = 12.5 # tool 1 in tool.tbl +b, cc = math.radians(-22.5), math.radians(45) +want = [-L * math.sin(math.pi - b) * math.cos(cc), + -L * math.sin(math.pi - b) * math.sin(cc), + -L * (1 + math.cos(math.pi - b))] +got = [after[j] - before[j] for j in (0, 1, 2)] +if max(abs(g - w) for g, w in zip(got, want)) > 1e-3: + error("G43.4 in the tilted pose moved the joints by %s, not %s" + % (" ".join("%.4f" % v for v in got), " ".join("%.4f" % v for v in want))) +else: + print("G43.4 moved the joints by the tilt term of the tool length") +# G49 would drop to identity and hold the joints where they are; a zero +# offset without a switch takes the length back out +back = mdi("G43.1 Z0") +if max(abs(back[j] - before[j]) for j in (0, 1, 2)) > 1e-3: + error("a zero tool length did not take the length back out of the joints") +mdi("G49") + +# ---- a negative kinematics number is refused ----------------------------- + drain() c.mdi("G12.1 P-1") c.wait_complete(30) From d992970fdac87e8233eb69f4d7815ec24a1d8516 Mon Sep 17 00:00:00 2001 From: Luca Toniolo <10792599+grandixximo@users.noreply.github.com> Date: Sat, 5 Sep 2026 00:11:48 +1000 Subject: [PATCH 41/60] kinematics: reach the frames and the tool frame inverse from outside realtime The loader gains the two frames, the tool frame inverse and a survey of which joints turn the work, on kins_util.c code the library already carries; only the glue between the block form and the joint-only frame functions is new. The survey lets a caller hold the table still without a config entry naming it. A second survey tells the two orienting rotaries apart, the primary whose axis is fixed in the machine from the secondary whose axis it carries, by turning each a little and reading the axis of the resulting rotation; the sign of the secondary is how a caller names a pose rather than counting them. It answers only for exactly two such rotaries, so a robot wrist gets nothing. tests/kins-params checks the pair every module reports. kinematicsUserInitString() takes [KINS] KINEMATICS as the HAL file hands it to loadrt. --- src/emc/kinematics/kinematics.h | 24 +++ src/emc/kinematics/kins_util.c | 114 ++++++++++++++ .../kinematics_userspace/kinematics_user.c | 143 ++++++++++++++++++ .../kinematics_userspace/kinematics_user.h | 54 +++++++ tests/kins-params/check.py | 26 ++++ tests/kins-params/test.sh | 39 +++-- tests/tool-frame/test_tool_frame.c | 19 +++ 7 files changed, 403 insertions(+), 16 deletions(-) diff --git a/src/emc/kinematics/kinematics.h b/src/emc/kinematics/kinematics.h index a87266e5067..4984e1f1859 100644 --- a/src/emc/kinematics/kinematics.h +++ b/src/emc/kinematics/kinematics.h @@ -355,6 +355,30 @@ typedef int (*kinsFrameFunc)(const double *joint, PmRotationMatrix *rot, const KINEMATICS_FORWARD_FLAGS *fflags); +/* Which joints turn the work: a bit per joint whose motion changes the + work frame at the seed. This is what a caller needs to hold the table + still while the head orients the tool (Heidenhain COORD ROT), or to let + it take part (TABLE ROT), without a config entry naming it. Returns 0 + or -1 if the frame cannot be evaluated. */ +extern int toolFrameWorkJoints(kinsFrameFunc work, int num_joints, + const double *seed, unsigned int *mask); + +/* The two rotaries that orient the tool, told apart. One has its axis + fixed in the machine frame, the primary, and the other has its axis + carried by the first, the secondary. The two poses that reach one tool + direction differ in the sign of the secondary, which is what a caller + needs to name a pose rather than count them, Heidenhain's SEQ+ and SEQ-. + + Both are found from the module's own tool frame, by turning each joint a + little and reading the axis of the rotation that results, so a module + declares nothing and a switchkins type that turns nothing answers -1. + Returns 0 with both joints set, or -1 where the machine has any number + of orienting rotaries but two, a robot wrist among them, or where the + frame cannot be evaluated. */ +extern int toolFrameOrientJoints(kinsFrameFunc tool, int num_joints, + const double *seed, + int *primary, int *secondary); + extern int toolFrameSolve(kinsFrameFunc work, kinsFrameFunc tool, int num_joints, diff --git a/src/emc/kinematics/kins_util.c b/src/emc/kinematics/kins_util.c index 656c6ebb6c7..d97f228cf20 100644 --- a/src/emc/kinematics/kins_util.c +++ b/src/emc/kinematics/kins_util.c @@ -562,6 +562,9 @@ int identityKinematicsToolFrame(const double *joints, #define TFS_ITERS 60 #define TFS_FD_STEP 1e-6 // internal radians #define TFS_MOVED_TOL 1e-9 // frame difference that counts as movement +#define TFS_PROBE_STEP 0.05 // joint units, to read a joint's own axis +#define TFS_CARRY_STEP 40.0 // joint units, far enough to swing a carried axis +#define TFS_CARRY_TOL 1e-6 // axes closer than this counted as the same #define TFS_RANK_TOL 1e-4 // a direction worth less than this is free #define TFS_SOLVED 1e-18 // sum of squared residuals #define TFS_STEP_LIMIT 0.4 // internal radians per iteration @@ -970,6 +973,117 @@ static int tfs_spin(tfs_ctx *c, const double *joint, return 0; } +int toolFrameWorkJoints(kinsFrameFunc work, int num_joints, + const double *seed, unsigned int *mask) +{ + KINEMATICS_FORWARD_FLAGS fflags = 0; + PmRotationMatrix base, moved; + double joint[EMCMOT_MAX_JOINTS]; + int i, j; + + if (!work || !seed || !mask || num_joints <= 0 || num_joints > EMCMOT_MAX_JOINTS) { + return -1; + } + *mask = 0; + for (i = 0; i < EMCMOT_MAX_JOINTS; i++) { joint[i] = (i < num_joints) ? seed[i] : 0; } + if (work(joint, &base, &fflags)) { return -1; } + + // a step of one joint unit: a degree on every module in the tree, and + // a linear joint never turns a frame whatever its unit + for (j = 0; j < num_joints; j++) { + double diff = 0; + const double *a = &base.x.x, *b = &moved.x.x; + + joint[j] = seed[j] + 1.0; + if (work(joint, &moved, &fflags)) { return -1; } + joint[j] = seed[j]; + for (i = 0; i < 9; i++) { diff += fabs(a[i] - b[i]); } + if (diff > TFS_MOVED_TOL) { *mask |= 1u << j; } + } + return 0; +} + +// The axis a joint turns the tool frame about, in machine coordinates: move +// the joint a little and read the rotation that took the frame there. +// Returns 0 and a unit axis where the joint turns the tool, 1 where it does +// not, which is every linear joint and every joint the module ignores. +static int tfs_joint_axis(kinsFrameFunc tool, const double *joint, + int j, double step, double axis[3]) +{ + KINEMATICS_FORWARD_FLAGS fflags = 0; + PmRotationMatrix r1, r2; + double moved[EMCMOT_MAX_JOINTS]; + const double *a, *b; + double len; + int i; + + for (i = 0; i < EMCMOT_MAX_JOINTS; i++) { moved[i] = joint[i]; } + moved[j] += step; + if (tool(joint, &r1, &fflags)) { return -1; } + if (tool(moved, &r2, &fflags)) { return -1; } + + // The rotation from one frame to the other is r2 * transpose(r1), and + // the axis of a small rotation is the skew part of it. Both matrices + // are columns of axes, so element (row, col) is (&r.x.x)[3*col + row]. + a = &r1.x.x; + b = &r2.x.x; + axis[0] = axis[1] = axis[2] = 0; + for (i = 0; i < 3; i++) { + // m[2][1] - m[1][2], m[0][2] - m[2][0], m[1][0] - m[0][1] + axis[0] += b[3*i + 2] * a[3*i + 1] - b[3*i + 1] * a[3*i + 2]; + axis[1] += b[3*i + 0] * a[3*i + 2] - b[3*i + 2] * a[3*i + 0]; + axis[2] += b[3*i + 1] * a[3*i + 0] - b[3*i + 0] * a[3*i + 1]; + } + len = sqrt(axis[0]*axis[0] + axis[1]*axis[1] + axis[2]*axis[2]); + if (len < 1e-9) { return 1; } + for (i = 0; i < 3; i++) { axis[i] /= len; } + return 0; +} + +int toolFrameOrientJoints(kinsFrameFunc tool, int num_joints, + const double *seed, int *primary, int *secondary) +{ + double joint[EMCMOT_MAX_JOINTS], elsewhere[EMCMOT_MAX_JOINTS]; + double axis[3], turned[3]; + int turns[2], count = 0, carried = -1; + int i, j, k, r; + + if (!tool || !seed || !primary || !secondary + || num_joints <= 0 || num_joints > EMCMOT_MAX_JOINTS) { + return -1; + } + *primary = *secondary = -1; + for (i = 0; i < EMCMOT_MAX_JOINTS; i++) { joint[i] = (i < num_joints) ? seed[i] : 0; } + + for (j = 0; j < num_joints; j++) { + r = tfs_joint_axis(tool, joint, j, TFS_PROBE_STEP, axis); + if (r < 0) { return -1; } + if (r > 0) { continue; } + if (count >= 2) { return -1; } // a wrist, not a head + turns[count++] = j; + } + if (count != 2) { return -1; } + + // whichever axis swings when the other joint moves is the carried one + for (i = 0; i < 2; i++) { + j = turns[i]; + k = turns[1 - i]; + if (tfs_joint_axis(tool, joint, j, TFS_PROBE_STEP, axis) != 0) { return -1; } + for (r = 0; r < EMCMOT_MAX_JOINTS; r++) { elsewhere[r] = joint[r]; } + elsewhere[k] += TFS_CARRY_STEP; + if (tfs_joint_axis(tool, elsewhere, j, TFS_PROBE_STEP, turned) != 0) { return -1; } + if (fabs(axis[0]*turned[0] + axis[1]*turned[1] + axis[2]*turned[2] - 1.0) + > TFS_CARRY_TOL) { + if (carried >= 0) { return -1; } // both carried: not a head + carried = j; + } + } + if (carried < 0) { return -1; } + *secondary = carried; + *primary = (turns[0] == carried) ? turns[1] : turns[0]; + return 0; +} + int toolFrameSolve(kinsFrameFunc work, kinsFrameFunc tool, int num_joints, diff --git a/src/emc/kinematics_userspace/kinematics_user.c b/src/emc/kinematics_userspace/kinematics_user.c index b8b339f564e..25e8b5c374a 100644 --- a/src/emc/kinematics_userspace/kinematics_user.c +++ b/src/emc/kinematics_userspace/kinematics_user.c @@ -589,6 +589,149 @@ int kinematicsUserIsRtOnly(KinematicsUserContext* ctx) return ctx->rt_only; } +/* ======================================================================== + * Frames and the tool frame inverse + * + * toolFrameSolve() drives a pair of frame functions that take joints alone, + * the shape the RT modules export; the block form takes the parameters as + * well. The context the solver is running for is parked in a file static + * for the duration of the call, which is fine for the single threaded + * callers this has (the interpreter, a planner), and would not be for two + * threads solving at once. + * ======================================================================== */ + +static KinematicsUserContext *frame_ctx; + +static int frame_work(const double *joint, PmRotationMatrix *rot, + const KINEMATICS_FORWARD_FLAGS *fflags) +{ + KinematicsUserContext *ctx = frame_ctx; + return kinsOpsWorkFrame(ctx->info.ops[ctx->ktype], &ctx->params, joint, rot, fflags); +} + +static int frame_tool(const double *joint, PmRotationMatrix *rot, + const KINEMATICS_FORWARD_FLAGS *fflags) +{ + KinematicsUserContext *ctx = frame_ctx; + return kinsOpsToolFrame(ctx->info.ops[ctx->ktype], &ctx->params, joint, rot, fflags); +} + +static void pad_joints(KinematicsUserContext *ctx, const double *in, double *out) +{ + int i; + for (i = 0; i < EMCMOT_MAX_JOINTS; i++) { + out[i] = (i < ctx->num_joints) ? in[i] : 0.0; + } +} + +int kinematicsUserWorkFrame(KinematicsUserContext* ctx, const double* joints, + PmRotationMatrix* rot) +{ + KINEMATICS_FORWARD_FLAGS fflags = 0; + double j[EMCMOT_MAX_JOINTS]; + + if (!ctx || !ctx->initialized || ctx->rt_only || !joints || !rot) return -1; + refresh(ctx); + pad_joints(ctx, joints, j); + return kinsOpsWorkFrame(ctx->info.ops[ctx->ktype], &ctx->params, j, rot, &fflags); +} + +int kinematicsUserToolFrame(KinematicsUserContext* ctx, const double* joints, + PmRotationMatrix* rot) +{ + KINEMATICS_FORWARD_FLAGS fflags = 0; + double j[EMCMOT_MAX_JOINTS]; + + if (!ctx || !ctx->initialized || ctx->rt_only || !joints || !rot) return -1; + refresh(ctx); + pad_joints(ctx, joints, j); + return kinsOpsToolFrame(ctx->info.ops[ctx->ktype], &ctx->params, j, rot, &fflags); +} + +int kinematicsUserToolFrameInverse(KinematicsUserContext* ctx, + const PmCartesian* axis_in_work, + const PmCartesian* x_in_work, + const double* seed, + unsigned int held, + double* solutions, + int max_solutions, + int* free_directions, + double* tool_spin) +{ + double j[EMCMOT_MAX_JOINTS]; + int found; + + if (!ctx || !ctx->initialized || ctx->rt_only || !seed) return -1; + if (!ctx->info.ops[ctx->ktype]->work || !ctx->info.ops[ctx->ktype]->tool) return -1; + refresh(ctx); + pad_joints(ctx, seed, j); + frame_ctx = ctx; + found = toolFrameSolve(frame_work, frame_tool, ctx->num_joints, + axis_in_work, x_in_work, j, held, + solutions, max_solutions, free_directions, tool_spin); + frame_ctx = NULL; + return found; +} + +int kinematicsUserWorkJoints(KinematicsUserContext* ctx, const double* seed, + unsigned int* mask) +{ + double j[EMCMOT_MAX_JOINTS]; + int r; + + if (!ctx || !ctx->initialized || ctx->rt_only || !seed || !mask) return -1; + if (!ctx->info.ops[ctx->ktype]->work) return -1; + refresh(ctx); + pad_joints(ctx, seed, j); + frame_ctx = ctx; + r = toolFrameWorkJoints(frame_work, ctx->num_joints, j, mask); + frame_ctx = NULL; + return r; +} + +int kinematicsUserOrientJoints(KinematicsUserContext* ctx, const double* seed, + int* primary, int* secondary) +{ + double j[EMCMOT_MAX_JOINTS]; + int r; + + if (!ctx || !ctx->initialized || ctx->rt_only || !seed || !primary || !secondary) return -1; + if (!ctx->info.ops[ctx->ktype]->tool) return -1; + refresh(ctx); + pad_joints(ctx, seed, j); + frame_ctx = ctx; + r = toolFrameOrientJoints(frame_tool, ctx->num_joints, j, primary, secondary); + frame_ctx = NULL; + return r; +} + +KinematicsUserContext* kinematicsUserInitString(const char* kinematics, + int num_joints, + int comp_id, + const char* prefix) +{ + char buf[256], *tok, *save = NULL; + char module[64] = "", coords[64] = "", sparm[64] = ""; + + if (!kinematics) return NULL; + snprintf(buf, sizeof(buf), "%s", kinematics); + for (tok = strtok_r(buf, " \t", &save); tok; tok = strtok_r(NULL, " \t", &save)) { + if (!module[0]) { + snprintf(module, sizeof(module), "%s", tok); + } else if (!strncmp(tok, "coordinates=", 12)) { + snprintf(coords, sizeof(coords), "%s", tok + 12); + } else if (!strncmp(tok, "sparm=", 6)) { + snprintf(sparm, sizeof(sparm), "%s", tok + 6); + } + /* kinstype= and anything else is the RT loader's business */ + } + if (!module[0]) return NULL; + return kinematicsUserInitSparm(module, num_joints, + coords[0] ? coords : NULL, + sparm[0] ? sparm : NULL, + comp_id, prefix); +} + void kinematicsUserFree(KinematicsUserContext* ctx) { int i; diff --git a/src/emc/kinematics_userspace/kinematics_user.h b/src/emc/kinematics_userspace/kinematics_user.h index e3090a34bd5..6d9b07a0859 100644 --- a/src/emc/kinematics_userspace/kinematics_user.h +++ b/src/emc/kinematics_userspace/kinematics_user.h @@ -201,6 +201,60 @@ int kinematicsUserRefreshParams(KinematicsUserContext* ctx); */ int kinematicsUserIsRtOnly(KinematicsUserContext* ctx); +/** + * The frames, as the module reports them: the work frame and the tool + * frame at a joint set, each against the machine (see kinematics.h). + * + * @return 0, or -1 if the module supplies no frame for the selected type + */ +int kinematicsUserWorkFrame(KinematicsUserContext* ctx, const double* joints, + PmRotationMatrix* rot); +int kinematicsUserToolFrame(KinematicsUserContext* ctx, const double* joints, + PmRotationMatrix* rot); + +/** + * The tool frame inverse of kinematics.h, on the loaded module and the + * selected type: the joint sets that point the tool axis, and where given + * the tool x, along the directions asked for, in work coordinates. Same + * arguments and answers as kinematicsToolFrameInverse(). + */ +int kinematicsUserToolFrameInverse(KinematicsUserContext* ctx, + const PmCartesian* axis_in_work, + const PmCartesian* x_in_work, + const double* seed, + unsigned int held, + double* solutions, + int max_solutions, + int* free_directions, + double* tool_spin); + +/** + * Which joints turn the work at the seed, a bit per joint; what a caller + * passes as held to keep the table still. See toolFrameWorkJoints(). + */ +int kinematicsUserWorkJoints(KinematicsUserContext* ctx, const double* seed, + unsigned int* mask); + +/** + * The two rotaries that orient the tool, primary and secondary, told apart + * by which one carries the other's axis. The sign of the secondary names + * the pose a five axis machine reaches a tool direction in. Returns 0, or + * -1 where the machine has any number of orienting rotaries but two. + * See toolFrameOrientJoints(). + */ +int kinematicsUserOrientJoints(KinematicsUserContext* ctx, const double* seed, + int* primary, int* secondary); + +/** + * kinematicsUserInitSparm() from the value of [KINS] KINEMATICS as the + * HAL file hands it to loadrt: the module name first, then any of + * coordinates=, sparm= and kinstype=, in any order. + */ +KinematicsUserContext* kinematicsUserInitString(const char* kinematics, + int num_joints, + int comp_id, + const char* prefix); + /** * Free kinematics context * diff --git a/tests/kins-params/check.py b/tests/kins-params/check.py index 265fb606e8c..a3133840f52 100755 --- a/tests/kins-params/check.py +++ b/tests/kins-params/check.py @@ -42,6 +42,9 @@ class EmcPose(ctypes.Structure): if coords == "-": coords = "" sparm = sys.argv[8].encode() if len(sys.argv) > 8 and sys.argv[8] not in ("", "-") else None +# the orientation joints this machine is known to have, "primary,secondary"; +# "-" where it has no such pair, "no-frame" where it reports no tool frame +orient_in = sys.argv[9] if len(sys.argv) > 9 else "no-frame" pose_in += [0.0] * (AXES - len(pose_in)) jnt_in += [0.0] * (MAX_JOINTS - len(jnt_in)) @@ -157,6 +160,29 @@ def compare(what, ours, theirs): for j in range(joints): compare("inverse joint %d after the tool is handed back" % j, qt[j], qi[j]) +# the two rotaries that orient the tool, told apart by which carries the +# other. The sign of the secondary is what G53.1 P names, so a module that +# gets this wrong sends the machine to the other pose without saying so. +kins.kinematicsUserOrientJoints.argtypes = [ctypes.c_void_p, Joints, + ctypes.POINTER(ctypes.c_int), + ctypes.POINTER(ctypes.c_int)] +primary, secondary = ctypes.c_int(-1), ctypes.c_int(-1) +r_orient = kins.kinematicsUserOrientJoints(ctx, Joints(*jnt_in), + ctypes.byref(primary), ctypes.byref(secondary)) +class Rot(ctypes.Structure): + _fields_ = [(n, ctypes.c_double * 3) for n in "xyz"] +kins.kinematicsUserToolFrame.argtypes = [ctypes.c_void_p, Joints, ctypes.POINTER(Rot)] +frame = Rot() +has_frame = kins.kinematicsUserToolFrame(ctx, Joints(*jnt_in), ctypes.byref(frame)) == 0 +if r_orient == 0: + got = "%d,%d" % (primary.value, secondary.value) +else: + got = "-" if has_frame else "no-frame" +if got != orient_in: + fail("orientation joints are %s, expected %s" % (got, orient_in)) +else: + print("kins-params: %s type %d orientation joints %s" % (module, ktype, got)) + kins.kinematicsUserFree(ctx) halc.hal_exit(comp_id) diff --git a/tests/kins-params/test.sh b/tests/kins-params/test.sh index 82f5d294e23..e429bf5029c 100755 --- a/tests/kins-params/test.sh +++ b/tests/kins-params/test.sh @@ -11,7 +11,7 @@ ${SUDO} halcompile --install paritycheck.c >/dev/null # ONLY= in the environment runs the entries for that module alone run() { local loadrt="$1" setp="$2" parms="$3" ktype="$4" - local module coords sparm joints frompose pose jnt hal tok + local module coords sparm joints frompose pose jnt orient rtparms hal tok case "$loadrt" in "${ONLY:-}"*) ;; *) return 0 ;; esac module=${loadrt%% *} coords=""; sparm="" @@ -22,21 +22,28 @@ run() { esac done joints=3; frompose=0; pose="0,0,0,0,0,0,0,0,0"; jnt="10,20,30,40,50,60,70,80,90" + # the two rotaries that orient the tool, "primary,secondary", or "-" + # where the machine has no such pair and "no-frame" where the module + # reports no tool frame at all, which most of the tree still does + orient="no-frame" for tok in $parms; do case "$tok" in joints=*) joints=${tok#joints=} ;; frompose=*) frompose=${tok#frompose=} ;; pose=*) pose=${tok#pose=} ;; jnt=*) jnt=${tok#jnt=} ;; + orient=*) orient=${tok#orient=} ;; esac done + # what the module under test is loaded with: everything but our own word + rtparms=$(printf ' %s ' "$parms" | sed 's/ orient=[^ ]*//g') hal=$(mktemp --suffix=.hal) { printf 'loadrt %s\n' "$loadrt" printf '%s\n' "$setp" - printf 'loadrt paritycheck %s ktype=%s\n' "$parms" "${ktype:-0}" + printf 'loadrt paritycheck %s ktype=%s\n' "$rtparms" "${ktype:-0}" # halcmd keeps quotes, so an absent value travels as a dash - printf 'loadusr -w python3 check.py %s %s %s %s %s %s %s %s\n' \ - "$module" "$joints" "${coords:--}" "${ktype:-0}" "$frompose" "$pose" "$jnt" "${sparm:--}" + printf 'loadusr -w python3 check.py %s %s %s %s %s %s %s %s %s\n' \ + "$module" "$joints" "${coords:--}" "${ktype:-0}" "$frompose" "$pose" "$jnt" "${sparm:--}" "$orient" } > "$hal" echo "=== $loadrt type ${ktype:-0}" halrun -f "$hal" @@ -44,11 +51,11 @@ run() { } # identity, a gantry included -run "trivkins coordinates=XYZ" "" "joints=3 jnt=10,20,30" -run "trivkins coordinates=XYZY kinstype=BOTH" "" "joints=4 jnt=10,20,30,20" -run "trivkins coordinates=XYZABCUVW" "" "joints=9" +run "trivkins coordinates=XYZ" "" "joints=3 jnt=10,20,30 orient=-" +run "trivkins coordinates=XYZY kinstype=BOTH" "" "joints=4 jnt=10,20,30,20 orient=-" +run "trivkins coordinates=XYZABCUVW" "" "joints=9 orient=-" run "userkins" "" "joints=3 jnt=10,20,30" -run "millturn" "" "joints=4 jnt=10,20,30,40" +run "millturn" "" "joints=4 jnt=10,20,30,40 orient=-" run "millturn" "" "joints=4 jnt=10,20,30,40" 1 # linear maps and one rotation @@ -69,8 +76,8 @@ run "maxkins" \ "setp maxkins.pivot-length 100" \ "joints=9 jnt=10,20,30,0,15,25,7,0,3" -run "5axiskins coordinates=XYZBCW" "" "joints=6 jnt=10,20,30,15,25,5" -run "5axiskins coordinates=XYZBCW sparm=identityfirst" "" "joints=6 jnt=10,20,30,15,25,5" 1 +run "5axiskins coordinates=XYZBCW" "" "joints=6 jnt=10,20,30,15,25,5 orient=4,3" +run "5axiskins coordinates=XYZBCW sparm=identityfirst" "" "joints=6 jnt=10,20,30,15,25,5 orient=4,3" 1 run "xyzac-trt-kins coordinates=XYZAC" \ "setp xyzac-trt-kins.y-offset 3 @@ -79,7 +86,7 @@ setp xyzac-trt-kins.tool-offset 7 setp xyzac-trt-kins.x-rot-point 1 setp xyzac-trt-kins.y-rot-point 2 setp xyzac-trt-kins.z-rot-point 5" \ - "joints=5 jnt=10,20,30,15,25" + "joints=5 jnt=10,20,30,15,25 orient=-" run "xyzbc-trt-kins coordinates=XYZBC" \ "setp xyzbc-trt-kins.conventional-directions 1 @@ -89,7 +96,7 @@ setp xyzbc-trt-kins.tool-offset 7 setp xyzbc-trt-kins.x-rot-point 1 setp xyzbc-trt-kins.y-rot-point 2 setp xyzbc-trt-kins.z-rot-point 5" \ - "joints=5 jnt=10,20,30,15,25" + "joints=5 jnt=10,20,30,15,25 orient=-" run "xyzab_tdr_kins" \ "setp xyzab_tdr_kins.x-offset 3 @@ -112,7 +119,7 @@ setp xyzacb_trsrn_kins.tool-offset-z 50 setp xyzacb_trsrn_kins.pre-rot 0.3 setp xyzacb_trsrn_kins.primary-angle 20 setp xyzacb_trsrn_kins.secondary-angle 35" \ - "joints=6 jnt=10,20,30,15,25,35" 1 + "joints=6 jnt=10,20,30,15,25,35 orient=5,4" 1 run "xyzacb_trsrn" \ "setp xyzacb_trsrn_kins.nut-angle 45 @@ -121,7 +128,7 @@ setp xyzacb_trsrn_kins.z-pivot 200 setp xyzacb_trsrn_kins.pre-rot 0.3 setp xyzacb_trsrn_kins.primary-angle 20 setp xyzacb_trsrn_kins.secondary-angle 35" \ - "joints=6 jnt=10,20,30,15,25,35" 2 + "joints=6 jnt=10,20,30,15,25,35 orient=-" 2 run "xyzbca_trsrn" \ "setp xyzbca_trsrn_kins.nut-angle 45 @@ -135,7 +142,7 @@ setp xyzbca_trsrn_kins.tool-offset-z 50 setp xyzbca_trsrn_kins.pre-rot 0.3 setp xyzbca_trsrn_kins.primary-angle 20 setp xyzbca_trsrn_kins.secondary-angle 35" \ - "joints=6 jnt=10,20,30,15,25,35" 1 + "joints=6 jnt=10,20,30,15,25,35 orient=5,3" 1 # polar run "rosekins" "" "joints=3 jnt=10,5,30" @@ -143,7 +150,7 @@ run "rosekins" "" "joints=3 jnt=10,5,30" # arms run "scarakins" "" "joints=6 jnt=30,40,20,10,0,0" run "scorbot-kins" "" "joints=5 jnt=40,60,-20,0,0" -run "pumakins" "setp pumakins.D6 50" "joints=6 jnt=15,20,-35,10,70,20" +run "pumakins" "setp pumakins.D6 50" "joints=6 jnt=15,20,-35,10,70,20 orient=-" run "three21kins" "" "joints=6 jnt=15,20,-35,10,70,20" run "genserkins" "" "joints=9 jnt=15,20,-35,10,70,20,0,0,0" run "genserkins" "setp genserkins.unrotate-3 1" "joints=9 jnt=15,20,-35,10,70,20,0,0,0" diff --git a/tests/tool-frame/test_tool_frame.c b/tests/tool-frame/test_tool_frame.c index 00103cb4678..9e38a5f6933 100644 --- a/tests/tool-frame/test_tool_frame.c +++ b/tests/tool-frame/test_tool_frame.c @@ -217,10 +217,29 @@ static int holds(const double *sols, int count, int njoints, return 0; } +/* the joints that turn the work, read off the work frame rather than + declared, so that a caller can hold the table without naming it */ +static void test_work_joints(void) +{ + const double seed[6] = {0, 0, 0, 10, 20, 30}; + unsigned int mask = 99; + + check(toolFrameWorkJoints(xyzacWork, 5, seed, &mask) == 0 && mask == ((1u << 3) | (1u << 4)), + "xyzac: both rotaries carry the work"); + check(toolFrameWorkJoints(identityFrame, 5, seed, &mask) == 0 && mask == 0, + "a head machine: nothing turns the work"); + check(toolFrameWorkJoints(mixedWork, 6, seed, &mask) == 0 && mask == (1u << 3), + "table and head: the table joint alone"); + check(toolFrameWorkJoints(NULL, 5, seed, &mask) == -1, + "no frame function is refused"); +} + int main(void) { PmRotationMatrix m, r; + test_work_joints(); + /* the supplied constants are usable as declarations */ check(toolFrameIsProper(&TOOL_FRAME_SPINDLE), "TOOL_FRAME_SPINDLE is proper"); check(toolFrameIsProper(&TOOL_FRAME_FLANGE), "TOOL_FRAME_FLANGE is proper"); From 3470709921db15a9310e5fe6a97882bafa72ba93 Mon Sep 17 00:00:00 2001 From: Luca Toniolo <10792599+grandixximo@users.noreply.github.com> Date: Mon, 14 Sep 2026 16:48:22 +1000 Subject: [PATCH 42/60] kinematics_user: read the module's pins by name, making nothing in HAL The loader read the RT instance's pins through pins of its own, linked to the signal each RT pin was on, and where a pin had no signal it made one and linked the RT pin to it. That leaves the module's pins connected to signals the config never made, so a net of one of them after the module was first evaluated fails on a pin that is already connected. Read each pin by name with hal_get_p() instead, at every refresh: nothing is made in HAL, a pin netted later reads the signal from then on, and the loader no longer has to be initialised before the caller's hal_ready(). --- .../kinematics_userspace/kinematics_user.c | 199 +++++------------- .../kinematics_userspace/kinematics_user.h | 7 +- 2 files changed, 59 insertions(+), 147 deletions(-) diff --git a/src/emc/kinematics_userspace/kinematics_user.c b/src/emc/kinematics_userspace/kinematics_user.c index 25e8b5c374a..559bba46dc2 100644 --- a/src/emc/kinematics_userspace/kinematics_user.c +++ b/src/emc/kinematics_userspace/kinematics_user.c @@ -4,9 +4,10 @@ * * Loads a kinematics .so with dlopen, asks it to describe itself through * kinsDescribe(), and evaluates its kinematics through the parameter - * block (see kinematics.h). The block is filled from HAL: one input pin - * of the caller's component per table entry, connected to the signal the - * RT instance's pin reads, so the values are the live ones. The tool is + * block (see kinematics.h). The block is filled from HAL: the RT + * instance's own pins, read by name whenever the block is refreshed, so + * the values are the live ones and nothing is made in HAL to get at them. + * The tool is * the caller's where it has given one, since a planner knows what a * segment runs under better than the machine does; otherwise it comes * from motion's own tooloffset pins where motion is loaded, so that the @@ -35,8 +36,7 @@ typedef int (*kins_describe_fn)(const char *coordinates, const char *sparm, kins_module_info *info); -#define MAX_BOUND_PINS (KINS_MAX_PARAMS + AXIS_COUNT) -#define MAX_MADE_SIGNALS MAX_BOUND_PINS +#define MAX_PINS (KINS_MAX_PARAMS + AXIS_COUNT) struct KinematicsUserContext { int initialized; @@ -49,14 +49,11 @@ struct KinematicsUserContext { int ktype; /* kinematics type being evaluated */ int num_joints; char module_name[64]; - int comp_id; /* the caller's component, owns the pins made here */ - const char *prefix; /* its name, which those pin names start with */ - char made_signal[MAX_MADE_SIGNALS][HAL_NAME_LEN + 1]; - int num_made_signals; - hal_refs_u *cell; /* HAL storage those pins are made against */ - int num_cells; - int cell_of_param[KINS_MAX_PARAMS]; /* -1 if not bound */ - int cell_of_tool[AXIS_COUNT]; /* motion.tooloffset.*, -1 if absent */ + int comp_id; /* the caller's component, which has HAL mapped */ + char pin_name[MAX_PINS][HAL_NAME_LEN + 1]; /* the RT instance's pins read here */ + int num_pins; + int pin_of_param[KINS_MAX_PARAMS]; /* -1 if not read */ + int pin_of_tool[AXIS_COUNT]; /* motion.tooloffset.*, -1 if absent */ int tool_param; /* the table's tool entry, -1 if none */ int warned_tool; EmcPose caller_tool; /* from kinematicsUserSetTool() */ @@ -65,55 +62,28 @@ struct KinematicsUserContext { }; /* ======================================================================== - * Pin binding + * Pin reading * ======================================================================== */ -/* - * Give the block a reference to a value it needs. - * - * The reference is to a pin of ours rather than into the RT instance's, - * so that its lifetime is ours. Ours is connected to the signal the RT - * pin reads, or, when the RT pin has no signal, to one made here and - * removed again in kinematicsUserFree(). - * - * The reference has to 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 and the block reads what a cell holds once - * the connection is in place. - */ -static int make_signal(KinematicsUserContext *ctx, const char *pin_name, - hal_type_t type, char *out, size_t outlen) -{ - if (ctx->num_made_signals >= MAX_MADE_SIGNALS) { - fprintf(stderr, "kinematicsUserInit: too many signals to create\n"); - return -1; - } - if ((size_t)snprintf(out, outlen, "%s-nonrt", pin_name) >= outlen) { - fprintf(stderr, "kinematicsUserInit: signal name for '%s' too long\n", - pin_name); - return -1; - } - if (hal_signal_new(out, type) != 0) return -1; - if (hal_link(pin_name, out) != 0) { - hal_signal_delete(out); - return -1; - } - snprintf(ctx->made_signal[ctx->num_made_signals++], - sizeof(ctx->made_signal[0]), "%s", out); - return 0; -} - -static int new_pin(int comp_id, hal_type_t type, hal_refs_u *out, - const char *name) +/* The block is read from the RT instance's own pins by name at every + refresh, so a pin netted later reads its signal; nothing is made in HAL + to get at them. */ +static int pin_value(const char *pin_name, hal_type_t type, double *out) { + hal_query_t q; + + memset(&q, 0, sizeof(q)); + q.name = pin_name; + q.qtype = HAL_QTYPE_PIN; + q.pp.type = type; + if (hal_get_p(&q, NULL, NULL) != 0) return -1; switch (type) { - case HAL_BIT: return hal_pin_new_bool(comp_id, HAL_IN, &out->b, 0, "%s", name); - case HAL_FLOAT: return hal_pin_new_real(comp_id, HAL_IN, &out->r, 0.0, "%s", name); - case HAL_S32: return hal_pin_new_si32(comp_id, HAL_IN, &out->s, 0, "%s", name); - case HAL_U32: return hal_pin_new_ui32(comp_id, HAL_IN, &out->u, 0, "%s", name); - default: break; + case HAL_BIT: *out = q.pp.value.b ? 1.0 : 0.0; break; + case HAL_S32: *out = q.pp.value.s; break; + case HAL_U32: *out = q.pp.value.u; break; + default: *out = q.pp.value.r; break; } - return -1; + return 0; } /* Does a pin of this name exist? Silent: absence is an answer, not an error. */ @@ -126,61 +96,24 @@ static int pin_exists(const char *pin_name) return hal_getref_p(&q) == 0; } -/* Bind pin_name; returns the cell index, or -1. */ -static int bind_pin(KinematicsUserContext *ctx, const char *pin_name, +/* Note pin_name for reading, once it has answered with the type; returns + its index, or -1. */ +static int note_pin(KinematicsUserContext *ctx, const char *pin_name, hal_type_t type) { - char signal[HAL_NAME_LEN + 1]; - char mine[HAL_NAME_LEN + 1]; - hal_refs_u *cell; - hal_query_t q; - int idx; - - memset(&q, 0, sizeof(q)); - q.name = pin_name; - q.qtype = HAL_QTYPE_PIN; + double value; - if (hal_getref_p(&q) != 0) { - fprintf(stderr, "kinematicsUserInit: no such pin '%s'\n", pin_name); - return -1; - } - if (q.pp.type != type) { - fprintf(stderr, "kinematicsUserInit: pin '%s' has the wrong type\n", + if (pin_value(pin_name, type, &value) != 0) { + fprintf(stderr, "kinematicsUserInit: no pin '%s' of the type expected\n", pin_name); return -1; } - - if (q.pp.signal) { - snprintf(signal, sizeof(signal), "%s", q.pp.signal); - } else if (make_signal(ctx, pin_name, type, signal, sizeof(signal))) { - fprintf(stderr, "kinematicsUserInit: cannot reach '%s'\n", pin_name); + if (ctx->num_pins >= MAX_PINS) { + fprintf(stderr, "kinematicsUserInit: too many pins to read\n"); return -1; } - - if ((size_t)snprintf(mine, sizeof(mine), "%s.%s", ctx->prefix, pin_name) - >= sizeof(mine)) { - fprintf(stderr, "kinematicsUserInit: pin name for '%s' too long\n", - pin_name); - return -1; - } - if (ctx->num_cells >= MAX_BOUND_PINS) { - fprintf(stderr, "kinematicsUserInit: too many pins to bind\n"); - return -1; - } - idx = ctx->num_cells; - cell = &ctx->cell[idx]; - - if (new_pin(ctx->comp_id, type, cell, mine) != 0) { - fprintf(stderr, "kinematicsUserInit: cannot create pin '%s'\n", mine); - return -1; - } - if (hal_link(mine, signal) != 0) { - fprintf(stderr, "kinematicsUserInit: cannot link '%s' to '%s'\n", - mine, signal); - return -1; - } - ctx->num_cells++; - return idx; + snprintf(ctx->pin_name[ctx->num_pins], sizeof(ctx->pin_name[0]), "%s", pin_name); + return ctx->num_pins++; } static hal_type_t hal_type_of(kins_param_type t) @@ -193,25 +126,15 @@ static hal_type_t hal_type_of(kins_param_type t) } } -static double cell_value(const hal_refs_u *cell, kins_param_type t) -{ - switch (t) { - case KINS_PARAM_BIT: return hal_get_bool(cell->b) ? 1.0 : 0.0; - case KINS_PARAM_S32: return hal_get_si32(cell->s); - case KINS_PARAM_U32: return hal_get_ui32(cell->u); - default: return hal_get_real(cell->r); - } -} - -/* Bind every input of the table, and motion's tool where motion is there. */ -static int bind_all(KinematicsUserContext *ctx) +/* Note every input of the table, and motion's tool where motion is there. */ +static int note_all(KinematicsUserContext *ctx) { static const char letter[AXIS_COUNT] = { 'x','y','z','a','b','c','u','v','w' }; char name[HAL_NAME_LEN + 1]; int i; - for (i = 0; i < KINS_MAX_PARAMS; i++) ctx->cell_of_param[i] = -1; - for (i = 0; i < AXIS_COUNT; i++) ctx->cell_of_tool[i] = -1; + for (i = 0; i < KINS_MAX_PARAMS; i++) ctx->pin_of_param[i] = -1; + for (i = 0; i < AXIS_COUNT; i++) ctx->pin_of_tool[i] = -1; ctx->tool_param = -1; for (i = 0; i < ctx->info.nparams; i++) { @@ -219,8 +142,8 @@ static int bind_all(KinematicsUserContext *ctx) if (d->dir == KINS_OUT) continue; if (d->tool) ctx->tool_param = i; snprintf(name, sizeof(name), "%s.%s", ctx->info.halprefix, d->name); - ctx->cell_of_param[i] = bind_pin(ctx, name, hal_type_of(d->type)); - if (ctx->cell_of_param[i] < 0) return -1; + ctx->pin_of_param[i] = note_pin(ctx, name, hal_type_of(d->type)); + if (ctx->pin_of_param[i] < 0) return -1; } /* motion publishes the tool it applies; take it from there when it is @@ -230,8 +153,8 @@ static int bind_all(KinematicsUserContext *ctx) for (i = 0; i < AXIS_COUNT; i++) { snprintf(name, sizeof(name), "motion.tooloffset.%c", letter[i]); if (!pin_exists(name)) continue; - ctx->cell_of_tool[i] = bind_pin(ctx, name, HAL_FLOAT); - if (ctx->cell_of_tool[i] < 0) return -1; + ctx->pin_of_tool[i] = note_pin(ctx, name, HAL_FLOAT); + if (ctx->pin_of_tool[i] < 0) return -1; } return 0; } @@ -244,10 +167,14 @@ static void refresh(KinematicsUserContext *ctx) double tool[AXIS_COUNT]; int have_motion_tool = 0; + /* a pin that stops answering, its module unloaded, keeps its last value */ for (i = 0; i < ctx->info.nparams; i++) { - int c = ctx->cell_of_param[i]; + int c = ctx->pin_of_param[i]; + double value; if (c < 0) continue; - ctx->params.geometry[i] = cell_value(&ctx->cell[c], ctx->info.params[i].type); + if (pin_value(ctx->pin_name[c], hal_type_of(ctx->info.params[i].type), &value) == 0) { + ctx->params.geometry[i] = value; + } } if (ctx->tool_param >= 0) { ctx->params.tool.tran.z = ctx->params.geometry[ctx->tool_param]; @@ -262,10 +189,10 @@ static void refresh(KinematicsUserContext *ctx) } for (i = 0; i < AXIS_COUNT; i++) { - int c = ctx->cell_of_tool[i]; + int c = ctx->pin_of_tool[i]; tool[i] = 0.0; if (c < 0) continue; - tool[i] = hal_get_real(ctx->cell[c].r); + if (pin_value(ctx->pin_name[c], HAL_FLOAT, &tool[i]) != 0) continue; have_motion_tool = 1; } if (!have_motion_tool) return; @@ -398,19 +325,12 @@ KinematicsUserContext* kinematicsUserInitSparm(const char* kins_type, ctx->num_joints = num_joints; ctx->comp_id = comp_id; - ctx->prefix = prefix; - - ctx->cell = (hal_refs_u *)hal_malloc(MAX_BOUND_PINS * sizeof(hal_refs_u)); - if (!ctx->cell) { - fprintf(stderr, "kinematicsUserInit: out of HAL memory\n"); - free(ctx); - return NULL; - } + (void)prefix; strncpy(ctx->module_name, kins_type, sizeof(ctx->module_name) - 1); if (load_module(ctx, kins_type, coordinates, sparm) == 0) { - if (bind_all(ctx) != 0) { - fprintf(stderr, "kinematicsUserInit: cannot bind the pins of '%s'\n", + if (note_all(ctx) != 0) { + fprintf(stderr, "kinematicsUserInit: cannot read the pins of '%s'\n", kins_type); ctx->rt_only = 1; } @@ -734,15 +654,8 @@ KinematicsUserContext* kinematicsUserInitString(const char* kinematics, void kinematicsUserFree(KinematicsUserContext* ctx) { - int i; - if (!ctx) return; - /* Removing one hands its value back to the RT pin, leaving the - machine as it was found. */ - for (i = 0; i < ctx->num_made_signals; i++) { - hal_signal_delete(ctx->made_signal[i]); - } if (ctx->rt_handle) dlclose(ctx->rt_handle); free(ctx); } diff --git a/src/emc/kinematics_userspace/kinematics_user.h b/src/emc/kinematics_userspace/kinematics_user.h index 6d9b07a0859..ff023113b95 100644 --- a/src/emc/kinematics_userspace/kinematics_user.h +++ b/src/emc/kinematics_userspace/kinematics_user.h @@ -47,15 +47,14 @@ typedef struct KinematicsUserContext KinematicsUserContext; /** * Initialize userspace kinematics context * - * The pins this creates belong to the caller's component, so call this - * after hal_init() and before hal_ready(): HAL refuses new pins once a - * component is ready. + * The module's pins are read through HAL, by name, so call this after + * hal_init(). Nothing is made in HAL: no pin, no signal. * * @param kins_type Kinematics module name (e.g., "trivkins", "5axiskins", "maxkins") * @param num_joints Number of joints in the machine * @param coordinates Coordinate string (e.g., "XYZABC", "XYZBCW") * @param comp_id Caller's HAL component, from hal_init() - * @param prefix Its name, which the created pin names start with + * @param prefix Its name * @return Allocated context, or NULL if kinematics type not supported */ KinematicsUserContext* kinematicsUserInit(const char* kins_type, From ad8f8c83344bfa62c54d5f8f272a228c353a47a6 Mon Sep 17 00:00:00 2001 From: Luca Toniolo <10792599+grandixximo@users.noreply.github.com> Date: Fri, 11 Sep 2026 20:25:25 +0800 Subject: [PATCH 43/60] kinematics: retire the KINS_NOT_SWITCHABLE macro Nothing uses it: every single-type module is on the parameter block and gets these entry points from kins_single.c, and the switchkins modules write their own. The man page now says so instead of pointing module authors at the macro. --- docs/src/motion/kinematics.adoc | 7 +++---- src/emc/kinematics/kinematics.h | 9 --------- 2 files changed, 3 insertions(+), 13 deletions(-) diff --git a/docs/src/motion/kinematics.adoc b/docs/src/motion/kinematics.adoc index 26f3e8a03d0..e1e25b57181 100644 --- a/docs/src/motion/kinematics.adoc +++ b/docs/src/motion/kinematics.adoc @@ -310,7 +310,6 @@ between joint numbers and axis letters when in joint mode ---- int kinematicsSwitchable(void) int kinematicsSwitch(int switchkins_type) -KINS_NOT_SWITCHABLE ---- The function kinematicsSwitchable() returns 1 if multiple @@ -320,9 +319,9 @@ See <>. [NOTE] The majority of provided kinematics modules support a single -kinematics type and use the directive "*KINS_NOT_SWITCHABLE*" to -supply defaults for the required kinematicsSwitchable() and -kinematicsSwitch() functions. +kinematics type; they are written on the parameter block and link +'kins_single.c', which supplies these entry points and answers "not +switchable" for them. ---- int kinematicsHome(EmcPose *world, double *joint, diff --git a/src/emc/kinematics/kinematics.h b/src/emc/kinematics/kinematics.h index 4984e1f1859..456257a78de 100644 --- a/src/emc/kinematics/kinematics.h +++ b/src/emc/kinematics/kinematics.h @@ -704,15 +704,6 @@ extern int kinematicsSetTool(const EmcPose *tool); // before/after invoking kinematicsSwitch() // A convenient command to synch is: M66 E0 L0 -#define KINS_NOT_SWITCHABLE \ -extern int kinematicsSwitchable() {return 0;} \ -extern int kinematicsSwitch(int switchkins_type) { (void)switchkins_type; return 0;} \ -extern int kinematicsTypeFlags(int ktype) { (void)ktype; return -1;} \ -EXPORT_SYMBOL(kinematicsSwitchable); \ -EXPORT_SYMBOL(kinematicsSwitch); \ -EXPORT_SYMBOL(kinematicsTypeFlags); - - // support for template for user-defined switchkins_type==2 extern const kins_ops USERK_OPS; From 7afc3d8ec7ca6483143f922c2bda24cff6a033be Mon Sep 17 00:00:00 2001 From: Luca Toniolo <10792599+grandixximo@users.noreply.github.com> Date: Fri, 4 Sep 2026 18:46:45 +1000 Subject: [PATCH 44/60] tests: put the nutating head kinematics next to the tilted work plane maths The two trsrn configs carry their orientation maths in python, written apart from the kinematics modules, and nothing had compared the two. tests/kins-twp loads each module in realtime with a small component answering frame and inverse requests over HAL pins and drives it from python holding the twp functions. Over a grid of primary, secondary and table angles the module's tool frame equals the python transformation matrix to 1e-9; for a set of requested tool axes with the table held as the remap holds it, the joint pairs the module finds are the python's candidates and the spin it reports is the python's virtual rotation. With nothing held the module may turn the table, which the python never does, so there each side is judged by the other's maths. --- tests/kins-twp/README | 9 ++ tests/kins-twp/check.py | 273 ++++++++++++++++++++++++++++++++++ tests/kins-twp/checkresult | 3 + tests/kins-twp/skip | 4 + tests/kins-twp/test.sh | 30 ++++ tests/kins-twp/twp-xyzacb.ini | 16 ++ tests/kins-twp/twp-xyzbca.ini | 16 ++ tests/kins-twp/twpcheck.c | 165 ++++++++++++++++++++ 8 files changed, 516 insertions(+) create mode 100644 tests/kins-twp/README create mode 100755 tests/kins-twp/check.py create mode 100755 tests/kins-twp/checkresult create mode 100755 tests/kins-twp/skip create mode 100755 tests/kins-twp/test.sh create mode 100644 tests/kins-twp/twp-xyzacb.ini create mode 100644 tests/kins-twp/twp-xyzbca.ini create mode 100644 tests/kins-twp/twpcheck.c diff --git a/tests/kins-twp/README b/tests/kins-twp/README new file mode 100644 index 00000000000..b9b3b7275fa --- /dev/null +++ b/tests/kins-twp/README @@ -0,0 +1,9 @@ +The C kinematics against the tilted work plane maths. + +The two nutating-head configs carry their orientation maths in python, +remap_funcs_twp.py, written independently of the kinematics modules. +This test loads each module in realtime and puts its tool frame next to +the python transformation matrix over a grid of head angles, and its +tool frame inverse next to the python candidate joint angles and virtual +rotation for a set of requested tool axes. Where they disagree, one of +the two has the sign or the order of a rotation wrong. diff --git a/tests/kins-twp/check.py b/tests/kins-twp/check.py new file mode 100755 index 00000000000..2b8d52dd683 --- /dev/null +++ b/tests/kins-twp/check.py @@ -0,0 +1,273 @@ +#!/usr/bin/env python3 +# The python half of the tilted work plane cross-check. +# +# Imports the machine's remap_funcs_twp.py, the maths the tilted work +# plane remap orients the head with, and drives twpcheck, loaded after +# the kinematics module, to get the module's answers to the same +# questions. Two comparisons: +# +# frames over a grid of primary angle, secondary angle, virtual +# rotation and table angle, the module's tool frame in +# machine coordinates against the python transformation +# matrix Rp * Rs * Rtc +# inverse for a set of requested tool axes, with the table held as +# the remap holds it, the joint angle pairs the module's +# kinematicsToolFrameInverse() finds against the pairs the +# python candidate search keeps, and the spin about the tool +# the module reports for the python's horizontal tool x +# against the python's own virtual rotation; then with nothing +# held, where the module may turn the table, each side judged +# by the other's maths +# +# Usage: check.py MACHINE CONFIGDIR INIFILE +# MACHINE is xyzacb or xyzbca; CONFIGDIR holds remap_funcs_twp.py; +# INIFILE is what that file reads its letters and limits from. + +import os +import sys +import time +from math import radians, degrees, pi, sin, cos, atan2 + +import numpy as np +import hal + +machine, cfgdir, inifile = sys.argv[1:4] +os.environ["INI_FILE_NAME"] = inifile +sys.path.insert(0, cfgdir) +import remap_funcs_twp as twp + +# joint numbers: the table, the secondary and the primary rotary +TABLE, SECONDARY, PRIMARY = {"xyzacb": (3, 4, 5), "xyzbca": (4, 3, 5)}[machine] +PREROT = "%s_trsrn_kins.pre-rot" % machine +TOL = 1e-9 +ANGLE_TOL = 1e-6 # degrees + +failures = 0 +def fail(what): + global failures + failures += 1 + print("kins-twp: FAIL %s: %s" % (machine, what)) + +class Log: + def debug(self, *a, **k): pass + def error(self, *a, **k): print("kins-twp: python error:", a[0] % tuple(a[1:]) if len(a) > 1 else a[0]) +log = Log() + +# ---- driving twpcheck + +request = 0 +def ask(j, axis=None, xdir=None, held=0): + """set the joints and the request, wait for the answer""" + global request + for i, v in enumerate(j): + hal.set_p("twpcheck.j-%d" % i, str(v)) + hal.set_p("twpcheck.held", str(held)) + for i, c in enumerate("xyz"): + hal.set_p("twpcheck.axis-%s" % c, str(axis[i] if axis is not None else 0.0)) + hal.set_p("twpcheck.xdir-%s" % c, str(xdir[i] if xdir is not None else 0.0)) + hal.set_p("twpcheck.have-x", "1" if xdir is not None else "0") + request += 1 + hal.set_p("twpcheck.request", str(request)) + deadline = time.time() + 5 + while hal.get_value("twpcheck.done") != request: + if time.time() > deadline: + print("kins-twp: FAIL twpcheck did not answer") + sys.exit(1) + time.sleep(0.002) + +def read_matrix(name): + return np.array([[hal.get_value("twpcheck.%s-%d%d" % (name, r, c)) for c in range(3)] + for r in range(3)]) + +def read_solutions(): + n = hal.get_value("twpcheck.nsol") + sols = [] + for k in range(max(n, 0)): + sols.append(([hal.get_value("twpcheck.sol-%d-%d" % (k, i)) for i in range(6)], + hal.get_value("twpcheck.spin-%d" % k), + hal.get_value("twpcheck.free-%d" % k))) + return n, sols + +def joints_at(table, secondary, primary): + j = [10.0, 20.0, 30.0, 0.0, 0.0, 0.0] + j[TABLE], j[SECONDARY], j[PRIMARY] = table, secondary, primary + return j + +# ---- the python's answers + +def py_matrix(primary_deg, secondary_deg, tc): + m = twp.kins_calc_transformation_matrix(radians(primary_deg), radians(secondary_deg), tc, + np.asmatrix(np.identity(4)), 'inv') + return np.array(m)[:3, :3] + +def py_pairs(z): + """the (primary, secondary) pairs in degrees the remap would keep for a + tool axis, following remap.py: every combination of the candidate + lists, kept where it reaches the axis""" + t1, t2 = twp.kins_calc_possible_joint_angles(log, np.array(z), None) + if t1 is None or t2 is None: + return [] + pairs = [] + for a in set(t1): + for b in set(t2): + m = py_matrix(degrees(a), degrees(b), 0.0) + if np.allclose(m[:, 2], z, atol=1e-6): + pairs.append((degrees(a), degrees(b))) + return pairs + +def same_angle(a, b): + d = (a - b + 180.0) % 360.0 - 180.0 + return abs(d) < ANGLE_TOL + +def same_pair(p, q): + return same_angle(p[0], q[0]) and same_angle(p[1], q[1]) + +def fmt(m): + return np.array2string(m, precision=6, suppress_small=True) + +# ---- frames +# +# The module's frame is the head's rotation from its joints alone, so it +# is compared with the python matrix at zero virtual rotation; whether the +# frame should carry the virtual rotation too is a convention question the +# test does not settle. + +frames = 0 +hal.set_p(PREROT, "0") +for table in (0.0, 20.0): + for primary in (0.0, 30.0, -25.0, 90.0, 180.0, -135.0): + for secondary in (0.0, 30.0, -25.0, 90.0, -90.0, 180.0): + ask(joints_at(table, secondary, primary)) + if hal.get_value("twpcheck.frame-rc") != 0: + fail("no frame at primary %g secondary %g" % (primary, secondary)) + continue + tool = read_matrix("tool") + want = py_matrix(primary, secondary, 0.0) + frames += 1 + if not np.allclose(tool, want, atol=TOL): + fail("tool frame differs at primary %g secondary %g table %g\n module:\n%s\n python:\n%s" + % (primary, secondary, table, fmt(tool), fmt(want))) + +# ---- inverse, the table held +# +# The remap holds the table and orients the head, so ask the module the +# same: the joint pairs must then be the python's, and the spin about the +# tool for the python's horizontal tool x must be the python's virtual +# rotation. + +def rz(a): + return np.array([[cos(a), -sin(a), 0.0], [sin(a), cos(a), 0.0], [0.0, 0.0, 1.0]]) + +def frames_at(j): + ask(j) + return read_matrix("work"), read_matrix("tool") + +def in_work(work, tool): + return work.T @ tool + +HOLD_TABLE = 1 << TABLE +REQUESTS = ((30.0, 30.0), (-25.0, 60.0), (120.0, -45.0), (180.0, 90.0), + (0.0, 0.0), (90.0, 135.0), (45.0, 170.0), (-100.0, -20.0)) + +requests = 0 +for primary, secondary in REQUESTS: + z = py_matrix(primary, secondary, 0.0)[:, 2] + pairs = py_pairs(list(z)) + seed = joints_at(0.0, 0.0, 0.0) + where = "axis %s (from primary %g secondary %g)" % (fmt(z), primary, secondary) + if not any(same_pair(p, (primary, secondary)) for p in pairs): + fail("the python does not find the pair (%g, %g) the axis was made from" % (primary, secondary)) + + ask(seed, axis=z, held=HOLD_TABLE) + n, sols = read_solutions() + requests += 1 + if n <= 0: + fail("with the table held, the module finds no solution for " + where) + continue + found = [(s[0][PRIMARY], s[0][SECONDARY]) for s in sols] + for s in sols: + j, spin, free = s + if abs(j[TABLE] - seed[TABLE]) > 1e-12: + fail("the held table moved for " + where) + if free != 0 and (primary, secondary) != (0.0, 0.0): + fail("with the table held a solution is still a family for " + where) + for p in pairs: + if not any(same_pair(p, f) for f in found): + fail("python pair (%.6f, %.6f) not among the module's %s for %s" + % (p[0], p[1], ["(%.6f, %.6f)" % f for f in found], where)) + for f in found: + if not any(same_pair(p, f) for p in pairs): + fail("module pair (%.6f, %.6f) not among the python's %s for %s" + % (f[0], f[1], ["(%.6f, %.6f)" % p for p in pairs], where)) + + # tool x as the python's virtual rotation places it, horizontal: the + # module, holding the table, must answer the same pair with that spin + for p in pairs: + tc = twp.kins_calc_virtual_rot_for_g683(radians(p[0]), radians(p[1])) + full = py_matrix(p[0], p[1], tc) + if abs(full[2, 0]) > 1e-9: + fail("python virtual rotation %g leaves tool x off horizontal for pair (%.6f, %.6f)" % (tc, p[0], p[1])) + ask(seed, axis=z, xdir=full[:, 0], held=HOLD_TABLE) + n, sols = read_solutions() + requests += 1 + match = [s for s in sols if same_pair((s[0][PRIMARY], s[0][SECONDARY]), p)] + if not match: + fail("with tool x given and the table held, pair (%.6f, %.6f) is gone from the module's answers" % p) + continue + spin = match[0][1] + if abs((spin - tc + pi) % (2 * pi) - pi) > 1e-6: + fail("module spin %.9f and python virtual rotation %.9f differ for pair (%.6f, %.6f)" + % (spin, tc, p[0], p[1])) + +# ---- inverse, nothing held +# +# The module may now turn the table, since it turns the tool against the +# work as surely as the head does, and reports one member of the family +# that results. Not the python's answer, so each is judged by the other's +# maths: a module solution must reach the axis through the python head +# matrix composed with the module's table frame, and with tool x given it +# must reach the whole frame. + +for primary, secondary in REQUESTS: + z = py_matrix(primary, secondary, 0.0)[:, 2] + pairs = py_pairs(list(z)) + seed = joints_at(0.0, 0.0, 0.0) + where = "axis %s (from primary %g secondary %g)" % (fmt(z), primary, secondary) + + ask(seed, axis=z) + n, sols = read_solutions() + requests += 1 + if n <= 0: + fail("the module finds no solution for " + where) + continue + for s in sols: + j, spin, free = s + work, tool = frames_at(j) + if not np.allclose(tool, py_matrix(j[PRIMARY], j[SECONDARY], 0.0), atol=TOL): + fail("module frame at its own solution differs from the python head matrix for " + where) + if not np.allclose(in_work(work, py_matrix(j[PRIMARY], j[SECONDARY], 0.0))[:, 2], z, atol=1e-6): + fail("module solution %s does not reach %s by the python head matrix" % (fmt(np.array(j)), where)) + for i in (0, 1, 2): + if abs(j[i] - seed[i]) > 1e-9: + fail("solution moved linear joint %d for %s" % (i, where)) + + for p in pairs: + tc = twp.kins_calc_virtual_rot_for_g683(radians(p[0]), radians(p[1])) + full = py_matrix(p[0], p[1], tc) + ask(seed, axis=z, xdir=full[:, 0]) + n, sols = read_solutions() + requests += 1 + if n <= 0: + fail("the module finds no solution with tool x given for pair (%.6f, %.6f)" % p) + continue + for s in sols: + j, spin, free = s + work, tool = frames_at(j) + achieved = in_work(work, py_matrix(j[PRIMARY], j[SECONDARY], 0.0) @ rz(spin)) + if not np.allclose(achieved, full, atol=1e-6): + fail("module solution %s spin %.6f does not reach the python frame for pair (%.6f, %.6f)\n achieved:\n%s\n wanted:\n%s" + % (fmt(np.array(j)), spin, p[0], p[1], fmt(achieved), fmt(full))) + +if failures: + sys.exit(1) +print("kins-twp: %s agrees, %d frames, %d requests" % (machine, frames, requests)) diff --git a/tests/kins-twp/checkresult b/tests/kins-twp/checkresult new file mode 100755 index 00000000000..011ea9232ad --- /dev/null +++ b/tests/kins-twp/checkresult @@ -0,0 +1,3 @@ +#!/bin/sh +[ "$(grep -c 'kins-twp: .* agrees' "$1")" = 2 ] \ + && ! grep -q "FAIL" "$1" diff --git a/tests/kins-twp/skip b/tests/kins-twp/skip new file mode 100755 index 00000000000..a12f31a77c2 --- /dev/null +++ b/tests/kins-twp/skip @@ -0,0 +1,4 @@ +#!/bin/sh +# Builds a realtime component with halcompile, which needs the build +# tools present. Skip when testing installed packages. +[ -z "$SYSTEM_BUILD" ] diff --git a/tests/kins-twp/test.sh b/tests/kins-twp/test.sh new file mode 100755 index 00000000000..86b77f6ff44 --- /dev/null +++ b/tests/kins-twp/test.sh @@ -0,0 +1,30 @@ +#!/bin/bash +set -e + +# RIP layout: $HEADERS is $TOPDIR/include +TOPDIR=$(dirname "$HEADERS") +CONFIGS=$TOPDIR/configs/sim/axis/vismach/5axis/table-rotary_spindle-rotary-nutating + +${SUDO} halcompile --install twpcheck.c >/dev/null + +# One hal file per machine. twpcheck answers frame and inverse requests +# from check.py over HAL pins; check.py holds the python maths. +run() { + local machine=$1 hal + hal=$(mktemp --suffix=.hal) + { printf 'loadrt %s_trsrn\n' "$machine" + printf 'setp %s_trsrn_kins.nut-angle 45\n' "$machine" + printf 'loadrt twpcheck joints=6 ktype=1\n' + printf 'loadrt threads name1=t1 period1=1000000\n' + printf 'addf twpcheck t1\n' + printf 'start\n' + printf 'loadusr -w python3 check.py %s %s/%s-trsrn_twp %s/twp-%s.ini\n' \ + "$machine" "$CONFIGS" "$machine" "$PWD" "$machine" + } > "$hal" + echo "=== $machine" + halrun -f "$hal" + rm -f "$hal" +} + +run xyzacb +run xyzbca diff --git a/tests/kins-twp/twp-xyzacb.ini b/tests/kins-twp/twp-xyzacb.ini new file mode 100644 index 00000000000..7d276d86bf8 --- /dev/null +++ b/tests/kins-twp/twp-xyzacb.ini @@ -0,0 +1,16 @@ +# what remap_funcs_twp.py reads: the primary and secondary letters, their +# limits, and the module name its pins hang off +[KINS] +KINEMATICS = xyzacb_trsrn + +[TWP] +PRIMARY = C +SECONDARY = B + +[AXIS_C] +MIN_LIMIT = -181 +MAX_LIMIT = 181 + +[AXIS_B] +MIN_LIMIT = -181 +MAX_LIMIT = 181 diff --git a/tests/kins-twp/twp-xyzbca.ini b/tests/kins-twp/twp-xyzbca.ini new file mode 100644 index 00000000000..b6bfd198e60 --- /dev/null +++ b/tests/kins-twp/twp-xyzbca.ini @@ -0,0 +1,16 @@ +# what remap_funcs_twp.py reads: the primary and secondary letters, their +# limits, and the module name its pins hang off +[KINS] +KINEMATICS = xyzbca_trsrn + +[TWP] +PRIMARY = C +SECONDARY = A + +[AXIS_C] +MIN_LIMIT = -181 +MAX_LIMIT = 181 + +[AXIS_A] +MIN_LIMIT = -181 +MAX_LIMIT = 181 diff --git a/tests/kins-twp/twpcheck.c b/tests/kins-twp/twpcheck.c new file mode 100644 index 00000000000..90a86025569 --- /dev/null +++ b/tests/kins-twp/twpcheck.c @@ -0,0 +1,165 @@ +/* + * twpcheck: the realtime half of the tilted work plane cross-check. + * + * Loaded after a kinematics module, it answers requests made over HAL + * pins: for the joint values on its inputs it reports the module's tool + * frame and work frame, and for the tool axis (and optionally tool x) + * on its inputs it reports what kinematicsToolFrameInverse() finds, the + * joint sets and the spin about the tool each needs, with the joints + * named on the held pin kept where they are. check.py drives it and + * holds the python maths the answers are compared with. + * + * A request is made by raising the request pin; done follows it when + * the answers are on the pins. + * + * Module parameters + * joints joint count the module was loaded for + * ktype switchkins type to select first, 0 for none + */ +#include +#include +#include +#include +#include +#include + +MODULE_LICENSE("GPL"); + +static int joints = 6; +RTAPI_MP_INT(joints, "joint count the module under test was loaded for"); +static int ktype = 0; +RTAPI_MP_INT(ktype, "switchkins type to select first"); + +static int comp_id = -1; + +#define NSOL TOOL_FRAME_MAX_SOLUTIONS + +static struct { + hal_real_t j[EMCMOT_MAX_JOINTS]; + hal_real_t axis[3]; + hal_real_t xdir[3]; + hal_bool_t have_x; + hal_uint_t held; /* bit per joint the inverse may not move */ + hal_uint_t request; + hal_uint_t done; + hal_real_t tool[3][3]; /* [row][column], columns are the frame's axes */ + hal_real_t work[3][3]; + hal_sint_t frame_rc; + hal_sint_t nsol; + hal_real_t sol[NSOL][EMCMOT_MAX_JOINTS]; + hal_real_t spin[NSOL]; + hal_sint_t free[NSOL]; +} *pins; + +static void publish(hal_real_t out[3][3], const PmRotationMatrix *m) +{ + hal_set_real(out[0][0], m->x.x); hal_set_real(out[0][1], m->y.x); hal_set_real(out[0][2], m->z.x); + hal_set_real(out[1][0], m->x.y); hal_set_real(out[1][1], m->y.y); hal_set_real(out[1][2], m->z.y); + hal_set_real(out[2][0], m->x.z); hal_set_real(out[2][1], m->y.z); hal_set_real(out[2][2], m->z.z); +} + +static void update(void *arg, long period) +{ + KINEMATICS_FORWARD_FLAGS ff = 0; + PmRotationMatrix tool, work; + PmCartesian axis, xdir; + double j[EMCMOT_MAX_JOINTS]; + double sols[NSOL * EMCMOT_MAX_JOINTS]; /* rows of joints doubles, packed */ + double spin[NSOL]; + int freed[NSOL]; + int i, k, n, rc; + (void)arg; + (void)period; + + if (hal_get_ui32(pins->request) == hal_get_ui32(pins->done)) { return; } + + for (i = 0; i < EMCMOT_MAX_JOINTS; i++) { + j[i] = i < joints ? hal_get_real(pins->j[i]) : 0.0; + } + + rc = kinematicsToolFrame(j, &tool, &ff); + if (!rc) { rc = kinematicsWorkFrame(j, &work, &ff); } + hal_set_si32(pins->frame_rc, rc); + if (!rc) { + publish(pins->tool, &tool); + publish(pins->work, &work); + } + + axis.x = hal_get_real(pins->axis[0]); + axis.y = hal_get_real(pins->axis[1]); + axis.z = hal_get_real(pins->axis[2]); + xdir.x = hal_get_real(pins->xdir[0]); + xdir.y = hal_get_real(pins->xdir[1]); + xdir.z = hal_get_real(pins->xdir[2]); + n = -1; + if (axis.x != 0 || axis.y != 0 || axis.z != 0) { + n = kinematicsToolFrameInverse(&axis, hal_get_bool(pins->have_x) ? &xdir : NULL, + j, hal_get_ui32(pins->held), sols, NSOL, + freed, spin); + } + hal_set_si32(pins->nsol, n); + for (k = 0; k < NSOL; k++) { + for (i = 0; i < joints; i++) { + hal_set_real(pins->sol[k][i], k < n ? sols[k * joints + i] : 0.0); + } + hal_set_real(pins->spin[k], k < n ? spin[k] : 0.0); + hal_set_si32(pins->free[k], k < n ? freed[k] : 0); + } + + hal_set_ui32(pins->done, hal_get_ui32(pins->request)); +} + +int rtapi_app_main(void) +{ + static const char letter[3] = { 'x', 'y', 'z' }; + int i, k, r, res = 0; + + if (joints < 1 || joints > EMCMOT_MAX_JOINTS) { return -1; } + + comp_id = hal_init("twpcheck"); + if (comp_id < 0) { return comp_id; } + + pins = hal_malloc(sizeof(*pins)); + if (!pins) { hal_exit(comp_id); return -1; } + + for (i = 0; i < joints; i++) { + res += hal_pin_new_real(comp_id, HAL_IN, &pins->j[i], 0.0, "twpcheck.j-%d", i); + } + for (i = 0; i < 3; i++) { + res += hal_pin_new_real(comp_id, HAL_IN, &pins->axis[i], 0.0, "twpcheck.axis-%c", letter[i]); + res += hal_pin_new_real(comp_id, HAL_IN, &pins->xdir[i], 0.0, "twpcheck.xdir-%c", letter[i]); + } + res += hal_pin_new_bool(comp_id, HAL_IN, &pins->have_x, 0, "twpcheck.have-x"); + res += hal_pin_new_ui32(comp_id, HAL_IN, &pins->held, 0, "twpcheck.held"); + res += hal_pin_new_ui32(comp_id, HAL_IN, &pins->request, 0, "twpcheck.request"); + res += hal_pin_new_ui32(comp_id, HAL_OUT, &pins->done, 0, "twpcheck.done"); + for (r = 0; r < 3; r++) { + for (i = 0; i < 3; i++) { + res += hal_pin_new_real(comp_id, HAL_OUT, &pins->tool[r][i], 0.0, "twpcheck.tool-%d%d", r, i); + res += hal_pin_new_real(comp_id, HAL_OUT, &pins->work[r][i], 0.0, "twpcheck.work-%d%d", r, i); + } + } + res += hal_pin_new_si32(comp_id, HAL_OUT, &pins->frame_rc, 0, "twpcheck.frame-rc"); + res += hal_pin_new_si32(comp_id, HAL_OUT, &pins->nsol, 0, "twpcheck.nsol"); + for (k = 0; k < NSOL; k++) { + for (i = 0; i < joints; i++) { + res += hal_pin_new_real(comp_id, HAL_OUT, &pins->sol[k][i], 0.0, "twpcheck.sol-%d-%d", k, i); + } + res += hal_pin_new_real(comp_id, HAL_OUT, &pins->spin[k], 0.0, "twpcheck.spin-%d", k); + res += hal_pin_new_si32(comp_id, HAL_OUT, &pins->free[k], 0, "twpcheck.free-%d", k); + } + if (res) { hal_exit(comp_id); return -1; } + + if (ktype > 0 && kinematicsSwitchable()) { + if (kinematicsSwitch(ktype)) { hal_exit(comp_id); return -1; } + } + + if (hal_export_funct("twpcheck", update, NULL, 1, 0, comp_id)) { + hal_exit(comp_id); + return -1; + } + hal_ready(comp_id); + return 0; +} + +void rtapi_app_exit(void) { hal_exit(comp_id); } From b1313c5b5bc9f1699d2812bc36c84f95cb31957d Mon Sep 17 00:00:00 2001 From: Luca Toniolo <10792599+grandixximo@users.noreply.github.com> Date: Sat, 5 Sep 2026 00:22:08 +1000 Subject: [PATCH 45/60] motion: add the point-to-point move, interpolated in joint space A segment whose endpoint is Cartesian but whose interpolation is in joint space: the inverse runs once, at the endpoint, when the move is queued, and the planner runs every joint from where the queue ends to there together, the slowest setting the pace. Its length is the joint space distance with the tightest per-joint velocity, acceleration and jerk limits scaled onto it. Nothing blends into or out of it. While it runs the servo thread takes the joints from the planner and reports the tool from the forward kinematics; a forward that fails, as an iterating one can at the singularity the move is there to cross, leaves the last solved position and carte_pos_cmd_ok cleared. This is how a program crosses a singularity or flips a head without leaving its coordinate system. The move is a rapid with the rapid override or a feed that takes a given time with the feed override; the endpoint can also be given as joints, whose world position motion finds with the forward. Canon JOINT_TRAVERSE and JOINT_FEED, NML EMC_TRAJ_JOINT_MOVE and EMCMOT_SET_JOINT_LINE carry it; the preview draws the straight line between its ends. An external offset cannot ride on a joint segment: the queue refuses the segment while one is applied, and a request arriving while one is queued waits until the last joint segment is done. A queue reset forgets the joint-space bookkeeping with the queue. --- src/emc/motion/command.c | 158 ++++++++++++++++++++++++++++ src/emc/motion/control.c | 34 +++++- src/emc/motion/motion.h | 8 ++ src/emc/nml_intf/canon.hh | 18 ++++ src/emc/nml_intf/emc.cc | 15 +++ src/emc/nml_intf/emc.hh | 2 + src/emc/nml_intf/emc_nml.hh | 21 ++++ src/emc/rs274ngc/gcodemodule.cc | 22 ++++ src/emc/sai/saicanon.cc | 43 ++++++++ src/emc/task/emccanon.cc | 41 ++++++++ src/emc/task/emctaskmain.cc | 12 +++ src/emc/task/taskintf.cc | 17 +++ src/emc/tp/tc.c | 74 ++++++++++++- src/emc/tp/tc.h | 1 + src/emc/tp/tc_types.h | 20 +++- src/emc/tp/tp.c | 181 +++++++++++++++++++++++++++++++- src/emc/tp/tp.h | 9 ++ src/emc/tp/tp_types.h | 11 ++ 18 files changed, 682 insertions(+), 5 deletions(-) diff --git a/src/emc/motion/command.c b/src/emc/motion/command.c index 9cedc31f759..636bd9c11d4 100644 --- a/src/emc/motion/command.c +++ b/src/emc/motion/command.c @@ -1130,6 +1130,164 @@ void emcmotCommandHandler_locked(void *arg, long servo_period) } break; + case EMCMOT_SET_JOINT_LINE: { + /* a move interpolated in joint space to a Cartesian endpoint: the + inverse runs once here, at the endpoint, and the planner takes + the joints from there; or the endpoint is given as joints and + the forward says where that is */ + double start[EMCMOT_MAX_JOINTS], target[EMCMOT_MAX_JOINTS]; + EmcPose end = emcmotCommand->pos; + double length = 0.0, vmax = 0.0, amax = 0.0, jmax = 0.0; + int moving = 0, jerk_limited = 0, bad = 0, axis_num; + + rtapi_print_msg(RTAPI_MSG_DBG, "SET_JOINT_LINE"); + if (!GET_MOTION_COORD_FLAG() || !GET_MOTION_ENABLE_FLAG()) { + reportError(_("need to be enabled, in coord mode for joint interpolated move")); + emcmotStatus->commandStatus = EMCMOT_COMMAND_INVALID_COMMAND; + SET_MOTION_ERROR_FLAG(1); + break; + } + if (!limits_ok()) { + reportError(_("can't do joint interpolated move with limits exceeded")); + emcmotStatus->commandStatus = EMCMOT_COMMAND_INVALID_PARAMS; + tpAbort(&emcmotInternal->coord_tp); + SET_MOTION_ERROR_FLAG(1); + break; + } + /* an external offset is applied to the world position on the way + to the inverse every cycle, which a joint interpolated segment + does not go through */ + for (axis_num = 0; axis_num < EMCMOT_MAX_AXIS; axis_num++) { + if (axis_get_ext_offset_curr_pos(axis_num) != 0.0) { bad = 1; } + } + if (bad) { + reportError(_("can't do joint interpolated move on line %d with an external offset applied"), + emcmotCommand->id); + emcmotStatus->commandStatus = EMCMOT_COMMAND_INVALID_PARAMS; + tpAbort(&emcmotInternal->coord_tp); + SET_MOTION_ERROR_FLAG(1); + break; + } + + /* where the queue ends in joint space */ + if (!tpGetQueueEndJoints(&emcmotInternal->coord_tp, start)) { + EmcPose goal; + tpGetGoalPos(&emcmotInternal->coord_tp, &goal); + for (joint_num = 0; joint_num < EMCMOT_MAX_JOINTS; joint_num++) { + start[joint_num] = (joint_num < ALL_JOINTS) ? joints[joint_num].pos_cmd : 0.0; + } + if (kinematicsInverse(&goal, start, &iflags, &fflags) != 0) { + reportError(_("joint interpolated move on line %d: the queue end fails kinematicsInverse"), + emcmotCommand->id); + emcmotStatus->commandStatus = EMCMOT_COMMAND_INVALID_PARAMS; + tpAbort(&emcmotInternal->coord_tp); + SET_MOTION_ERROR_FLAG(1); + break; + } + } + + for (joint_num = 0; joint_num < EMCMOT_MAX_JOINTS; joint_num++) { target[joint_num] = start[joint_num]; } + if (emcmotCommand->have_joint_target) { + for (joint_num = 0; joint_num < NO_OF_KINS_JOINTS; joint_num++) { + target[joint_num] = emcmotCommand->joint_target[joint_num]; + } + if (kinematicsForward(target, &end, &fflags, &iflags) != 0) { + reportError(_("joint interpolated move on line %d fails kinematicsForward"), + emcmotCommand->id); + emcmotStatus->commandStatus = EMCMOT_COMMAND_INVALID_PARAMS; + tpAbort(&emcmotInternal->coord_tp); + SET_MOTION_ERROR_FLAG(1); + break; + } + } else { + if (!inRange(end, emcmotCommand->id, "Joint interpolated")) { + reportError(_("invalid params in joint interpolated move")); + emcmotStatus->commandStatus = EMCMOT_COMMAND_INVALID_PARAMS; + tpAbort(&emcmotInternal->coord_tp); + SET_MOTION_ERROR_FLAG(1); + break; + } + if (kinematicsInverse(&end, target, &iflags, &fflags) != 0) { + reportError(_("joint interpolated move on line %d fails kinematicsInverse"), + emcmotCommand->id); + emcmotStatus->commandStatus = EMCMOT_COMMAND_INVALID_PARAMS; + tpAbort(&emcmotInternal->coord_tp); + SET_MOTION_ERROR_FLAG(1); + break; + } + } + + /* the endpoint must be inside the joint limits, and every joint + that moves needs limits to move within; the segment length is + the joint space distance and each joint's limits are scaled + onto it so that the slowest joint sets the pace */ + for (joint_num = 0; joint_num < NO_OF_KINS_JOINTS; joint_num++) { + double d = target[joint_num] - start[joint_num]; + joint = &joints[joint_num]; + if (!GET_JOINT_ACTIVE_FLAG(joint)) { continue; } + if (!isfinite(target[joint_num])) { + reportError(_("joint interpolated move on line %d gave non-finite joint location on joint %d"), + emcmotCommand->id, joint_num); + bad = 1; + } else if (target[joint_num] > joint->max_pos_limit || target[joint_num] < joint->min_pos_limit) { + reportError(_("joint interpolated move on line %d would exceed joint %d's limit"), + emcmotCommand->id, joint_num); + bad = 1; + } + length += d * d; + } + length = sqrt(length); + for (joint_num = 0; joint_num < NO_OF_KINS_JOINTS && !bad; joint_num++) { + double d = fabs(target[joint_num] - start[joint_num]); + joint = &joints[joint_num]; + if (!GET_JOINT_ACTIVE_FLAG(joint) || d < TP_POS_EPSILON) { continue; } + if (joint->vel_limit <= 0.0 || joint->acc_limit <= 0.0) { + reportError(_("joint interpolated move on line %d: joint %d has no velocity or acceleration limit"), + emcmotCommand->id, joint_num); + bad = 1; + break; + } + if (!moving || joint->vel_limit * length / d < vmax) { vmax = joint->vel_limit * length / d; } + if (!moving || joint->acc_limit * length / d < amax) { amax = joint->acc_limit * length / d; } + if (joint->jerk_limit > 0.0) { + if (!jerk_limited || joint->jerk_limit * length / d < jmax) { jmax = joint->jerk_limit * length / d; } + jerk_limited = 1; + } + moving = 1; + } + if (bad) { + emcmotStatus->commandStatus = EMCMOT_COMMAND_INVALID_PARAMS; + tpAbort(&emcmotInternal->coord_tp); + SET_MOTION_ERROR_FLAG(1); + break; + } + + /* a feed asks for a time; the joint limits still cap it */ + double vreq = vmax; + if (emcmotCommand->joint_seconds > 0.0 && length / emcmotCommand->joint_seconds < vmax) { + vreq = length / emcmotCommand->joint_seconds; + } + tpSetId(&emcmotInternal->coord_tp, emcmotCommand->id); + int res_addjoint = tpAddJointLine(&emcmotInternal->coord_tp, + start, target, NO_OF_KINS_JOINTS, end, + emcmotCommand->motion_type, + vreq, vmax, amax, jmax, + emcmotStatus->enables_new, + emcmotCommand->tag); + if (res_addjoint < 0) { + reportError(_("can't add joint interpolated move at line %d, error code %d"), + emcmotCommand->id, res_addjoint); + emcmotStatus->commandStatus = EMCMOT_COMMAND_BAD_EXEC; + tpAbort(&emcmotInternal->coord_tp); + SET_MOTION_ERROR_FLAG(1); + break; + } else if (res_addjoint == 0) { + SET_MOTION_ERROR_FLAG(0); + rehomeAll = 1; + } + break; + } + case EMCMOT_SET_CIRCLE: /* emcmotInternal->coord_tp up a circular move */ /* requires coordinated mode, enable on, not on limits */ diff --git a/src/emc/motion/control.c b/src/emc/motion/control.c index b509a133d3e..35af182d3d3 100644 --- a/src/emc/motion/control.c +++ b/src/emc/motion/control.c @@ -1427,10 +1427,41 @@ static void get_pos_cmds(long period) emcmotStatus->syncOverrunSpindle = 0; SET_MOTION_ERROR_FLAG(1); } + + if (tpGetJointPos(&emcmotInternal->coord_tp, positions) > 0) { + /* a joint interpolated segment: the planner hands out the + joints and the forward kinematics says where the tool is, + for status and for the display; nothing is inverted, and + the planner's own position is the chord between the ends. + The joints are commanded either way; a forward that fails, + as an iterating one can at a singularity, leaves the last + solved position reported rather than an unsolved one */ + EmcPose pose = emcmotStatus->carte_pos_cmd; + if (kinematicsForward(positions, &pose, &fflags, &iflags) == 0) { + emcmotStatus->carte_pos_cmd = pose; + emcmotStatus->carte_pos_cmd_ok = 1; + } else { + emcmotStatus->carte_pos_cmd_ok = 0; + } + result = 0; + } else { + /* a joint interpolated segment that ended this cycle is gone + from the queue: its end joints seed the inverse, since the + modules that read their rotary angles from the seed would + otherwise get last cycle's */ + tpTakeJointEnd(&emcmotInternal->coord_tp, positions); /* get new commanded traj pos */ tpGetPos(&emcmotInternal->coord_tp, &emcmotStatus->carte_pos_cmd); - if (axis_update_coord_with_bound(pcmd_p, servo_period)) { + if (tpJointSegmentsQueued(&emcmotInternal->coord_tp)) { + /* an external offset cannot ride on a joint interpolated + segment: its joints were solved without one, and the + queue refused the segment while one was applied. A + request that arrives while one is queued waits here, + unplanned, and ramps in at its own limits once the last + joint segment is done, instead of landing as a step at + the segment's ends */ + } else if (axis_update_coord_with_bound(pcmd_p, servo_period)) { ext_offset_coord_limit = 1; } else { ext_offset_coord_limit = 0; @@ -1439,6 +1470,7 @@ static void get_pos_cmds(long period) /* OUTPUT KINEMATICS - convert to joints in local array */ result = kinematicsInverse(&emcmotStatus->carte_pos_cmd, positions, &iflags, &fflags); + } if(result == 0) { /* copy to joint structures and spline them up */ diff --git a/src/emc/motion/motion.h b/src/emc/motion/motion.h index 06ff25eb0ba..9525f91d4d6 100644 --- a/src/emc/motion/motion.h +++ b/src/emc/motion/motion.h @@ -116,6 +116,7 @@ extern "C" { EMCMOT_SET_LINE, /* queue up a linear move */ EMCMOT_SET_CIRCLE, /* queue up a circular move */ + EMCMOT_SET_JOINT_LINE, /* queue up a joint interpolated move */ EMCMOT_CLEAR_PROBE_FLAGS, /* clears probeTripped flag */ EMCMOT_PROBE, /* go to pos, stop if probe trips, record trip pos */ @@ -273,6 +274,13 @@ extern "C" { struct state_tag_t tag; int switchkins_type; /* switchkins type requested by G12.1 */ + + /* a joint interpolated move: either pos is the endpoint and the joints + come from the inverse, or these are the joints and pos comes from + the forward */ + double joint_target[EMCMOT_MAX_JOINTS]; + int have_joint_target; + double joint_seconds; /* 0 for a rapid, else the time the move is to take */ } emcmot_command_t; /*! \todo FIXME - these packed bits might be replaced with chars diff --git a/src/emc/nml_intf/canon.hh b/src/emc/nml_intf/canon.hh index 6e3d6c79a01..9211790508f 100644 --- a/src/emc/nml_intf/canon.hh +++ b/src/emc/nml_intf/canon.hh @@ -291,6 +291,24 @@ extern void STRAIGHT_TRAVERSE(int lineno, double x, double y, double z, double a, double b, double c, double u, double v, double w); + +/* A traverse interpolated in joint space. The endpoint x..w is in program + coordinates like STRAIGHT_TRAVERSE's; motion runs the inverse once there + and interpolates the joints to it. With have_joints the joints given + (machine units, one per joint) are the endpoint instead and x..w say + where the interpreter believes that is. Nothing blends into or out of + it. */ +extern void JOINT_TRAVERSE(int lineno, const double *joints, int have_joints, + double x, double y, double z, + double a, double b, double c, + double u, double v, double w); +/* The same move at feed: it is to take 'seconds' seconds at the programmed + feed, the feed override applies, and the joint limits still cap it. */ +extern void JOINT_FEED(int lineno, const double *joints, int have_joints, + double x, double y, double z, + double a, double b, double c, + double u, double v, double w, + double seconds); /* Move at traverse rate so that at any time during the move, all axes diff --git a/src/emc/nml_intf/emc.cc b/src/emc/nml_intf/emc.cc index 06b4fbe9688..cc65795be53 100644 --- a/src/emc/nml_intf/emc.cc +++ b/src/emc/nml_intf/emc.cc @@ -311,6 +311,9 @@ int emcFormat(NMLTYPE type, void *buffer, CMS * cms) case EMC_TRAJ_SET_G68_TYPE: ((EMC_TRAJ_SET_G68 *) buffer)->update(cms); break; + case EMC_TRAJ_JOINT_MOVE_TYPE: + ((EMC_TRAJ_JOINT_MOVE *) buffer)->update(cms); + break; case EMC_TRAJ_SET_SCALE_TYPE: ((EMC_TRAJ_SET_SCALE *) buffer)->update(cms); break; @@ -536,6 +539,8 @@ const char *emc_symbol_lookup(uint32_t type) return "EMC_TRAJ_SET_ROTATION"; case EMC_TRAJ_SET_G68_TYPE: return "EMC_TRAJ_SET_G68"; + case EMC_TRAJ_JOINT_MOVE_TYPE: + return "EMC_TRAJ_JOINT_MOVE"; case EMC_TRAJ_SET_SCALE_TYPE: return "EMC_TRAJ_SET_SCALE"; case EMC_TRAJ_SET_RAPID_SCALE_TYPE: @@ -1699,6 +1704,16 @@ void EMC_TRAJ_SET_G68::update(CMS * cms) cms->update(active); } +// cppcheck-suppress duplInheritedMember +void EMC_TRAJ_JOINT_MOVE::update(CMS * cms) +{ + EMC_TRAJ_CMD_MSG::update(cms); + EmcPose_update(cms, &end); + cms->update(joints, EMCMOT_MAX_JOINTS); + cms->update(have_joints); + cms->update(seconds); +} + /* * NML/CMS Update function for EMC_SPINDLE_BRAKE_ENGAGE * Automatically generated by NML CodeGen Java Applet. diff --git a/src/emc/nml_intf/emc.hh b/src/emc/nml_intf/emc.hh index f5e45d54d6e..e81ba91d4aa 100644 --- a/src/emc/nml_intf/emc.hh +++ b/src/emc/nml_intf/emc.hh @@ -112,6 +112,7 @@ struct PM_CARTESIAN; #define EMC_TRAJ_SET_FH_ENABLE_TYPE ((NMLTYPE) 236) #define EMC_TRAJ_RIGID_TAP_TYPE ((NMLTYPE) 237) #define EMC_TRAJ_SET_G68_TYPE ((NMLTYPE) 239) +#define EMC_TRAJ_JOINT_MOVE_TYPE ((NMLTYPE) 240) #define EMC_TRAJ_SELECT_KINS_TYPE ((NMLTYPE) 289) #define EMC_TRAJ_STAT_TYPE ((NMLTYPE) 299) @@ -375,6 +376,7 @@ extern int emcTrajResume(); extern int emcTrajDelay(double delay); extern int emcTrajLinearMove(const EmcPose& end, int type, double vel, double ini_maxvel, double acc, double ini_maxjerk, int indexer_jnum); +extern int emcTrajJointMove(const EmcPose& end, const double *joints, int have_joints, double seconds); extern int emcTrajCircularMove(const EmcPose& end, const PM_CARTESIAN& center, const PM_CARTESIAN& normal, int turn, int type, double vel, double ini_maxvel, double acc, double ini_maxjerk); extern int emcTrajSetTermCond(int cond, double tolerance); diff --git a/src/emc/nml_intf/emc_nml.hh b/src/emc/nml_intf/emc_nml.hh index 36ee2fed645..adc038f1068 100644 --- a/src/emc/nml_intf/emc_nml.hh +++ b/src/emc/nml_intf/emc_nml.hh @@ -960,6 +960,27 @@ class EMC_TRAJ_PROBE:public EMC_TRAJ_CMD_MSG { unsigned char probe_type; }; +// a move interpolated in joint space: to the world endpoint, whose joints +// motion finds with the inverse; or to the joints given, whose world +// position motion finds with the forward +class EMC_TRAJ_JOINT_MOVE:public EMC_TRAJ_CMD_MSG { + public: + EMC_TRAJ_JOINT_MOVE() + : EMC_TRAJ_CMD_MSG(EMC_TRAJ_JOINT_MOVE_TYPE, sizeof(EMC_TRAJ_JOINT_MOVE)), + end{}, joints{}, have_joints(0), seconds(0.0) + {}; + + // For internal NML/CMS use only. + // Sub-class update() calls base-class update() + // cppcheck-suppress duplInheritedMember + void update(CMS * cms); + + EmcPose end; + double joints[EMCMOT_MAX_JOINTS]; + int have_joints; + double seconds; /* 0 for a rapid, else the time it is to take */ +}; + class EMC_TRAJ_RIGID_TAP:public EMC_TRAJ_CMD_MSG { public: EMC_TRAJ_RIGID_TAP() diff --git a/src/emc/rs274ngc/gcodemodule.cc b/src/emc/rs274ngc/gcodemodule.cc index fdd6b4873a3..87f8542811e 100644 --- a/src/emc/rs274ngc/gcodemodule.cc +++ b/src/emc/rs274ngc/gcodemodule.cc @@ -636,6 +636,28 @@ void STRAIGHT_FEED(int line_number, Py_XDECREF(result); } +// the preview draws a joint interpolated move as the traverse between its +// ends: the path between them depends on the kinematics, which the +// preview does not have +void JOINT_TRAVERSE(int line_number, const double *joints, int have_joints, + double x, double y, double z, + double a, double b, double c, + double u, double v, double w) { + (void)joints; + (void)have_joints; + STRAIGHT_TRAVERSE(line_number, x, y, z, a, b, c, u, v, w); +} + +void JOINT_FEED(int line_number, const double *joints, int have_joints, + double x, double y, double z, + double a, double b, double c, + double u, double v, double w, double seconds) { + (void)joints; + (void)have_joints; + (void)seconds; + STRAIGHT_FEED(line_number, x, y, z, a, b, c, u, v, w); +} + void STRAIGHT_TRAVERSE(int line_number, double x, double y, double z, double a, double b, double c, diff --git a/src/emc/sai/saicanon.cc b/src/emc/sai/saicanon.cc index 5c0812dfc54..e552926e298 100644 --- a/src/emc/sai/saicanon.cc +++ b/src/emc/sai/saicanon.cc @@ -219,6 +219,49 @@ void SET_TRAVERSE_RATE(double rate) _sai._traverse_rate = rate; } +void JOINT_TRAVERSE(int /*line_number*/, const double *joints, int have_joints, + double x, double y, double z, + double a, double b, double c, + double /*u*/, double /*v*/, double /*w*/) +{ + if (have_joints && joints) { + ECHO_WITH_ARGS("[%.4f, %.4f, %.4f, %.4f, %.4f, %.4f, %.4f, %.4f, %.4f], " + "%.4f, %.4f, %.4f, %.4f, %.4f, %.4f", + joints[0], joints[1], joints[2], joints[3], joints[4], + joints[5], joints[6], joints[7], joints[8], x, y, z, a, b, c); + } else { + ECHO_WITH_ARGS("%.4f, %.4f, %.4f, %.4f, %.4f, %.4f", x, y, z, a, b, c); + } + _sai._program_position_x = x; + _sai._program_position_y = y; + _sai._program_position_z = z; + _sai._program_position_a = a; + _sai._program_position_b = b; + _sai._program_position_c = c; +} + +void JOINT_FEED(int /*line_number*/, const double *joints, int have_joints, + double x, double y, double z, + double a, double b, double c, + double /*u*/, double /*v*/, double /*w*/, + double seconds) +{ + if (have_joints && joints) { + ECHO_WITH_ARGS("[%.4f, %.4f, %.4f, %.4f, %.4f, %.4f, %.4f, %.4f, %.4f], " + "%.4f, %.4f, %.4f, %.4f, %.4f, %.4f, %.4f", + joints[0], joints[1], joints[2], joints[3], joints[4], + joints[5], joints[6], joints[7], joints[8], x, y, z, a, b, c, seconds); + } else { + ECHO_WITH_ARGS("%.4f, %.4f, %.4f, %.4f, %.4f, %.4f, %.4f", x, y, z, a, b, c, seconds); + } + _sai._program_position_x = x; + _sai._program_position_y = y; + _sai._program_position_z = z; + _sai._program_position_a = a; + _sai._program_position_b = b; + _sai._program_position_c = c; +} + void STRAIGHT_TRAVERSE( int /*line_number*/, double x, double y, double z , double a /*AA*/ diff --git a/src/emc/task/emccanon.cc b/src/emc/task/emccanon.cc index 5ef10868ebb..a954bbb4bed 100644 --- a/src/emc/task/emccanon.cc +++ b/src/emc/task/emccanon.cc @@ -1319,6 +1319,47 @@ void generate_fast_move(double x, double y, double z, canonUpdateEndPoint(x, y, z, a, b, c, u, v, w); } +static void joint_move(int line_number, const double *joints, int have_joints, + double x, double y, double z, + double a, double b, double c, + double u, double v, double w, + double seconds) +{ + auto msg = std::make_unique(); + + flush_segments(); + from_prog(x,y,z,a,b,c,u,v,w); + rotate_and_offset_pos(x,y,z,a,b,c,u,v,w); + + msg->end = to_ext_pose(x, y, z, a, b, c, u, v, w); + msg->have_joints = have_joints ? 1 : 0; + for (int i = 0; i < EMCMOT_MAX_JOINTS; i++) { + msg->joints[i] = (have_joints && joints) ? joints[i] : 0.0; + } + msg->seconds = seconds; + interp_list.set_line_number(line_number); + tag_and_send(std::move(msg), _tag); + + canonUpdateEndPoint(x, y, z, a, b, c, u, v, w); +} + +void JOINT_TRAVERSE(int line_number, const double *joints, int have_joints, + double x, double y, double z, + double a, double b, double c, + double u, double v, double w) +{ + joint_move(line_number, joints, have_joints, x, y, z, a, b, c, u, v, w, 0.0); +} + +void JOINT_FEED(int line_number, const double *joints, int have_joints, + double x, double y, double z, + double a, double b, double c, + double u, double v, double w, + double seconds) +{ + joint_move(line_number, joints, have_joints, x, y, z, a, b, c, u, v, w, seconds); +} + void generate_move(double vel,double x, double y, double z, double a, double b, double c, double u, double v, double w) diff --git a/src/emc/task/emctaskmain.cc b/src/emc/task/emctaskmain.cc index 7a1a1583275..2709fd0d2cf 100644 --- a/src/emc/task/emctaskmain.cc +++ b/src/emc/task/emctaskmain.cc @@ -475,6 +475,9 @@ static int checkInterpList(NML_INTERP_LIST * il, EMC_STAT * /*stat*/) case EMC_TRAJ_LINEAR_MOVE_TYPE: break; + case EMC_TRAJ_JOINT_MOVE_TYPE: + break; + case EMC_TRAJ_CIRCULAR_MOVE_TYPE: break; @@ -1521,6 +1524,7 @@ static EMC_TASK_EXEC emcTaskCheckPreconditions(NMLmsg * cmd) break; case EMC_TRAJ_LINEAR_MOVE_TYPE: + case EMC_TRAJ_JOINT_MOVE_TYPE: case EMC_TRAJ_CIRCULAR_MOVE_TYPE: case EMC_TRAJ_SET_VELOCITY_TYPE: case EMC_TRAJ_SET_ACCELERATION_TYPE: @@ -1834,6 +1838,13 @@ static int emcTaskIssueCommand(NMLmsg * cmd) emcTrajLinearMoveMsg->indexer_jnum); break; + case EMC_TRAJ_JOINT_MOVE_TYPE: { + EMC_TRAJ_JOINT_MOVE *jm = reinterpret_cast(cmd); + emcTrajUpdateTag(jm->tag); + retval = emcTrajJointMove(jm->end, jm->joints, jm->have_joints, jm->seconds); + break; + } + case EMC_TRAJ_CIRCULAR_MOVE_TYPE: emcTrajUpdateTag((reinterpret_cast(cmd))->tag); emcTrajCircularMoveMsg = reinterpret_cast(cmd); @@ -2497,6 +2508,7 @@ static EMC_TASK_EXEC emcTaskCheckPostconditions(NMLmsg * cmd) return EMC_TASK_EXEC::WAITING_FOR_SYSTEM_CMD; break; + case EMC_TRAJ_JOINT_MOVE_TYPE: case EMC_TRAJ_LINEAR_MOVE_TYPE: case EMC_TRAJ_CIRCULAR_MOVE_TYPE: case EMC_TRAJ_SET_VELOCITY_TYPE: diff --git a/src/emc/task/taskintf.cc b/src/emc/task/taskintf.cc index 585a22af698..a5fbeca3aa5 100644 --- a/src/emc/task/taskintf.cc +++ b/src/emc/task/taskintf.cc @@ -1498,6 +1498,23 @@ double emcTrajGetAngularUnits() return TrajConfig.AngularUnits; } +int emcTrajJointMove(const EmcPose& end, const double *joints, int have_joints, double seconds) +{ + int i; + + emcmotCommand.command = EMCMOT_SET_JOINT_LINE; + emcmotCommand.pos = end; + emcmotCommand.id = TrajConfig.MotionId; + emcmotCommand.tag = localEmcTrajTag; + emcmotCommand.motion_type = seconds > 0.0 ? EMC_MOTION_TYPE_FEED : EMC_MOTION_TYPE_TRAVERSE; + emcmotCommand.joint_seconds = seconds; + emcmotCommand.have_joint_target = have_joints; + for (i = 0; i < EMCMOT_MAX_JOINTS; i++) { + emcmotCommand.joint_target[i] = (have_joints && joints) ? joints[i] : 0.0; + } + return usrmotWriteEmcmotCommand(&emcmotCommand); +} + int emcTrajSetOffset(const EmcPose& tool_offset) { emcmotCommand.command = EMCMOT_SET_OFFSET; diff --git a/src/emc/tp/tc.c b/src/emc/tp/tc.c index e1c9fe5ffe3..177961cf4b3 100644 --- a/src/emc/tp/tc.c +++ b/src/emc/tp/tc.c @@ -203,6 +203,7 @@ int tcGetStartAccelUnitVector(TC_STRUCT const * const tc, PmCartesian * const ou tcCircleStartAccelUnitVector(tc,out); break; case TC_SPHERICAL: + case TC_JOINT: return -1; default: return -1; @@ -210,6 +211,24 @@ int tcGetStartAccelUnitVector(TC_STRUCT const * const tc, PmCartesian * const ou return 0; } +/** + * The world direction of a joint interpolated segment, end minus start, + * for the status fields that want a direction. The path between them is + * not straight, so this is the chord, and there is none when only the + * rotaries move. + */ +static int tcJointChordUnitVector(TC_STRUCT const * const tc, PmCartesian * const out) +{ + PmCartesian d; + double mag; + + pmCartCartSub(&tc->coords.joint.world_end.tran, &tc->coords.joint.world_start.tran, &d); + pmCartMag(&d, &mag); + if (mag < TP_POS_EPSILON) { return -1; } + pmCartScalMult(&d, 1.0 / mag, out); + return 0; +} + /** * Get the acceleration direction unit vector for blend velocity calculations. * This calculates the direction of acceleration at the end of a segment. @@ -328,6 +347,8 @@ int tcGetStartTangentUnitVector(TC_STRUCT const * const tc, PmCartesian * const case TC_CIRCULAR: pmCircleTangentVector(&tc->coords.circle.xyz, 0.0, out); break; + case TC_JOINT: + return tcJointChordUnitVector(tc, out); default: rtapi_print_msg(RTAPI_MSG_ERR, "Invalid motion type %d!\n",tc->motion_type); return -1; @@ -351,6 +372,8 @@ int tcGetEndTangentUnitVector(TC_STRUCT const * const tc, PmCartesian * const ou pmCircleTangentVector(&tc->coords.circle.xyz, tc->coords.circle.xyz.angle, out); break; + case TC_JOINT: + return tcJointChordUnitVector(tc, out); default: rtapi_print_msg(RTAPI_MSG_ERR, "Invalid motion type %d!\n",tc->motion_type); return -1; @@ -406,6 +429,8 @@ int tcGetCurrentTangentUnitVector(TC_STRUCT const * const tc, PmCartesian * cons arcTangent(arc, out, at_end); } break; + case TC_JOINT: + return tcJointChordUnitVector(tc, out); default: rtapi_print_msg(RTAPI_MSG_ERR, "Invalid motion type %d in tcGetCurrentTangentUnitVector!\n", tc->motion_type); return -1; @@ -529,6 +554,30 @@ int tcGetPosReal(TC_STRUCT const * const tc, int of_point, EmcPose * const pos) abc = tc->coords.arc.abc; uvw = tc->coords.arc.uvw; break; + case TC_JOINT: { + // the ends are exact; between them this is the chord, a proxy + // for the status fields, and the servo thread reports the + // real position from the forward kinematics + const PmJointLine *jl = &tc->coords.joint; + double f = (tc->target > 0.0) ? progress / tc->target : 0.0; + EmcPose d; + + emcPoseSub(&jl->world_end, &jl->world_start, &d); + pos->tran.x = jl->world_start.tran.x + f * d.tran.x; + pos->tran.y = jl->world_start.tran.y + f * d.tran.y; + pos->tran.z = jl->world_start.tran.z + f * d.tran.z; + pos->a = jl->world_start.a + f * d.a; + pos->b = jl->world_start.b + f * d.b; + pos->c = jl->world_start.c + f * d.c; + pos->u = jl->world_start.u + f * d.u; + pos->v = jl->world_start.v + f * d.v; + pos->w = jl->world_start.w + f * d.w; + if (of_point == TC_GET_ENDPOINT) { *pos = jl->world_end; } + return TP_ERR_OK; + } + default: + rtapi_print_msg(RTAPI_MSG_ERR, "Invalid motion type %d in tcGetPosReal!\n", tc->motion_type); + return TP_ERR_FAIL; } if (res_fit == TP_ERR_OK) { @@ -539,6 +588,26 @@ int tcGetPosReal(TC_STRUCT const * const tc, int of_point, EmcPose * const pos) } +/** + * The joints of a joint interpolated segment at its progress. + * Returns the joint count, or 0 for any other segment. + */ +int tcGetJointPos(TC_STRUCT const * const tc, double * const joints) +{ + const PmJointLine *jl; + double f; + int i; + + if (!tc || tc->motion_type != TC_JOINT) { return 0; } + jl = &tc->coords.joint; + f = (tc->target > 0.0) ? tc->progress / tc->target : 1.0; + if (f > 1.0) { f = 1.0; } + for (i = 0; i < jl->num_joints; i++) { + joints[i] = jl->start[i] + f * (jl->end[i] - jl->start[i]); + } + return jl->num_joints; +} + /** * Set the terminal condition of a segment. * This function will eventually handle state changes associated with altering a terminal condition. @@ -624,7 +693,7 @@ int tcIsBlending(TC_STRUCT * const tc) { //FIXME Disabling blends for rigid tap cycle until changes can be verified. int is_blending_next = (tc->term_cond == TC_TERM_COND_PARABOLIC ) && tc->on_final_decel && (tc->currentvel < tc->blend_vel) && - tc->motion_type != TC_RIGIDTAP; + tc->motion_type != TC_RIGIDTAP && tc->motion_type != TC_JOINT; //Latch up the blending_next status here, so that even if the prev conditions //aren't necessarily true we still blend to completion once the blend @@ -1071,6 +1140,9 @@ double pmRigidTapTarget(PmRigidTap * const tap, double uu_per_rev) /** Returns true if segment has ONLY rotary motion, false otherwise. */ int tcPureRotaryCheck(TC_STRUCT const * const tc) { + // a joint interpolated segment measures its velocity in joint units, + // so the cartesian limit does not apply to it either + if (tc->motion_type == TC_JOINT) { return 1; } return (tc->motion_type == TC_LINEAR) && (tc->coords.line.xyz.tmag_zero) && (tc->coords.line.uvw.tmag_zero); diff --git a/src/emc/tp/tc.h b/src/emc/tp/tc.h index 5558a55e280..dce3df6753c 100644 --- a/src/emc/tp/tc.h +++ b/src/emc/tp/tc.h @@ -37,6 +37,7 @@ int tcGetEndpoint(TC_STRUCT const * const tc, EmcPose * const out); int tcGetStartpoint(TC_STRUCT const * const tc, EmcPose * const out); int tcGetPos(TC_STRUCT const * const tc, EmcPose * const out); int tcGetPosReal(TC_STRUCT const * const tc, int of_endpoint, EmcPose * const out); +int tcGetJointPos(TC_STRUCT const * const tc, double * const joints); int tcGetEndAccelUnitVector(TC_STRUCT const * const tc, PmCartesian * const out); int tcGetStartAccelUnitVector(TC_STRUCT const * const tc, PmCartesian * const out); int tcGetEndTangentUnitVector(TC_STRUCT const * const tc, PmCartesian * const out); diff --git a/src/emc/tp/tc_types.h b/src/emc/tp/tc_types.h index 135b34586ae..95cdf540dde 100644 --- a/src/emc/tp/tc_types.h +++ b/src/emc/tp/tc_types.h @@ -33,7 +33,8 @@ typedef enum { TC_LINEAR = 1, TC_CIRCULAR = 2, TC_RIGIDTAP = 3, - TC_SPHERICAL = 4 + TC_SPHERICAL = 4, + TC_JOINT = 5 } tc_motion_type_t; typedef enum { @@ -117,6 +118,19 @@ typedef struct { RIGIDTAP_STATE state; } PmRigidTap; +/* A segment interpolated in joint space: every joint runs from start to + * end together, the longest one setting the pace. The world poses at the + * two ends are what the segments around it see; the position along the way + * is not a line in world space and the servo thread reports it from the + * forward kinematics. */ +typedef struct { + double start[EMCMOT_MAX_JOINTS]; + double end[EMCMOT_MAX_JOINTS]; + int num_joints; + EmcPose world_start; + EmcPose world_end; +} PmJointLine; + typedef struct { double cycle_time; //Position stuff @@ -160,11 +174,13 @@ typedef struct { PmCircle9 circle; PmRigidTap rigidtap; Arc9 arc; + PmJointLine joint; } coords; int motion_type; // TC_LINEAR (coords.line) or // TC_CIRCULAR (coords.circle) or - // TC_RIGIDTAP (coords.rigidtap) + // TC_RIGIDTAP (coords.rigidtap) or + // TC_JOINT (coords.joint) int active; // this motion is being executed int canon_motion_type; // this motion is due to which canon function? int term_cond; // gcode requests continuous feed at the end of diff --git a/src/emc/tp/tp.c b/src/emc/tp/tp.c index 80e7570222b..c705ca5bff1 100644 --- a/src/emc/tp/tp.c +++ b/src/emc/tp/tp.c @@ -158,6 +158,8 @@ STATIC int tcRotaryMotionCheck(TC_STRUCT const * const tc) { } case TC_SPHERICAL: return true; + case TC_JOINT: + return true; default: tp_debug_print("Unknown motion type!\n"); return false; @@ -440,12 +442,23 @@ STATIC void tpReleaseQueuedPlanners(TP_STRUCT * const tp) * intended to put the motion queue in the state it would be if all queued * motions finished at the current position. */ +/* What the planner knows in joint space belongs to the queue: with the + queue reset, the queue end is no longer at those joints, no segment is + left to hand its end joints out, and no joint segment is waiting. */ +STATIC void tpForgetJoints(TP_STRUCT * const tp) +{ + tp->queue_end_joints_valid = 0; + tp->joint_end_valid = 0; + tp->joint_segments_queued = 0; +} + int tpClear(TP_STRUCT * const tp) { tpReleaseQueuedPlanners(tp); tcqInit(&tp->queue); tp->queueSize = 0; tp->goalPos = tp->currentPos; + tpForgetJoints(tp); // Clear out status ID's tp->nextId = 0; tp->execId = 0; @@ -1649,6 +1662,8 @@ int tpAddRigidTap(TP_STRUCT * const tp, acc, ini_maxjerk); + tp->queue_end_joints_valid = 0; + // Setup rigid tap geometry pmRigidTapInit(&tc.coords.rigidtap, &tp->goalPos, @@ -2071,6 +2086,11 @@ tc_blend_type_t tpHandleBlendArc(TP_STRUCT * const tp, TC_STRUCT * const tc) { tp_debug_print(" queue empty\n"); return NO_BLEND; } + if (prev_tc->motion_type == TC_JOINT) { + // nothing blends with a joint interpolated segment + tcSetTermCond(prev_tc, tc, TC_TERM_COND_STOP); + return NO_BLEND; + } if (prev_tc->progress > prev_tc->target / 2.0) { tp_debug_print(" prev_tc progress (%f) is too large, aborting blend arc\n", prev_tc->progress); return NO_BLEND; @@ -2109,6 +2129,78 @@ tc_blend_type_t tpHandleBlendArc(TP_STRUCT * const tp, TC_STRUCT * const tc) { return blend_used; } +/** + * Add a joint interpolated segment to the tc queue. + * + * The joints run from start to end together over a "length" that is the + * joint space distance between them. vel and acc are already the tightest + * per-joint limits scaled onto that length, so no joint exceeds its own. + * Nothing blends into or out of it: the segment before it is made to stop + * and so is this one, since the path between the two world poses is not a + * line and the next segment has to start from rest at world_end. + */ +int tpAddJointLine(TP_STRUCT * const tp, const double *start, const double *end, + int num_joints, EmcPose world_end, int canon_motion_type, + double vel, double ini_maxvel, double acc, double ini_maxjerk, + unsigned char enables, struct state_tag_t tag) +{ + TC_STRUCT tc = {0}; + PmJointLine *jl = &tc.coords.joint; + TC_STRUCT *prev_tc; + double length = 0.0; + int i; + + if (!tp || !start || !end || num_joints <= 0 || num_joints > EMCMOT_MAX_JOINTS) { + return TP_ERR_MISSING_INPUT; + } + if (tp->aborting) { + rtapi_print_msg(RTAPI_MSG_ERR, "TP is aborting\n"); + return TP_ERR_FAIL; + } + + tcInit(&tc, TC_JOINT, canon_motion_type, tp->cycleTime, enables, 0); + tc.tag = tag; + tpSetupSyncedIO(tp, &tc); + tcSetupState(&tc, tp); + // a joint move has no path to synchronise to a spindle along + tc.synchronized = TC_SYNC_NONE; + tc.uu_per_rev = 0.0; + tcSetupMotion(&tc, vel, ini_maxvel, acc, ini_maxjerk); + + jl->num_joints = num_joints; + for (i = 0; i < EMCMOT_MAX_JOINTS; i++) { + jl->start[i] = (i < num_joints) ? start[i] : 0.0; + jl->end[i] = (i < num_joints) ? end[i] : 0.0; + length += (jl->end[i] - jl->start[i]) * (jl->end[i] - jl->start[i]); + } + jl->world_start = tp->goalPos; + jl->world_end = world_end; + + tc.target = pmSqrt(length); + if (tc.target < TP_POS_EPSILON) { + return TP_ERR_ZERO_LENGTH; + } + tc.nominal_length = tc.target; + tcClampVelocityByLength(&tc); + tc.indexer_jnum = -1; + tcSetTermCond(&tc, NULL, TC_TERM_COND_STOP); + + prev_tc = tcqLast(&tp->queue); + if (prev_tc) { + tcSetTermCond(prev_tc, &tc, TC_TERM_COND_STOP); + tcFinalizeLength(prev_tc); + } + + int retval = tpAddSegmentToQueue(tp, &tc, true); + if (retval == TP_ERR_OK) { + for (i = 0; i < EMCMOT_MAX_JOINTS; i++) { tp->queue_end_joints[i] = jl->end[i]; } + tp->queue_end_joints_valid = 1; + tp->joint_segments_queued++; + } + tpRunOptimization(tp); + return retval; +} + //TODO final setup steps as separate functions // /** @@ -2149,6 +2241,7 @@ int tpAddLine(TP_STRUCT * const tp, EmcPose end, int canon_motion_type, acc, ini_maxjerk); // Setup line geometry + tp->queue_end_joints_valid = 0; pmLine9Init(&tc.coords.line, &tp->goalPos, &end); @@ -2220,6 +2313,7 @@ int tpAddCircle(TP_STRUCT * const tp, tp->cycleTime, enables, atspeed); + tp->queue_end_joints_valid = 0; tc.tag = tag; // Setup any synced IO for this move tpSetupSyncedIO(tp, &tc); @@ -3314,6 +3408,7 @@ STATIC void tpHandleEmptyQueue(TP_STRUCT * const tp) tpReleaseQueuedPlanners(tp); tcqInit(&tp->queue); + tpForgetJoints(tp); tp->goalPos = tp->currentPos; tp->done = 1; tp->depth = tp->activeDepth = 0; @@ -3369,6 +3464,16 @@ STATIC int tpCompleteSegment(TP_STRUCT * const tp, return TP_ERR_FAIL; } + // a joint interpolated segment leaves its end joints behind for the + // servo thread: it asks after the segment is gone, and would otherwise + // invert the end position with the previous cycle's joints as seed + if (tc->motion_type == TC_JOINT) { + int i; + for (i = 0; i < EMCMOT_MAX_JOINTS; i++) { tp->joint_end[i] = tc->coords.joint.end[i]; } + tp->joint_end_valid = 1; + if (tp->joint_segments_queued > 0) { tp->joint_segments_queued--; } + } + //Clear status flags associated since segment is done //TODO stuff into helper function? tc->active = 0; @@ -3415,6 +3520,7 @@ STATIC tp_err_t tpHandleAbort(TP_STRUCT * const tp, TC_STRUCT * const tc, (tc->currentvel == 0.0 && (!nexttc || nexttc->currentvel == 0.0))) { tpReleaseQueuedPlanners(tp); tcqInit(&tp->queue); + tpForgetJoints(tp); tp->goalPos = tp->currentPos; tp->done = 1; tp->depth = tp->activeDepth = 0; @@ -3502,7 +3608,8 @@ STATIC tp_err_t tpActivateSegment(TP_STRUCT * const tp, TC_STRUCT * const tc) { return TP_ERR_MISSING_INPUT; } - if (tp->reverse_run && (tc->motion_type == TC_RIGIDTAP || tc->synchronized != TC_SYNC_NONE)) { + if (tp->reverse_run && (tc->motion_type == TC_RIGIDTAP || tc->motion_type == TC_JOINT + || tc->synchronized != TC_SYNC_NONE)) { //Can't activate a segment with synced motion in reverse return TP_ERR_REVERSE_EMPTY; } @@ -4379,6 +4486,72 @@ int tpGetPos(TP_STRUCT const * const tp, EmcPose * const pos) return TP_ERR_OK; } +int tpGetGoalPos(TP_STRUCT const * const tp, EmcPose * const pos) +{ + if (0 == tp) { + ZERO_EMC_POSE((*pos)); + return TP_ERR_FAIL; + } + *pos = tp->goalPos; + return TP_ERR_OK; +} + +/** + * The joints the active segment commands, when it is a joint interpolated + * one: the servo thread takes these instead of inverting the position. + * Returns the joint count, or 0 when the active segment is any other kind. + */ +int tpGetJointPos(TP_STRUCT const * const tp, double * const joints) +{ + TC_STRUCT const *tc; + + if (!tp || !joints) { return 0; } + tc = tcqItem((TC_QUEUE_STRUCT *)&tp->queue, 0); + if (!tc || !tc->active) { return 0; } + return tcGetJointPos(tc, joints); +} + +/** + * The end joints of a joint interpolated segment that completed this + * cycle, once: the seed for the servo thread's inverse of the position the + * planner is now at, which is that segment's end and whatever a following + * segment added in the rest of the cycle. Returns 1 and fills the joints, + * or 0. + */ +int tpTakeJointEnd(TP_STRUCT * const tp, double * const joints) +{ + int i; + + if (!tp || !joints || !tp->joint_end_valid) { return 0; } + for (i = 0; i < EMCMOT_MAX_JOINTS; i++) { joints[i] = tp->joint_end[i]; } + tp->joint_end_valid = 0; + return 1; +} + +/** + * How many joint interpolated segments the queue holds, active one + * included. An external offset cannot ride on one, so its planning waits + * while any is queued. + */ +int tpJointSegmentsQueued(TP_STRUCT const * const tp) +{ + return tp ? tp->joint_segments_queued : 0; +} + +/** + * Where the queue ends in joint space, if the last segment queued was a + * joint interpolated one. Returns 1 and fills the joints, or 0 when the + * answer is the inverse of the goal position. + */ +int tpGetQueueEndJoints(TP_STRUCT const * const tp, double * const joints) +{ + int i; + + if (!tp || !joints || !tp->queue_end_joints_valid) { return 0; } + for (i = 0; i < EMCMOT_MAX_JOINTS; i++) { joints[i] = tp->queue_end_joints[i]; } + return 1; +} + int tpIsDone(TP_STRUCT * const tp) { if (0 == tp) { @@ -4471,6 +4644,12 @@ EXPORT_SYMBOL(tpAbort); EXPORT_SYMBOL(tpActiveDepth); EXPORT_SYMBOL(tpAddCircle); EXPORT_SYMBOL(tpAddLine); +EXPORT_SYMBOL(tpAddJointLine); +EXPORT_SYMBOL(tpGetGoalPos); +EXPORT_SYMBOL(tpGetJointPos); +EXPORT_SYMBOL(tpTakeJointEnd); +EXPORT_SYMBOL(tpJointSegmentsQueued); +EXPORT_SYMBOL(tpGetQueueEndJoints); EXPORT_SYMBOL(tpAddRigidTap); EXPORT_SYMBOL(tpClear); EXPORT_SYMBOL(tpCreate); diff --git a/src/emc/tp/tp.h b/src/emc/tp/tp.h index e00b457ad23..a1af63deb11 100644 --- a/src/emc/tp/tp.h +++ b/src/emc/tp/tp.h @@ -63,6 +63,15 @@ int tpAddCircle(TP_STRUCT * const tp, EmcPose end, PmCartesian center, double ini_maxvel, double acc, double ini_maxjerk, unsigned char enables, char atspeed, struct state_tag_t tag); int tpGetPos(TP_STRUCT const * const tp, EmcPose * const pos); +int tpGetGoalPos(TP_STRUCT const * const tp, EmcPose * const pos); +int tpAddJointLine(TP_STRUCT * const tp, const double *start, const double *end, + int num_joints, EmcPose world_end, int canon_motion_type, + double vel, double ini_maxvel, double acc, double ini_maxjerk, + unsigned char enables, struct state_tag_t tag); +int tpGetJointPos(TP_STRUCT const * const tp, double * const joints); +int tpTakeJointEnd(TP_STRUCT * const tp, double * const joints); +int tpJointSegmentsQueued(TP_STRUCT const * const tp); +int tpGetQueueEndJoints(TP_STRUCT const * const tp, double * const joints); int tpIsDone(TP_STRUCT * const tp); int tpQueueDepth(TP_STRUCT * const tp); int tpActiveDepth(TP_STRUCT * const tp); diff --git a/src/emc/tp/tp_types.h b/src/emc/tp/tp_types.h index 0e9ab844322..26ed50933bd 100644 --- a/src/emc/tp/tp_types.h +++ b/src/emc/tp/tp_types.h @@ -109,6 +109,17 @@ typedef struct { EmcPose currentPos; EmcPose goalPos; + /* where the queue ends in joint space, known when the last segment + queued was a joint interpolated one; a world segment after it makes + the answer the inverse of goalPos again */ + double queue_end_joints[EMCMOT_MAX_JOINTS]; + int queue_end_joints_valid; + /* the end joints of a joint interpolated segment that completed this + cycle, for the servo thread to seed its inverse with: the segment is + gone from the queue by the time it asks */ + double joint_end[EMCMOT_MAX_JOINTS]; + int joint_end_valid; + int joint_segments_queued; /* joint interpolated segments in the queue */ int queueSize; double cycleTime; From d28bfa227d87339e89de351b25c5e6b601f9dff5 Mon Sep 17 00:00:00 2001 From: Luca Toniolo <10792599+grandixximo@users.noreply.github.com> Date: Sat, 5 Sep 2026 00:33:42 +1000 Subject: [PATCH 46/60] interpreter: the point-to-point moves and the tool orientation codes, G53.1 to G53.6, and G68.3 The interpreter evaluates the kinematics module ahead of motion through the loader in kinematics_userspace, opened on first use on a HAL component of its own, so G53.n can ask the tool frame inverse where the rotaries go, G68.3 can read the tool direction, and the point-to-point codes can tell where their joints put the tool. G53.4, G53.5 and G53.7 are point-to-point moves, non-modal modifiers of G0 and G1 like G53: the path is interpolated in joint space, only the endpoint is defined. G53.4 takes a program point through the offsets. G53.5 reads the axis letters as though the kinematics were the identity, a letter naming the joints the module's identity mapping gives it, no offset applied; it is refused on a machine whose letters name joints of the other unit class, as a serial robot's X. G53.7 takes joint values as J= words, a form the lexer reads when the J value is followed by an equals sign, the joint's own position in its own units, which works on every machine. With G1 the move takes the time the straight move would at the programmed feed. The inverse ahead of motion is run to a fixed point, because a module may read the joints it is handed. G53.1 moves the rotaries to the plane's normal with the linear joints where they are; G53.6 does the same holding the tool centre point, a Cartesian move; G53.3 X Y Z goes to a point in the plane with the tool oriented. P picks among the poses: nearest by default, P1 and P2 by the sign of the secondary rotary, the one whose axis the other carries, found by turning each rotary through the module's tool frame, Heidenhain's SEQ+ and SEQ-. Q says whether the joints that carry the work take part: Q0 holds them, COORD ROT, falling back to everything free; Q1 frees them, TABLE ROT. G68.3 takes the plane from the tool, Z the tool axis as the joints have it, X the default tool X of the conventions chapter. tests/twp-native runs the xyzacb nutating head through all of it against the python maths in tests/kins-twp; tests/ptp-robot puts pumakins through the point-to-point codes. Reversing the joint interpolation fails three checks in twp-native. --- docs/src/gcode/g-code.adoc | 229 +++++++- docs/src/gcode/overview.adoc | 6 +- docs/src/motion/kinematics-conventions.adoc | 22 + docs/src/motion/kinematics.adoc | 22 + src/emc/motion/command.c | 24 +- src/emc/rs274ngc/Submakefile | 2 +- src/emc/rs274ngc/interp_array.cc | 4 +- src/emc/rs274ngc/interp_check.cc | 37 +- src/emc/rs274ngc/interp_convert.cc | 34 +- src/emc/rs274ngc/interp_internal.cc | 6 +- src/emc/rs274ngc/interp_internal.hh | 18 + src/emc/rs274ngc/interp_read.cc | 18 +- src/emc/rs274ngc/interp_setup.cc | 6 + src/emc/rs274ngc/interp_workplane.cc | 508 ++++++++++++++++++ src/emc/rs274ngc/rs274ngc_interp.hh | 11 + src/emc/rs274ngc/rs274ngc_pre.cc | 17 + tests/kins-twp/README | 6 +- tests/kins-twp/test.sh | 8 +- .../kins-twp/xyzacb}/remap_funcs_twp.py | 0 .../kins-twp/xyzbca}/remap_funcs_twp.py | 0 tests/ptp-robot/README | 6 + tests/ptp-robot/checkresult | 3 + tests/ptp-robot/sim.hal | 16 + tests/ptp-robot/skip | 4 + tests/ptp-robot/test-ui.py | 99 ++++ tests/ptp-robot/test.ini | 134 +++++ tests/ptp-robot/test.sh | 4 + tests/ptp-robot/tool.tbl | 1 + tests/twp-native/README | 13 + tests/twp-native/abort.ngc | 11 + tests/twp-native/sim.hal | 16 + tests/twp-native/skip | 4 + tests/twp-native/test-ui.py | 477 ++++++++++++++++ tests/twp-native/test.ini | 147 +++++ tests/twp-native/test.sh | 4 + tests/twp-native/tool.tbl | 1 + 36 files changed, 1893 insertions(+), 25 deletions(-) rename {configs/sim/axis/vismach/5axis/table-rotary_spindle-rotary-nutating/xyzacb-trsrn_twp => tests/kins-twp/xyzacb}/remap_funcs_twp.py (100%) rename {configs/sim/axis/vismach/5axis/table-rotary_spindle-rotary-nutating/xyzbca-trsrn_twp => tests/kins-twp/xyzbca}/remap_funcs_twp.py (100%) create mode 100644 tests/ptp-robot/README create mode 100755 tests/ptp-robot/checkresult create mode 100644 tests/ptp-robot/sim.hal create mode 100755 tests/ptp-robot/skip create mode 100755 tests/ptp-robot/test-ui.py create mode 100644 tests/ptp-robot/test.ini create mode 100755 tests/ptp-robot/test.sh create mode 100644 tests/ptp-robot/tool.tbl create mode 100644 tests/twp-native/README create mode 100644 tests/twp-native/abort.ngc create mode 100644 tests/twp-native/sim.hal create mode 100755 tests/twp-native/skip create mode 100755 tests/twp-native/test-ui.py create mode 100644 tests/twp-native/test.ini create mode 100755 tests/twp-native/test.sh create mode 100644 tests/twp-native/tool.tbl diff --git a/docs/src/gcode/g-code.adoc b/docs/src/gcode/g-code.adoc index d0692eab77c..ddaf3f22d98 100644 --- a/docs/src/gcode/g-code.adoc +++ b/docs/src/gcode/g-code.adoc @@ -88,11 +88,13 @@ as the 'L number', and so on for any other letter. |<> |Cancel Tool Length Offset |<> |Local Coordinate System Offset |<> |Move in Machine Coordinates +|<> |Orient the Tool to the Tilted Work Plane +|<> |Point-to-Point Move |<> |Select Coordinate System (1 - 9) |<> |Exact Path Mode |<> |Exact Stop Mode |<> |Path Control Mode with Optional Tolerance -|<> |Tilted Work Plane +|<> |Tilted Work Plane |<> |Lathe finishing cycle |<> |Lathe roughing cycle |<> |Drilling Cycle with Chip Breaking @@ -1741,6 +1743,214 @@ It is an error if: * G53 is used without G0 or G1 being active, * or G53 is used while cutter compensation is on. +[[gcode:g53.1]] +== G53.1, G53.3, G53.6 Orient the Tool to the Work Plane(((G53.1 Orient the Tool))) + +[source,ngc] +---- +G53.1 +G53.3 X- Y- Z- +G53.6 +---- + +Each of these moves the rotary joints so that the tool axis is normal to the +active <>, the plane's Z. They differ in what +happens to the tool tip on the way: + +* 'G53.1' moves the rotaries alone. The linear joints stay where they are, + and the tool tip swings to wherever that carries it. It is a + point-to-point move, like <>. +* 'G53.6' keeps the tool centre point where it is. It is a Cartesian move of + the rotary words, so the kinematics compensates the linear joints all along. +* 'G53.3' moves the rotaries and takes the tool to 'X Y Z', given in the + plane, in one point-to-point move. A word left out keeps the present value. + +The kinematics module answers where the rotaries have to go, with its tool +frame inverse (see the kinematics conventions chapter), so no configuration +carries the formula. That answer has more than one solution on most +machines, two on a five-axis one, and often a choice of which joints to use. + +'P' picks which of them. Without 'P', or with 'P0', the machine takes the +one nearest where its rotaries are standing, which is the shortest move and +depends on where that is. 'P1' and 'P2' name the pose instead, so a program +reaches the same one wherever it starts from: on a five axis machine the two +poses lean the head or the table opposite ways, and they differ in the sign +of the secondary rotary, the one whose axis the other carries. 'P1' is the +pose with that rotary positive and 'P2' the pose with it negative. The +interpreter works out which rotary that is by asking the kinematics module, +so nothing is configured for it. This is the choice Heidenhain writes as +`SEQ+` and `SEQ-`. + +The two poses become one where the tool direction asked for lies along the +primary rotary's axis, straight up on a vertical mill, and there every form +gives the same answer. A machine that is not of this shape, a robot among +them, has no such sign to name, and there only the nearest form is +available. + +'Q' says whether the joints that carry the work, the table, take part. + +* 'Q0', the default, holds them and lets the head do the work. On a machine + whose head has only two rotaries the plane's X is then not something the + joints can place, and it is left to the coordinate system, which the plane + already carries; nothing else is done with it. This is what Heidenhain + calls `COORD ROT`. If nothing is reachable with the table held, the move + is tried again with every joint free. +* 'Q1' frees the table from the start. With the plane's X asked for as well, + a machine with the joints for it turns the table so that the plane's X is + reached by the machine, Heidenhain's `TABLE ROT`. + +Which joints carry the work is read off the kinematics module's work frame, +so 'Q' means the same thing on every module and needs no INI entry. + +The orientation is evaluated by the interpreter, ahead of the moves before +it, on the kinematics type the program is in. It needs a kinematics type +whose frames describe the machine: on a switchable module that is its TCP +kinematics, selected with <>, and not the identity +kinematics the module starts in. + +.G53.1 Example +[source,ngc] +---- +G12.1 P1 (the TCP kinematics) +G68.2 X50 Y50 Z0 I30 J20 K0 (a plane) +G53.1 (rotaries to its normal, the head does it) +G0 X0 Y0 Z10 (rapid to a point in the plane, above its origin) +G1 Z-2 F200 (down, along the plane's normal) +G69 +---- + +It is an error if: + +* No tilted work plane is active. +* The kinematics type in force is the identity kinematics, the module cannot + be evaluated by the interpreter, or it reports no frames, so it cannot say + where its joints point the tool. +* The plane's normal cannot be reached by the rotary joints. +* 'P' is anything but 0, 1 or 2, no reachable pose has the secondary rotary + the way 'P' asks for, or the machine has no pair of poses a tilting joint + tells apart. +* 'Q' is anything but 0 or 1. +* Axis words are used with 'G53.1' or 'G53.6', or words other than 'X', 'Y' + and 'Z' with 'G53.3'. +* Cutter compensation is on. + +[[gcode:g53.4]] +== G53.4, G53.5, G53.7 Point-to-Point Move(((G53.4 Point-to-Point Move))) + +[source,ngc] +---- +G53.4 G0 +G53.4 G1 F- +G53.5 G0 +G53.5 G1 F- +G53.7 G0 J= ... +G53.7 G1 F- J= ... +---- + +A point-to-point move defines its two ends and leaves the path between them +to the joints. The inverse kinematics runs once, at the destination, and every +joint then travels from where it is to where it must be, all together, the +slowest setting the pace. The tool does not follow a straight line; on a +machine with rotary joints it swings. The joints are the motion controller's, +<>, the ones the kinematics module maps the axes +to; what HAL connects behind each of them is not part of the interpolation. + +All three are non-modal, like 'G53': they apply to the one block they are +written in, which must have 'G0' or 'G1' in force or on the line. Each strips +one more layer of interpretation from the destination than the one before: + +* 'G53.4' takes a destination in program coordinates, through the offsets and + the tilted work plane like any other move. This is the move a program uses + to cross a kinematic singularity, or to turn a rotary head right round, + without leaving its coordinate system and without the trajectory planner + trying to hold the tool tip on a line the joints cannot follow at speed. +* 'G53.5' takes the same axis letters and sends the slides there. The words + are read as though the kinematics were the identity: each letter names the + joints the module's identity mapping gives it, which the `coordinates=` + parameter and the `[TRAJ]COORDINATES` line describe, and the two joints of + a gantry take one value together. Values are in program units, so 'G20' + scales them like any other axis word. No offset, tool length, rotation or + work plane applies. On a mill whose joint 2 carries the Z slide, 'G53.5 G0 + Z0' puts that slide at zero whatever the head is doing, where 'G53 G0 Z0' + puts the tool tip at machine Z zero. This is the move for parking, tool + change and home positions, where the slides matter and the part does not. + It is not a joint jog: the machine stays in world mode and the position on + the screen stays true through the move. ++ +An axis letter carries a unit class and a joint does not, so 'G53.5' is +refused on a machine whose letters name joints of the other kind. A serial +robot answers X with its first rotary joint, which turns in degrees, and +there the whole code is refused rather than that one letter. Which joints +turn is what `[JOINT_n] TYPE` declares. + +* 'G53.7' takes joint values, one word per joint, and works on every machine: + 'J2=-5' sends joint 2 to -5. The number after 'J' is the joint number, the + `[JOINT_n]` section and the `joint.n` HAL pins, and the value after '=' is + the joint's own position, what `joint.n.pos-cmd` shows, in the joint's own + units. Nothing is converted, not even 'G20' and 'G21'. A joint left out + keeps its position, and the two joints of a gantry pair must both be given, + with one value. This is the form for a robot, and for any machine where the + letters do not name the joints they look like. + +With 'G0' the speed comes from the joint limits in the INI file, +`[JOINT_n] MAX_VELOCITY` and `MAX_ACCELERATION`, scaled so that no joint +exceeds its own; the rapid override applies. With 'G1' the move takes the time +the straight move to the same destination would take at the programmed feed: +under G94 the F word applies to the distance between the two ends by the same +rule as a straight move, XYZ if any of them move, else UVW, else the rotary +words; under G93 the move takes 1/F minutes exactly, which is the form to use +when the ends coincide or only the joints move. The feed override applies, and +the joint limits still cap it. Nothing blends into or out of a point-to-point +move: the move before it comes to a stop and so does the move itself. + +'G53.4' is also the move <> and <> make. + +.G53.4 Example +[source,ngc] +---- +G0 X0 Y0 Z100 A0 C0 +G53.4 G0 A90 C180 (swing the rotaries round, the tip goes where the joints take it) +G0 X0 Y0 Z100 (and back on a straight line) +---- + +.G53.5 Example +[source,ngc] +---- +G53.5 G0 Z0 (the Z slide to its zero, whatever the head's tilt) +G53.5 G0 X0 Y0 B0 C0 (park X, Y and both head joints; the Z slide stays) +G93 G53.5 G1 F0.5 C180 (turn the C joint to 180 in two minutes) +G94 +---- + +.G53.7 Example +[source,ngc] +---- +G53.7 G0 J2=0 (the Z slide, joint 2, to its zero) +G53.7 G0 J0=0 J1=0 J4=0 J5=0 (park those four joints; joint 2 stays) +G93 G53.7 G1 F0.5 J5=180 (turn joint 5 to 180 in two minutes) +G94 +---- + +It is an error if: + +* Neither 'G0' nor 'G1' is in force. +* An axis letter is without a real value, or one is used that is not + configured. +* With 'G53.5', no axis word is given, a letter is used that is not a joint of + the kinematics, or the machine's letters name joints of the other kind. +* With 'G53.7', an axis word or a plain 'J' word is used, no joint word is + given, a joint number is not a whole number from 0 to 15 or is beyond the + joints of the kinematics, or one joint of a gantry pair is given without the + other or with a different value. +* With 'G53.5' or 'G53.7', polar coordinates are used, incremental distance + mode is in force, or the kinematics module cannot be evaluated by the + interpreter. +* A joint word, 'J=', is used without 'G53.7'. +* With 'G1', the feed is zero, or under G94 the two ends coincide, or feed per + revolution (G95) is in force. +* Cutter compensation is on. +* An external offset is applied when the move reaches motion. + [[gcode:g54-g59.3]] == G54-G59.3 Select Coordinate System(((G54-G59.3 Select Coordinate System))) @@ -1916,7 +2126,7 @@ G64 P0.015 Q2 image::images/G64_Heart_Q2.png["G64 Heart",align="center"] [[gcode:g68.2]] -== G68.2, G68.4, G69 Tilted Work Plane(((G68.2 Tilted Work Plane))) +== G68.2, G68.3, G68.4, G69 Tilted Work Plane(((G68.2 Tilted Work Plane))) [source,ngc] ---- @@ -1928,6 +2138,7 @@ G68.2 P2 Q2 X- Y- Z- (a second point on the plane's +X,) G68.2 P2 Q3 X- Y- Z- (a third point on its +Y side) G68.2 P3 Q1 X- Y- Z- I- J- K- (two vectors: the origin and +X, then) G68.2 P3 Q2 I- J- K- (+Z, the normal) +G68.3 X- Y- Z- (the plane from the tool direction) G68.4 ... (any G68.2 form, on the active plane) G69 (cancel) ---- @@ -1976,6 +2187,14 @@ other block in between is an error. A definition with a plane already active replaces it, with the words in the coordinate system underneath, not in the old plane. +'G68.3' takes the plane from the tool: Z is the tool axis as the rotary +joints have it at that moment, X is the default tool X of the kinematics +conventions, tool X turned about the tool axis by the smaller angle that +makes it parallel to the machine XY plane, and machine X when the tool is +vertical; 'R' turns the plane from there. It asks the kinematics module for +the tool direction, so it needs a kinematics type whose frames describe the +machine, as <> does. + 'G68.4' takes any 'G68.2' form and composes it onto the active plane: the words are in the plane, and the result is a new plane relative to the old one. It needs a plane to build on. @@ -2001,7 +2220,8 @@ from MDI shows the same way, as a square with nothing under it. an abort: the plane is not persistent and nothing about it is written to the parameter file. -Defining the plane does not move anything. +Defining the plane does not move anything. To bring the tool normal to it +use <>. While a plane is active the codes that define the coordinate system the plane sits on are refused: 'G92', 'G92.1', 'G92.2', 'G92.3', 'G52', 'G10 L2', @@ -2018,6 +2238,7 @@ plane coordinates or draw the plane. ---- G54 G68.2 X50 Y50 Z0 I30 J20 K0 (Euler: 30 about Z, 20 about the new X) +G53.1 (tool normal to it) G0 X0 Y0 Z5 (5 above the plane's origin, along its normal) G1 Z-3 F150 (a hole 3 deep, straight into the plane) G0 Z5 @@ -2035,6 +2256,8 @@ It is an error if: * The points of a 'P2' definition coincide or lie on one line, or a vector of a 'P3' definition is zero or the X direction lies along the normal. * 'G68.4' is used with no plane active. +* 'G68.3' is used where the kinematics cannot be evaluated, is the identity + kinematics, or reports no frames. * Cutter compensation is on. * Polar coordinates or a motion code are used on the same line. diff --git a/docs/src/gcode/overview.adoc b/docs/src/gcode/overview.adoc index d66e25e09ff..a05f01628c4 100644 --- a/docs/src/gcode/overview.adoc +++ b/docs/src/gcode/overview.adoc @@ -121,7 +121,7 @@ The table includes N and O for completeness, even though, as defined above, line |G | General function (See table <>) |H | Tool length offset index |I | X offset for arcs and G87 canned cycles -|J | Y offset for arcs and G87 canned cycles +|J | Y offset for arcs and G87 canned cycles; as J=, the position of joint n for <> .2+|K | Z offset for arcs and G87 canned cycles. <| Spindle-Motion Ratio for G33 synchronized movements. |L | generic parameter word for G10, M66 and others @@ -964,7 +964,7 @@ The modal groups are shown in the following Table. [width="80%",cols="4,6",options="header"] |=== |Modal Group Meaning | Member Words -|Non-modal codes (Group 0) | G4, G10 G28, G30, G52, G53, G92, G92.1, G92.2, G92.3, +|Non-modal codes (Group 0) | G4, G10 G28, G30, G52, G53, G53.1, G53.3, G53.4, G53.5, G53.6, G53.7, G92, G92.1, G92.2, G92.3, |Motion (Group 1) | G0, G1, G2, G3, G33, G38.n, G73, G76, G80, G81 G82, G83, G84, G85, G86, G87, G88, G89 |Plane selection (Group 2) | G17, G18, G19, G17.1, G18.1, G19.1 @@ -974,7 +974,7 @@ The modal groups are shown in the following Table. |Units (Group 6) | G20, G21 |Cutter Diameter Compensation (Group 7) | G40, G41, G42, G41.1, G42.1 |Tool Length Offset (Group 8) | G43, G43.1, G43.2, G43.4, G49 -|Tilted Work Plane (Group 9) | G68.2, G68.4, G69 +|Tilted Work Plane (Group 9) | G68.2, G68.3, G68.4, G69 |Canned Cycles Return Mode (Group 10) | G98, G99 |Coordinate System (Group 12) | G54, G55, G56, G57, G58, G59, G59.1, G59.2, G59.3 |Control Mode (Group 13) | G61, G61.1, G64 diff --git a/docs/src/motion/kinematics-conventions.adoc b/docs/src/motion/kinematics-conventions.adoc index f0a4846ae4c..9b22064570e 100644 --- a/docs/src/motion/kinematics-conventions.adoc +++ b/docs/src/motion/kinematics-conventions.adoc @@ -63,6 +63,16 @@ against, and the two halves of this chapter use different pairs deliberately. Positions are measured in the work frame, which is what makes a program independent of how the table is set. +With every rotary joint at zero, the work frame position is what the linear +joints read. The shipped head and table modules fold the pivot lengths and +the offsets in, so that the tool tip and the slides agree at the zero pose +and the pivot shows only as the compensation, `L(1 - cos B)` on a head, +once a rotary leaves zero. A module with a switchable identity type keeps +this of necessity, since the identity and the geometric type must agree at +the zero pose or a switch would jump the reported position; a module without +one keeps it too, so that `G53.5` slide positions and `G53` positions coincide +with the rotaries at zero. + Orientations are measured against the machine frame, and there are two of them. A module reports the tool frame and the work frame separately, each in machine coordinates. A consumer that wants the tool in workpiece coordinates composes @@ -329,6 +339,13 @@ its X points, and `G53.1` moves the rotaries to align the tool axis. Neither refuses a program for naming both directions on a five-axis machine, and neither should this. +In tree the G-code side of that is `G68.2`, which defines the plane, and +`G53.1`, `G53.3` and `G53.6`, which ask this inverse where the rotaries go, +with the plane's normal and its X as the request. Their `Q` word is the +`held` mask: `Q0` holds the joints the work frame survey finds and takes the +reported turn as the coordinate rotation it is, `Q1` holds nothing. See the +G-code chapter. + Some requests still do not pin the machine down. A five-axis machine asked to point the tool along the axis its primary rotary turns about can hold any primary angle; a machine with three orientation joints asked only for a tool @@ -563,6 +580,11 @@ Frames:: separately, each against the machine frame, so that a consumer placing both bodies can. +Zero pose:: + With every rotary joint at zero the work frame position equals the linear + joints. Pivots and offsets live in the module and show only as compensation + once a rotary moves. + Signs:: Positive A, B and C are counterclockwise about work X, Y and Z viewed from the positive end, describing the motion of the tool relative to the diff --git a/docs/src/motion/kinematics.adoc b/docs/src/motion/kinematics.adoc index e1e25b57181..7fd97dd8eaf 100644 --- a/docs/src/motion/kinematics.adoc +++ b/docs/src/motion/kinematics.adoc @@ -48,6 +48,28 @@ typically refer to the usual Cartesian coordinates. The A B C axes refer to rotational coordinates about the X Y Z axes respectively. The U V W axes refer to additional coordinates that are commonly made colinear to the X Y Z axes respectively. +[[sec:joint-space]] +=== Joint Space + +The joints the motion controller works with are the ones the kinematics +module defines: the numbers `kinematicsInverse()` produces and +`kinematicsForward()` reads, numbered as the `[JOINT_n]` sections of the INI +file and the `joint.N` HAL pins. Their zero is where homing put it and their +limits are the `[JOINT_n]` limits. _Joint space_ is the set of these +positions taken together, one coordinate per joint, as opposed to the +Cartesian coordinates of the axes. + +What sits behind a joint is a HAL matter. On most machines +`joint.N.motor-pos-cmd` drives one motor, so a joint and an actuator are the +same thing, but a joint can also drive two motors, or one motor through a +ratio, or feed a linkage computed in HAL, and the motion controller does not +know the difference. A point-to-point move, <>, runs each +of the controller's joints on a straight line from its start to its end +position at a rate within that joint's limits; the actuators behind them +follow through whatever HAL puts in between. With switchable kinematics the +joints do not change when the kinematics type does, only the mapping between +them and the axes. + == Trivial Kinematics The simplest machines are those in which which each joint is placed diff --git a/src/emc/motion/command.c b/src/emc/motion/command.c index 636bd9c11d4..7270a6abdfd 100644 --- a/src/emc/motion/command.c +++ b/src/emc/motion/command.c @@ -120,6 +120,26 @@ void emcmotApplyPendingPlannerType(void) } /* ===== END PLANNER_SWITCH_DEFER ==================================================== */ +/* the inverse once, for an endpoint, run to a fixed point: some modules + read the joints they are handed (a nutating head takes its rotary angles + from them), so one pass from a stale seed answers for the wrong angles, + and running again from its own answer settles it */ +static int inverse_settled(EmcPose *pos, double *joints, + KINEMATICS_INVERSE_FLAGS *iflags, + KINEMATICS_FORWARD_FLAGS *fflags) +{ + int pass, j; + + for (pass = 0; pass < 8; pass++) { + double prev[EMCMOT_MAX_JOINTS], worst = 0.0; + for (j = 0; j < EMCMOT_MAX_JOINTS; j++) { prev[j] = joints[j]; } + if (kinematicsInverse(pos, joints, iflags, fflags) != 0) { return -1; } + for (j = 0; j < NO_OF_KINS_JOINTS; j++) { worst = fmax(worst, fabs(joints[j] - prev[j])); } + if (worst < 1e-9) { break; } + } + return 0; +} + /* limits_ok() returns 1 if none of the hard limits are set, 0 if any are set. Called on a linear and circular move. */ STATIC int limits_ok(void) @@ -1176,7 +1196,7 @@ void emcmotCommandHandler_locked(void *arg, long servo_period) for (joint_num = 0; joint_num < EMCMOT_MAX_JOINTS; joint_num++) { start[joint_num] = (joint_num < ALL_JOINTS) ? joints[joint_num].pos_cmd : 0.0; } - if (kinematicsInverse(&goal, start, &iflags, &fflags) != 0) { + if (inverse_settled(&goal, start, &iflags, &fflags) != 0) { reportError(_("joint interpolated move on line %d: the queue end fails kinematicsInverse"), emcmotCommand->id); emcmotStatus->commandStatus = EMCMOT_COMMAND_INVALID_PARAMS; @@ -1207,7 +1227,7 @@ void emcmotCommandHandler_locked(void *arg, long servo_period) SET_MOTION_ERROR_FLAG(1); break; } - if (kinematicsInverse(&end, target, &iflags, &fflags) != 0) { + if (inverse_settled(&end, target, &iflags, &fflags) != 0) { reportError(_("joint interpolated move on line %d fails kinematicsInverse"), emcmotCommand->id); emcmotStatus->commandStatus = EMCMOT_COMMAND_INVALID_PARAMS; diff --git a/src/emc/rs274ngc/Submakefile b/src/emc/rs274ngc/Submakefile index 1a6f76f58d5..895258a6e24 100644 --- a/src/emc/rs274ngc/Submakefile +++ b/src/emc/rs274ngc/Submakefile @@ -42,7 +42,7 @@ TARGETS += ../lib/librs274.so ../lib/librs274.so.0 ../lib/librs274.so.0: $(patsubst %.cc,objects/%.o,$(LIBRS274SRCS)) \ ../lib/liblinuxcncini.so ../lib/libpyplugin.so ../lib/liblinuxcnchal.so.0 \ - ../lib/libtooldata.so.0 + ../lib/libtooldata.so.0 ../lib/libkinslimits.so.0 ../lib/libposemath.so.0 $(ECHO) Linking $(notdir $@) @mkdir -p ../lib @rm -f $@ diff --git a/src/emc/rs274ngc/interp_array.cc b/src/emc/rs274ngc/interp_array.cc index 5c115cab1ad..afcb22141b2 100644 --- a/src/emc/rs274ngc/interp_array.cc +++ b/src/emc/rs274ngc/interp_array.cc @@ -96,7 +96,7 @@ const int Interp::gees[] = { /* 460 */ -1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1, /* 480 */ -1,-1,-1,-1,-1,-1,-1,-1,-1,-1, 8,-1,-1,-1,-1,-1,-1,-1,-1,-1, /* 500 */ -1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1, -/* 520 */ 0,-1,-1,-1,-1,-1,-1,-1,-1,-1, 0,-1,-1,-1,-1,-1,-1,-1,-1,-1, +/* 520 */ 0,-1,-1,-1,-1,-1,-1,-1,-1,-1, 0, 0,-1, 0, 0, 0, 0, 0,-1,-1, /* 540 */ 12,-1,-1,-1,-1,-1,-1,-1,-1,-1,12,-1,-1,-1,-1,-1,-1,-1,-1,-1, /* 560 */ 12,-1,-1,-1,-1,-1,-1,-1,-1,-1,12,-1,-1,-1,-1,-1,-1,-1,-1,-1, /* 580 */ 12,-1,-1,-1,-1,-1,-1,-1,-1,-1,12,12,12,12,-1,-1,-1,-1,-1,-1, @@ -104,7 +104,7 @@ const int Interp::gees[] = { /* 620 */ -1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1, /* 640 */ 13,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1, /* 660 */ -1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1, -/* 680 */ -1,-1, 9,-1, 9,-1,-1,-1,-1,-1, 9,-1,-1,-1,-1,-1,-1,-1,-1,-1, +/* 680 */ -1,-1, 9, 9, 9,-1,-1,-1,-1,-1, 9,-1,-1,-1,-1,-1,-1,-1,-1,-1, /* 700 */ 1,-1,-1,-1,-1,-1,-1,-1,-1,-1, 1, 1, 1,-1,-1,-1,-1,-1,-1,-1, /* 720 */ 1, 1, 1,-1,-1,-1,-1,-1,-1,-1, 1,-1,-1,-1,-1,-1,-1,-1,-1,-1, /* 740 */ 1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1, diff --git a/src/emc/rs274ngc/interp_check.cc b/src/emc/rs274ngc/interp_check.cc index b0e398faf85..261e5060705 100644 --- a/src/emc/rs274ngc/interp_check.cc +++ b/src/emc/rs274ngc/interp_check.cc @@ -109,6 +109,30 @@ int Interp::check_g_codes(block_pointer block, //!< pointer to a block to be c (settings->distance_mode == DISTANCE_MODE::INCREMENTAL))), NCE_CANNOT_USE_G53_INCREMENTAL); } else if (mode0 == G_92) { + } else if (mode0 == G_53_1 || mode0 == G_53_6) { + CHKS((block->x_flag || block->y_flag || block->z_flag || block->a_flag || block->b_flag || + block->c_flag || block->u_flag || block->v_flag || block->w_flag), + _("Cannot use axis words with G53.1 or G53.6")); + } else if (mode0 == G_53_3) { + CHKS((block->a_flag || block->b_flag || block->c_flag || block->u_flag || block->v_flag || block->w_flag), + _("Only X, Y and Z words can be used with G53.3")); + } else if (mode0 == G_53_4 || mode0 == G_53_5 || mode0 == G_53_7) { + CHKS(((block->motion_to_be != G_0) && (block->motion_to_be != G_1)), + _("G53.4, G53.5 and G53.7 need G0 or G1")); + if (mode0 == G_53_5 || mode0 == G_53_7) { + CHKS((block->radius_flag || block->theta_flag), + _("Cannot use polar coordinates with G53.5 or G53.7")); + CHKS(((block->g_modes[GM_DISTANCE_MODE] == G_91) || + ((block->g_modes[GM_DISTANCE_MODE] != G_90) && + (settings->distance_mode == DISTANCE_MODE::INCREMENTAL))), + _("Cannot use G53.5 or G53.7 in incremental distance mode")); + } + if (mode0 == G_53_7) { + CHKS((block->x_flag || block->y_flag || block->z_flag || block->a_flag || block->b_flag || + block->c_flag || block->u_flag || block->v_flag || block->w_flag), + _("G53.7 takes joint words, J=, not axis words; G53.5 takes the axis words")); + CHKS((block->j_flag), _("Cannot use a J word with G53.7; a joint is J=")); + } } else if (mode0 == G_12_1){ // kins-switch CHKS((!block->p_flag), NCE_P_WORD_MISSING_WITH_G121); @@ -307,6 +331,13 @@ int Interp::check_other_codes(block_pointer block) //!< pointer to a block _("J word with no G2, G3, G5, G5.1, G6, G6.1, G10, G68.2, G76 or G87 to use it")); } + for (int n = 0; n < EMCMOT_MAX_JOINTS; n++) { + if (block->joint_flag[n]) { + CHKS((block->g_modes[GM_MODAL_0] != G_53_7), _("J%d= word with no G53.7 to use it"), n); + break; + } + } + if (block->k_flag) { /* could still be useless if xy_plane arc */ CHKS(((motion != G_2) && (motion != G_3) && (motion != G_6_2) && (motion != G_33) && (motion != G_33_1) && (motion != G_76) && (motion != G_87) && (block->g_modes[GM_WORK_PLANE] == -1)), @@ -329,6 +360,7 @@ int Interp::check_other_codes(block_pointer block) //!< pointer to a block if (block->p_flag) { CHKS(((block->g_modes[GM_MODAL_0] != G_10) && (block->g_modes[GM_MODAL_0] != G_4) && (block->g_modes[GM_CONTROL_MODE] != G_64 && (block->g_modes[GM_MODAL_0] != G_12_1)) && (block->g_modes[GM_WORK_PLANE] == -1) && + (block->g_modes[GM_MODAL_0] != G_53_1) && (block->g_modes[GM_MODAL_0] != G_53_3) && (block->g_modes[GM_MODAL_0] != G_53_6) && (motion != G_76) && (motion != G_82) && (motion != G_86) && (motion != G_88) && (motion != G_89) && (motion != G_5) && (motion != G_5_2) && (motion != G_70) && @@ -340,7 +372,7 @@ int Interp::check_other_codes(block_pointer block) //!< pointer to a block (block->m_modes[5] != 64) && (block->m_modes[5] != 65) && (block->m_modes[5] != 66) && (block->m_modes[7] != 19) && (block->user_m != 1) && (block->o_type != M_98)), - _("P word with no G2 G3 G4 G10 G12.1 G64 G68.2 G5 G5.2 G6, G6.2, G76 G82 G86 G88 G89" + _("P word with no G2 G3 G4 G10 G12.1 G53.1 G53.3 G53.6 G64 G68.2 G5 G5.2 G6, G6.2, G76 G82 G86 G88 G89" " or M50 M51 M52 M53 M62 M63 M64 M65 M66 M98 " "or user M code to use it")); int p_value = round_to_int(block->p_number); @@ -358,11 +390,12 @@ int Interp::check_other_codes(block_pointer block) //!< pointer to a block (block->m_modes[5] != 66) && (block->m_modes[5] != 67) && (block->m_modes[5] != 68) && (block->g_modes[GM_MODAL_0] != G_10) && (block->m_modes[6] != 61) && (block->g_modes[GM_CONTROL_MODE] != G_64) && (block->g_modes[GM_WORK_PLANE] == -1) && + (block->g_modes[GM_MODAL_0] != G_53_1) && (block->g_modes[GM_MODAL_0] != G_53_3) && (block->g_modes[GM_MODAL_0] != G_53_6) && (motion != G_70) && (motion != G_71) && (motion != G_71_1) && (motion != G_71_2) && (motion != G_72) && (motion != G_72_1) && (motion != G_72_2) && (block->m_modes[7] != 19), - _("Q word with no G5, G6, G10, G64, G68.2, G73, G76, G83, M19, M66, M67, M68 or user M code that uses it")); + _("Q word with no G5, G6, G10, G53.1, G53.3, G53.6, G64, G68.2, G73, G76, G83, M19, M66, M67, M68 or user M code that uses it")); } if (block->r_flag) { diff --git a/src/emc/rs274ngc/interp_convert.cc b/src/emc/rs274ngc/interp_convert.cc index 2412281c3c3..377e649e113 100644 --- a/src/emc/rs274ngc/interp_convert.cc +++ b/src/emc/rs274ngc/interp_convert.cc @@ -4369,7 +4369,11 @@ int Interp::convert_modal_0(int code, //!< G-code, must be from group 0 CHP(convert_axis_offsets(code, block, settings)); } else if ((code == G_5_3)||(code == G_6_3)) { // jjf CHP(convert_nurbs(code, block, settings)); - } else if ((code == G_4) || (code == G_53)); // handled elsewhere + } else if ((code == G_4) || (code == G_53) || (code == G_53_4) || (code == G_53_5) + || (code == G_53_7)); // handled elsewhere + else if ((code == G_53_1) || (code == G_53_3) || (code == G_53_6)) { + CHP(convert_orient_tool(code, block, settings)); + } else if ((code == G_12_1) || (code == G_13_1)) { // The flag makes the interpreter wait for motion to drain, so that no // motion is planned across a change of kinematics. Reading runs far @@ -5558,6 +5562,11 @@ int Interp::convert_straight(int move, //!< either G_0 or G_1 } settings->motion_mode = move; + if (block->g_modes[GM_MODAL_0] == G_53_5 || block->g_modes[GM_MODAL_0] == G_53_7) { + // the words name joints, by letter or by number: nothing below applies + CHP(convert_ptp_joints(block->g_modes[GM_MODAL_0], move, block, settings)); + return INTERP_OK; + } CHP(find_ends(block, settings, &end_x, &end_y, &end_z, &AA_end, &BB_end, &CC_end, &u_end, &v_end, &w_end)); @@ -5571,7 +5580,28 @@ int Interp::convert_straight(int move, //!< either G_0 or G_1 // Create a state tag and dump it to canon write_canon_state_tag(block, settings); - if ((settings->cutter_comp_side != CUTTER_COMP::OFF) && /* ! "== true" */ + if (block->g_modes[GM_MODAL_0] == G_53_4) { + // point-to-point: the endpoint is this Cartesian point, the path to it + // is whatever the joints make of it + CHKS((settings->cutter_comp_side != CUTTER_COMP::OFF), + _("Cannot use G53.4 with cutter radius compensation on")); + tag_straight(block,end_x, end_y); + if (move == G_0) { + JOINT_TRAVERSE(block->line_number, NULL, 0, end_x, end_y, end_z, + AA_end, BB_end, CC_end, + u_end, v_end, w_end); + } else { + double seconds; + CHP(ptp_seconds(block, settings, end_x, end_y, end_z, + AA_end, BB_end, CC_end, u_end, v_end, w_end, &seconds)); + JOINT_FEED(block->line_number, NULL, 0, end_x, end_y, end_z, + AA_end, BB_end, CC_end, + u_end, v_end, w_end, seconds); + } + settings->current_x = end_x; + settings->current_y = end_y; + settings->current_z = end_z; + } else if ((settings->cutter_comp_side != CUTTER_COMP::OFF) && /* ! "== true" */ (settings->cutter_comp_radius > 0.0)) { /* radius always is >= 0 */ CHKS((block->g_modes[GM_MODAL_0] == G_53), diff --git a/src/emc/rs274ngc/interp_internal.cc b/src/emc/rs274ngc/interp_internal.cc index ad0f886ccf6..42de545b9b7 100644 --- a/src/emc/rs274ngc/interp_internal.cc +++ b/src/emc/rs274ngc/interp_internal.cc @@ -174,7 +174,7 @@ int Interp::enhance_block(block_pointer block, //!< pointer to a block to be c mode1 = block->g_modes[GM_MOTION]; mode_zero_covets_axes = ((mode0 == G_10) || (mode0 == G_28) || (mode0 == G_30) - || (mode0 == G_52) || (mode0 == G_92)); + || (mode0 == G_52) || (mode0 == G_92) || (mode0 == G_53_3)); // a tilted work plane definition takes the axis words the same way if (block->g_modes[GM_WORK_PLANE] == G_68_2 || block->g_modes[GM_WORK_PLANE] == G_68_4) { CHKS(polar_flag, _("Polar coordinates cannot define a tilted work plane")); @@ -286,6 +286,10 @@ int Interp::init_block(block_pointer block) //!< pointer to a block to be i block->h_number = -1; block->i_flag = false; block->j_flag = false; + for (n = 0; n < EMCMOT_MAX_JOINTS; n++) { + block->joint_flag[n] = false; + block->joint_value[n] = 0.0; + } block->k_flag = false; block->l_number = -1; block->l_flag = false; diff --git a/src/emc/rs274ngc/interp_internal.hh b/src/emc/rs274ngc/interp_internal.hh index 54b10e9b834..90565ae4342 100644 --- a/src/emc/rs274ngc/interp_internal.hh +++ b/src/emc/rs274ngc/interp_internal.hh @@ -253,6 +253,12 @@ enum GCodes G_51 = 510, G_52 = 520, G_53 = 530, + G_53_1 = 531, + G_53_3 = 533, + G_53_4 = 534, + G_53_5 = 535, + G_53_6 = 536, + G_53_7 = 537, G_54 = 540, G_55 = 550, G_56 = 560, @@ -263,6 +269,7 @@ enum GCodes G_59_2 = 592, G_59_3 = 593, G_68_2 = 682, + G_68_3 = 683, G_68_4 = 684, G_69 = 690, G_61 = 610, @@ -501,6 +508,9 @@ struct block_struct bool dollar_flag{}; + bool joint_flag[EMCMOT_MAX_JOINTS]{}; // J= words, for G53.5 + double joint_value[EMCMOT_MAX_JOINTS]{}; + double radius{}; double theta{}; int radius_flag{}; @@ -761,6 +771,14 @@ struct setup int g68_seq_p; unsigned g68_seq_have; // bit per Q received double g68_seq_word[4][7]; // per Q: x y z i j k r + // the kinematics, for G68.3 and the orientation moves: loaded on first + // use through the non-realtime loader, on a HAL component of our own + void *kins_ctx; // KinematicsUserContext + int kins_comp_id; + char kins_module[LINELEN]; // [KINS] KINEMATICS, as loadrt gets it + int kins_joints; // [KINS] JOINTS + int kins_angular_joints; // bit per joint, [JOINT_n] TYPE = ANGULAR + double kins_seed[EMCMOT_MAX_JOINTS]; // the last inverse, seeding the next double parameters[interp_param_global::RS274NGC_MAX_PARAMETERS]; // system parameters int parameter_occurrence; // parameter buffer index int parameter_numbers[MAX_NAMED_PARAMETERS]; // parameter number buffer diff --git a/src/emc/rs274ngc/interp_read.cc b/src/emc/rs274ngc/interp_read.cc index 6e58d1635dc..d0a34a21b8a 100644 --- a/src/emc/rs274ngc/interp_read.cc +++ b/src/emc/rs274ngc/interp_read.cc @@ -888,11 +888,15 @@ Returned Value: int NCE_BUG_FUNCTION_SHOULD_NOT_HAVE_BEEN_CALLED 2. A j_coordinate has already been inserted in the block. NCE_MULTIPLE_J_WORDS_ON_ONE_LINE + 3. The value is followed by '=' and is not a joint number, or that joint + already has a value in the block. Side effects: counter is reset. The j_flag in the block is turned on. A j_coordinate setting is inserted in the block. + For the J= form, joint_flag[n] is turned on and joint_value[n] + is set instead; j_flag is left alone. Called by: read_one_item @@ -918,8 +922,20 @@ int Interp::read_j(char *line, //!< string: line of RS274 code being processed CHKS((line[*counter] != 'j'), NCE_BUG_FUNCTION_SHOULD_NOT_HAVE_BEEN_CALLED); *counter = (*counter + 1); - CHKS((block->j_flag), NCE_MULTIPLE_J_WORDS_ON_ONE_LINE); CHP(read_real_value(line, counter, &value, parameters)); + if (line[*counter] == '=') { + // J=: a joint value for G53.5, n is the joint number + int n = (int)value; + CHKS((value != (double)n || n < 0 || n >= EMCMOT_MAX_JOINTS), + _("The joint number in a J= word must be a whole number from 0 to %d"), EMCMOT_MAX_JOINTS - 1); + CHKS((block->joint_flag[n]), _("Multiple J%d= words on one line"), n); + *counter = (*counter + 1); + CHP(read_real_value(line, counter, &value, parameters)); + block->joint_flag[n] = true; + block->joint_value[n] = value; + return INTERP_OK; + } + CHKS((block->j_flag), NCE_MULTIPLE_J_WORDS_ON_ONE_LINE); block->j_flag = true; block->j_number = value; return INTERP_OK; diff --git a/src/emc/rs274ngc/interp_setup.cc b/src/emc/rs274ngc/interp_setup.cc index 171f7e3a1c9..4a5e3ce866d 100644 --- a/src/emc/rs274ngc/interp_setup.cc +++ b/src/emc/rs274ngc/interp_setup.cc @@ -112,6 +112,12 @@ setup::setup() : g68_seq_p(0), g68_seq_have(0), g68_seq_word{}, + kins_ctx(nullptr), + kins_comp_id(0), + kins_module{}, + kins_joints(0), + kins_angular_joints(0), + kins_seed{}, parameters{0}, parameter_occurrence(0), diff --git a/src/emc/rs274ngc/interp_workplane.cc b/src/emc/rs274ngc/interp_workplane.cc index 5f14ceea40e..12f7a1afc08 100644 --- a/src/emc/rs274ngc/interp_workplane.cc +++ b/src/emc/rs274ngc/interp_workplane.cc @@ -412,6 +412,10 @@ int Interp::convert_work_plane(int g_code, block_pointer block, setup_pointer s) _("Cannot cancel a tilted work plane with cutter radius compensation on")); return work_plane_cancel(s, true); } + if (g_code == G_68_3) { + CHKS((s->g68_seq_code != 0), _("G68.3 cannot interrupt a G68.2 sequence")); + return convert_work_plane_from_tool(block, s); + } CHKS((g_code != G_68_2 && g_code != G_68_4), "BUG: code not G68.2, G68.4 or G69"); CHKS((s->cutter_comp_side != CUTTER_COMP::OFF), @@ -435,3 +439,507 @@ int Interp::convert_work_plane(int g_code, block_pointer block, setup_pointer s) } return work_plane_set(s, g_code, origin, rotation); } + +//---------------------------------------------------------------------- +// The kinematics. G68.3 and the orientation moves need the frames and +// the tool frame inverse of the module motion runs, evaluated here, ahead +// of motion, through the loader in kinematics_userspace/. The loader +// binds its pins to a HAL component, so the interpreter makes one, named +// by its process, the first time it is asked. +//---------------------------------------------------------------------- + +#include +#include +#include "units.h" +#include +#include + +#define KINS_CTX(s) ((KinematicsUserContext *)(s)->kins_ctx) + +// the loaded module, on the kinematics type the program is in +int Interp::kins_context(setup_pointer s, void **out) +{ + KinematicsUserContext *ctx; + + *out = NULL; + if (!s->kins_ctx) { + char name[HAL_NAME_LEN + 1]; + int comp; + + CHKS((!s->kins_module[0]), + _("the INI file names no [KINS] KINEMATICS, so the kinematics cannot be evaluated here")); + CHKS((s->kins_joints < 1), + _("the INI file gives no [KINS] JOINTS, so the kinematics cannot be evaluated here")); + snprintf(name, sizeof(name), "interp.%d", (int)getpid()); + comp = hal_init(name); + CHKS((comp < 0), + _("cannot connect to HAL to evaluate the kinematics (is realtime running?)")); + s->kins_comp_id = comp; + ctx = kinematicsUserInitString(s->kins_module, s->kins_joints, comp, name); + hal_ready(comp); + CHKS((!ctx), _("kinematics module %s cannot be loaded here"), s->kins_module); + s->kins_ctx = ctx; + for (int i = 0; i < EMCMOT_MAX_JOINTS; i++) { s->kins_seed[i] = 0.0; } + } + ctx = KINS_CTX(s); + CHKS((kinematicsUserIsRtOnly(ctx)), + _("kinematics module %s cannot be evaluated outside realtime"), s->kins_module); + CHKS((kinematicsUserSetType(ctx, s->kins_type) != 0), + _("kinematics type %d is not available outside realtime"), s->kins_type); + *out = ctx; + return INTERP_OK; +} + +void Interp::kins_release(setup_pointer s) +{ + if (s->kins_ctx) { + kinematicsUserFree(KINS_CTX(s)); + s->kins_ctx = NULL; + } + if (s->kins_comp_id > 0) { + hal_exit(s->kins_comp_id); + s->kins_comp_id = 0; + } +} + +// the current point as the machine sees it: the absolute frame, in the +// machine's units, which is what the kinematics works in +void Interp::current_machine_pose(setup_pointer s, EmcPose *pose) +{ + double abs_pos[9]; + + get_abs_position(s, abs_pos); + pose->tran.x = PROGRAM_TO_USER_LEN(abs_pos[0]); + pose->tran.y = PROGRAM_TO_USER_LEN(abs_pos[1]); + pose->tran.z = PROGRAM_TO_USER_LEN(abs_pos[2]); + pose->a = PROGRAM_TO_USER_ANG(abs_pos[3]); + pose->b = PROGRAM_TO_USER_ANG(abs_pos[4]); + pose->c = PROGRAM_TO_USER_ANG(abs_pos[5]); + pose->u = PROGRAM_TO_USER_LEN(abs_pos[6]); + pose->v = PROGRAM_TO_USER_LEN(abs_pos[7]); + pose->w = PROGRAM_TO_USER_LEN(abs_pos[8]); +} + +// and back: a machine pose as program coordinates, through the chain +void Interp::machine_pose_to_program(setup_pointer s, const EmcPose *pose, double prog[9]) +{ + world_to_program_xyz(s, + USER_TO_PROGRAM_LEN(pose->tran.x), + USER_TO_PROGRAM_LEN(pose->tran.y), + USER_TO_PROGRAM_LEN(pose->tran.z), + &prog[0], &prog[1], &prog[2]); + prog[3] = USER_TO_PROGRAM_ANG(pose->a) - s->tool_offset.a - s->AA_origin_offset - s->AA_axis_offset; + prog[4] = USER_TO_PROGRAM_ANG(pose->b) - s->tool_offset.b - s->BB_origin_offset - s->BB_axis_offset; + prog[5] = USER_TO_PROGRAM_ANG(pose->c) - s->tool_offset.c - s->CC_origin_offset - s->CC_axis_offset; + prog[6] = USER_TO_PROGRAM_LEN(pose->u) - s->tool_offset.u - s->u_origin_offset - s->u_axis_offset; + prog[7] = USER_TO_PROGRAM_LEN(pose->v) - s->tool_offset.v - s->v_origin_offset - s->v_axis_offset; + prog[8] = USER_TO_PROGRAM_LEN(pose->w) - s->tool_offset.w - s->w_origin_offset - s->w_axis_offset; +} + +// the joints the machine is at, as far as the interpreter can know ahead of +// motion: the seed while it still explains the current point, since a point +// does not name one joint set, else the joints the machine stands in +int Interp::current_joints(setup_pointer s, void *vctx, double *joints) +{ + KinematicsUserContext *ctx = (KinematicsUserContext *)vctx; + EmcPose pose; + int pass, i; + + current_machine_pose(s, &pose); + for (i = 0; i < EMCMOT_MAX_JOINTS; i++) { joints[i] = s->kins_seed[i]; } + for (pass = 0; pass < 8; pass++) { + double prev[EMCMOT_MAX_JOINTS], worst = 0.0; + for (i = 0; i < EMCMOT_MAX_JOINTS; i++) { prev[i] = joints[i]; } + CHKS((kinematicsUserInverse(ctx, &pose, joints) != 0), + _("the kinematics cannot invert the current position")); + for (i = 0; i < EMCMOT_MAX_JOINTS; i++) { worst = fmax(worst, fabs(joints[i] - prev[i])); } + if (worst < 1e-9) { break; } + } + for (i = 0; i < EMCMOT_MAX_JOINTS; i++) { s->kins_seed[i] = joints[i]; } + return INTERP_OK; +} + +// a direction of the plane in world coordinates: the plane's rotation +// then the XY rotation of the coordinate system it sits on +static void plane_axis_in_world(setup_pointer s, int column, double rotation_xy, PmCartesian *out) +{ + double x = s->g68_rotation[0][column]; + double y = s->g68_rotation[1][column]; + double z = s->g68_rotation[2][column]; + double t = rotation_xy * M_PI / 180.0; + + out->x = x * cos(t) - y * sin(t); + out->y = x * sin(t) + y * cos(t); + out->z = z; +} + +static void rotate_about(const PmCartesian *axis, double rad, PmCartesian *v) +{ + // Rodrigues, for a unit axis + PmCartesian c; + double d = axis->x*v->x + axis->y*v->y + axis->z*v->z; + + pmCartCartCross(axis, v, &c); + v->x = v->x*cos(rad) + c.x*sin(rad) + axis->x*d*(1 - cos(rad)); + v->y = v->y*cos(rad) + c.y*sin(rad) + axis->y*d*(1 - cos(rad)); + v->z = v->z*cos(rad) + c.z*sin(rad) + axis->z*d*(1 - cos(rad)); +} + +// G68.3: the plane from the tool. Z is the tool axis as the joints have it, +// X the default tool X of the conventions chapter, R turns the plane from +// there; X Y Z are the origin, as G68.2's +int Interp::convert_work_plane_from_tool(block_pointer block, setup_pointer s) +{ + void *vctx; + KinematicsUserContext *ctx; + double joints[EMCMOT_MAX_JOINTS]; + PmRotationMatrix work, tool, in_work; + PmCartesian zt, xt, yt, zm, xm, x, y; + double origin[3], rotation[3][3], r, t, along; + + CHKS((s->cutter_comp_side != CUTTER_COMP::OFF), + _("Cannot define a tilted work plane with cutter radius compensation on")); + CHP(kins_context(s, &vctx)); + ctx = (KinematicsUserContext *)vctx; + CHKS((kinematicsUserIsIdentity(ctx)), + _("G68.3 needs a kinematics type that describes the machine; select it with G12.1 first")); + CHP(current_joints(s, ctx, joints)); + CHKS((kinematicsUserWorkFrame(ctx, joints, &work) != 0 + || kinematicsUserToolFrame(ctx, joints, &tool) != 0), + _("the kinematics reports no tool frame, so G68.3 cannot read the tool direction")); + toolFrameInWork(&work, &tool, &in_work); + zt = in_work.z; + xt = in_work.x; + yt = in_work.y; + // machine Z and X seen from the work: the rows of the work frame + zm.x = work.x.z; zm.y = work.y.z; zm.z = work.z.z; + xm.x = work.x.x; xm.y = work.y.x; xm.z = work.z.x; + + pmCartCartCross(&zt, &zm, &x); + if (sqrt(x.x*x.x + x.y*x.y + x.z*x.z) < 1e-9) { + // vertical: tool x is machine x, less whatever of it lies along + // the tool axis, which is rounding + along = xm.x*zt.x + xm.y*zt.y + xm.z*zt.z; + x.x = xm.x - along*zt.x; x.y = xm.y - along*zt.y; x.z = xm.z - along*zt.z; + pmCartUnitEq(&x); + } else { + // the turn about the tool axis that takes tool x into the + // machine XY plane: (cos t x + sin t y) . zm = 0, the root nearer + // to no turn at all + double xz = xt.x*zm.x + xt.y*zm.y + xt.z*zm.z; + double yz = yt.x*zm.x + yt.y*zm.y + yt.z*zm.z; + + t = atan2(-xz, yz); + if (t > M_PI / 2) { t -= M_PI; } + if (t < -M_PI / 2) { t += M_PI; } + x = xt; + rotate_about(&zt, t, &x); + } + r = block->r_flag ? block->r_number : 0.0; + rotate_about(&zt, r * M_PI / 180.0, &x); + pmCartCartCross(&zt, &x, &y); + + // from world directions to the system the plane is defined in: the + // XY rotation comes off + { + PmCartesian cols[3] = { x, y, zt }; + double c = cos(-s->rotation_xy * M_PI / 180.0), sn = sin(-s->rotation_xy * M_PI / 180.0); + + for (int j = 0; j < 3; j++) { + rotation[0][j] = cols[j].x * c - cols[j].y * sn; + rotation[1][j] = cols[j].x * sn + cols[j].y * c; + rotation[2][j] = cols[j].z; + } + } + origin[0] = block->x_flag ? block->x_number : 0.0; + origin[1] = block->y_flag ? block->y_number : 0.0; + origin[2] = block->z_flag ? block->z_number : 0.0; + return work_plane_set(s, G_68_3, origin, rotation); +} + +// G53.1, G53.3 and G53.6: the rotaries to the plane's normal. G53.1 turns +// the rotaries alone, in joint space; G53.6 keeps the tool centre point, a +// Cartesian move; G53.3 goes to X Y Z in the plane. P picks the pose, +// nearest first or by the sign of the tilting joint; Q0 holds the joints that +// carry the work (Heidenhain COORD ROT), Q1 frees them (TABLE ROT). +int Interp::convert_orient_tool(int code, block_pointer block, setup_pointer s) +{ + void *vctx; + KinematicsUserContext *ctx; + double now[EMCMOT_MAX_JOINTS]; + double solutions[TOOL_FRAME_MAX_SOLUTIONS * EMCMOT_MAX_JOINTS]; + double spin[TOOL_FRAME_MAX_SOLUTIONS]; + double distance[TOOL_FRAME_MAX_SOLUTIONS]; + int order[TOOL_FRAME_MAX_SOLUTIONS], free_dirs[TOOL_FRAME_MAX_SOLUTIONS]; + PmCartesian axis, xdir; + EmcPose end_pose; + double end_prog[9]; + unsigned int held = 0; + int p, q, n, i, j, chosen, njoints; + const double *sol; + const char *name = (code == G_53_1) ? "G53.1" : (code == G_53_3) ? "G53.3" : "G53.6"; + + CHKS((!s->g68_active), _("%s needs a tilted work plane; define one with G68.2 first"), name); + CHKS((s->cutter_comp_side != CUTTER_COMP::OFF), + _("Cannot orient the tool with cutter radius compensation on")); + p = block->p_flag ? (int)round(block->p_number) : 0; + CHKS((block->p_flag && (fabs(block->p_number - p) > 1e-9 || p < 0 || p > 2)), + _("P word with %s must be 0, 1 or 2"), name); + q = block->q_flag ? (int)round(block->q_number) : 0; + CHKS((block->q_flag && (fabs(block->q_number - q) > 1e-9 || (q != 0 && q != 1))), + _("Q word with %s must be 0 or 1"), name); + + CHP(kins_context(s, &vctx)); + ctx = (KinematicsUserContext *)vctx; + CHKS((kinematicsUserIsIdentity(ctx)), + _("%s needs a kinematics type that describes the machine; select it with G12.1 first"), name); + njoints = kinematicsUserGetNumJoints(ctx); + CHP(current_joints(s, ctx, now)); + + plane_axis_in_world(s, 2, s->rotation_xy, &axis); + plane_axis_in_world(s, 0, s->rotation_xy, &xdir); + if (q == 0) { + if (kinematicsUserWorkJoints(ctx, now, &held) != 0) { held = 0; } + } + n = kinematicsUserToolFrameInverse(ctx, &axis, &xdir, now, held, + solutions, TOOL_FRAME_MAX_SOLUTIONS, free_dirs, spin); + if (n == 0 && held) { + // nothing reachable with the work held still: let it move + held = 0; + n = kinematicsUserToolFrameInverse(ctx, &axis, &xdir, now, held, + solutions, TOOL_FRAME_MAX_SOLUTIONS, free_dirs, spin); + } + CHKS((n < 0), _("%s: the kinematics cannot answer the orientation"), name); + CHKS((n == 0), _("%s: the plane's normal cannot be reached by the rotary joints"), name); + + // nearest first, by rotary travel in joint units + for (i = 0; i < n; i++) { + distance[i] = 0.0; + for (j = 0; j < njoints; j++) { distance[i] += fabs(solutions[i*njoints + j] - now[j]); } + order[i] = i; + } + for (i = 1; i < n; i++) { + int k = order[i]; + for (j = i; j > 0 && distance[order[j-1]] > distance[k]; j--) { order[j] = order[j-1]; } + order[j] = k; + } + if (p == 0) { + chosen = order[0]; + } else { + // P names the pose rather than its rank, so that the same program + // reaches the same pose from wherever the machine is standing + int primary, secondary; + CHKS((kinematicsUserOrientJoints(ctx, now, &primary, &secondary) != 0), + _("%s P%d: the poses of this machine cannot be told apart by a tilting" + " joint, so leave P out and take the nearest"), name, p); + chosen = -1; + for (i = 0; i < n; i++) { + double value = solutions[order[i]*njoints + secondary]; + if ((p == 1 && value > 1e-9) || (p == 2 && value < -1e-9)) { + chosen = order[i]; + break; + } + } + CHKS((chosen < 0), _("%s P%d: no reachable pose has joint %d %s"), + name, p, secondary, (p == 1) ? "positive" : "negative"); + } + sol = solutions + chosen * njoints; + + // where that puts the machine, and what the program calls it + end_pose = (EmcPose){}; + current_machine_pose(s, &end_pose); + { + double full[EMCMOT_MAX_JOINTS]; + for (i = 0; i < EMCMOT_MAX_JOINTS; i++) { full[i] = (i < njoints) ? sol[i] : 0.0; } + CHKS((kinematicsUserForward(ctx, full, &end_pose) != 0), + _("%s: the kinematics cannot place the orientation it found"), name); + for (i = 0; i < EMCMOT_MAX_JOINTS; i++) { s->kins_seed[i] = full[i]; } + } + machine_pose_to_program(s, &end_pose, end_prog); + + write_canon_state_tag(block, s); + if (code == G_53_1) { + // the rotaries alone: the linear joints are where they are, since + // the solver left them at the seed, and the tool goes wherever + // that carries it + JOINT_TRAVERSE(block->line_number, sol, 1, + end_prog[0], end_prog[1], end_prog[2], + end_prog[3], end_prog[4], end_prog[5], + end_prog[6], end_prog[7], end_prog[8]); + s->current_x = end_prog[0]; + s->current_y = end_prog[1]; + s->current_z = end_prog[2]; + } else if (code == G_53_6) { + // the tool centre point stays: a Cartesian move of the rotaries + STRAIGHT_TRAVERSE(block->line_number, s->current_x, s->current_y, s->current_z, + end_prog[3], end_prog[4], end_prog[5], + s->u_current, s->v_current, s->w_current); + } else { + double x = block->x_flag ? block->x_number : s->current_x; + double y = block->y_flag ? block->y_number : s->current_y; + double z = block->z_flag ? block->z_number : s->current_z; + + JOINT_TRAVERSE(block->line_number, NULL, 0, x, y, z, + end_prog[3], end_prog[4], end_prog[5], + s->u_current, s->v_current, s->w_current); + s->current_x = x; + s->current_y = y; + s->current_z = z; + } + s->AA_current = end_prog[3]; + s->BB_current = end_prog[4]; + s->CC_current = end_prog[5]; + if (code == G_53_1) { + s->u_current = end_prog[6]; + s->v_current = end_prog[7]; + s->w_current = end_prog[8]; + } + return INTERP_OK; +} + +// how long a point-to-point feed is to take: the time the same straight +// move would take at the programmed feed +int Interp::ptp_seconds(block_pointer block, setup_pointer s, + double x, double y, double z, double a, double b, double c, + double u, double v, double w, double *seconds) +{ + if (s->feed_mode == FEED_MODE::INVERSE_TIME) { + CHKS((block->f_number <= 0.0), _("F must be positive with G93")); + *seconds = 60.0 / block->f_number; + } else if (s->feed_mode == FEED_MODE::UNITS_PER_MINUTE) { + double length = find_straight_length(x, y, z, a, b, c, u, v, w, + s->current_x, s->current_y, s->current_z, + s->AA_current, s->BB_current, s->CC_current, + s->u_current, s->v_current, s->w_current); + CHKS((length <= 0.0), + _("a point-to-point feed with no displacement has nothing to apply F to in G94 mode; use G93 or G0")); + *seconds = 60.0 * length / s->feed_rate; + } else { + ERS(_("Cannot use feed per revolution with a point-to-point move")); + } + return INTERP_OK; +} + +// The two point-to-point codes that name joints: G53.5 by axis letter through +// the module's identity mapping, refused where a letter names joints of the +// other unit class; G53.7 by J= in the joint's own units. A joint +// left out keeps its position. +int Interp::convert_ptp_joints(int code, int move, block_pointer block, setup_pointer s) +{ + void *vctx; + KinematicsUserContext *ctx; + const kins_params *p; + double joints[EMCMOT_MAX_JOINTS]; + EmcPose pose; + double prog[9]; + const int flags[9] = { block->x_flag, block->y_flag, block->z_flag, + block->a_flag, block->b_flag, block->c_flag, + block->u_flag, block->v_flag, block->w_flag }; + const double words[9] = { block->x_number, block->y_number, block->z_number, + block->a_number, block->b_number, block->c_number, + block->u_number, block->v_number, block->w_number }; + static const char letters[9] = { 'X', 'Y', 'Z', 'A', 'B', 'C', 'U', 'V', 'W' }; + const char *name = (code == G_53_5) ? "G53.5" : "G53.7"; + int a, j, njoints, given = 0; + + CHKS((s->cutter_comp_side != CUTTER_COMP::OFF), + _("Cannot use %s with cutter radius compensation on"), name); + CHP(kins_context(s, &vctx)); + ctx = (KinematicsUserContext *)vctx; + njoints = kinematicsUserGetNumJoints(ctx); + p = kinematicsUserParams(ctx); + CHP(current_joints(s, ctx, joints)); + + if (code == G_53_7) { + for (j = 0; j < EMCMOT_MAX_JOINTS; j++) { + if (!block->joint_flag[j]) { continue; } + CHKS((j >= njoints), _("G53.7: this kinematics has no joint %d"), j); + joints[j] = block->joint_value[j]; + given++; + } + CHKS((given == 0), _("G53.7 needs at least one J= joint word")); + } else { + CHKS((!p), _("G53.5: the kinematics module gives no joint mapping")); + + // A letter carries a unit class and a joint does not, so the letters + // are only a way to name joints where the mapping agrees with them. + // On a serial robot the first joint answers to X and turns in degrees, + // and the whole machine is refused rather than that one letter, since + // a mapping that lies about X is telling nothing useful about A. + for (a = 0; a < 9; a++) { + const int angular = (a >= 3 && a <= 5); + for (j = 0; j < njoints; j++) { + int turns; + if (!(p->joints_of_axis[a] & (1 << j))) { continue; } + turns = (s->kins_angular_joints & (1 << j)) ? 1 : 0; + CHKS((turns != angular), + _("G53.5: on this machine %c names joint %d, which the INI file" + " declares %s, so the axis letters do not name the joints they" + " look like; give joints by number with G53.7 J%d="), + letters[a], j, turns ? "angular" : "linear", j); + } + } + + for (a = 0; a < 9; a++) { + const int angular = (a >= 3 && a <= 5); + double value; + if (!flags[a]) { continue; } + CHKS((p->joints_of_axis[a] == 0), + _("G53.5: %c is not a joint of this kinematics"), letters[a]); + value = angular ? PROGRAM_TO_USER_ANG(words[a]) : PROGRAM_TO_USER_LEN(words[a]); + for (j = 0; j < njoints; j++) { + if (p->joints_of_axis[a] & (1 << j)) { joints[j] = value; } + } + given++; + } + CHKS((given == 0), _("G53.5 needs at least one axis word")); + } + + // the joints of a gantry pair move together: both given, one value + if (code == G_53_7) { + for (a = 0; p && a < EMCMOT_MAX_AXIS; a++) { + int bits = p->joints_of_axis[a]; + int first = -1; + if (!(bits & (bits - 1))) { continue; } + for (j = 0; j < njoints; j++) { + if (!(bits & (1 << j))) { continue; } + if (first < 0) { first = j; continue; } + CHKS((block->joint_flag[j] != block->joint_flag[first]), + _("G53.7: joints %d and %d are a pair on this kinematics, give both"), first, j); + CHKS((block->joint_flag[j] && block->joint_value[j] != block->joint_value[first]), + _("G53.7: joints %d and %d are a pair on this kinematics, give them one value"), first, j); + } + } + } + + // where that puts the tool, and what the program calls it + current_machine_pose(s, &pose); + CHKS((kinematicsUserForward(ctx, joints, &pose) != 0), + _("%s: the kinematics cannot place those joints"), name); + for (j = 0; j < EMCMOT_MAX_JOINTS; j++) { s->kins_seed[j] = joints[j]; } + machine_pose_to_program(s, &pose, prog); + + write_canon_state_tag(block, s); + if (move == G_0) { + JOINT_TRAVERSE(block->line_number, joints, 1, + prog[0], prog[1], prog[2], prog[3], prog[4], prog[5], + prog[6], prog[7], prog[8]); + } else { + double seconds; + CHP(ptp_seconds(block, s, prog[0], prog[1], prog[2], prog[3], prog[4], prog[5], + prog[6], prog[7], prog[8], &seconds)); + JOINT_FEED(block->line_number, joints, 1, + prog[0], prog[1], prog[2], prog[3], prog[4], prog[5], + prog[6], prog[7], prog[8], seconds); + } + s->current_x = prog[0]; + s->current_y = prog[1]; + s->current_z = prog[2]; + s->AA_current = prog[3]; + s->BB_current = prog[4]; + s->CC_current = prog[5]; + s->u_current = prog[6]; + s->v_current = prog[7]; + s->w_current = prog[8]; + return INTERP_OK; +} diff --git a/src/emc/rs274ngc/rs274ngc_interp.hh b/src/emc/rs274ngc/rs274ngc_interp.hh index 0b480f4ab0f..795d8d718f2 100644 --- a/src/emc/rs274ngc/rs274ngc_interp.hh +++ b/src/emc/rs274ngc/rs274ngc_interp.hh @@ -369,6 +369,17 @@ public: const double origin[3], const double rotation[3][3]); int work_plane_cancel(setup_pointer settings, bool tell_canon_anyway = false); int work_plane_check_sequence(block_pointer block, setup_pointer settings); + int convert_work_plane_from_tool(block_pointer block, setup_pointer settings); + int convert_orient_tool(int code, block_pointer block, setup_pointer settings); + int convert_ptp_joints(int code, int move, block_pointer block, setup_pointer settings); + int ptp_seconds(block_pointer block, setup_pointer settings, + double x, double y, double z, double a, double b, double c, + double u, double v, double w, double *seconds); + int kins_context(setup_pointer settings, void **ctx); + void kins_release(setup_pointer settings); + void current_machine_pose(setup_pointer settings, EmcPose *pose); + void machine_pose_to_program(setup_pointer settings, const EmcPose *pose, double prog[9]); + int current_joints(setup_pointer settings, void *ctx, double *joints); void g68_apply(setup_pointer settings, double *x, double *y, double *z); void g68_remove(setup_pointer settings, double *x, double *y, double *z); void g68_unrotate(setup_pointer settings, double *x, double *y, double *z); diff --git a/src/emc/rs274ngc/rs274ngc_pre.cc b/src/emc/rs274ngc/rs274ngc_pre.cc index ad04ae98bbe..dcb221c0e39 100644 --- a/src/emc/rs274ngc/rs274ngc_pre.cc +++ b/src/emc/rs274ngc/rs274ngc_pre.cc @@ -178,6 +178,7 @@ InterpBase *makeInterp() } Interp::~Interp() { + kins_release(&_setup); if(log_file) { if(log_file != stderr) fclose(log_file); @@ -889,6 +890,22 @@ int Interp::init() _setup.random_toolchanger = inifile.findBoolV("RANDOM_TOOLCHANGER", "EMCIO", false); _setup.num_spindles = inifile.findIntV("SPINDLES", "TRAJ", 1); + // the kinematics, for the codes that ask it something + if (auto kins = inifile.findString("KINEMATICS", "KINS")) { + snprintf(_setup.kins_module, sizeof(_setup.kins_module), "%s", kins->c_str()); + } + _setup.kins_joints = inifile.findIntV("JOINTS", "KINS", 0); + // which joints turn rather than slide, so that an axis letter is + // refused where it would name a joint of the other kind + _setup.kins_angular_joints = 0; + for (int jno = 0; jno < _setup.kins_joints && jno < EMCMOT_MAX_JOINTS; jno++) { + char section[16]; + snprintf(section, sizeof(section), "JOINT_%d", jno); + if (auto type = inifile.findString("TYPE", section)) { + if (*type == "ANGULAR") { _setup.kins_angular_joints |= 1 << jno; } + } + } + _setup.tolerance_default = inifile.findRealV("G64_DEFAULT_TOLERANCE", "RS274NGC", 0.0); _setup.naivecam_tolerance_default = inifile.findRealV("G64_DEFAULT_NAIVETOLERANCE", "RS274NGC", 0.0); diff --git a/tests/kins-twp/README b/tests/kins-twp/README index b9b3b7275fa..d114c3a2afc 100644 --- a/tests/kins-twp/README +++ b/tests/kins-twp/README @@ -1,7 +1,9 @@ The C kinematics against the tilted work plane maths. -The two nutating-head configs carry their orientation maths in python, -remap_funcs_twp.py, written independently of the kinematics modules. +The tilted work plane maths for the two nutating-head machines was first +written in python, remap_funcs_twp.py, independently of the kinematics +modules, for the remap the configs used before the interpreter learned +the codes. It lives on here, one copy per machine, as the oracle. This test loads each module in realtime and puts its tool frame next to the python transformation matrix over a grid of head angles, and its tool frame inverse next to the python candidate joint angles and virtual diff --git a/tests/kins-twp/test.sh b/tests/kins-twp/test.sh index 86b77f6ff44..d50c9bea68e 100755 --- a/tests/kins-twp/test.sh +++ b/tests/kins-twp/test.sh @@ -1,10 +1,6 @@ #!/bin/bash set -e -# RIP layout: $HEADERS is $TOPDIR/include -TOPDIR=$(dirname "$HEADERS") -CONFIGS=$TOPDIR/configs/sim/axis/vismach/5axis/table-rotary_spindle-rotary-nutating - ${SUDO} halcompile --install twpcheck.c >/dev/null # One hal file per machine. twpcheck answers frame and inverse requests @@ -18,8 +14,8 @@ run() { printf 'loadrt threads name1=t1 period1=1000000\n' printf 'addf twpcheck t1\n' printf 'start\n' - printf 'loadusr -w python3 check.py %s %s/%s-trsrn_twp %s/twp-%s.ini\n' \ - "$machine" "$CONFIGS" "$machine" "$PWD" "$machine" + printf 'loadusr -w python3 check.py %s %s/%s %s/twp-%s.ini\n' \ + "$machine" "$PWD" "$machine" "$PWD" "$machine" } > "$hal" echo "=== $machine" halrun -f "$hal" diff --git a/configs/sim/axis/vismach/5axis/table-rotary_spindle-rotary-nutating/xyzacb-trsrn_twp/remap_funcs_twp.py b/tests/kins-twp/xyzacb/remap_funcs_twp.py similarity index 100% rename from configs/sim/axis/vismach/5axis/table-rotary_spindle-rotary-nutating/xyzacb-trsrn_twp/remap_funcs_twp.py rename to tests/kins-twp/xyzacb/remap_funcs_twp.py diff --git a/configs/sim/axis/vismach/5axis/table-rotary_spindle-rotary-nutating/xyzbca-trsrn_twp/remap_funcs_twp.py b/tests/kins-twp/xyzbca/remap_funcs_twp.py similarity index 100% rename from configs/sim/axis/vismach/5axis/table-rotary_spindle-rotary-nutating/xyzbca-trsrn_twp/remap_funcs_twp.py rename to tests/kins-twp/xyzbca/remap_funcs_twp.py diff --git a/tests/ptp-robot/README b/tests/ptp-robot/README new file mode 100644 index 00000000000..8fda2b1b350 --- /dev/null +++ b/tests/ptp-robot/README @@ -0,0 +1,6 @@ +The point-to-point moves on a serial robot. + +pumakins maps X, Y and Z to its first three joints, which turn rather +than slide, so G53.5 refuses the machine and says which joint gives it +away. G53.7 names joints by number and works, and G53.4 still takes a +Cartesian destination. diff --git a/tests/ptp-robot/checkresult b/tests/ptp-robot/checkresult new file mode 100755 index 00000000000..9d48d3f180e --- /dev/null +++ b/tests/ptp-robot/checkresult @@ -0,0 +1,3 @@ +#!/bin/sh +# the test script counts its own failures +grep -q "^Exiting with 0 errors" "$1" diff --git a/tests/ptp-robot/sim.hal b/tests/ptp-robot/sim.hal new file mode 100644 index 00000000000..e92c60eb526 --- /dev/null +++ b/tests/ptp-robot/sim.hal @@ -0,0 +1,16 @@ +loadrt [KINS]KINEMATICS +loadrt [EMCMOT]EMCMOT servo_period_nsec=[EMCMOT]SERVO_PERIOD num_joints=[KINS]JOINTS + +addf motion-command-handler servo-thread +addf motion-controller servo-thread + +net J0 joint.0.motor-pos-cmd => joint.0.motor-pos-fb +net J1 joint.1.motor-pos-cmd => joint.1.motor-pos-fb +net J2 joint.2.motor-pos-cmd => joint.2.motor-pos-fb +net J3 joint.3.motor-pos-cmd => joint.3.motor-pos-fb +net J4 joint.4.motor-pos-cmd => joint.4.motor-pos-fb +net J5 joint.5.motor-pos-cmd => joint.5.motor-pos-fb + +net estop-loop iocontrol.0.user-enable-out iocontrol.0.emc-enable-in +net tool-prep-loop iocontrol.0.tool-prepare iocontrol.0.tool-prepared +net tool-change-loop iocontrol.0.tool-change iocontrol.0.tool-changed diff --git a/tests/ptp-robot/skip b/tests/ptp-robot/skip new file mode 100755 index 00000000000..a12f31a77c2 --- /dev/null +++ b/tests/ptp-robot/skip @@ -0,0 +1,4 @@ +#!/bin/sh +# Builds a realtime component with halcompile, which needs the build +# tools present. Skip when testing installed packages. +[ -z "$SYSTEM_BUILD" ] diff --git a/tests/ptp-robot/test-ui.py b/tests/ptp-robot/test-ui.py new file mode 100755 index 00000000000..92362c901a7 --- /dev/null +++ b/tests/ptp-robot/test-ui.py @@ -0,0 +1,99 @@ +#!/usr/bin/env python3 +# A serial robot has no axis letter that names the joint it looks like, so +# the letter form of the point-to-point move is refused here and the joint +# form is the one that works. +import linuxcnc +import sys +import time + +JOINTS = 6 + +c = linuxcnc.command() +s = linuxcnc.stat() +e = linuxcnc.error_channel() + +errors = 0 + +def error(what): + global errors + errors += 1 + print("*** ERROR %s" % what) + +def settled(): + deadline = time.time() + 60 + last = None + while time.time() < deadline: + s.poll() + now = [s.joint_position[i] for i in range(JOINTS)] + if s.inpos and not s.queue and now == last: + return now + last = now + time.sleep(0.05) + error("timed out waiting for the move") + return last + +def drain(): + while e.poll(): + pass + +def mdi(cmd): + c.mdi(cmd) + c.wait_complete(60) + return settled() + +def refused(cmd, expect): + c.mdi(cmd) + c.wait_complete(30) + m = e.poll() + if not m or m[0] not in (linuxcnc.NML_ERROR, linuxcnc.OPERATOR_ERROR): + error("%s was accepted" % cmd) + return + if expect not in m[1]: + error("%s said %r, which does not mention %r" % (cmd, m[1].strip(), expect)) + else: + print("refused as expected: %s" % m[1].strip()) + drain() + +c.state(linuxcnc.STATE_ESTOP_RESET) +c.state(linuxcnc.STATE_ON) +c.wait_complete(30) +c.home(-1) +c.wait_complete(60) +c.mode(linuxcnc.MODE_MDI) +c.wait_complete(30) +drain() + +# the joint form moves the joints it names and leaves the rest alone +before = mdi("G53.7 G0 J0=0 J1=0 J2=0 J3=0 J4=0 J5=0") +after = mdi("G53.7 G0 J1=-20 J4=35") +print("G53.7 G0 J1=-20 J4=35 %s" % " ".join("%.4f" % v for v in after)) +drain() +if abs(after[1] + 20) > 1e-6 or abs(after[4] - 35) > 1e-6: + error("G53.7 left joints 1 and 4 at %.6f and %.6f" % (after[1], after[4])) +for j in (0, 2, 3, 5): + if abs(after[j] - before[j]) > 1e-6: + error("G53.7 moved joint %d from %.9f to %.9f" % (j, before[j], after[j])) + +# the letter form is refused whichever letter is used, because X names the +# first rotary joint here; the message says so and points at G53.7 +refused("G53.5 G0 X10", "joint 0") +refused("G53.5 G0 A10", "G53.7") +refused("G53.5 G0 Z0", "angular") + +# and the code that takes a Cartesian target still works: the point the +# robot is standing on is reachable by definition, so ask for it +s.poll() +here = list(s.position[:3]) +mdi("G53.7 G0 J1=0 J4=0") +# a serial robot reaches one point with more than one set of joints, so +# only the point is checked, not the pose it comes back in +back = mdi("G53.4 G0 X%.6f Y%.6f Z%.6f" % (here[0], here[1], here[2])) +drain() +s.poll() +if max(abs(a - b) for a, b in zip(s.position[:3], here)) > 1e-3: + error("G53.4 landed at %s, not at %s" + % (["%.4f" % v for v in s.position[:3]], ["%.4f" % v for v in here])) +print("G53.4 back to the same point %s" % " ".join("%.4f" % v for v in back)) + +print("Exiting with %d errors" % errors) +sys.exit(1 if errors else 0) diff --git a/tests/ptp-robot/test.ini b/tests/ptp-robot/test.ini new file mode 100644 index 00000000000..4570f1389e8 --- /dev/null +++ b/tests/ptp-robot/test.ini @@ -0,0 +1,134 @@ +[EMC] +VERSION = 1.1 +DEBUG = 0 + +[DISPLAY] +DISPLAY = ./test-ui.py + +[RS274NGC] +RS274NGC_STARTUP_CODE = G17 G21 G40 G49 G54 G64 P0.001 G80 G90 G92.1 G94 G97 G98 +PARAMETER_FILE = sim.var + +[KINS] +KINEMATICS = pumakins +JOINTS = 6 + +[HAL] +HALFILE = sim.hal +HALCMD = setp pumakins.A2 300 +HALCMD = setp pumakins.A3 50 +HALCMD = setp pumakins.D3 70 +HALCMD = setp pumakins.D4 400 +HALCMD = setp pumakins.D6 80 + +[TRAJ] +COORDINATES = XYZABC +LINEAR_UNITS = mm +ANGULAR_UNITS = deg +DEFAULT_LINEAR_VELOCITY = 100 +MAX_LINEAR_VELOCITY = 120 +MAX_LINEAR_ACCELERATION = 700 +DEFAULT_LINEAR_ACCELERATION = 300 +NO_FORCE_HOMING = 1 + +[EMCMOT] +EMCMOT = motmod +SERVO_PERIOD = 1000000 +COMM_TIMEOUT = 4 + +[TASK] +TASK = milltask +CYCLE_TIME = 0.010 + +[EMCIO] +TOOL_TABLE = tool.tbl + +[AXIS_X] +MIN_LIMIT = -5000 +MAX_LIMIT = 5000 +MAX_VELOCITY = 200 +MAX_ACCELERATION = 700 + +[AXIS_Y] +MIN_LIMIT = -5000 +MAX_LIMIT = 5000 +MAX_VELOCITY = 200 +MAX_ACCELERATION = 700 + +[AXIS_Z] +MIN_LIMIT = -5000 +MAX_LIMIT = 5000 +MAX_VELOCITY = 200 +MAX_ACCELERATION = 700 + +[AXIS_A] +MIN_LIMIT = -360 +MAX_LIMIT = 360 +MAX_VELOCITY = 90 +MAX_ACCELERATION = 900 + +[AXIS_B] +MIN_LIMIT = -185 +MAX_LIMIT = 185 +MAX_VELOCITY = 90 +MAX_ACCELERATION = 900 + +[AXIS_C] +MIN_LIMIT = -320 +MAX_LIMIT = 320 +MAX_VELOCITY = 90 +MAX_ACCELERATION = 900 + +[JOINT_0] +TYPE = ANGULAR +HOME = 0 +MAX_VELOCITY = 90 +MAX_ACCELERATION = 900 +MIN_LIMIT = -360 +MAX_LIMIT = 360 +HOME_SEQUENCE = 0 + +[JOINT_1] +TYPE = ANGULAR +HOME = 0 +MAX_VELOCITY = 90 +MAX_ACCELERATION = 900 +MIN_LIMIT = -360 +MAX_LIMIT = 360 +HOME_SEQUENCE = 0 + +[JOINT_2] +TYPE = ANGULAR +HOME = 0 +MAX_VELOCITY = 90 +MAX_ACCELERATION = 900 +MIN_LIMIT = -360 +MAX_LIMIT = 360 +HOME_SEQUENCE = 0 + +[JOINT_3] +TYPE = ANGULAR +HOME = 0 +MAX_VELOCITY = 90 +MAX_ACCELERATION = 900 +MIN_LIMIT = -360 +MAX_LIMIT = 360 +HOME_SEQUENCE = 0 + +[JOINT_4] +TYPE = ANGULAR +HOME = 0 +MAX_VELOCITY = 90 +MAX_ACCELERATION = 900 +MIN_LIMIT = -360 +MAX_LIMIT = 360 +HOME_SEQUENCE = 0 + +[JOINT_5] +TYPE = ANGULAR +HOME = 0 +MAX_VELOCITY = 90 +MAX_ACCELERATION = 900 +MIN_LIMIT = -360 +MAX_LIMIT = 360 +HOME_SEQUENCE = 0 diff --git a/tests/ptp-robot/test.sh b/tests/ptp-robot/test.sh new file mode 100755 index 00000000000..765cf14fed6 --- /dev/null +++ b/tests/ptp-robot/test.sh @@ -0,0 +1,4 @@ +#!/bin/bash -e +# a failed run leaves the var file behind, and it carries offsets +rm -f sim.var sim.var.bak +linuxcnc -r test.ini diff --git a/tests/ptp-robot/tool.tbl b/tests/ptp-robot/tool.tbl new file mode 100644 index 00000000000..2028da29213 --- /dev/null +++ b/tests/ptp-robot/tool.tbl @@ -0,0 +1 @@ +T1 P1 Z25 D6 diff --git a/tests/twp-native/README b/tests/twp-native/README new file mode 100644 index 00000000000..9a49937006d --- /dev/null +++ b/tests/twp-native/README @@ -0,0 +1,13 @@ +The tilted work plane on the xyzacb nutating-head sim, natively. + +G12.1 P1 selects the TCP kinematics, G68.2 defines a plane, and the +orientation moves are checked against the python maths in +tests/kins-twp/xyzacb: G53.1 lands the head on one of the oracle's +angle pairs and leaves the linear joints where they were all through the +move; G53.6 leaves the tool tip where it was; G53.3 ends at the point +asked for in the plane; a move along plane X goes along plane X in the +world; G68.3 reads the plane back off the oriented tool; G69 cancels. +Q1 lets the table take part. The point-to-point moves are checked too: +G53.4 G0 to a program point, G53.5 and G53.7 G0 to a slide position with +the head tilted, G53.4 G1 taking the time the straight move would in G94 and +in G93, and what they refuse. diff --git a/tests/twp-native/abort.ngc b/tests/twp-native/abort.ngc new file mode 100644 index 00000000000..217d67e407a --- /dev/null +++ b/tests/twp-native/abort.ngc @@ -0,0 +1,11 @@ +(a plane, then a long move to be stopped in the middle) +(the cancel at the end is one the read ahead reaches long before the machine does) +G21 G90 G94 +G12.1 P1 +G0 X0 Y0 Z0 A0 B0 C0 +G68.2 P1 Q123 I30 J20 K0 +G53.1 +G1 F300 Z-40 +G1 Z0 +G69 +M2 diff --git a/tests/twp-native/sim.hal b/tests/twp-native/sim.hal new file mode 100644 index 00000000000..e92c60eb526 --- /dev/null +++ b/tests/twp-native/sim.hal @@ -0,0 +1,16 @@ +loadrt [KINS]KINEMATICS +loadrt [EMCMOT]EMCMOT servo_period_nsec=[EMCMOT]SERVO_PERIOD num_joints=[KINS]JOINTS + +addf motion-command-handler servo-thread +addf motion-controller servo-thread + +net J0 joint.0.motor-pos-cmd => joint.0.motor-pos-fb +net J1 joint.1.motor-pos-cmd => joint.1.motor-pos-fb +net J2 joint.2.motor-pos-cmd => joint.2.motor-pos-fb +net J3 joint.3.motor-pos-cmd => joint.3.motor-pos-fb +net J4 joint.4.motor-pos-cmd => joint.4.motor-pos-fb +net J5 joint.5.motor-pos-cmd => joint.5.motor-pos-fb + +net estop-loop iocontrol.0.user-enable-out iocontrol.0.emc-enable-in +net tool-prep-loop iocontrol.0.tool-prepare iocontrol.0.tool-prepared +net tool-change-loop iocontrol.0.tool-change iocontrol.0.tool-changed diff --git a/tests/twp-native/skip b/tests/twp-native/skip new file mode 100755 index 00000000000..a12f31a77c2 --- /dev/null +++ b/tests/twp-native/skip @@ -0,0 +1,4 @@ +#!/bin/sh +# Builds a realtime component with halcompile, which needs the build +# tools present. Skip when testing installed packages. +[ -z "$SYSTEM_BUILD" ] diff --git a/tests/twp-native/test-ui.py b/tests/twp-native/test-ui.py new file mode 100755 index 00000000000..4c15ce9bfca --- /dev/null +++ b/tests/twp-native/test-ui.py @@ -0,0 +1,477 @@ +#!/usr/bin/env python3 +# The tilted work plane on the xyzacb nutating-head sim, natively: see README. + +import linuxcnc +import hal +import sys +import os +import time +import math +import numpy as np + +TOPDIR = os.path.abspath(os.path.join(os.path.dirname(__file__), "..", "..")) +sys.path.insert(0, os.path.join(TOPDIR, "tests", "kins-twp", "xyzacb")) +import remap_funcs_twp as twp + +JOINTS = 6 +TABLE, SECONDARY, PRIMARY = 3, 4, 5 # A, B, C +I4 = np.asmatrix(np.identity(4)) + +class Log: + def debug(self, *a): pass + def error(self, *a): print("oracle:", a) +log = Log() + +c = linuxcnc.command() +s = linuxcnc.stat() +e = linuxcnc.error_channel() + +c.state(linuxcnc.STATE_ESTOP_RESET) +c.state(linuxcnc.STATE_ON) +c.home(-1) +c.wait_complete() +c.mode(linuxcnc.MODE_MDI) + +errors = 0 + +def error(msg): + global errors + errors += 1 + print("*** ERROR " + msg) + +def drain(): + while True: + m = e.poll() + if not m: + return + print("channel:", m) + if m[0] in (linuxcnc.NML_ERROR, linuxcnc.OPERATOR_ERROR): + error("reported: %s" % m[1]) + +def settled(): + deadline = time.time() + 60 + last = None + while time.time() < deadline: + s.poll() + now = [s.joint_position[i] for i in range(JOINTS)] + if s.inpos and not s.queue and now == last: + return now + last = now + time.sleep(0.05) + error("timed out waiting for the move") + return last + +def mdi(*cmds): + for cmd in cmds: + c.mdi(cmd) + c.wait_complete(60) + return settled() + +# run one command and sample joints and positions on the way +# status is a task-cycle snapshot with the feedback a servo cycle behind the +# command, so two equal polls inside one task cycle do not mean the move is +# over: the last sample is taken once the move has settled +def sampled(cmd): + c.mdi(cmd) + samples = [] + t0 = time.time() + while time.time() - t0 < 60: + s.poll() + samples.append(([s.joint_position[i] for i in range(JOINTS)], list(s.position))) + if s.inpos and not s.queue and len(samples) > 20 and samples[-1] == samples[-2]: + break + time.sleep(0.005) + c.wait_complete(60) + end = settled() + s.poll() + samples.append(([s.joint_position[i] for i in range(JOINTS)], list(s.position))) + return end, samples + +# a point-to-point move runs every joint on a straight line in joint space, +# all together: the fraction of the way each moving joint has gone is the +# same for all of them at every sample, never goes back, and reaches one +def joint_line(what, samples, start, end): + moving = [i for i in range(JOINTS) if abs(end[i] - start[i]) > 1e-6] + if not moving: + error("%s: no joint moved" % what) + return + last = 0.0 + for n, (j, p) in enumerate(samples): + fs = [(j[i] - start[i]) / (end[i] - start[i]) for i in moving] + f = sum(fs) / len(fs) + if max(abs(x - f) for x in fs) > 1e-3: + error("%s: joints out of step at sample %d of %d: %s" % (what, n, len(samples), fs)) + return + if f < last - 1e-6: + error("%s: the joint path ran backwards at sample %d of %d (%.4f after %.4f)" % (what, n, len(samples), f, last)) + return + if f < -1e-6 or f > 1 + 1e-6: + error("%s: a joint left its segment at sample %d of %d (fraction %.4f)" % (what, n, len(samples), f)) + return + last = f + if last < 1 - 1e-6: + error("%s: the last sample is short of the end (fraction %.4f)" % (what, last)) + print("%s: %d joints on one line in joint space through %d samples" % (what, len(moving), len(samples))) + +def show(what, j): + print("%-26s %s" % (what, " ".join("%.4f" % v for v in j))) + +def wrap(d): + return (d + 180.0) % 360.0 - 180.0 + +def close(a, b, tol=1e-6): + return all(abs(x - y) <= tol for x, y in zip(a, b)) + +# the pairs the oracle would keep for a tool axis, in machine coordinates, +# as (b, c) in degrees, and which is nearest to the head's present angles +def oracle_pairs(z): + t1s, t2s = twp.kins_calc_possible_joint_angles(log, list(z), None) + pairs = [] + for t1 in t1s or []: + for t2 in t2s or []: + m = twp.kins_calc_transformation_matrix(t1, t2, 0, I4, 'inv') + got = [m[0, 2], m[1, 2], m[2, 2]] + if close(got, z, 1e-6): + pairs.append((math.degrees(t2), math.degrees(t1))) + return pairs + +def nearest_pair(pairs, b_now, c_now): + return min(pairs, key=lambda p: abs(wrap(p[0] - b_now)) + abs(wrap(p[1] - c_now))) + +def tool_axis(joints): + # the tool axis in work coordinates: the head as the oracle models it, + # brought into the table's frame the way the module reports the work + m = twp.kins_calc_transformation_matrix(math.radians(joints[PRIMARY]), + math.radians(joints[SECONDARY]), 0, I4, 'inv') + zm = np.array([m[0, 2], m[1, 2], m[2, 2]]) + a = math.radians(joints[TABLE]) + W = np.array([[1, 0, 0], [0, math.cos(a), math.sin(a)], [0, -math.sin(a), math.cos(a)]]) + return W.T.dot(zm) + +def plane_axes(): + s.poll() + r = s.g68_rotation + return ([r[0], r[3], r[6]], [r[1], r[4], r[7]], [r[2], r[5], r[8]]) + +def rot_x(d): + r = math.radians(d) + return np.array([[1, 0, 0], [0, math.cos(r), -math.sin(r)], [0, math.sin(r), math.cos(r)]]) + +def rot_y(d): + r = math.radians(d) + return np.array([[math.cos(r), 0, math.sin(r)], [0, 1, 0], [-math.sin(r), 0, math.cos(r)]]) + +# --- the plane, and G53.1 with the table held --------------------------- +start = mdi("G12.1 P1", "G0 X0 Y0 Z0 A0 B0 C0") +show("start", start) +R = rot_y(20).dot(rot_x(30)) +mdi("G68.2 P1 Q123 I30 J20 K0") +after, samples = sampled("G53.1") +show("G53.1", after) +drain() +s.poll() +if not s.g68_active: + error("G68.2 did not leave a plane active") +if not close(plane_axes()[2], list(R[:, 2]), 1e-9): + error("status reports a different plane normal than the program defined") +pairs = oracle_pairs(list(R[:, 2])) +print("oracle pairs (b, c):", ["(%.4f, %.4f)" % p for p in pairs]) +want = nearest_pair(pairs, start[SECONDARY], start[PRIMARY]) +if not pairs or abs(wrap(after[SECONDARY] - want[0])) > 1e-3 or abs(wrap(after[PRIMARY] - want[1])) > 1e-3: + error("G53.1 landed on (%.4f, %.4f), the oracle's nearest pair is (%.4f, %.4f)" + % (after[SECONDARY], after[PRIMARY], want[0], want[1])) +if abs(after[TABLE] - start[TABLE]) > 1e-9: + error("G53.1 moved the table with Q0") +worst = max(abs(smp[0][i] - start[i]) for smp in samples for i in range(3)) +print("linear joints moved at most %.9f through G53.1" % worst) +if worst > 1e-6: + error("G53.1 moved a linear joint") +if not close(tool_axis(after), list(R[:, 2]), 1e-6): + error("the tool axis after G53.1 is not the plane normal") +joint_line("G53.1", samples, start, after) + +# P names the pose rather than its rank: P1 is the one with the secondary +# rotary positive and P2 the one with it negative, from wherever the +# machine is standing, while no P is the nearest and so does depend on it +poses = {} +for where in ("G0 A0 B0 C0", "G0 A0 B-40 C170"): + for word in ("", "P1", "P2"): + mdi("G69") + mdi(where) + mdi("G68.2 P1 Q123 I30 J20 K0") + got = mdi("G53.1 %s" % word) + poses.setdefault(word, []).append(got) + drain() +for word, sign in (("P1", 1), ("P2", -1)): + a, b = poses[word] + rot = lambda j: [j[TABLE], j[SECONDARY], j[PRIMARY]] + if max(abs(wrap(x - y)) for x, y in zip(rot(a), rot(b))) > 1e-4: + error("G53.1 %s landed differently from two starting poses: %s and %s" + % (word, rot(a), rot(b))) + if sign * a[SECONDARY] <= 0: + error("G53.1 %s put the secondary rotary at %.4f" % (word, a[SECONDARY])) +if abs(poses["P1"][0][SECONDARY] - poses["P2"][0][SECONDARY]) < 1e-6: + error("G53.1 P1 and P2 chose the same pose") +if abs(poses[""][0][SECONDARY] - poses[""][1][SECONDARY]) < 1e-6: + error("G53.1 with no P gave the same pose from both starts, so it is not the nearest") +print("G53.1 P1 %.4f, P2 %.4f, no P %.4f then %.4f (secondary rotary)" + % (poses["P1"][0][SECONDARY], poses["P2"][0][SECONDARY], + poses[""][0][SECONDARY], poses[""][1][SECONDARY])) +c.mdi("G53.1 P3") +c.wait_complete(30) +m = e.poll() +if not m or m[0] not in (linuxcnc.NML_ERROR, linuxcnc.OPERATOR_ERROR): + error("G53.1 P3 was accepted") +drain() +mdi("G69") +mdi("G0 X0 Y0 Z0 A0 B0 C0") +mdi("G68.2 P1 Q123 I30 J20 K0") +after = mdi("G53.1") + +# --- moves in the plane go along the plane's axes in the world ---------- +# G53.1 swung the tool tip, so it is somewhere in the plane; a move to X10 +# travels along the plane's X by the difference +def in_plane(): + s.poll() + return R.T.dot(np.array(s.position[:3]) - np.array(s.g68_offset[:3])), list(s.position[:3]) +q0, p0 = in_plane() +mdi("G0 X10") +q1, p1 = in_plane() +d = [p1[i] - p0[i] for i in range(3)] +want = list((10 - q0[0]) * R[:, 0]) +if not close(d, want, 1e-3) or abs(q1[0] - 10) > 1e-3: + error("G0 X10 in the plane moved the tool by %s, expected %s" % (d, want)) +mdi("G0 Z5") +q2, p2 = in_plane() +d = [p2[i] - p1[i] for i in range(3)] +want = list((5 - q1[2]) * R[:, 2]) +if not close(d, want, 1e-3) or abs(q2[2] - 5) > 1e-3: + error("G0 Z5 in the plane moved the tool by %s, expected %s" % (d, want)) + +# --- G53.6 keeps the tool centre point -------------------------------- +mdi("G69") +R2 = rot_y(20).dot(rot_x(-30)) +mdi("G68.2 P1 Q123 I-30 J20 K0") +s.poll(); before = list(s.position) +after, samples = sampled("G53.6") +show("G53.6", after) +drain() +worst = max(abs(smp[1][i] - before[i]) for smp in samples for i in range(3)) +print("tool tip moved at most %.6f through G53.6" % worst) +if worst > 1e-3: + error("G53.6 moved the tool tip") +if not close(tool_axis(after), list(R2[:, 2]), 1e-6): + error("the tool axis after G53.6 is not the plane normal") + +# --- G53.3 goes to a point in the plane with the tool oriented ---------- +before = mdi("G69") +R3 = rot_y(-25).dot(rot_x(35)) +mdi("G68.2 P1 Q123 I35 J-25 K0") +after, samples = sampled("G53.3 X5 Y5 Z5") +show("G53.3", after) +drain() +joint_line("G53.3", samples, before, after) +s.poll() +prog = R3.T.dot(np.array(s.position[:3]) - np.array(s.g68_offset[:3])) +if not close(list(prog), [5, 5, 5], 1e-3): + error("G53.3 ended at %s in the plane, not 5 5 5" % list(prog)) +if not close(tool_axis(after), list(R3[:, 2]), 1e-6): + error("the tool axis after G53.3 is not the plane normal") + +# --- G68.3 reads the plane back off the tool --------------------------- +mdi("G69", "G68.3 X1 Y2 Z3") +drain() +s.poll() +x, y, z = plane_axes() +if not s.g68_active or not close(list(s.g68_offset[:3]), [1, 2, 3], 1e-9): + error("G68.3 did not set the origin asked for") +if not close(z, list(R3[:, 2]), 1e-6): + error("G68.3's normal %s is not the tool axis %s" % (z, list(R3[:, 2]))) +if abs(x[2]) > 1e-6: + error("G68.3's X is not parallel to the machine XY plane: %s" % x) +mdi("G69", "G68.3 R90") +xr, yr, zr = plane_axes() +if not close(xr, y, 1e-6): + error("G68.3 R90 did not turn the plane about its normal") + +# --- Q1 lets the table take part --------------------------------------- +mdi("G69", "G0 X0 Y0 Z0 A0 B0 C0") +R4 = rot_x(30) +mdi("G68.2 P1 Q123 I30 J0 K0") +after = mdi("G53.1 Q1") +show("G53.1 Q1", after) +drain() +if not close(tool_axis(after), list(R4[:, 2]), 1e-6): + error("the tool axis after G53.1 Q1 is not the plane normal") +# with the plane X requested as well and the table free, three joints +# place three constraints: the plane's X is reached by the machine rather +# than by the frame +m = twp.kins_calc_transformation_matrix(math.radians(after[PRIMARY]), + math.radians(after[SECONDARY]), 0, I4, 'inv') +a = math.radians(after[TABLE]) +W = np.array([[1, 0, 0], [0, math.cos(a), math.sin(a)], [0, -math.sin(a), math.cos(a)]]) +xm = np.array([m[0, 0], m[1, 0], m[2, 0]]) +if not close(list(W.T.dot(xm)), list(R4[:, 0]), 1e-6): + error("with Q1 the machine did not place the plane's X") + +# --- G53.4, G53.5 and G69 ------------------------------------------------ +before = mdi("G69") +after, samples = sampled("G53.4 G0 X0 Y0 Z0 A0 B0 C0") +show("G53.4 G0", after) +drain() +joint_line("G53.4 G0", samples, before, after) +s.poll() +if s.g68_active: + error("G69 left the plane active") +if not close(list(s.g68_rotation), [1, 0, 0, 0, 1, 0, 0, 0, 1], 1e-12): + error("G69 left a rotation in status") +if not close(list(s.position[:3]), [0, 0, 0], 1e-6) or any(abs(after[j]) > 1e-6 for j in (TABLE, SECONDARY, PRIMARY)): + error("G53.4 G0 did not bring the tool and the rotaries back to zero") + +# G53.7 takes joint values by joint number: with the head tilted, J2=-5 +# puts joint 2 at -5 whatever that does to the tool tip, and touches no +# other joint; the value is the joint's own, untouched by G20 +before = mdi("G0 B30") +show("before G53.7", before) +after = mdi("G20 G53.7 G0 J2=-5") +mdi("G21") +show("G53.7 G0 J2=-5", after) +drain() +if abs(after[2] + 5) > 1e-6: + error("G53.7 J2=-5 left joint 2 at %.6f" % after[2]) +for j in (0, 1, 3, 4, 5): + if abs(after[j] - before[j]) > 1e-6: + error("G53.7 J2=-5 moved joint %d from %.9f to %.9f" % (j, before[j], after[j])) +after = mdi("G53.7 G0 J2=0 J[2+2]=0") +if abs(after[2]) > 1e-6 or abs(after[SECONDARY]) > 1e-6: + error("G53.7 J2=0 J4=0 did not put joints 2 and 4 at zero") + +# G53.5 takes the same destination by axis letter, in program units: Z-5 +# is joint 2 in millimetres, and under G20 the same words are inches +before = mdi("G0 B30") +after = mdi("G53.5 G0 Z-5") +show("G53.5 G0 Z-5", after) +drain() +if abs(after[2] + 5) > 1e-6: + error("G53.5 Z-5 left joint 2 at %.6f" % after[2]) +for j in (0, 1, 3, 4, 5): + if abs(after[j] - before[j]) > 1e-6: + error("G53.5 Z-5 moved joint %d from %.9f to %.9f" % (j, before[j], after[j])) +after = mdi("G20 G53.5 G0 Z-1") +mdi("G21") +show("G20 G53.5 G0 Z-1", after) +if abs(after[2] + 25.4) > 1e-6: + error("an inch of G53.5 Z left joint 2 at %.6f, not -25.4" % after[2]) +after = mdi("G53.5 G0 Z0 B0") +if abs(after[2]) > 1e-6 or abs(after[SECONDARY]) > 1e-6: + error("G53.5 Z0 B0 did not put joints 2 and 4 at zero") + +# a point-to-point feed takes the time the straight move would: 10 mm at +# F600 is one second, and F30 in G93 is two +def timed(cmd): + c.mdi(cmd) + first = last = None + t0 = time.time() + s.poll(); start = [s.joint_position[i] for i in range(JOINTS)] + while time.time() - t0 < 60: + s.poll() + now = [s.joint_position[i] for i in range(JOINTS)] + if now != start: + if first is None: + first = time.time() + last = time.time() + start = now + elif first is not None and s.inpos and not s.queue and time.time() - last > 0.3: + break + time.sleep(0.005) + c.wait_complete(60) + settled() + return (last - first) if first else 0.0 +mdi("G0 X0 Y0 Z0 A0 B0 C0") +took = timed("G53.4 G1 X10 F600") +print("G53.4 G1 X10 F600 took %.3f s" % took) +if not 0.7 < took < 1.5: + error("a 10 mm point-to-point feed at F600 took %.3f s, not about one" % took) +took = timed("G93 G53.4 G1 X0 F30") +mdi("G94") +print("G93 G53.4 G1 X0 F30 took %.3f s" % took) +if not 1.6 < took < 2.6: + error("a point-to-point feed at G93 F30 took %.3f s, not about two" % took) +drain() + +# what the point-to-point codes refuse +for cmd in ("G53.4 G2 X1 I1", "G91 G53.7 G0 J0=1", "G53.7 G0 J9=1", "G53.7 G0 X1", + "G53.7 G0 J1", "G53.7 G0", "G0 J0=1", "G53.7 G0 J0.5=1", "G53.7 G0 J0=1 J0=2", + "G53.5 G0 J0=1", "G53.5 G0", "G91 G53.5 G0 X1", "G53.4 G1 F0 X1"): + c.mdi(cmd) + c.wait_complete(30) + m = e.poll() + if not m or m[0] not in (linuxcnc.NML_ERROR, linuxcnc.OPERATOR_ERROR): + error("%s was accepted" % cmd) + else: + print("refused as expected:", m[1]) + c.mode(linuxcnc.MODE_MDI) +mdi("G90 G94 G0 X0 Y0 Z0 A0 B0 C0") + +# --- a plane refuses what would move the ground under it --------------- +# each refusal is an interpreter error, and the abort that follows cancels +# the plane, so it is defined afresh before every one +for cmd in ("G92 X1", "G55", "G10 L2 P1 X1"): + mdi("G68.2 P1 Q123 I30 J0 K0") + c.mdi(cmd) + c.wait_complete(30) + m = e.poll() + if not m or m[0] not in (linuxcnc.NML_ERROR, linuxcnc.OPERATOR_ERROR): + error("%s was accepted while a plane is active" % cmd) + else: + print("refused as expected:", m[1]) + c.mode(linuxcnc.MODE_MDI) + s.poll() + if s.g68_active: + error("the abort after %s left the plane active" % cmd) +mdi("G69") + +# --- stopping a program that has a plane --------------------------------- +# The read ahead runs the program's own G69 long before the machine gets +# there, so the cancel is sitting in the queue when the stop button throws +# the queue away. The plane in status has to end up cancelled all the same, +# and a G69 typed afterwards has to be able to say so again. +c.mode(linuxcnc.MODE_AUTO) +c.wait_complete(30) +c.program_open("abort.ngc") +c.auto(linuxcnc.AUTO_RUN, 0) +deadline = time.time() + 30 +while time.time() < deadline: + s.poll() + if s.g68_active and s.current_line >= 8: + break + time.sleep(0.02) +s.poll() +if not s.g68_active: + error("the program never reported a plane to stop in the middle of") +c.abort() +c.wait_complete(30) +c.mode(linuxcnc.MODE_MDI) +c.wait_complete(30) +drain() +s.poll() +if s.g68_active: + error("stopping the program left the plane active in status") +if not close(list(s.g68_rotation), [1, 0, 0, 0, 1, 0, 0, 0, 1], 1e-12): + error("stopping the program left a rotation in status") +mdi("G69") +s.poll() +if s.g68_active: + error("G69 after the stop did not clear the plane") +print("a stopped program leaves no plane behind") +mdi("G0 X0 Y0 Z0 A0 B0 C0") +drain() + +for f in ("sim.var", "sim.var.bak"): + try: + os.unlink(f) + except OSError: + pass + +print("Exiting with %d errors" % errors) +sys.exit(1 if errors else 0) diff --git a/tests/twp-native/test.ini b/tests/twp-native/test.ini new file mode 100644 index 00000000000..a3a6bcf9e1a --- /dev/null +++ b/tests/twp-native/test.ini @@ -0,0 +1,147 @@ +[EMC] +VERSION = 1.1 +DEBUG = 0 + +[DISPLAY] +DISPLAY = ./test-ui.py + +[RS274NGC] +RS274NGC_STARTUP_CODE = G17 G21 G40 G49 G54 G64 P0.001 G80 G90 G92.1 G94 G97 G98 +PARAMETER_FILE = sim.var + +[KINS] +KINEMATICS = xyzacb_trsrn +JOINTS = 6 + +# what the python oracle reads +[TWP] +PRIMARY = C +SECONDARY = B + +[HAL] +HALFILE = sim.hal +HALCMD = setp xyzacb_trsrn_kins.nut-angle 45 +HALCMD = setp xyzacb_trsrn_kins.y-pivot 100 +HALCMD = setp xyzacb_trsrn_kins.z-pivot 200 +HALCMD = setp xyzacb_trsrn_kins.x-offset 5 +HALCMD = setp xyzacb_trsrn_kins.y-offset 7 +HALCMD = setp xyzacb_trsrn_kins.y-rot-axis 300 +HALCMD = setp xyzacb_trsrn_kins.z-rot-axis 400 + +[TRAJ] +COORDINATES = XYZABC +LINEAR_UNITS = mm +ANGULAR_UNITS = deg +DEFAULT_LINEAR_VELOCITY = 100 +MAX_LINEAR_VELOCITY = 120 +MAX_LINEAR_ACCELERATION = 700 +DEFAULT_LINEAR_ACCELERATION = 300 +NO_FORCE_HOMING = 1 + +[EMCMOT] +EMCMOT = motmod +SERVO_PERIOD = 1000000 +COMM_TIMEOUT = 4 + +[TASK] +TASK = milltask +CYCLE_TIME = 0.010 + +[EMCIO] +TOOL_TABLE = tool.tbl + +[AXIS_X] +MIN_LIMIT = -5000 +MAX_LIMIT = 5000 +MAX_VELOCITY = 200 +MAX_ACCELERATION = 700 + +[AXIS_Y] +MIN_LIMIT = -5000 +MAX_LIMIT = 5000 +MAX_VELOCITY = 200 +MAX_ACCELERATION = 700 + +[AXIS_Z] +MIN_LIMIT = -5000 +MAX_LIMIT = 5000 +MAX_VELOCITY = 200 +MAX_ACCELERATION = 700 + +[AXIS_A] +MIN_LIMIT = -360 +MAX_LIMIT = 360 +MAX_VELOCITY = 90 +MAX_ACCELERATION = 900 + +[AXIS_B] +MIN_LIMIT = -185 +MAX_LIMIT = 185 +MAX_VELOCITY = 90 +MAX_ACCELERATION = 900 + +[AXIS_C] +MIN_LIMIT = -320 +MAX_LIMIT = 320 +MAX_VELOCITY = 90 +MAX_ACCELERATION = 900 + +[JOINT_0] +TYPE = LINEAR +HOME = 0 +MAX_VELOCITY = 200 +MAX_ACCELERATION = 700 +MAX_JERK = 7000 +MIN_LIMIT = -5000 +MAX_LIMIT = 5000 +HOME_SEQUENCE = 0 + +[JOINT_1] +TYPE = LINEAR +HOME = 0 +MAX_VELOCITY = 200 +MAX_ACCELERATION = 700 +MAX_JERK = 7000 +MIN_LIMIT = -5000 +MAX_LIMIT = 5000 +HOME_SEQUENCE = 0 + +[JOINT_2] +TYPE = LINEAR +HOME = 0 +MAX_VELOCITY = 200 +MAX_ACCELERATION = 700 +MAX_JERK = 7000 +MIN_LIMIT = -5000 +MAX_LIMIT = 5000 +HOME_SEQUENCE = 0 + +[JOINT_3] +TYPE = ANGULAR +HOME = 0 +MAX_VELOCITY = 90 +MAX_ACCELERATION = 900 +MAX_JERK = 9000 +MIN_LIMIT = -360 +MAX_LIMIT = 360 +HOME_SEQUENCE = 0 + +[JOINT_4] +TYPE = ANGULAR +HOME = 0 +MAX_VELOCITY = 90 +MAX_ACCELERATION = 900 +MAX_JERK = 9000 +MIN_LIMIT = -185 +MAX_LIMIT = 185 +HOME_SEQUENCE = 0 + +[JOINT_5] +TYPE = ANGULAR +HOME = 0 +MAX_VELOCITY = 90 +MAX_ACCELERATION = 900 +MAX_JERK = 9000 +MIN_LIMIT = -320 +MAX_LIMIT = 320 +HOME_SEQUENCE = 0 diff --git a/tests/twp-native/test.sh b/tests/twp-native/test.sh new file mode 100755 index 00000000000..765cf14fed6 --- /dev/null +++ b/tests/twp-native/test.sh @@ -0,0 +1,4 @@ +#!/bin/bash -e +# a failed run leaves the var file behind, and it carries offsets +rm -f sim.var sim.var.bak +linuxcnc -r test.ini diff --git a/tests/twp-native/tool.tbl b/tests/twp-native/tool.tbl new file mode 100644 index 00000000000..2028da29213 --- /dev/null +++ b/tests/twp-native/tool.tbl @@ -0,0 +1 @@ +T1 P1 Z25 D6 From 9d924ab65ebab719e755a32d0a92ae9fdf43eb1c Mon Sep 17 00:00:00 2001 From: Luca Toniolo <10792599+grandixximo@users.noreply.github.com> Date: Sat, 5 Sep 2026 00:58:37 +1000 Subject: [PATCH 47/60] configs: the nutating-head sims on the native tilted work plane The two trsrn_twp configs drop the python remap of G68.2, G68.3, G68.4, G69, G53.1, G53.3 and G53.6, its ngc wrappers, the abort handler that unwound it and the twp-status analog pin: the interpreter does all of it now, with nothing to restore on abort. The helper component that feeds the vismach model reads the plane from status. The demos select the TCP kinematics up front, since the orientation codes need it, and put kinematics 0 back at the end; square.ngc loses its G52, which a plane refuses. --- .../README | 33 +- .../demos/incremental_repetition.ngc | 2 + .../incremental_repetition_back_and_forth.ngc | 2 + .../demos/incremental_repetition_g533.ngc | 4 +- .../demos/simple_example.ngc | 2 + .../demos/square.ngc | 1 - .../python/remap.py | 1574 ----------------- .../python/toplevel.py | 20 - .../python/twp-helper-comp.py | 94 +- .../python/util.py | 67 - .../remap_subs/g531remap.ngc | 14 - .../remap_subs/g533remap.ngc | 20 - .../remap_subs/g536remap.ngc | 14 - .../remap_subs/g69remap.ngc | 11 - .../remap_subs/on_abort_with_twp_reset.ngc | 15 - .../xyzacb-trsrn_twp/xyzacb-trsrn.ini | 39 - .../xyzbca-trsrn_twp/xyzbca-trsrn.ini | 40 +- tests/twp-native/checkresult | 3 + tests/twp-native/test-ui.py | 12 +- 19 files changed, 63 insertions(+), 1904 deletions(-) delete mode 100755 configs/sim/axis/vismach/5axis/table-rotary_spindle-rotary-nutating/python/remap.py delete mode 100755 configs/sim/axis/vismach/5axis/table-rotary_spindle-rotary-nutating/python/toplevel.py delete mode 100755 configs/sim/axis/vismach/5axis/table-rotary_spindle-rotary-nutating/python/util.py delete mode 100755 configs/sim/axis/vismach/5axis/table-rotary_spindle-rotary-nutating/remap_subs/g531remap.ngc delete mode 100755 configs/sim/axis/vismach/5axis/table-rotary_spindle-rotary-nutating/remap_subs/g533remap.ngc delete mode 100755 configs/sim/axis/vismach/5axis/table-rotary_spindle-rotary-nutating/remap_subs/g536remap.ngc delete mode 100755 configs/sim/axis/vismach/5axis/table-rotary_spindle-rotary-nutating/remap_subs/g69remap.ngc delete mode 100755 configs/sim/axis/vismach/5axis/table-rotary_spindle-rotary-nutating/remap_subs/on_abort_with_twp_reset.ngc create mode 100755 tests/twp-native/checkresult diff --git a/configs/sim/axis/vismach/5axis/table-rotary_spindle-rotary-nutating/README b/configs/sim/axis/vismach/5axis/table-rotary_spindle-rotary-nutating/README index fea7c5d4882..ba7fafdbe6d 100644 --- a/configs/sim/axis/vismach/5axis/table-rotary_spindle-rotary-nutating/README +++ b/configs/sim/axis/vismach/5axis/table-rotary_spindle-rotary-nutating/README @@ -1,27 +1,20 @@ This is a simulation configuration for a 6 axis machine with one table rotary and two spindle rotary joints -This simulation also includes a python remap of Gcodes for tilted workplane (TWP) functionality. -Both the kinematic and the twp remap support nutation of the secondary rotary joints (ie A or B ) form 0 to 90°. -Hence this also works for the 'usual' orthogonal spindle rotary-tilt type machines by setting the nutation angle to 90°. +This simulation uses the interpreter's tilted work plane codes, documented in the G-code section of the manual: -Implemented TWP functionality: -G68.2 : defines twp using euler-angles, pitch-roll-yaw, 2-vectors, 3 points, optionally with offset in XYZ and rotation in XY -G68.3 : defines twp from current tool orientation, optionally with offset in XYZ and rotation in XY -G68.4 : same as G68.2 but as an incremental definition from an active TWP plane -G69 : cancels the current twp (resets all parameters, moves to G54 and sets Identity kinematics) -G53.1 (P) : spindle orientation without tcp, switches to G59 and activates tool kinematics -G53.3 (P XYZ) : same as G53.1 but with simultaneous move the the XYZ coords on the twp plane -G53.6 (P) : same as G53.1 but spindle orientation with tcp +G68.2 : defines the plane by three angles, three points or two vectors, with an origin in XYZ and a turn R about the plane's Z +G68.3 : defines the plane from the current tool direction, with an origin in XYZ and a turn R +G68.4 : any G68.2 form, composed onto the active plane +G69 : cancels the plane +G53.1 (P Q) : orients the tool to the plane, rotaries only, the linear joints stay where they are +G53.3 (P Q XYZ) : orients the tool and moves to XYZ in the plane, interpolated in joint space +G53.6 (P Q) : orients the tool with the tool centre point held -- Spindle is C primary, A secondary or B secondary as defined in the [TWP] section of the ini file -- All G53.x commands will respect axis limits as set in the ini file for the respective primary and secondary spindle joints. -- The P word sets the orientation strategy: 0(default)=shortest distance, - 1=positive rotation only, - 2=negative rotation only - (this applies to the primary rotary, the secondary moves the shortest distance) +The orientation codes need the TCP kinematics (G12.1 P1). P picks the solution, nearest to the present rotary position first. Q0 holds the table and lets the head do it, Q1 lets the table take part as well. + +The kinematic supports nutation of the secondary rotary joint (A or B) from 0 to 90 degrees, so this also covers the usual orthogonal spindle rotary-tilt machines by setting the nutation angle to 90 degrees. + +The python maths this configuration used to carry as a remap lives on in tests/kins-twp, where the kinematics module is checked against it. For more: https://forum.linuxcnc.org/show-your-stuff/49103-kinematic-model-for-a-5axis-mill-with-universal-nutating-head?start=0#271334 - -Full Documentation can be found at: -https://github.com/Sigma1912/LinuxCNC_Demo_Configs/tree/main/table-rotary_spindle-rotary-nutating/Documentation diff --git a/configs/sim/axis/vismach/5axis/table-rotary_spindle-rotary-nutating/demos/incremental_repetition.ngc b/configs/sim/axis/vismach/5axis/table-rotary_spindle-rotary-nutating/demos/incremental_repetition.ngc index 31feddcff12..4dc18624cf2 100644 --- a/configs/sim/axis/vismach/5axis/table-rotary_spindle-rotary-nutating/demos/incremental_repetition.ngc +++ b/configs/sim/axis/vismach/5axis/table-rotary_spindle-rotary-nutating/demos/incremental_repetition.ngc @@ -1,4 +1,5 @@ G69 +g12.1 p1 (the TCP kinematics: the plane codes need it) g10 l2 p0 x1000 y-1000 z-1000 m6 t3 g43 h3 g68.2 q121 i25 j-10 @@ -13,5 +14,6 @@ o100 REPEAT[100] g0 y50 o100 ENDREPEAT g69 +g13.1 M2 diff --git a/configs/sim/axis/vismach/5axis/table-rotary_spindle-rotary-nutating/demos/incremental_repetition_back_and_forth.ngc b/configs/sim/axis/vismach/5axis/table-rotary_spindle-rotary-nutating/demos/incremental_repetition_back_and_forth.ngc index 37b6f85032d..e1d5bb5d34b 100644 --- a/configs/sim/axis/vismach/5axis/table-rotary_spindle-rotary-nutating/demos/incremental_repetition_back_and_forth.ngc +++ b/configs/sim/axis/vismach/5axis/table-rotary_spindle-rotary-nutating/demos/incremental_repetition_back_and_forth.ngc @@ -1,4 +1,5 @@ g69 +g12.1 p1 (the TCP kinematics: the plane codes need it) g10 l2 p0 x1300 y-200 z-1400 m6 t3 g43 h3 g68.2 q121 i0 j5 @@ -20,4 +21,5 @@ o100 REPEAT[1000] o100 ENDREPEAT g69 +g13.1 M2 diff --git a/configs/sim/axis/vismach/5axis/table-rotary_spindle-rotary-nutating/demos/incremental_repetition_g533.ngc b/configs/sim/axis/vismach/5axis/table-rotary_spindle-rotary-nutating/demos/incremental_repetition_g533.ngc index f01324916b5..0a2e4731539 100644 --- a/configs/sim/axis/vismach/5axis/table-rotary_spindle-rotary-nutating/demos/incremental_repetition_g533.ngc +++ b/configs/sim/axis/vismach/5axis/table-rotary_spindle-rotary-nutating/demos/incremental_repetition_g533.ngc @@ -1,4 +1,5 @@ G69 +g12.1 p1 (the TCP kinematics: the plane codes need it) g10 l2 p0 x1000 y-1000 z-1000 m6 t3 g43 h3 g68.2 q121 i25 j-10 @@ -7,7 +8,7 @@ o100 REPEAT[100] g68.4 q131 i-35 j-35 k0 ;g53.3 p0 x50y50z150 g53.6 - x50y50z150 + g0 x50y50z150 g0 z100 g0 x-50 g0 y-50 @@ -16,5 +17,6 @@ o100 REPEAT[100] g0 x0y0z120 o100 ENDREPEAT g69 +g13.1 M2 diff --git a/configs/sim/axis/vismach/5axis/table-rotary_spindle-rotary-nutating/demos/simple_example.ngc b/configs/sim/axis/vismach/5axis/table-rotary_spindle-rotary-nutating/demos/simple_example.ngc index 7586f2f1d3c..bc2f46c6b6b 100644 --- a/configs/sim/axis/vismach/5axis/table-rotary_spindle-rotary-nutating/demos/simple_example.ngc +++ b/configs/sim/axis/vismach/5axis/table-rotary_spindle-rotary-nutating/demos/simple_example.ngc @@ -1,4 +1,6 @@ g69 +g13.1 +g12.1 p1 (the TCP kinematics: the plane codes need it) g10 l2 p0 x1300 y-200 z-1400 m6 t3 g43 h3 g0 x0y0z100 diff --git a/configs/sim/axis/vismach/5axis/table-rotary_spindle-rotary-nutating/demos/square.ngc b/configs/sim/axis/vismach/5axis/table-rotary_spindle-rotary-nutating/demos/square.ngc index c7e263ce601..c1f2bbf2abe 100644 --- a/configs/sim/axis/vismach/5axis/table-rotary_spindle-rotary-nutating/demos/square.ngc +++ b/configs/sim/axis/vismach/5axis/table-rotary_spindle-rotary-nutating/demos/square.ngc @@ -5,5 +5,4 @@ osub g0 x50 g0 y50 g0 x0y0z120 - g52 x0y0z0 oendsub diff --git a/configs/sim/axis/vismach/5axis/table-rotary_spindle-rotary-nutating/python/remap.py b/configs/sim/axis/vismach/5axis/table-rotary_spindle-rotary-nutating/python/remap.py deleted file mode 100755 index 05fc53261a1..00000000000 --- a/configs/sim/axis/vismach/5axis/table-rotary_spindle-rotary-nutating/python/remap.py +++ /dev/null @@ -1,1574 +0,0 @@ -# This is a python remap for LinuxCNC implementing 'Tilted Work Plane' -# G68.2, G68.3, G68.4 and related Gcodes G53.1, G53.3, G53.6, G69 -# -# Copyright ()c) 2025 David Mueller -# -# This program is free software: you can redistribute it and/or modify -# it under the terms of the GNU General Public License as published by -# the Free Software Foundation, either version 2 of the License, or -# (at your option) any later version. -# -# This program is distributed in the hope that it will be useful, -# but WITHOUT ANY WARRANTY; without even the implied warranty of -# MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the -# GNU General Public License for more details. -# -''' -The remap does the following: - -- Parses the G68.[2,4] gcodes and constructs the requested tool orientation vectors (x,z). -- Writes and reads hal pins created and updated by'twp-helper-comp.py' (mostly for updating the gui). -- Parses the G53.[1,3,6] and uses the functions in 'remap_funcs_twp.py' to calculate all rotary joint position that result in the correct tool orientation (there may be more than just one). -- Selects the appropriate rotary angles that will respect rotary limits set in the ini file and also follow any orientation strategy requested by the operator using the 'P' word. -- Sets the kinematic modes -- Calculates new work offset values so the WCS origin after switching to TWP mode is in the requested physical position. -- Used MDI commands to: - - Move the rotary joints to the calculated positions - - Switch the WCS system to 'G59' and set the values of G59, G59.[1,2.3] to the calculated coordinates -- Parses the G69 gcodes, resets the relevant parameters and switches back to Identity kinematic mode -''' - - -import sys -import traceback -import numpy as np -from math import sin,cos,tan,asin,acos,atan,atan2,sqrt,pi,degrees,radians,fabs -from interpreter import * -import emccanon -from util import lineno, call_pydevd -import hal - -# logging -import logging -# this name will be printed first on each log message -log = logging.getLogger('remap.py; TWP') -# we have to setup a handler to be able to set the log level for this module -handler = logging.StreamHandler() -formatter = logging.Formatter('%(name)s %(levelname)s: %(message)s') -handler.setFormatter(formatter) -log.addHandler(handler) - -# set up parsing of the inifile -import os -import linuxcnc -# get the path for the ini file used to start this config -inifile = os.environ.get("INI_FILE_NAME") - -# adding the remap_funcs folder to the system path. The machine specific -# functions live beside the ini file, which is the working directory, and the -# parent is searched too so a config may keep them one level up and share them -# between variants. -cwd = os.getcwd() -parent = os.path.abspath(os.path.join(cwd, os.pardir)) -sys.path.insert(0, parent) -sys.path.insert(0, cwd) -from remap_funcs_twp import * - -# instantiate the LinuxCNC ini-parser -config = linuxcnc.ini(inifile) - -# debug setting -try: - debug_setting = config.getint('TWP', 'LOG_LEVEL', fallback=1) - if debug_setting > 4: debug_setting = 4 - if debug_setting < 0: debug_setting = 0 -except Exception as error: - debug_setting = 1 - log.warning("Unable to parse debug setting given in INI. Setting it to 1.") -debug_levels = (logging.CRITICAL, logging.ERROR, logging.WARNING, logging.INFO, logging.DEBUG) -log.setLevel(debug_levels[debug_setting]) - -## ROTARY JOINT LETTERS -# primary rotary joint (independent of the secondary joint) -joint_letter_primary = config.getstring('TWP', 'PRIMARY', fallback="").capitalize() -# secondary rotary joint (dependent on the primary joint) -joint_letter_secondary = config.getstring('TWP', 'SECONDARY', fallback="").capitalize() - -if not joint_letter_primary in ('A','B','C') or not joint_letter_secondary in ('A','B','C'): - log.error("Unable to parse joint letters given in INI [TWP].") -elif joint_letter_primary == joint_letter_secondary: - log.error("Letters for primary and secondary joints in INI [TWP] must not be the same.") -else: - # get the MIN/MAX limits of the respective rotary joint letters - category = 'AXIS_' + joint_letter_primary - primary_min_limit = radians(config.getreal(category, 'MIN_LIMIT', fallback=0.0)) - primary_max_limit = radians(config.getreal(category, 'MAX_LIMIT', fallback=0.0)) - log.info('Joint letter for primary is %s with MIN/MAX limits: %s,%s', - joint_letter_primary, degrees(primary_min_limit), degrees(primary_max_limit)) - category = 'AXIS_' + joint_letter_secondary - secondary_min_limit = radians(config.getreal(category, 'MIN_LIMIT', fallback=0.0)) - secondary_max_limit = radians(config.getreal(category, 'MAX_LIMIT', fallback=0.0)) - log.info('Joint letter for secondary is %s with MIN/MAX Limits: %s,%s', - joint_letter_secondary, degrees(secondary_min_limit), degrees(secondary_max_limit)) - - -## CONNECTIONS TO THE HELPER COMPONENT -twp_comp = 'twp-helper-comp.' -twp_is_defined = twp_comp + 'twp-is-defined' -twp_is_active = twp_comp + 'twp-is-active' - -# Which rotary joint should be prioritized when calculating optimal joint rotation angles -try: - optimization_priority = config.getint('TWP', 'PRIORITY', fallback=1) -except Exception as error: - log.warning("Unable to parse orientation priority given in INI. Setting it to 1.") - optimization_priority = 1 - -# raise InterpreterException if execute() or read() fail -throw_exceptions = 1 - -## VALUE INITIALIZATION -# we start with the identity matrix (ie the twp is equal to the world coordinates) -twp_matrix = np.asmatrix(np.identity(4)) - -# some g68.2 p-word modes require several calls to enter all the required parameters so we -# need a flag that indicates when the twp has been defined and is ready for G53.n -# [current p-word, number of calls required, (state of calls required for that p mode added by g68.2)] -# note that we use string since boolean True == 1, which gives wrong results if we want -# to count the elements that are True because it is counted as integer '1' -# eg: twp_flag = [0, 1, 'empty'] -twp_flag = [] -# we need a place to store the twp-build-parameters if the mode needs more than one call -twp_build_params = {} -# container to store the current work offset during twp operations -current_work_offset_number = 1 -saved_work_offset = [0,0,0] -# orientation mode refers to the strategy used to choose from the different rotary angles for a given -# z-vector vector. The optimization is applied to the primary axis only with mode 0 (shortest path) being -# the default. (0=shortest_path , 1=positive_rotation only, 2=negative_rotation only, ) -orient_mode = 0 - - - -# define the basic rotation matrices -def Rx(th): - return np.array([[1, 0 , 0 ], - [0, cos(th), -sin(th)], - [0, sin(th), cos(th)]]) - -def Ry(th): - return np.array([[ cos(th), 0, sin(th)], - [ 0 , 1, 0 ], - [-sin(th), 0, cos(th)]]) - -def Rz(th): - return np.array([[cos(th), -sin(th), 0], - [sin(th), cos(th), 0], - [0 , 0 , 1]]) - - - -def calc_euler_rot_matrix(th1, th2, th3, order): # expects radians - # returns the rotation matrices for given order and angles - log.debug('Entering: %s', sys._getframe( ).f_code.co_name) - debug_msg = (f' Euler order {order} requested with angles: ' - f'{degrees(th1):.4f}, {degrees(th2):.4f}, {degrees(th3):.4f}') - log.debug(debug_msg) - if order == '131': - matrix = np.dot(np.dot(Rx(th1), Rz(th2)), Rx(th3)) - elif order=='121': - matrix = np.dot(np.dot(Rx(th1), Ry(th2)), Rx(th3)) - elif order=='212': - matrix = np.dot(np.dot(Ry(th1), Rx(th2)), Ry(th3)) - elif order=='232': - matrix = np.dot(np.dot(Ry(th1), Rz(th2)), Ry(th3)) - elif order=='323': - matrix = np.dot(np.dot(Rz(th1), Ry(th2)), Rz(th3)) - elif order=='313': - matrix = np.dot(np.dot(Rz(th1), Rx(th2)), Rz(th3)) - elif order=='123': - matrix = np.dot(np.dot(Rx(th1), Ry(th2)), Rz(th3)) - elif order=='132': - matrix = np.dot(np.dot(Rx(th1), Rz(th2)), Ry(th3)) - elif order=='213': - matrix = np.dot(np.dot(Ry(th1), Rx(th2)), Rz(th3)) - elif order=='231': - matrix = np.dot(np.dot(Ry(th1), Rz(th2)), Rx(th3)) - elif order=='321': - matrix = np.dot(np.dot(Rz(th1), Ry(th2)), Rx(th3)) - elif order=='312': - matrix = np.dot(np.dot(Rz(th1), Rx(th2)), Ry(th3)) - #log.debug(' Returning euler rotation as matrix: \n %s', matrix) - return matrix - - -def calc_joint_angles(z_vector_req, x_vector_req): - # returns a list of valid primary/secondary rotary joint positions in radians for a given orientation vector - # returns an empty list if no valid position could be found - log.debug('Entering: %s', sys._getframe( ).f_code.co_name) - log.debug(' z_vector_requested: %s', z_vector_req) - log.debug(' x_vector_requested: %s', x_vector_req) - # set the tolerance value - epsilon = 0.0001 - # create np.array so we can easily calculate differences and check elements - z_vector_req = np.array([z_vector_req[0], z_vector_req[1], z_vector_req[2]]) - # calculate joint values using kinematic specific formula - try: - (theta_1_calcd, theta_2_calcd) = kins_calc_possible_joint_angles(log, z_vector_req, x_vector_req) - except Exception as error: - log.error('Remap_funcs: kins_calc_possible_joint_angles failure, %s', error) - - # remove any duplicate values from the results - theta_1_calcd = tuple(set(theta_1_calcd)) - theta_2_calcd = tuple(set(theta_2_calcd)) - log.debug(' Got possible angles theta_1: ' + ' '.join("{:.4f}°".format(degrees(theta)) for theta in theta_1_calcd)) - log.debug(' Got possible angles theta_2: ' + ' '.join("{:.4f}°".format(degrees(theta)) for theta in theta_2_calcd)) - if theta_1_calcd == None or theta_2_calcd == None: - return [] - angle_pairs_list = [] - # create a list of paired combinations of returned angles (theta_1 , theta_2) - for i in range(len(theta_1_calcd)): - for j in range(len(theta_2_calcd)): - angle_pairs_list.append((theta_1_calcd[i], theta_2_calcd[j])) - angle_pairs_list = list(set(angle_pairs_list)) - # iterate through the list and check if a particular pair actually produces the requested z-vector orientation - joint_angles_list = [] - for i in range(len(angle_pairs_list)): - debug_msg = (f' Checking angle pair {i}: ({angle_pairs_list[i][0]:.4f}, {angle_pairs_list[i][1]:.4f}) ' - f'({degrees(angle_pairs_list[i][0]):.4f}°, {degrees(angle_pairs_list[i][1]):.4f}°)') - log.debug(debug_msg) - # we start with an identity matrix (ie oriented to world) - matrix_in = np.asmatrix(np.identity(4)) - try: - direction = kins_calc_transformation_get_direction() - except Exception as error: - log.error('kins_calc_transformation_get_direction, %s', error) - try: - matrix_out = kins_calc_transformation_matrix(angle_pairs_list[i][0], angle_pairs_list[i][1], 0, matrix_in, direction) - except Exception as error: - log.error('kins_calc_transformation_matrix, %s', error) - # the resulting z-vector for this pair of (theta_1, theta_2) is found in the third column - z_vector_would_be = np.array([matrix_out[0,2], matrix_out[1,2], matrix_out[2,2]]) - # calculate the difference of the respective elements - z_vector_diff = z_vector_req - z_vector_would_be - log.debug(' z_vector_diff: %s', z_vector_diff) - # and check if all elements are within [-epsilon,epsilon] - match_z = np.all((z_vector_diff > -epsilon) & (z_vector_diff < epsilon)) - log.debug(' Is the z-vector-vector close enough ? %s', match_z) - if match_z: - joint_angles_list.append((angle_pairs_list[i][0], angle_pairs_list[i][1])) - for (theta_1, theta_2) in joint_angles_list: - log.debug(f'Returning valid joint angles found: {degrees(theta_1):.4f}°, {degrees(theta_2):.4f}°') - return joint_angles_list # returns radians - - -def calc_shortest_distance(pos, trgt, mode): - pos = degrees(pos) - trgt = degrees(trgt) - # calculate the shortest distance in [-180°, 180°] eg if pos=170° and trgt=-170° then dist will be 20° - # If the operator requests positive or negative rotation we may need to return the long distance instead - log.debug('Entering: %s', sys._getframe( ).f_code.co_name) - dist_short = (trgt - pos + 180) % 360 - 180 - # calculate short and long distance - if dist_short >= 0: # ie dist_long should be negative - dist_long = -(360 - dist_short) - else: - dist_long = 360 + dist_short - log.debug(f' Calculated dist_short: {dist_short:.4f}°, dist_long: {dist_long:.4f}°') - if mode == 1: # positive rotation only, ie we want a positive distance - if dist_short >= 0: # ie we want this one - dist = dist_short - else: # ie we need to go the other way - dist = dist_long - elif mode == 2: # negative rotation only ie we want a positive distance - if dist_short > 0: # ie we need to go the other way - dist = dist_long - else: # ie we want this one - dist = dist_short - else: # mode = 0 ie we want the shortest distance either way - dist = dist_short - log.debug(f'Returning distance: {dist:.4f}°') - return radians(dist) - - -def calc_rotary_move_with_joint_limits(pos, trgt, max_limit, min_limit, mode): # expects radians - # this takes a target angle in [-pi,pi] and finds the closest move within [min_limit, max_limit] - # from a given position in [min_limit, max_limit], returns the optimized target angle and the distance - # from the given position to that target angle - log.debug('Entering: %s', sys._getframe( ).f_code.co_name) - log.debug(f' Current position: {degrees(pos):.4f}°, target position: {degrees(trgt):.4f}°') - # calculate the shortest distance from position to target for the strategy given by - # the operator (ie shortest (= default), positive rotation only, negative rotation only ) - dist = calc_shortest_distance(pos, trgt, mode) - # check that the result is within the rotary axis limits defined in the ini file - if dist >= 0: # shortest way is in the positive direction - if (pos + dist) <= max_limit: # if the limits allow we rotate the joint in the positive sense - log.debug(f' Max_limit OK, setting target to: {degrees(pos + dist):.4f}°') - theta = pos + dist - else: # if positive limits would be exceeded we need to go the longer way in the other direction - log.debug(f' Maximum axis limit of {degrees(max_limit):.4f} would be violated.') - if mode == 0: - dist = dist - 2*pi - log.debug(f' Changing target to: {degrees(trgt):.4f}°, distance to: {degrees(dist):.4f}°') - theta = trgt - else: # if the rotation direction was set by the operator then we can not change direction - log.debug(f' Unable to change direction because orient mode is set to {mode:.0f}.\n') - theta = None - else: # shortest way is in the negative direction - if (pos + dist) >= min_limit: # if the limits allow we rotate the joint in the negative sense - log.debug(f' Min_limit OK, setting target to: {degrees(pos + dist):.4f}°') - theta = pos + dist - else: # if negative limits would be exceeded we need to go the longer way int the other direction - log.debug(f' Minimum axis limit of {degrees(min_limit):.4f} would be violated.') - if mode == 0: - dist = dist + 2*pi - log.debug(f' Changing target to: {degrees(trgt):.4f}°, distance to: {degrees(dist):.4f}°') - theta = trgt - else: # if the rotation direction was set by the operator then we can not change direction - log.debug(f' Unable to change direction because orient mode is set to {mode:.0f}.\n') - theta = None - if theta is not None: - log.debug(f'Returning: angle {degrees(theta):.4f}° with distance {degrees(dist):.4f}° for requested mode {mode:.0f}\n') - # we also attach the distance for this particular move and mode - return theta, dist # returns radians - - -def calc_angle_pairs_and_distances(self, possible_prim_sec_angle_pairs): # expects radians - # this takes a list of joint angle pairs in [-pi,pi] and optimizes them for shortest moves - # in (min_limit, max_linit) from the current joint positions using the orient_mode set by - # the operator: 0=shortest (default), 1=positive rotation only, 2=negative rotation only - log.debug('Entering: %s', sys._getframe( ).f_code.co_name) - global primary_min_limit, primary_max_limit, secondary_min_limit, secondary_max_limit - global orient_mode - # get the current joint positions - prim_pos, sec_pos = get_current_rotary_positions(self) # returns radians - # we want to return a list of angles that are optimized for the orient_mode and the - # rotary axes limits as set in the ini file - target_dist_list= [] - for prim_trgt, sec_trgt in possible_prim_sec_angle_pairs: - # For the priortized joint we apply the orient mode requested by the operator - # the other we optimize for shortest move - if optimization_priority == 2: - primary_strategy = 0 - secondary_strategy = orient_mode - else: - primary_strategy = orient_mode - secondary_strategy = 0 - # primary joint - prim_move, prim_dist = calc_rotary_move_with_joint_limits(prim_pos, prim_trgt, - primary_max_limit, primary_min_limit, - primary_strategy) - # secondary joint - sec_move, sec_dist = calc_rotary_move_with_joint_limits(sec_pos, sec_trgt, - secondary_max_limit, secondary_min_limit, - secondary_strategy) - # if a solution has been found for this particular pair then we add it to the list - if not (prim_move == None) and not (sec_move == None): - target_dist_list.append(((prim_move, sec_move),(prim_dist, sec_dist))) - for ((prim_move, sec_move),(prim_dist, sec_dist)) in target_dist_list: - debug_msg = (f'Returning prim_move: {degrees(prim_move):.4f}°, sec_move: {degrees(sec_move):.4f}°, ' - f'prim_dist: {degrees(prim_dist):.4f}°, sec_dist: {degrees(sec_dist):.4f}°') - log.debug(debug_msg) - return target_dist_list # returns radians - - -def calc_optimal_joint_move(self, possible_prim_sec_angle_pairs): - # find the optimal joint move from current to target positions in the list - # orient_mode is 0=shortest, 1=positive rotation only, 2=negative rotation only - # For orient_mode=(1,2): If no move can be found within joint limits we return None - log.debug('Entering: %s', sys._getframe( ).f_code.co_name) - global orient_mode - # this returns a list with all moves ((prim_move, sec_move),(prim_dist, sec_dist)) that - # will result in correct tool orientation, stay within the rotary axis limits and respect the - # orient_mode if set by the operator - valid_joint_moves_and_distances = calc_angle_pairs_and_distances(self, possible_prim_sec_angle_pairs) - if len(valid_joint_moves_and_distances) < 1: - log.error(f' No valid joint moves found.') - return (None, None) - # now we need to pick and return the (primary angle, secondary angle) that results in the - # shortest move of the prioritized joint - (theta_1, theta_2) = (None, None) - joint = optimization_priority - 1 - dist = 10 # some large initial value - for trgt_angles, dists in valid_joint_moves_and_distances: - if orient_mode == 0 and fabs(dists[joint]) < fabs(dist): # shortest move requested - (theta_1, theta_2) = trgt_angles - dist = dists[0] - elif orient_mode == 1 and fabs(dists[joint]) < fabs(dist) and dists[joint] >= 0: # positive primary rotation only - (theta_1, theta_2) = trgt_angles - dist = dists[0] - elif orient_mode == 2 and fabs(dists[joint]) < fabs(dist) and dists[joint] <= 0: # negative primary rotation only - (theta_1, theta_2) = trgt_angles - dist = dists[0] - if theta_1 is not None: - debug_msg = (f'Returning shortest move selected for orient_mode {orient_mode:.0f}: ' - f'primary: {degrees(theta_1):.4f}°, secondary: {degrees(theta_2):.4f}°\n') - log.debug(debug_msg) - return theta_1, theta_2 # returns radians - - -def calc_virtual_rotation(theta_1, theta_2, x_vector_req, z_vector_req, matrix_in, direction): # expects radians - # calculates a required virtual-rotation around tool- or work-z so the x-vector matches the requested - # orientation after rotation - log.debug('Entering: %s', sys._getframe( ).f_code.co_name) - # tolerance setting for check if x-vector-vector needs to be rotated at all - epsilon = 0.00000001 - log.info(" x-vector-requested: %s", x_vector_req) - debug_msg = (f' got joint angles: primary {theta_1:.4f} {degrees(theta_1):.4f}°, ' - f'secondary {theta_2:.4f}° {degrees(theta_2):.4f}°') - log.debug(debug_msg) - # run matrix_in through the kinematic transformation in the requested direction - # using the given joint angles and zero virtual-rotation - try: - matrix_out = kins_calc_transformation_matrix(theta_1, theta_2, 0, matrix_in, direction) - except Exception as error: - log.error('calc_virtual_rotation, %s', error) - # the x-vector for the given machine joint rotations is found directly in the first column - x_vector_is = [matrix_out[0,0], matrix_out[1,0], matrix_out[2,0]] - log.debug(" X-vector after machine rotation would be: %s", x_vector_is) - # we calculate the angular difference between the two vectors so we can add a virtual rotation - # around z-vector or work-z to match the requested x orientation after machine rotation - # just to be sure we normalize the two vectors - x_vector_is = x_vector_is / np.linalg.norm(x_vector_is) - x_vector_req = x_vector_req / np.linalg.norm(x_vector_req) - # check if the x-vector is already in the required orientation (ie parallel) - log.debug(" checking if vectors are parallel: %s", np.dot(x_vector_is,x_vector_req)) - if np.dot(x_vector_is, x_vector_req) > 1 - epsilon: - log.info(" X-vector already oriented, setting virtual-rotation = 0") - # if we are already parallel then we don't need to add a virtual rotation - virtual_rot = 0 - else: - # we can use the cross product to determine the direction we need to rotate - cross = np.cross(x_vector_req, x_vector_is) - log.debug(" cross product (x_vector_req, x_vector_is): %s", cross) - virtual_rot = np.arccos(np.dot(x_vector_req, x_vector_is)) - log.debug(f' raw virtual_rot: {virtual_rot:.4f} {degrees(virtual_rot):.4f}°') - # To find out which quadrant we need the angle to be in we create a list of them all - virtual_rot_list = [virtual_rot, -virtual_rot, 2*pi-virtual_rot, -(2*pi-virtual_rot)] - log.debug(' Got possible virtual_rot angles: ' + ' '.join("{:.4f}°".format(degrees(angle)) for angle in virtual_rot_list)) - # then we run all of them through the kinematic model and see which gives us the requested x-vector-vector - for virtual_rot in virtual_rot_list: - log.debug(f' Checking virtual_rot = {degrees(virtual_rot):.4f}°') - zeta = 0.0001 - # run the identity matrix through the kinematic transformation in the requested direction - # using the given joint angles and virtual-rotation angle in the list - try: - matrix_out = kins_calc_transformation_matrix(theta_1, theta_2, virtual_rot, matrix_in, direction) - except Exception as error: - log.error('calc_virtual_rotation, %s', error) - # the oriented x-vector is found directly in the first column - x_vector_would_be = [matrix_out[0,0], matrix_out[1,0], matrix_out[2,0]] - log.debug(' x_vector_would_be: %s', x_vector_would_be) - # calculate the difference of the respective elements - x_vector_diff = x_vector_req - x_vector_would_be - # and check if all elements are within [-epsilon,epsilon] - match = np.all((x_vector_diff > -zeta) & (x_vector_diff < zeta)) - log.debug(' Is the X-vector close enough ? %s', match) - if match: - # if we have a match we leave the loop and use this angle - break - log.info(f'Returning virtual-rotation calculated {degrees(virtual_rot):.4f}°') - return virtual_rot # returns radians - - -def calc_twp_matrix_from_joint_position(self, matrix_in, virtual_rot, direction): # expects radians - # transforms a 4x4 input matrix using the current transformation matrix - # (forward or inverse) using the kinematic model of the machine - log.debug('Entering: %s', sys._getframe( ).f_code.co_name) - global kins_virtual_rotation - # read current spindle rotary angles (radians) - theta_1, theta_2 = get_current_rotary_positions(self) - # virtual-rot is the virtual rotary axis around the z-vector or work-z axis to align the x-vector - log.debug(f" requested virtual-rot value {degrees(virtual_rot):.4f}°") - # run matrix_in through the kinematic transformation in the requested direction - # using the current joint angles and virtual-rotation as requested - try: - twp_matrix = kins_calc_transformation_matrix(theta_1, theta_2, virtual_rot, matrix_in, direction) - except Exception as error: - log.error('calc_twp_matrix_from_joint_position, %s', error) - return twp_matrix - - -def gui_update_twp(): - # The tilted-work-plane is created in identity mode and must NOT be updated after a switch - log.debug('Entering: %s', sys._getframe( ).f_code.co_name) - global twp_matrix, saved_work_offset - # twp origin as vector (in world coords) from current work-offset to the origin of the twp - try: - hal.set_p("twp-helper-comp.twp-ox-in",str(twp_matrix[0,3])) - hal.set_p("twp-helper-comp.twp-oy-in",str(twp_matrix[1,3])) - hal.set_p("twp-helper-comp.twp-oz-in",str(twp_matrix[2,3])) - # twp x-vector - hal.set_p("twp-helper-comp.twp-xx-in",str(twp_matrix[0,0])) - hal.set_p("twp-helper-comp.twp-xy-in",str(twp_matrix[1,0])) - hal.set_p("twp-helper-comp.twp-xz-in",str(twp_matrix[2,0])) - # twp z-vector - hal.set_p("twp-helper-comp.twp-zx-in",str(twp_matrix[0,2])) - hal.set_p("twp-helper-comp.twp-zy-in",str(twp_matrix[1,2])) - hal.set_p("twp-helper-comp.twp-zz-in",str(twp_matrix[2,2])) - except Exception as error: - log.error('gui_update_twp failed, %s', error) - # publish the twp offset coordinates in world coordinates (ie identity) - [work_offset_x, work_offset_y, work_offset_z] = saved_work_offset - log.debug(" Setting work_offsets in the simulation: %s", (work_offset_x, work_offset_y, work_offset_z)) - # this is used to translate the rotated twp to the correct position - # care must be taken that only the work_offsets in identity mode are sent as that is - # what the model uses. The visuals for the offsets are created in the origin, - # then rotated according to the rotary joint position and then translated. - # The twp has to be rotated out of the machine xy plane using the g68.2 parameters and is then - # translated by the offset values of the identity mode. - try: - hal.set_p("twp-helper-comp.twp-ox-world-in",str(work_offset_x)) - hal.set_p("twp-helper-comp.twp-oy-world-in",str(work_offset_y)) - hal.set_p("twp-helper-comp.twp-oz-world-in",str(work_offset_z)) - except Exception as error: - log.error('gui_update_twp failed, %s', error) - - -# NOTE: Due to easier abort handling we currently restrict the use of twp to G54 -# as LinuxCNC seems to revert to G54 as the default system -def get_current_work_offset(self): - log.debug('Entering: %s', sys._getframe( ).f_code.co_name) - # get which offset is active (g54=1 .. g59.3=9) - active_offset = int(self.params[5220]) - current_work_offset_number = active_offset - # set the relevant parameter numbers that hold the active offset values - # (G54_x: #5221, G55_x:#[5221+20], G56_x:#[5221+40] ....) - work_offset_x = (active_offset-1)*20 + 5221 - work_offset_y = work_offset_x + 1 - work_offset_z = work_offset_x + 2 - co_x = self.params[work_offset_x] - co_y = self.params[work_offset_y] - co_z = self.params[work_offset_z] - current_work_offset = (co_x, co_y, co_z) - return [current_work_offset_number, current_work_offset] - - -def get_current_rotary_positions(self): - log.debug('Entering: %s', sys._getframe( ).f_code.co_name) - global joint_letter_primary, joint_letter_secondary - if joint_letter_primary == 'A': - theta_1 = radians(self.AA_current) - elif joint_letter_primary == 'B': - theta_1 = radians(self.BB_current) - elif joint_letter_primary == 'C': - theta_1 = radians(self.CC_current) - log.debug(f' Current position Primary joint: {degrees(theta_1):.4f}°') - # read current spindle rotary angles and convert to radians - if joint_letter_secondary == 'A': - theta_2 = radians(self.AA_current) - elif joint_letter_secondary == 'B': - theta_2 = radians(self.BB_current) - elif joint_letter_secondary == 'C': - theta_2 = radians(self.CC_current) - log.debug(f' Current position Secondary joint: {degrees(theta_2):.4f}°') - return theta_1, theta_2 - - -def reset_twp_params(): - log.debug('Entering: %s', sys._getframe( ).f_code.co_name) - global virtual_rot, twp_matrix, twp_flag, twp_build_params - virtual_rot = 0 - # we must not change tool kins parameters when TOOL kins are active or we get sudden joint position changes - # ie don't do this: kins_comp_set_virtual_rot(0)! - twp_flag = [] - twp_build_params = {} - log.info(" Resetting TWP-matrix") - twp_matrix = np.asmatrix(np.identity(4)) - - -def g53n_core(self): - log.debug('Entering: %s', sys._getframe( ).f_code.co_name) - # Orient the tool to the current twp (with TCP for G53.1 or IDENTITY for G53.6) - # Note: To avoid that this python code is run prematurely by the read ahead we need a quebuster at the - # beginning but because we need self.execute() to switch the WCS properly this remap needs to be called from - # an ngc reamp that contains a quebuster before calling this code. - # IMPORTANT: - # The correct kinematic mode (ie TCP for 53.1 / IDENTITY for G53.6) must be active when this code is called - # (ie do it in the ngc remap mentioned above!) - global saved_work_offset, twp_matrix, twp_flag, virtual_rot - global joint_letter_primary, joint_letter_secondary, twp_error_status - global orient_mode - if self.task == 0: # ignore the preview interpreter - yield INTERP_EXECUTE_FINISH - return INTERP_OK - - if not hal.get_value(twp_is_defined): - # reset the twp parameters - reset_twp_params() - msg = "G53.n: No TWP defined." - log.debug(' ' + msg) - emccanon.CANON_ERROR(msg) - yield INTERP_EXECUTE_FINISH # w/o this the error message is not displayed - yield INTERP_EXIT # w/o this the error does not abort a running gcode program - return INTERP_ERROR - elif hal.get_value(twp_is_active): - # reset the twp parameters - reset_twp_params() - msg = "G53.n: TWP already active" - log.debug(' ' + msg) - emccanon.CANON_ERROR(msg) - yield INTERP_EXECUTE_FINISH # w/o this the error message is not displayed - yield INTERP_EXIT # w/o this the error does not abort a running gcode program - return INTERP_ERROR - - # Check if any words have been passed with the respective G53.n command - c = self.blocks[self.remap_level] - p = c.p_number if c.p_flag else 0 - x = c.i_number if c.i_flag else None - y = c.j_number if c.j_flag else None - z = c.k_number if c.k_flag else None - log.debug(' G53.n Words passed: (P, X,Y,Z): %s', (p,x,y,z)) - - if p not in [0,1,2]: - # reset the twp parameters - reset_twp_params() - msg = "G53.n : unrecognised P-Word found." - log.debug(' ' + msg) - emccanon.CANON_ERROR(msg) - yield INTERP_EXECUTE_FINISH # w/o this the error message is not displayed - yield INTERP_EXIT # w/o this the error does not abort a running gcode program - return INTERP_ERROR - - orient_mode = p - z_vector_requested = [twp_matrix[0,2],twp_matrix[1,2],twp_matrix[2,2]] - x_vector_requested = [twp_matrix[0,0],twp_matrix[1,0],twp_matrix[2,0]] - # calculate all possible pairs of (primary, secondary) angles to matches the requested orientation - try: - # angles are returned in [-pi,pi] - possible_prim_sec_angle_pairs = calc_joint_angles(z_vector_requested, x_vector_requested) # returns radians - except Exception as error: - log.error('calc_joint_angles, %s', error) - # reset the twp parameters - reset_twp_params() - msg = ("G53.n ERROR: Calculation of joint angles has failed. -> aborting G53.n") - log.debug(' ' + msg) - emccanon.CANON_ERROR(msg) - yield INTERP_EXECUTE_FINISH # w/o this the error message is not displayed - yield INTERP_EXIT # w/o this the error does not abort a running gcode program - return INTERP_ERROR - - if possible_prim_sec_angle_pairs == []: - # reset the twp parameters - log.error('G53.n: No possible primary/secondary angle pairs found.') - reset_twp_params() - msg = "G53.n ERROR: Requested tool orientation not reachable -> aborting G53.n" - log.debug(' ' + msg) - emccanon.CANON_ERROR(msg) - yield INTERP_EXECUTE_FINISH # w/o this the error message is not displayed - yield INTERP_EXIT # w/o this the error does not abort a running gcode program - return INTERP_ERROR - - # this returns one pair of optimized angles in degrees, or (None, None) if no solution could be found - try: - theta_1, theta_2 = calc_optimal_joint_move(self, possible_prim_sec_angle_pairs) # returns radians - except Exception as error: - log.error('G53.n: Calculation of optimal joint move failed, %s', error) - if theta_1 == None or theta_2 == None: - # reset the twp parameters - reset_twp_params() - msg = ("G53.n ERROR: Requested tool orientation not reachable -> aborting G53.n") - log.debug(' ' + msg) - emccanon.CANON_ERROR(msg) - yield INTERP_EXECUTE_FINISH # w/o this the error message is not displayed - yield INTERP_EXIT # w/o this the error does not abort a running gcode program - return INTERP_ERROR - - # get the particular conditions to be met for the kinematic at hand - try: - (x_vector_requested, z_vector_requested, matrix_in, direction) = kins_calc_virtual_rot_get_values(x_vector_requested, - z_vector_requested, - twp_matrix) - except Exception as error: - log.error('G53.n: kins_calc_virtual_rot_get_values failed, %s', error) - # calculate the virtual-rotation needed - virtual_rot = calc_virtual_rotation(theta_1, - theta_2, - x_vector_requested, - z_vector_requested, - matrix_in, - direction) # returns radians - log.debug(f" Calculated virtual-rotation to match requested x-vector: {degrees(virtual_rot):.4f}°") - - # mark twp-flag as active - twp_flag = [0, 'active'] - gui_update_twp() - - # set the virtual-rotation value in the kinematic component - debug_msg = (f' G53.n: Setting angle values in kins comp to theta1: {degrees(theta_1):.4f}°, ' - f'theta2: {degrees(theta_2):.4f}°, virtual_rot: {degrees(virtual_rot):.4f}°') - log.debug(debug_msg) - try: - kins_set_values(theta_1, theta_2, virtual_rot) - except Exception as error: - log.error('G53.n: kins_set_values failed, %s', error) - - # calculate the work offset in transformed-coordinatess - log.debug(" G53.n: Saved work offset: %s", saved_work_offset) - twp_offset = (twp_matrix[0,3],twp_matrix[1,3],twp_matrix[2,3]) - try: - new_offset = kins_calc_transformed_work_offset(saved_work_offset, twp_offset, theta_1, theta_2, virtual_rot) - except Exception as error: - log.error('G53.n: Calculation of kins_calc_transformed_work_offset failed, %s', error) - debug_msg = (f' G53.n: Setting transformed work-offsets for twp-kins in G59, G59.1, ' - f'G59.2 and G59.3 to: {new_offset[0]:.4f}, {new_offset[1]:.4f}, {new_offset[2]:.4f}') - log.debug(debug_msg) - # set the dedicated TWP work offset values (G53, G53.1, G53.2, G53.3) - self.execute("G10 L2 P6 X%f Y%f Z%f" % (new_offset[0], new_offset[1], new_offset[2]), lineno()) - self.execute("G10 L2 P7 X%f Y%f Z%f" % (new_offset[0], new_offset[1], new_offset[2]), lineno()) - self.execute("G10 L2 P8 X%f Y%f Z%f" % (new_offset[0], new_offset[1], new_offset[2]), lineno()) - self.execute("G10 L2 P9 X%f Y%f Z%f" % (new_offset[0], new_offset[1], new_offset[2]), lineno()) - - log.debug(f" G53.n: Moving primary joint to {degrees(theta_1):.4f}° and secondary joint to {degrees(theta_2):.4f}° ") - if (x,y,z) == (None,None,None): - # Move rotary joints to align the tool and the requested work plane - self.execute("G0 %s%f %s%f" % (joint_letter_primary, degrees(theta_1), joint_letter_secondary, degrees(theta_2)), lineno()) - # switch to the dedicated TWP work offsets - self.execute("G59", lineno()) - # activate TWP kinematics - self.execute("G12.1 P2") - if (x,y,z) != (None,None,None): - log.debug(' G53.3 called') - self.execute("G0 X%s Y%s Z%s %s%f %s%f" % - (x, y, z, joint_letter_primary, degrees(theta_1), joint_letter_secondary, degrees(theta_2)), lineno()) - # set twp-state to 'active' (2) - self.execute("M68 E2 Q2") - yield INTERP_EXECUTE_FINISH - return INTERP_OK - - -# Cancel an active TWP definition and reset the parameters to zero -# Note: To avoid that this python code is run prematurely by the read ahead we need a quebuster at the beginning but -# because we need self.execute() to switch the WCS properly this remap needs to be called from -# an ngc that contains a quebuster before calling this code -def g69_core(self): - log.debug('Entering: %s', sys._getframe( ).f_code.co_name) - global twp_flag, saved_work_offset_number, saved_work_offset - if self.task == 0: # ignore the preview interpreter - yield INTERP_EXECUTE_FINISH - return INTERP_OK - log.info('G69 called') - # reset the twp parameters - reset_twp_params() - gui_update_twp() - # set twp-state to 'undefined' (0) - self.execute("M68 E2 Q0") - yield INTERP_EXECUTE_FINISH - return INTERP_OK - - -# define a virtual tilted-work-plane (twp) that is perpendicular to the current tool-orientation -def g683(self, **words): - log.debug('Entering: %s', sys._getframe( ).f_code.co_name) - global twp_matrix, virtual_rot, twp_flag, saved_work_offset_number, saved_work_offset - - if self.task == 0: # ignore the preview interpreter - yield INTERP_EXECUTE_FINISH - return INTERP_OK - - # ! IMPORTANT ! - # We need to use 'yield INTERP_EXECUTE_FINISH' here to stop the read ahead - # and avoid it executing the rest of the remap ahead of time - ## NOTE: No 'self.execute(..)' command can be used after 'yield INTERP_EXECUTE_FINISH' - yield INTERP_EXECUTE_FINISH - - if hal.get_value(twp_is_defined): - # reset the twp parameters - reset_twp_params() - msg =("G68.3 ERROR: TWP already defined.") - log.debug(msg) - emccanon.CANON_ERROR(msg) - yield INTERP_EXECUTE_FINISH # w/o this the error message is not displayed - yield INTERP_EXIT # w/o this the error does not abort a running gcode program - return INTERP_ERROR - - # NOTE: Due to easier abort handling we currently restrict the use of twp to G54 - # as LinuxCNC seems to revert to G54 as the default system - # get which offset is active (g54=1 .. g59.3=9) - (n, offsets) = get_current_work_offset(self) - if n != 1: - # reset the twp parameters - reset_twp_params() - msg = "G68.3 ERROR: Must be in G54 to define TWP." - log.debug(msg) - emccanon.CANON_ERROR(msg) - yield INTERP_EXECUTE_FINISH # w/o this the error message is not displayed - yield INTERP_EXIT # w/o this the error does not abort a running gcode program - return INTERP_ERROR - - c = self.blocks[self.remap_level] - # parse the requested origin - x = c.x_number if c.x_flag else 0 - y = c.y_number if c.y_flag else 0 - z = c.z_number if c.z_flag else 0 - # parse the requested rotation of x-vector around the origin - r = radians(c.r_number) if c.r_flag else 0 - - twp_flag = [0, 1, 'empty'] # one call to define the twp in this mode - theta_1, theta_2 = get_current_rotary_positions(self) # radians - # calculate virtual rotation to have the oriented x-vector in the direction required for the kinematic at hand - try: - virtual_rot = kins_calc_virtual_rot_for_g683(theta_1, theta_2 ) - except Exception as error: - log.error('remap_func: kins_calc_virtual_rot_for_g683 failed, %s', error) - log.info("G68.3: virtual-Rotation calculated for x-vector in machine-xy plane [deg]: %s", degrees(virtual_rot)) - # then we need to calculate the transformation matrix of the current orientation with the including the - # calculated virtual-rotation. - # for this we take the 4x4 identity matrix and pass it through the kinematic transformation using the - # current rotary joint positions and the calculated virtual-rotation angle plus any additional angle - # passed in the R word of the G68.3 command - start_matrix = np.asmatrix(np.identity(4)) - log.info('G68.3: Requested R-word rotation [deg]: %s', degrees(r)) - # the required transformation direction may depend on the kinematic at hand - try: - direction = kins_calc_transformation_get_direction() - except Exception as error: - log.error('kins_calc_transformation_get_direction, %s', error) - twp_matrix = calc_twp_matrix_from_joint_position(self, start_matrix, virtual_rot + r, direction) - log.debug("G68.3: TWP matrix with oriented x-vector: \n%s", twp_matrix) - # put the requested origin into the twp_matrix - (twp_matrix[0,3], twp_matrix[1,3], twp_matrix[2,3]) = (x, y, z) - # update the build state of the twp call - twp_flag[2] = 'done' - log.info("G68.3: Built twp-transformation-matrix: \n%s", twp_matrix) - # collect the currently active work offset values (ie g54, g55 or other) - saved_work_offset = offsets - saved_work_offset_number = n - log.debug("G68.3: Saved work offsets: %s", (n, saved_work_offset)) - # set twp-state to 'defined' (1) - self.execute("M68 E2 Q1") - yield INTERP_EXECUTE_FINISH - - gui_update_twp() - return INTERP_OK - - -# definition of a virtual work-plane (twp) using different methods set by the 'p'-word -def g682(self, **words): - log.debug('Entering: %s', sys._getframe( ).f_code.co_name) - global twp_matrix, virtual_rot, twp_flag, twp_build_params, saved_work_offset_number, saved_work_offset - - if self.task == 0: # ignore the preview interpreter - yield INTERP_EXECUTE_FINISH - return INTERP_OK - - # ! IMPORTANT ! - # We need to use 'yield INTERP_EXECUTE_FINISH' here to stop the read ahead - # and avoid it executing the rest of the remap ahead of time - ## NOTE: No 'self.execute(..)' command can be used after 'yield INTERP_EXECUTE_FINISH' - yield INTERP_EXECUTE_FINISH - - if hal.get_value(twp_is_defined): # ie TWP has already been defined - # reset the twp parameters - reset_twp_params() - msg = ("G68.2: TWP already defined.") - log.debug(' ' + msg) - emccanon.CANON_ERROR(msg) - yield INTERP_EXECUTE_FINISH # w/o this the error message is not displayed - yield INTERP_EXIT # w/o this the error does not abort a running gcode program - return INTERP_ERROR - - # NOTE: Due to easier abort handling we currently restrict the use of twp to G54 - # as LinuxCNC seems to revert to G54 as the default system - (n, offsets) = get_current_work_offset(self) - if n != 1: - # reset the twp parameters - reset_twp_params() - msg = "G68.2 ERROR: Must be in G54 to define TWP." - log.debug(' ' + msg) - emccanon.CANON_ERROR(msg) - yield INTERP_EXECUTE_FINISH # w/o this the error message is not displayed - yield INTERP_EXIT # w/o this the error does not abort a running gcode program - return INTERP_ERROR - - # collect the currently active work offset values (ie g54, g55 or other) - saved_work_offset_number = n - saved_work_offset = offsets - log.debug(" G68.2: Saved work offsets %s", (n, saved_work_offset)) - - c = self.blocks[self.remap_level] - p = c.p_number if c.p_flag else 0 - if p == 0: # true euler angles (this is the default mode) - twp_flag = [int(p), 1, 'empty'] # one call to define the twp in this mode - # parse requested order of rotations (default is '313' ie: ZXZ) - q = str(int(c.q_number if c.q_flag else 313)) - if q not in ['121','131','212','232','313','323']: - # reset the twp parameters - reset_twp_params() - msg = ("G68.2 (P0): No recognised Q-Word found.") - log.debug(' ' + msg) - emccanon.CANON_ERROR(msg) - yield INTERP_EXECUTE_FINISH # w/o this the error message is not displayed - yield INTERP_EXIT # w/o this the error does not abort a running gcode program - return INTERP_ERROR - - # parse the requested origin - x = c.x_number if c.x_flag else 0 - y = c.y_number if c.y_flag else 0 - z = c.z_number if c.z_flag else 0 - # parse the requested xy-rotation around the origin - r = radians(c.r_number) if c.r_flag else 0 - # parse the requested euler rotation angles - th1 = radians(c.i_number) if c.i_flag else 0 - th2 = radians(c.j_number) if c.j_flag else 0 - th3 = radians(c.k_number) if c.k_flag else 0 - - # build the translation vector of the twp_matrix - twp_origin = [[x], [y], [z]] - # create the rotation matrix for the requested origin rotation - try: - twp_origin_rotation = kins_calc_twp_origin_rot_matrix(r) - except Exception as error: - log.error('remap_func: kins_calc_twp_origin_rot_matrix failed, %s', error) - log.debug(' G68.2 (P0): Twp_origin_rotation \n%s',twp_origin_rotation) - # build the rotation matrix for the requested euler rotation - twp_euler_rotation = calc_euler_rot_matrix(th1, th2, th3, q) - log.debug(' G68.2 (P0): Twp_euler_rotation \n%s',twp_euler_rotation) - # calculate the total twp_rotation using matrix multiplication - twp_rotation = np.asmatrix(twp_origin_rotation) * np.asmatrix(twp_euler_rotation) - # combine rotation and translation and form the 4x4 twp-transformation matrix - twp_matrix = np.hstack((twp_rotation, twp_origin)) - twp_row_4 = [0,0,0,1] - twp_matrix = np.vstack((twp_matrix, twp_row_4)) - twp_matrix = np.asmatrix(twp_matrix) - # update the build state of the twp call - twp_flag[2] = 'done' - - elif p == 1: # non-true euler angles, eg: 'pitch,roll,yaw' - twp_flag = [int(p), 1, 'empty'] # one call to define the twp in this mode - # parse requested order of rotations (default is '123' ie: XYZ) - q = str(int(c.q_number if c.q_flag else 123)) - - if q not in ['123','132','213','231','312','321']: - # reset the twp parameters - reset_twp_params() - msg = ("G68.2 P1: No recognised Q-Word found.") - log.debug(' ' + msg) - emccanon.CANON_ERROR(msg) - yield INTERP_EXECUTE_FINISH # w/o this the error message is not displayed - yield INTERP_EXIT # w/o this the error does not abort a running gcode program - return INTERP_ERROR - - # parse the requested origin - x = c.x_number if c.x_flag else 0 - y = c.y_number if c.y_flag else 0 - z = c.z_number if c.z_flag else 0 - # parse the requested xy-rotation around the origin - r = radians(c.r_number) if c.r_flag else 0 - # parse the requested euler rotation angles - th1 = radians(c.i_number) if c.i_flag else 0 - th2 = radians(c.j_number) if c.j_flag else 0 - th3 = radians(c.k_number) if c.k_flag else 0 - - # build the translation vector of the twp_matrix - twp_origin = [[x], [y], [z]] - # create the rotation matrix for the requested origin rotation - try: - twp_origin_rotation = kins_calc_twp_origin_rot_matrix(r) - except Exception as error: - log.error('remap_func: kins_calc_twp_origin_rot_matrix failed, %s', error) - log.debug(' G68.2 P1: Twp_origin_rotation \n%s',twp_origin_rotation) - # build the rotation matrix for the requested euler rotation - twp_euler_rotation = calc_euler_rot_matrix(th1, th2, th3, q) - log.debug(' G68.2 P1: Twp_euler_rotation \n%s',twp_euler_rotation) - # calculate the total twp_rotation using matrix multiplication - twp_rotation = np.asmatrix(twp_origin_rotation) * np.asmatrix(twp_euler_rotation) - # combine rotation and translation and form the 4x4 twp-transformation matrix - twp_matrix = np.hstack((twp_rotation, twp_origin)) - twp_row_4 = [0,0,0,1] - twp_matrix = np.vstack((twp_matrix, twp_row_4)) - twp_matrix = np.asmatrix(twp_matrix) - # update the build state of the twp call - twp_flag[2] = 'done' - - elif p == 2: # twp defined py 3 points on the plane - # TODO implement operator errors as outlined in the twp README - #- G68.2 P2 (Q0),Q1,Q2,Q3 commands are not entered consecutively - #- two to the points entered in Q1,Q2,Q3 are identical - #- all three points entered in Q1,Q2,Q3 are on a line - #- the distance between a line defined by any two points entered in (Q1,Q2,Q3) and - #the remaining point is less than 10mm or 0.5inch (just some arbitrary values for now) - - # if this is the first call for this mode reset the twp_flag flag - if not twp_flag: - twp_flag = [int(p), 4 , 'empty', 'empty', 'empty', 'empty'] # four calls needed - twp_build_params = {'q0':[], 'q1':[], 'q2':[], 'q3':[]} - # Point 1: defines the origin of the twp - # Point 2: direction from P1 to P2 defines the positive x direction on the twp (x-vector) - # Point 3: defines the positive y side and with P1 and P2 defines the xy work plane (z-vector) - q = int(c.q_number if c.q_flag else 0) - # this mode needs four calls to fill all required parameters - if q == 0: # define new origin and rotation - x = c.x_number if c.x_flag else 0 - y = c.y_number if c.y_flag else 0 - z = c.z_number if c.z_flag else 0 - # parse the requested xy-rotation around the origin - r = radians(c.r_number) if c.r_flag else 0 - twp_build_params['q0'] = [x,y,z,r] - twp_flag[2] = 'done' - elif q == 1: # define point 1 - x1 = c.x_number if c.x_flag else 0 - y1 = c.y_number if c.y_flag else 0 - z1 = c.z_number if c.z_flag else 0 - twp_build_params['q1'] = [x1,y1,z1] - twp_flag[3] = 'done' - elif q == 2: # define point 2 - x2 = c.x_number if c.x_flag else 0 - y2 = c.y_number if c.y_flag else 0 - z2 = c.z_number if c.z_flag else 0 - twp_build_params['q2'] = [x2,y2,z2] - twp_flag[4] = 'done' - elif q == 3: # define point 3 - x3 = c.x_number if c.x_flag else 0 - y3 = c.y_number if c.y_flag else 0 - z3 = c.z_number if c.z_flag else 0 - twp_build_params['q3'] = [x3,y3,z3] - twp_flag[5] = 'done' - else: - # reset the twp parameters - reset_twp_params() - msg = ("G68.2 P2: No recognised Q-Word found.") - log.debug(' ' + msg) - emccanon.CANON_ERROR(msg) - yield INTERP_EXECUTE_FINISH # w/o this the error message is not displayed - yield INTERP_EXIT # w/o this the error does not abort a running gcode program - return INTERP_ERROR - - # only start calculations once all the parameters have been passed - if twp_flag.count('done') == twp_flag[1]: - [x, y, z, r] = twp_build_params['q0'][0:4] - # build the translation vector of the twp_matrix - twp_origin = [[x], [y], [z]] - p1 = twp_build_params['q1'][0:3] - p2 = twp_build_params['q2'] - p3 = twp_build_params['q3'] - log.debug(" G68.2 P2: Point 1: %s",p1) - log.debug(" G68.2 P2: Point 2: %s",p2) - log.debug(" G68.2 P2: Point 3: %s",p3) - # build vectors x:P1->P2 and v2:P1->P3 - twp_vect_x = [p2[0]-p1[0], p2[1]-p1[1], p2[2]-p1[2]] - log.debug(" G68.2 P2: Twp_vect_x: \n%s",twp_vect_x) - v2 = [p3[0]-p1[0], p3[1]-p1[1], p3[2]-p1[2]] - log.debug(" G68.2 P2 (v2): %s",v2) - # normalize the two vectors - twp_vect_x = twp_vect_x / np.linalg.norm(twp_vect_x) - v2 = v2 / np.linalg.norm(v2) - # we can use the cross product to calculate the z-vector vector - # note: if P3 is on the right side of the vector P1->P2 - # then the z-vector will be below the twp (ie z-vector will be downwards) - twp_vect_z = np.cross(twp_vect_x , v2) - log.debug(" G68.2 P2: Twp_vect_z %s",twp_vect_z) - # we can use the cross product to calculate the y vector - twp_vect_y = np.cross(twp_vect_z, twp_vect_x) - log.debug(" G68.2 P2: Twp_vect_y %s",twp_vect_y) - # build the rotation matrix of the twp_matrix from the calculated vectors - # first stack the vectors (lists) and then flip diagonally (transpose) - # so the vectors are now vertical - twp_vect_rotation_t = np.vstack((twp_vect_x, twp_vect_y)) - twp_vect_rotation_t = np.vstack((twp_vect_rotation_t, twp_vect_z)) - twp_vect_rotation = np.transpose(twp_vect_rotation_t) - log.debug(" G68.2 P2: Built the twp-rotation-matrix: \n%s", twp_vect_rotation) - # create the rotation matrix for the requested origin rotation - try: - twp_origin_rotation = kins_calc_twp_origin_rot_matrix(r) - except Exception as error: - log.error('remap_func: kins_calc_twp_origin_rot_matrix failed, %s', error) - log.debug(' G68.2 P2: Twp-origin-rotation-matrix \n%s',twp_origin_rotation) - # calculate the total twp_rotation using matrix multiplication - twp_rotation = np.asmatrix(twp_origin_rotation) * np.asmatrix(twp_vect_rotation) - # add the origin translation on the right - twp_matrix = np.hstack((twp_rotation, twp_origin)) - # expand to 4x4 array and make into a matrix - twp_row_4 = [0,0,0,1] - twp_matrix = np.vstack((twp_matrix, twp_row_4)) - twp_matrix = np.asmatrix(twp_matrix) - log.debug(" G68.2 P2: Built twp-transformation-matrix: \n%s", twp_matrix) - - elif p == 3: # two vectors (vector 1 defines the x-vector and vector 2 defines the z-vector) - # TODO implement operator errors as outlined in the twp README - #- G68.2 P3 Q1 and Q2 commands are not entered consecutively - #- one of the vectors is the zero vector - #- the enclosed angle between the 1. and 2. vector is <85° or >95° (re fanuc twp pdf) - q = int(c.q_number if c.q_flag else 0) - # if this is the first call for this mode reset the twp_flag flag - if not twp_flag: - log.info(' first call') - twp_flag = [int(p), 2 , 'empty', 'empty'] # two calls needed - twp_build_params = {'q0':[], 'q1':[]} - log.debug(' twp_build_params: %s', twp_build_params) - if q == 0: # define new origin of the twp - x = c.x_number if c.x_flag else 0 - y = c.y_number if c.y_flag else 0 - z = c.z_number if c.z_flag else 0 - # parse the requested xy-rotation around the origin - r = radians(c.r_number) if c.r_flag else 0 - # first vector (direction of x in the twp) - i = c.i_number if c.i_flag else 0 - j = c.j_number if c.j_flag else 0 - k = c.k_number if c.k_flag else 0 - twp_build_params['q0'] = [x,y,z,i,j,k,r] - twp_flag[2] = 'done' - elif q == 1: # define second vector (the normal vector of the twp - i1 = c.i_number if c.i_flag else 0 - j1 = c.j_number if c.j_flag else 0 - k1 = c.k_number if c.k_flag else 0 - twp_build_params['q1'] = [i1,j1,k1] - twp_flag[3] = 'done' - else: - # reset the twp parameters - reset_twp_params() - msg = ("G68.2 P3: No recognised Q-Word found.") - log.debug(' ' + msg) - emccanon.CANON_ERROR(msg) - yield INTERP_EXECUTE_FINISH # w/o this the error message is not displayed - yield INTERP_EXIT # w/o this the error does not abort a running gcode program - return INTERP_ERROR - - # only start calculations once all the parameters have been passed - if twp_flag.count('done') == twp_flag[1]: - twp_origin = (x ,y, z) = twp_build_params['q0'][0:3] - r = twp_build_params['q0'][6] - (i, j, k) = twp_build_params['q0'][3:6] - (i1, j1, k1) = twp_build_params['q1'] - log.debug("(x, y, z): %s", (x, y, z)) - log.debug("(i, j, k): %s", (i, j, k)) - log.debug("(i1, j1, k1): %s", (i1, j1, k1)) - # build unit vector defining x-vector direction - twp_vect_x = [i-x, j-y, k-z] - twp_vect_x = twp_vect_x / np.linalg.norm(twp_vect_x) - twp_vect_z = [i1, j1, k1] - twp_vect_z = twp_vect_z / np.linalg.norm(twp_vect_z) - orth = np.dot(twp_vect_x, twp_vect_z) - log.debug(" orth check: %s", orth) - # the two vectors must be orthogonal - if orth > 0.001: - reset_twp_params() - msg = ("G68.2 P3: Vectors are not orthogonal.") - log.debug(' ' + msg) - emccanon.CANON_ERROR(msg) - yield INTERP_EXECUTE_FINISH # w/o this the error message is not displayed - yield INTERP_EXIT # w/o this the error does not abort a running gcode program - return INTERP_ERROR - - # we can use the cross product to calculate the y vector - twp_vect_y = np.cross(twp_vect_z, twp_vect_x) - log.debug(" G68.2 P3: twp_vect_y %s",twp_vect_y) - # build the rotation matrix of the twp_matrix from the calculated vectors - # first stack the vectors (lists) and then flip diagonally (transpose) - # so the vectors are now vertical - twp_vect_rotation_t = np.vstack((twp_vect_x, twp_vect_y)) - twp_vect_rotation_t = np.vstack((twp_vect_rotation_t, twp_vect_z)) - twp_vect_rotation = np.transpose(twp_vect_rotation_t) - log.debug(" G68.2 P3: Built twp-rotation-matrix: \n%s", twp_vect_rotation) - # create the rotation matrix for the requested origin rotation - try: - twp_origin_rotation = kins_calc_twp_origin_rot_matrix(r) - except Exception as error: - log.error('remap_func: kins_calc_twp_origin_rot_matrix failed, %s', error) - log.debug(' G68.2 P3: Twp-origin-rotation-matrix \n%s',twp_origin_rotation) - # calculate the total twp_rotation using matrix multiplication - twp_rotation = np.asmatrix(twp_origin_rotation) * np.asmatrix(twp_vect_rotation) - # add the origin translation on the right - twp_origin = [[x], [y], [z]] - twp_matrix = np.hstack((twp_rotation, twp_origin)) - # expand to 4x4 array and make into a matrix - twp_row_4 = [0,0,0,1] - twp_matrix = np.vstack((twp_matrix, twp_row_4)) - twp_matrix = np.asmatrix(twp_matrix) - log.debug(" G68.2 P3: Built twp-transformation-matrix: \n%s", twp_matrix) - - # TODO implement G68.2 P4 as outlined in the fanuc twp pdf (the exact meaning of which is unclear to me) - - else: - # reset the twp parameters - reset_twp_params() - msg = ("G68.2: No recognised P-Word found.") - log.debug(' ' + msg) - emccanon.CANON_ERROR(msg) - yield INTERP_EXECUTE_FINISH # w/o this the error message is not displayed - yield INTERP_EXIT # w/o this the error does not abort a running gcode program - return INTERP_ERROR - - log.debug(" G68.2: twp_flag: %s", twp_flag) - log.debug(" G68.2: calls required: %s", twp_flag.count('done')) - log.debug(" G68.2: number of calls made: %s", twp_flag.count('done')) - - if twp_flag.count('done') == twp_flag[1]: - log.info(' G68.2: requested rotation (degrees): %s', degrees(r)) - log.info(" G68.2: twp-tranformation-matrix: \n%s",twp_matrix) - twp_origin = [twp_matrix[0,3],twp_matrix[1,3],twp_matrix[2,3]] - log.info(" G68.2: twp origin: %s", twp_origin) - twp_vect_x = [twp_matrix[0,0],twp_matrix[1,0],twp_matrix[2,0]] - log.info(" G68.2: twp vector-x: %s", twp_vect_x) - twp_vect_z = [twp_matrix[0,2],twp_matrix[1,2],twp_matrix[2,2]] - log.info(" G68.2: twp vector-z: %s", twp_vect_z) - # set twp-state to 'defined' (1) - self.execute("M68 E2 Q1") - yield INTERP_EXECUTE_FINISH - - gui_update_twp() - return INTERP_OK - - -# incremental definition of a virtual work-plane (twp) using different methods set by the 'p'-word -def g684(self, **words): - log.debug('Entering: %s', sys._getframe( ).f_code.co_name) - global twp_matrix, virtual_rot, twp_flag, twp_build_params, saved_work_offset_number, saved_work_offset - - if self.task == 0: # ignore the preview interpreter - yield INTERP_EXECUTE_FINISH - return INTERP_OK - - # ! IMPORTANT ! - # We need to use 'yield INTERP_EXECUTE_FINISH' here to stop the read ahead - # and avoid it executing the rest of the remap ahead of time - ## NOTE: No 'self.execute(..)' command can be used after 'yield INTERP_EXECUTE_FINISH' - yield INTERP_EXECUTE_FINISH - - if not hal.get_value(twp_is_active): # ie there is currently no TWP defined - # reset the twp parameters - reset_twp_params() - msg = ("G68.4: No TWP active to increment from. Run G68.2 or G68.3 first.") - log.debug(' ' + msg) - emccanon.CANON_ERROR(msg) - yield INTERP_EXECUTE_FINISH # w/o this the error message is not displayed - yield INTERP_EXIT # w/o this the error does not abort a running gcode program - return INTERP_ERROR - - # collect the currently active work offset values (ie g54, g55 or other) - n = get_current_work_offset(self)[0] - # Must be in one of the dedicated offset systems for TWP - if False: #n < 6: - # reset the twp parameters - reset_twp_params() - msg = ("G68.4 ERROR: Must be in G59, G59.x to increment TWP.") - log.debug(' ' + msg) - emccanon.CANON_ERROR(msg) - yield INTERP_EXECUTE_FINISH # w/o this the error message is not displayed - yield INTERP_EXIT # w/o this the error does not abort a running gcode program - return INTERP_ERROR - - # store the current TWP to - twp_matrix_current = np.matrix.copy(twp_matrix) - c = self.blocks[self.remap_level] - p = c.p_number if c.p_flag else 0 - - if p == 0: # true euler angles (this is the default mode) - twp_flag = [int(p), 1, 'empty'] # one call to define the twp in this mode - # parse requested order of rotations (default is '313' ie: ZXZ) - q = str(int(c.q_number if c.q_flag else 313)) - - if q not in ['121','131','212','232','313','323']: - # reset the twp parameters - reset_twp_params() - msg = ("G68.4 (P0): No recognised Q-Word found.") - log.debug(' ' + msg) - emccanon.CANON_ERROR(msg) - yield INTERP_EXECUTE_FINISH # w/o this the error message is not displayed - yield INTERP_EXIT # w/o this the error does not abort a running gcode program - return INTERP_ERROR - - # parse the requested origin - x = c.x_number if c.x_flag else 0 - y = c.y_number if c.y_flag else 0 - z = c.z_number if c.z_flag else 0 - # parse the requested xy-rotation around the origin - r = radians(c.r_number) if c.r_flag else 0 - # parse the requested euler rotation angles - th1 = radians(c.i_number) if c.i_flag else 0 - th2 = radians(c.j_number) if c.j_flag else 0 - th3 = radians(c.k_number) if c.k_flag else 0 - - # build the translation vector of the twp_matrix - twp_origin = [[x], [y], [z]] - # create the rotation matrix for the requested origin rotation - try: - twp_origin_rotation = kins_calc_twp_origin_rot_matrix(r) - except Exception as error: - log.error('remap_func: kins_calc_twp_origin_rot_matrix failed, %s', error) - log.debug(' G68.4 (P0): Twp_origin_rotation \n%s',twp_origin_rotation) - # build the rotation matrix for the requested euler rotation - twp_euler_rotation = calc_euler_rot_matrix(th1, th2, th3, q) - log.debug(' G68.4 (P0): Twp_euler_rotation \n%s',twp_euler_rotation) - # calculate the total twp_rotation using matrix multiplication - twp_rotation = np.asmatrix(twp_origin_rotation) * np.asmatrix(twp_euler_rotation) - # combine rotation and translation and form the 4x4 twp-transformation matrix - twp_matrix = np.hstack((twp_rotation, twp_origin)) - twp_row_4 = [0,0,0,1] - twp_matrix = np.vstack((twp_matrix, twp_row_4)) - twp_matrix = np.asmatrix(twp_matrix) - # update the build state of the twp call - twp_flag[2] = 'done' - - elif p == 1: # non-true euler angles, eg: 'pitch,roll,yaw' - twp_flag = [int(p), 1, 'empty'] # one call to define the twp in this mode - # parse requested order of rotations (default is '123' ie: XYZ) - q = str(int(c.q_number if c.q_flag else 123)) - - if q not in ['123','132','213','231','312','321']: - # reset the twp parameters - reset_twp_params() - msg = ("G68.4 P1: No recognised Q-Word found.") - log.debug(' ' + msg) - emccanon.CANON_ERROR(msg) - yield INTERP_EXECUTE_FINISH # w/o this the error message is not displayed - yield INTERP_EXIT # w/o this the error does not abort a running gcode program - return INTERP_ERROR - - # parse the requested origin - x = c.x_number if c.x_flag else 0 - y = c.y_number if c.y_flag else 0 - z = c.z_number if c.z_flag else 0 - # parse the requested xy-rotation around the origin - r = radians(c.r_number) if c.r_flag else 0 - # parse the requested euler rotation angles - th1 = radians(c.i_number) if c.i_flag else 0 - th2 = radians(c.j_number) if c.j_flag else 0 - th3 = radians(c.k_number) if c.k_flag else 0 - - # build the translation vector of the twp_matrix - twp_origin = [[x], [y], [z]] - # create the rotation matrix for the requested origin rotation - try: - twp_origin_rotation = kins_calc_twp_origin_rot_matrix(r) - except Exception as error: - log.error('remap_func: kins_calc_twp_origin_rot_matrix failed, %s', error) - log.debug(' G68.4 P1: Twp_origin_rotation \n%s',twp_origin_rotation) - # build the rotation matrix for the requested euler rotation - twp_euler_rotation = calc_euler_rot_matrix(th1, th2, th3, q) - log.debug(' G68.4 P1: Twp_euler_rotation \n%s',twp_euler_rotation) - # calculate the total twp_rotation using matrix multiplication - twp_rotation = np.asmatrix(twp_origin_rotation) * np.asmatrix(twp_euler_rotation) - # combine rotation and translation and form the 4x4 twp-transformation matrix - twp_matrix = np.hstack((twp_rotation, twp_origin)) - twp_row_4 = [0,0,0,1] - twp_matrix = np.vstack((twp_matrix, twp_row_4)) - twp_matrix = np.asmatrix(twp_matrix) - # update the build state of the twp call - twp_flag[2] = 'done' - - elif p == 2: # twp defined py 3 points on the plane - # TODO implement operator errors as outlined in the twp README - #- G68.2 P2 (Q0),Q1,Q2,Q3 commands are not entered consecutively - #- two to the points entered in Q1,Q2,Q3 are identical - #- all three points entered in Q1,Q2,Q3 are on a line - #- the distance between a line defined by any two points entered in (Q1,Q2,Q3) and - #the remaining point is less than 10mm or 0.5inch (just some arbitrary values for now) - - # if this is the first call for this mode reset the twp_flag flag - if not twp_flag: - twp_flag = [int(p), 4 , 'empty', 'empty', 'empty', 'empty'] # four calls needed - twp_build_params = {'q0':[], 'q1':[], 'q2':[], 'q3':[]} - # Point 1: defines the origin of the twp - # Point 2: direction from P1 to P2 defines the positive x direction on the twp (x-vector) - # Point 3: defines the positive y side and with P1 and P2 defines the xy work plane (z-vector) - q = int(c.q_number if c.q_flag else 0) - # this mode needs four calls to fill all required parameters - if q == 0: # define new origin and rotation - x = c.x_number if c.x_flag else 0 - y = c.y_number if c.y_flag else 0 - z = c.z_number if c.z_flag else 0 - # parse the requested xy-rotation around the origin - r = radians(c.r_number) if c.r_flag else 0 - twp_build_params['q0'] = [x,y,z,r] - twp_flag[2] = 'done' - elif q == 1: # define point 1 - x1 = c.x_number if c.x_flag else 0 - y1 = c.y_number if c.y_flag else 0 - z1 = c.z_number if c.z_flag else 0 - twp_build_params['q1'] = [x1,y1,z1] - twp_flag[3] = 'done' - elif q == 2: # define point 2 - x2 = c.x_number if c.x_flag else 0 - y2 = c.y_number if c.y_flag else 0 - z2 = c.z_number if c.z_flag else 0 - twp_build_params['q2'] = [x2,y2,z2] - twp_flag[4] = 'done' - elif q == 3: # define point 3 - x3 = c.x_number if c.x_flag else 0 - y3 = c.y_number if c.y_flag else 0 - z3 = c.z_number if c.z_flag else 0 - twp_build_params['q3'] = [x3,y3,z3] - twp_flag[5] = 'done' - else: - # reset the twp parameters - reset_twp_params() - msg = ("G68.4 P2: No recognised Q-Word found.") - log.debug(' ' + msg) - emccanon.CANON_ERROR(msg) - yield INTERP_EXECUTE_FINISH # w/o this the error message is not displayed - yield INTERP_EXIT # w/o this the error does not abort a running gcode program - return INTERP_ERROR - - # only start calculations once all the parameters have been passed - if twp_flag.count('done') == twp_flag[1]: - [x, y, z, r] = twp_build_params['q0'][0:4] - # build the translation vector of the twp_matrix - twp_origin = [[x], [y], [z]] - p1 = twp_build_params['q1'][0:3] - p2 = twp_build_params['q2'] - p3 = twp_build_params['q3'] - log.debug(" G68.4 P2: Point 1: %s",p1) - log.debug(" G68.4 P2: Point 2: %s",p2) - log.debug(" G68.4 P2: Point 3: %s",p3) - # build vectors x:P1->P2 and v2:P1->P3 - twp_vect_x = [p2[0]-p1[0], p2[1]-p1[1], p2[2]-p1[2]] - log.debug(" G68.4 P2: Twp_vect_x: \n%s",twp_vect_x) - v2 = [p3[0]-p1[0], p3[1]-p1[1], p3[2]-p1[2]] - log.debug(" G68.4 P2: (v2) %s", v2) - # normalize the two vectors - twp_vect_x = twp_vect_x / np.linalg.norm(twp_vect_x) - v2 = v2 / np.linalg.norm(v2) - # we can use the cross product to calculate the z-vector vector - # note: if P3 is on the right side of the vector P1->P2 - # then the z-vector will be below the twp (ie z-vector will be downwards) - twp_vect_z = np.cross(twp_vect_x , v2) - log.debug(" G68.4 P2: Twp_vect_z %s",twp_vect_z) - # we can use the cross product to calculate the y vector - twp_vect_y = np.cross(twp_vect_z, twp_vect_x) - log.debug(" G68.4 P2: Twp_vect_y %s",twp_vect_y) - # build the rotation matrix of the twp_matrix from the calculated vectors - # first stack the vectors (lists) and then flip diagonally (transpose) - # so the vectors are now vertical - twp_vect_rotation_t = np.vstack((twp_vect_x, twp_vect_y)) - twp_vect_rotation_t = np.vstack((twp_vect_rotation_t, twp_vect_z)) - twp_vect_rotation = np.transpose(twp_vect_rotation_t) - log.debug(" G68.4 P2: Built the twp-rotation-matrix: \n%s", twp_vect_rotation) - # create the rotation matrix for the requested origin rotation - try: - twp_origin_rotation = kins_calc_twp_origin_rot_matrix(r) - except Exception as error: - log.error('remap_func: kins_calc_twp_origin_rot_matrix failed, %s', error) - log.debug(' G68.4 P2: Twp-origin-rotation-matrix \n%s',twp_origin_rotation) - # calculate the total twp_rotation using matrix multiplication - twp_rotation = np.asmatrix(twp_origin_rotation) * np.asmatrix(twp_vect_rotation) - # add the origin translation on the right - twp_matrix = np.hstack((twp_rotation, twp_origin)) - # expand to 4x4 array and make into a matrix - twp_row_4 = [0,0,0,1] - twp_matrix = np.vstack((twp_matrix, twp_row_4)) - twp_matrix = np.asmatrix(twp_matrix) - log.debug(" G68.4 P2: Built twp-transformation-matrix: \n%s", twp_matrix) - - elif p == 3: # two vectors (vector 1 defines the x-vector and vector 2 defines the z-vector) - # TODO implement operator errors as outlined in the twp README - #- G68.2 P3 Q1 and Q2 commands are not entered consecutively - #- one of the vectors is the zero vector - #- the enclosed angle between the 1. and 2. vector is <85° or >95° (re fanuc twp pdf) - q = int(c.q_number if c.q_flag else 0) - # if this is the first call for this mode reset the twp_flag flag - if not twp_flag: - twp_flag = [int(p), 2 , 'empty', 'empty'] # two calls needed - twp_build_params = {'q0':[], 'q1':[]} - if q == 0: # define new origin and first vector (direction of x in the twp) - x = c.x_number if c.x_flag else 0 - y = c.y_number if c.y_flag else 0 - z = c.z_number if c.z_flag else 0 - # parse the requested xy-rotation around the origin - r = radians(c.r_number) if c.r_flag else 0 - # first vector (direction of x in the twp) - i = c.i_number if c.i_flag else 0 - j = c.j_number if c.j_flag else 0 - k = c.k_number if c.k_flag else 0 - twp_build_params['q0'] = [x,y,z,i,j,k,r] - twp_flag[2] = 'done' - elif q == 1: # define second vector (the normal vector of the twp - i1 = c.i_number if c.i_flag else 0 - j1 = c.j_number if c.j_flag else 0 - k1 = c.k_number if c.k_flag else 0 - twp_build_params['q1'] = [i1,j1,k1] - twp_flag[3] = 'done' - else: - # reset the twp parameters - reset_twp_params() - msg = ("G68.4 P3: No recognised Q-Word found.") - log.debug(' ' + msg) - emccanon.CANON_ERROR(msg) - yield INTERP_EXECUTE_FINISH # w/o this the error message is not displayed - yield INTERP_EXIT # w/o this the error does not abort a running gcode program - return INTERP_ERROR - - # only start calculations once all the parameters have been passed - if twp_flag.count('done') == twp_flag[1]: - twp_origin = (x ,y, z) = twp_build_params['q0'][0:3] - r = twp_build_params['q0'][6] - (i, j, k) = twp_build_params['q0'][3:6] - (i1, j1, k1) = twp_build_params['q1'] - log.debug("(x, y, z) %s", (x, y, z)) - log.debug("(i, j, k) %s", (i, j, k)) - log.debug("(i1, j1, k1) %s", (i1, j1, k1)) - # build unit vector defining x-vector direction - twp_vect_x = [i-x, j-y, k-z] - twp_vect_x = twp_vect_x / np.linalg.norm(twp_vect_x) - twp_vect_z = [i1, j1, k1] - twp_vect_z = twp_vect_z / np.linalg.norm(twp_vect_z) - orth = np.dot(twp_vect_x, twp_vect_z) - log.debug(" orth check: %s", orth) - # the two vectors must be orthogonal - if orth != 0: - # reset the twp parameters - reset_twp_params() - ## reset the parameter values - #twp_flag = [int(p), 2 , 'empty', 'empty'] # two calls needed - #twp_build_params = {'q0':[], 'q1':[]} - msg = ("G68.4 P3: Vectors are not orthogonal.") - log.debug(' ' + msg) - emccanon.CANON_ERROR(msg) - yield INTERP_EXECUTE_FINISH # w/o this the error message is not displayed - yield INTERP_EXIT # w/o this the error does not abort a running gcode program - return INTERP_ERROR - - # we can use the cross product to calculate the y vector - twp_vect_y = np.cross(twp_vect_z, twp_vect_x) - log.debug(" G68.4 P3: twp_vect_y %s",twp_vect_y) - # build the rotation matrix of the twp_matrix from the calculated vectors - # first stack the vectors (lists) and then flip diagonally (transpose) - # so the vectors are now vertical - twp_vect_rotation_t = np.vstack((twp_vect_x, twp_vect_y)) - twp_vect_rotation_t = np.vstack((twp_vect_rotation_t, twp_vect_z)) - twp_vect_rotation = np.transpose(twp_vect_rotation_t) - log.debug(" G68.4 P3: Built twp-rotation-matrix: \n%s", twp_vect_rotation) - # create the rotation matrix for the requested origin rotation - try: - twp_origin_rotation = kins_calc_twp_origin_rot_matrix(r) - except Exception as error: - log.error('remap_func: kins_calc_twp_origin_rot_matrix failed, %s', error) - log.debug(' G68.4 P3: Twp-origin-rotation-matrix \n%s',twp_origin_rotation) - # calculate the total twp_rotation using matrix multiplication - twp_rotation = np.asmatrix(twp_origin_rotation) * np.asmatrix(twp_vect_rotation) - # add the origin translation on the right - twp_origin = [[x], [y], [z]] - twp_matrix = np.hstack((twp_rotation, twp_origin)) - # expand to 4x4 array and make into a matrix - twp_row_4 = [0,0,0,1] - twp_matrix = np.vstack((twp_matrix, twp_row_4)) - twp_matrix = np.asmatrix(twp_matrix) - log.debug(" G68.4 P3: Built twp-transformation-matrix: \n%s", twp_matrix) - - # TODO implement G68.4 P4 as outlined in the fanuc twp pdf (the exact meaning of which is unclear to me) - - else: - # reset the twp parameters - reset_twp_params() - msg = ("G68.4: No recognised P-Word found.") - log.debug(' ' + msg) - emccanon.CANON_ERROR(msg) - yield INTERP_EXECUTE_FINISH # w/o this the error message is not displayed - yield INTERP_EXIT # w/o this the error does not abort a running gcode program - return INTERP_ERROR - - log.debug(" G68.4: twp_flag: %s", twp_flag) - log.debug(" G68.4: calls required: %s", twp_flag.count('done')) - log.debug(" G68.4: number of calls made: %s", twp_flag.count('done')) - - if twp_flag.count('done') == twp_flag[1]: - log.info(' G68.4: requested rotation (degrees) %s', degrees(r)) - log.info(" G68.4: twp_matrix_current: \n%s", twp_matrix_current) - log.info(" G68.4: incremental twp_matrix requested: \n%s",twp_matrix) - log.info(" G68.4: calculating new twp_matrix...") - twp_matrix_new = twp_matrix_current * twp_matrix - log.info(" G68.4: twp_matrix_new: \n%s",twp_matrix_new) - twp_origin = [twp_matrix[0,3],twp_matrix[1,3],twp_matrix[2,3]] - log.info(" G68.4: twp origin: %s", twp_origin) - twp_vect_x = [twp_matrix[0,0],twp_matrix[1,0],twp_matrix[2,0]] - log.info(" G68.4: twp vector-x: %s", twp_vect_x) - twp_vect_z = [twp_matrix[0,2],twp_matrix[1,2],twp_matrix[2,2]] - log.info(" G68.4: twp vector-z: %s", twp_vect_z) - log.info(" G68.4: incremented twp_matrix: \n%s", twp_matrix_new) - twp_matrix = twp_matrix_new - # set twp-state to 'defined' (1) - self.execute("M68 E2 Q1") - yield INTERP_EXECUTE_FINISH - - gui_update_twp() - return INTERP_OK diff --git a/configs/sim/axis/vismach/5axis/table-rotary_spindle-rotary-nutating/python/toplevel.py b/configs/sim/axis/vismach/5axis/table-rotary_spindle-rotary-nutating/python/toplevel.py deleted file mode 100755 index c7ce432a045..00000000000 --- a/configs/sim/axis/vismach/5axis/table-rotary_spindle-rotary-nutating/python/toplevel.py +++ /dev/null @@ -1,20 +0,0 @@ -# This is a component of LinuxCNC -# Copyright 2011, 2012, 2013 Dewey Garrett , -# Michael Haberler -# -# This program is free software; you can redistribute it and/or modify -# it under the terms of the GNU General Public License as published by -# the Free Software Foundation; either version 2 of the License, or -# (at your option) any later version. -# -# This program is distributed in the hope that it will be useful, -# but WITHOUT ANY WARRANTY; without even the implied warranty of -# MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the -# GNU General Public License for more details. -# -# You should have received a copy of the GNU General Public License -# along with this program; if not, write to the Free Software -# Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA. -# -import remap - diff --git a/configs/sim/axis/vismach/5axis/table-rotary_spindle-rotary-nutating/python/twp-helper-comp.py b/configs/sim/axis/vismach/5axis/table-rotary_spindle-rotary-nutating/python/twp-helper-comp.py index 44748312c4e..ee6994e7196 100755 --- a/configs/sim/axis/vismach/5axis/table-rotary_spindle-rotary-nutating/python/twp-helper-comp.py +++ b/configs/sim/axis/vismach/5axis/table-rotary_spindle-rotary-nutating/python/twp-helper-comp.py @@ -1,87 +1,47 @@ #!/usr/bin/env python3 +# Publishes the tilted work plane for the vismach model: the origin and the +# X and Z directions in the coordinate system the plane was defined in, +# read from status where the interpreter keeps them (G68.2, G68.3, G68.4, +# G69), and the active work offset the model translates the plane by. import hal import linuxcnc +import time h = hal.component("twp-helper-comp") -# this pin reflects the machine.analog pin used for the -# twp-status -h.newpin("twp-status", hal.Type.REAL, hal.Dir.IN) -# these pins are created here from 'twp-status'' -h.newpin("twp-is-redefined", hal.Type.BOOL, hal.Dir.OUT) +h.newpin("twp-status", hal.Type.REAL, hal.Dir.OUT) # 0 undefined, 1 defined h.newpin("twp-is-defined", hal.Type.BOOL, hal.Dir.OUT) h.newpin("twp-is-active", hal.Type.BOOL, hal.Dir.OUT) -# twp origin vector -h.newpin("twp-ox-in", hal.Type.REAL, hal.Dir.IN) -h.newpin("twp-oy-in", hal.Type.REAL, hal.Dir.IN) -h.newpin("twp-oz-in", hal.Type.REAL, hal.Dir.IN) -h.newpin("twp-ox", hal.Type.REAL, hal.Dir.OUT) -h.newpin("twp-oy", hal.Type.REAL, hal.Dir.OUT) -h.newpin("twp-oz", hal.Type.REAL, hal.Dir.OUT) -# twp x-orientation vector -h.newpin("twp-xx-in", hal.Type.REAL, hal.Dir.IN) -h.newpin("twp-xy-in", hal.Type.REAL, hal.Dir.IN) -h.newpin("twp-xz-in", hal.Type.REAL, hal.Dir.IN) -h.newpin("twp-xx", hal.Type.REAL, hal.Dir.OUT) -h.newpin("twp-xy", hal.Type.REAL, hal.Dir.OUT) -h.newpin("twp-xz", hal.Type.REAL, hal.Dir.OUT) -# twp z-orientation vector -h.newpin("twp-zx-in", hal.Type.REAL, hal.Dir.IN) -h.newpin("twp-zy-in", hal.Type.REAL, hal.Dir.IN) -h.newpin("twp-zz-in", hal.Type.REAL, hal.Dir.IN) -h.newpin("twp-zx", hal.Type.REAL, hal.Dir.OUT) -h.newpin("twp-zy", hal.Type.REAL, hal.Dir.OUT) -h.newpin("twp-zz", hal.Type.REAL, hal.Dir.OUT) -# twp origin vector in machine coordinate system -h.newpin("twp-ox-world-in", hal.Type.REAL, hal.Dir.IN) -h.newpin("twp-oy-world-in", hal.Type.REAL, hal.Dir.IN) -h.newpin("twp-oz-world-in", hal.Type.REAL, hal.Dir.IN) -h.newpin("twp-ox-world", hal.Type.REAL, hal.Dir.OUT) -h.newpin("twp-oy-world", hal.Type.REAL, hal.Dir.OUT) -h.newpin("twp-oz-world", hal.Type.REAL, hal.Dir.OUT) +for name in ("twp-ox", "twp-oy", "twp-oz", + "twp-xx", "twp-xy", "twp-xz", + "twp-zx", "twp-zy", "twp-zz", + "twp-ox-world", "twp-oy-world", "twp-oz-world"): + h.newpin(name, hal.Type.REAL, hal.Dir.OUT) h.ready() -# create a connection to the status channel s = linuxcnc.stat() try: while 1: - # publish twp-status - if h['twp-status'] == 1: - h['twp-is-defined'] = 1 - h['twp-is-active'] = 0 - elif h['twp-status'] == 2: - h['twp-is-defined'] = 1 - h['twp-is-active'] = 1 - else: - h['twp-is-defined'] = 0 - h['twp-is-active'] = 0 - - # passthrough the twp arguments - h['twp-ox'] = h['twp-ox-in'] - h['twp-oy'] = h['twp-oy-in'] - h['twp-oz'] = h['twp-oz-in'] - h['twp-xx'] = h['twp-xx-in'] - h['twp-xy'] = h['twp-xy-in'] - h['twp-xz'] = h['twp-xz-in'] - h['twp-zx'] = h['twp-zx-in'] - h['twp-zy'] = h['twp-zy-in'] - h['twp-zz'] = h['twp-zz-in'] - - # we only want to expose offsets when twp is not defined - if not h['twp-is-defined']: - s.poll() # get current values - g5x_offset = s.g5x_offset - h['twp-ox-world'] = g5x_offset[0] - h['twp-oy-world'] = g5x_offset[1] - h['twp-oz-world'] = g5x_offset[2] - else : # use the values from the remap - h['twp-ox-world'] = h['twp-ox-world-in'] - h['twp-oy-world'] = h['twp-oy-world-in'] - h['twp-oz-world'] = h['twp-oz-world-in'] + s.poll() + active = 1 if s.g68_active else 0 + h['twp-status'] = active + h['twp-is-defined'] = active + h['twp-is-active'] = active + + o = s.g68_offset + r = s.g68_rotation + h['twp-ox'], h['twp-oy'], h['twp-oz'] = o[0], o[1], o[2] + # columns of the rotation: the plane's X and Z + h['twp-xx'], h['twp-xy'], h['twp-xz'] = r[0], r[3], r[6] + h['twp-zx'], h['twp-zy'], h['twp-zz'] = r[2], r[5], r[8] + + g5x = s.g5x_offset + h['twp-ox-world'], h['twp-oy-world'], h['twp-oz-world'] = g5x[0], g5x[1], g5x[2] + time.sleep(0.05) except KeyboardInterrupt: raise SystemExit diff --git a/configs/sim/axis/vismach/5axis/table-rotary_spindle-rotary-nutating/python/util.py b/configs/sim/axis/vismach/5axis/table-rotary_spindle-rotary-nutating/python/util.py deleted file mode 100755 index 59b012058ad..00000000000 --- a/configs/sim/axis/vismach/5axis/table-rotary_spindle-rotary-nutating/python/util.py +++ /dev/null @@ -1,67 +0,0 @@ -# This is a component of LinuxCNC -# Copyright 2011, 2013 Dewey Garrett , Michael -# Haberler -# -# This program is free software; you can redistribute it and/or modify -# it under the terms of the GNU General Public License as published by -# the Free Software Foundation; either version 2 of the License, or -# (at your option) any later version. -# -# This program is distributed in the hope that it will be useful, -# but WITHOUT ANY WARRANTY; without even the implied warranty of -# MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the -# GNU General Public License for more details. -# -# You should have received a copy of the GNU General Public License -# along with this program; if not, write to the Free Software -# Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA. -# -import inspect -import emccanon - -# O-word procedure to trap into the Pydevd debugger -# start debug server in Eclipse, then -# call as 'O call' from MDI - -# example setup for debugging embedded Python code -# see http://pydev.org/manual_adv_remote_debugger.html -# if this points to a valid directory, - -def call_pydevd(): - """ trap into the pydevd debugger""" - - import os,sys - - pydevdir= '/home/mah/.eclipse/org.eclipse.platform_3.5.0_155965261/plugins/org.python.pydev.debug_2.0.0.2011040403/pysrc/' - - # the 'emctask' module is present only in the milltask instance, otherwise both the UI and - # milltask would try to connect to the debug server. - - if os.path.isdir(pydevdir) and 'emctask' in sys.builtin_module_names: - sys.path.append(pydevdir) - sys.path.insert(0,pydevdir) - try: - import pydevd - emccanon.MESSAGE("pydevd imported, connecting to Eclipse debug server...") - pydevd.settrace() - except: - emccanon.MESSAGE("no pydevd module found") - pass - - - -def lineno(): - """ return line number in the current Python script """ - return inspect.currentframe().f_back.f_lineno - -def error_stack(self): - """ print the Interpreters error stack (function names) """ - print("error stack level=%d" % (self.stack_index)) - for s in self.stack(): - print("--'%s'" % (s)) - -def callstack(self): - """ print the O-Word call stack """ - for i in range(self.call_level): - c = self.sub_context[i] - print("%d: pos=%d seq=%d filename=%s sub=%s" % (i,c.position, c.sequence_number,c.filename,c.subname)) diff --git a/configs/sim/axis/vismach/5axis/table-rotary_spindle-rotary-nutating/remap_subs/g531remap.ngc b/configs/sim/axis/vismach/5axis/table-rotary_spindle-rotary-nutating/remap_subs/g531remap.ngc deleted file mode 100755 index 4b2fd293c61..00000000000 --- a/configs/sim/axis/vismach/5axis/table-rotary_spindle-rotary-nutating/remap_subs/g531remap.ngc +++ /dev/null @@ -1,14 +0,0 @@ -; this is the wrapper remap to orient the spindle using IDENTITY kinematics G53.1 - -osub -M66 L0 E0 ;force sync, stop read ahead -o100 if [EXISTS [#

]] -o100 else - #

= 0 ;if no P word has been passed we use the default (0) -o100 endif -G13.1 ;back to identity kinematic -M66 L0 E0 -M530 P#

;orient the spindle with P word -M66 L0 E0 -oendsub -m2 diff --git a/configs/sim/axis/vismach/5axis/table-rotary_spindle-rotary-nutating/remap_subs/g533remap.ngc b/configs/sim/axis/vismach/5axis/table-rotary_spindle-rotary-nutating/remap_subs/g533remap.ngc deleted file mode 100755 index 16d9687cbe8..00000000000 --- a/configs/sim/axis/vismach/5axis/table-rotary_spindle-rotary-nutating/remap_subs/g533remap.ngc +++ /dev/null @@ -1,20 +0,0 @@ -; this is the wrapper remap to orient the spindle using IDENTITY kinematics G53.3 with simultaneous move to XYZ (in tool coordinates) - -osub -M66 L0 E0 ;force sync, stop read ahead -o100 if [[EXISTS [#]] AND [EXISTS [#]] AND [EXISTS [#]]] - G13.1 ;back to identity kinematic -o100 else - (abort, G53.3: X,Y and Z words are required) ;it is an error if X,Y or Z word is missing -o100 endif -o105 if [EXISTS [#

]] ;check if a P word has been passed - ;(print, P=#

) -o105 else - #

= 0 ;if no P word has been passed we use the default (0) -o105 endif -M66 L0 E0 -;Note we can not pass XYZ words to an m-code so we send coords as ijk and handle it in remap.py -M530 P#

I# J# K# ;orient the spindle with P word and xyz as ijk -M66 L0 E0 -oendsub -m2 diff --git a/configs/sim/axis/vismach/5axis/table-rotary_spindle-rotary-nutating/remap_subs/g536remap.ngc b/configs/sim/axis/vismach/5axis/table-rotary_spindle-rotary-nutating/remap_subs/g536remap.ngc deleted file mode 100755 index a8b628a930c..00000000000 --- a/configs/sim/axis/vismach/5axis/table-rotary_spindle-rotary-nutating/remap_subs/g536remap.ngc +++ /dev/null @@ -1,14 +0,0 @@ -; this is the wrapper remap to orient the spindle using TCP kinematics G53.6 - -osub -M66 L0 E0 ;force sync, stop read ahead -o100 if [EXISTS [#

]] -o100 else - #

= 0 ;if no P word has been passed we use the default (0) -o100 endif -G12.1 P1 ;switch to tcp kinematic -M66 L0 E0 -M530 P#

;orient the spindle with P word -M66 L0 E0 -oendsub -m2 diff --git a/configs/sim/axis/vismach/5axis/table-rotary_spindle-rotary-nutating/remap_subs/g69remap.ngc b/configs/sim/axis/vismach/5axis/table-rotary_spindle-rotary-nutating/remap_subs/g69remap.ngc deleted file mode 100755 index 9efd1b7db29..00000000000 --- a/configs/sim/axis/vismach/5axis/table-rotary_spindle-rotary-nutating/remap_subs/g69remap.ngc +++ /dev/null @@ -1,11 +0,0 @@ -; this is the wrapper remap to cancel TWP - -osub -M66 L0 E0 ; force sync, stop read ahead -M469 ; call the python G69_core code -G13.1 ; back to identity kins -M68 E2 Q0 ; reset twp-state to 'undefined' (0) -G54 ; switch to G54 -M66 L0 E0 ; force sync, stop read ahead -oendsub -m2 diff --git a/configs/sim/axis/vismach/5axis/table-rotary_spindle-rotary-nutating/remap_subs/on_abort_with_twp_reset.ngc b/configs/sim/axis/vismach/5axis/table-rotary_spindle-rotary-nutating/remap_subs/on_abort_with_twp_reset.ngc deleted file mode 100755 index 1cbf3d41db7..00000000000 --- a/configs/sim/axis/vismach/5axis/table-rotary_spindle-rotary-nutating/remap_subs/on_abort_with_twp_reset.ngc +++ /dev/null @@ -1,15 +0,0 @@ -;This is a workaround for a bug that leads to stat.gcodes and parameter[5250] -;to get out of sync after some program aborts -;in [RS274NGC] section of the ini add: ON_ABORT_COMMAND = o call -;save this to a path specified in SUBROUTINE_PATH = -;NOTE: we cannot run remapped codes here only custom Mcodes (ie M100..M199) - -o sub -;(msg, on_abort START) -M68 E2 Q0 ; reset twp-state to 'undefined' (0) -G13.1 ; back to identity kins -G64 P0.01 ; reset the toolpath tolerance as this sometimes gets set to zero on estop events -G54 ; switch to G54 -(msg, on_abort END) -o endsub -M2 diff --git a/configs/sim/axis/vismach/5axis/table-rotary_spindle-rotary-nutating/xyzacb-trsrn_twp/xyzacb-trsrn.ini b/configs/sim/axis/vismach/5axis/table-rotary_spindle-rotary-nutating/xyzacb-trsrn_twp/xyzacb-trsrn.ini index 2be585e6ef8..0c4389ca1a9 100644 --- a/configs/sim/axis/vismach/5axis/table-rotary_spindle-rotary-nutating/xyzacb-trsrn_twp/xyzacb-trsrn.ini +++ b/configs/sim/axis/vismach/5axis/table-rotary_spindle-rotary-nutating/xyzacb-trsrn_twp/xyzacb-trsrn.ini @@ -29,46 +29,18 @@ MAX_ANGULAR_VELOCITY = 360 [RS274NGC] RS274NGC_STARTUP_CODE = G17 G21 G40 G49 G54 G64 P0.001 G80 G90 G92.1 G94 G97 G98 ON_ABORT_COMMAND = o call -#ON_ABORT_COMMAND = o call SUBROUTINE_PATH = ../remap_subs:../demos HAL_PIN_VARS = 1 REMAP = M428 modalgroup=10 ngc=428remap REMAP = M429 modalgroup=10 ngc=429remap REMAP = M430 modalgroup=10 ngc=430remap - REMAP = G53.1 modalgroup=1 argspec=p ngc=g531remap - REMAP = G53.3 modalgroup=1 argspec=pxyz ngc=g533remap - REMAP = G53.6 modalgroup=1 argspec=p ngc=g536remap - REMAP = M530 modalgroup=10 python=g53n_core - - REMAP = G68.2 modalgroup=1 argspec=pqxyzijkr python=g682 - REMAP = G68.3 modalgroup=1 argspec=xyzr python=g683 - REMAP = G68.4 modalgroup=1 argspec=pqxyzijkr python=g684 - - REMAP = G69 modalgroup=1 ngc=g69remap - REMAP = M469 modalgroup=10 python=g69_core - PARAMETER_FILE = xyzacb-trsrn.var -[PYTHON] -# where to find the Python code: -# code specific for this configuration -PATH_APPEND = ../python -# import the following Python module -TOPLEVEL = ../python/toplevel.py -# the higher the more verbose tracing of the Python plugin -LOG_LEVEL = 3 - [KINS] KINEMATICS = xyzacb_trsrn JOINTS = 6 -[TWP] -# this defines the primary spindle rotation -PRIMARY = C -# this defines the secnodary spindle rotation (ie the one closest to the tool) -SECONDARY = B - [HAL] HALUI = halui HALFILE = LIB:basic_sim.tcl @@ -76,11 +48,6 @@ POSTGUI_HALFILE = xyzacb-trsrn_postgui.hal #HALCMD = loadusr ../python/feed_zero.py - -# signal reflecting twp states (0=undefined, 1=defined, 2=active) -HALCMD = net twp-status <= motion.analog-out-02 - - # connections required for the kinematics component HALCMD = net :tool-offset motion.tooloffset.z xyzacb_trsrn_kins.tool-offset-z HALCMD = net :rot-axis-y xyzacb_trsrn_kins.y-rot-axis @@ -93,8 +60,6 @@ HALCMD = net :offset-y xyzacb_trsrn_kins.y- # load the required twp-helper component and its hal connections HALCMD = loadusr -W ../python/twp-helper-comp.py -#twp-status -HALCMD = net twp-status => twp-helper-comp.twp-status HALCMD = net twp-is-defined <= twp-helper-comp.twp-is-defined HALCMD = net twp-is-active <= twp-helper-comp.twp-is-active # current twp parameters @@ -150,7 +115,6 @@ HALCMD = net twp-status xyzacb-trsrn-gui.twp HALCMD = net twp-is-defined xyzacb-trsrn-gui.twp_defined HALCMD = net twp-is-active xyzacb-trsrn-gui.twp_active - [HALUI] # NOTE: kinstype==0 is identity kins because sparm=identityfirst # M428:identity kins (kinstype 0, startupDEFAULT) @@ -220,7 +184,6 @@ MAX_ACCELERATION = 302 MAX_VELOCITY = 30 MAX_ACCELERATION = 301 - [JOINT_0] TYPE = LINEAR HOME = 0 @@ -231,7 +194,6 @@ MAX_ACCELERATION = 301 HOME_SEARCH_VEL = 0 HOME_SEQUENCE = 0 - [JOINT_1] TYPE = LINEAR HOME = 0 @@ -252,7 +214,6 @@ MAX_ACCELERATION = 301 HOME_SEARCH_VEL = 0 HOME_SEQUENCE = 0 - #table rotary [JOINT_3] TYPE = ANGULAR diff --git a/configs/sim/axis/vismach/5axis/table-rotary_spindle-rotary-nutating/xyzbca-trsrn_twp/xyzbca-trsrn.ini b/configs/sim/axis/vismach/5axis/table-rotary_spindle-rotary-nutating/xyzbca-trsrn_twp/xyzbca-trsrn.ini index d3032855aee..5223bc375f6 100644 --- a/configs/sim/axis/vismach/5axis/table-rotary_spindle-rotary-nutating/xyzbca-trsrn_twp/xyzbca-trsrn.ini +++ b/configs/sim/axis/vismach/5axis/table-rotary_spindle-rotary-nutating/xyzbca-trsrn_twp/xyzbca-trsrn.ini @@ -28,56 +28,24 @@ MAX_ANGULAR_VELOCITY = 360 [RS274NGC] RS274NGC_STARTUP_CODE = G17 G21 G40 G49 G54 G64 P0.001 G80 G90 G92.1 G94 G97 G98 -#ON_ABORT_COMMAND = o call -ON_ABORT_COMMAND = o call +ON_ABORT_COMMAND = o call SUBROUTINE_PATH = ../remap_subs:../demos HAL_PIN_VARS = 1 REMAP = M428 modalgroup=10 ngc=428remap REMAP = M429 modalgroup=10 ngc=429remap REMAP = M430 modalgroup=10 ngc=430remap - REMAP = G53.1 modalgroup=1 argspec=p ngc=g531remap - REMAP = G53.3 modalgroup=1 argspec=pxyz ngc=g533remap - REMAP = G53.6 modalgroup=1 argspec=p ngc=g536remap - REMAP = M530 modalgroup=10 python=g53n_core - - REMAP = G68.2 modalgroup=1 argspec=pqxyzijkr python=g682 - REMAP = G68.3 modalgroup=1 argspec=xyzr python=g683 - REMAP = G68.4 modalgroup=1 argspec=pqxyzijkr python=g684 - - REMAP = G69 modalgroup=1 ngc=g69remap - REMAP = M469 modalgroup=10 python=g69_core - PARAMETER_FILE = xyzbca-trsrn.var -[PYTHON] -# where to find the Python code: -# code specific for this configuration -PATH_APPEND = ../python -# import the following Python module -TOPLEVEL = ../python/toplevel.py -# the higher the more verbose tracing of the Python plugin -LOG_LEVEL = 3 - [KINS] KINEMATICS = xyzbca_trsrn JOINTS = 6 -[TWP] -# this defines the primary spindle rotation -PRIMARY = C -# this defines the secnodary spindle rotation (ie the one closest to the tool) -SECONDARY = A - [HAL] HALUI = halui HALFILE = LIB:basic_sim.tcl POSTGUI_HALFILE = xyzbca-trsrn_postgui.hal -# signal reflecting twp states (0=undefined, 1=defined, 2=active) -HALCMD = net twp-status <= motion.analog-out-02 - - # connections required for the kinematics component HALCMD = net :tool-offset motion.tooloffset.z xyzbca_trsrn_kins.tool-offset-z HALCMD = net :rot-axis-x xyzbca_trsrn_kins.x-rot-axis @@ -90,8 +58,6 @@ HALCMD = net :offset-y xyzbca_trsrn_kins.y- # load the required twp-helper component and its hal connections HALCMD = loadusr -W ../python/twp-helper-comp.py -#twp-status -HALCMD = net twp-status => twp-helper-comp.twp-status HALCMD = net twp-is-defined <= twp-helper-comp.twp-is-defined HALCMD = net twp-is-active <= twp-helper-comp.twp-is-active # current twp parameters @@ -147,7 +113,6 @@ HALCMD = net twp-status xyzbca-trsrn-gui.twp HALCMD = net twp-is-defined xyzbca-trsrn-gui.twp_defined HALCMD = net twp-is-active xyzbca-trsrn-gui.twp_active - [HALUI] # NOTE: kinstype==0 is identity kins because sparm=identityfirst # M428:identity kins (kinstype 0, startupDEFAULT) @@ -217,7 +182,6 @@ MAX_ACCELERATION = 302 MAX_VELOCITY = 30 MAX_ACCELERATION = 301 - [JOINT_0] TYPE = LINEAR HOME = 0 @@ -228,7 +192,6 @@ MAX_ACCELERATION = 301 HOME_SEARCH_VEL = 0 HOME_SEQUENCE = 0 - [JOINT_1] TYPE = LINEAR HOME = 0 @@ -249,7 +212,6 @@ MAX_ACCELERATION = 301 HOME_SEARCH_VEL = 0 HOME_SEQUENCE = 0 - # spindle secondary joint [JOINT_3] TYPE = ANGULAR diff --git a/tests/twp-native/checkresult b/tests/twp-native/checkresult new file mode 100755 index 00000000000..9d48d3f180e --- /dev/null +++ b/tests/twp-native/checkresult @@ -0,0 +1,3 @@ +#!/bin/sh +# the test script counts its own failures +grep -q "^Exiting with 0 errors" "$1" diff --git a/tests/twp-native/test-ui.py b/tests/twp-native/test-ui.py index 4c15ce9bfca..c37ebd2579f 100755 --- a/tests/twp-native/test-ui.py +++ b/tests/twp-native/test-ui.py @@ -182,8 +182,16 @@ def rot_y(d): % (after[SECONDARY], after[PRIMARY], want[0], want[1])) if abs(after[TABLE] - start[TABLE]) > 1e-9: error("G53.1 moved the table with Q0") -worst = max(abs(smp[0][i] - start[i]) for smp in samples for i in range(3)) -print("linear joints moved at most %.9f through G53.1" % worst) +worst = 0.0 +where = (0, 0, start[0], len(samples)) +for n, smp in enumerate(samples): + for i in range(3): + d = abs(smp[0][i] - start[i]) + if d > worst: + worst = d + where = (n, i, smp[0][i], len(samples)) +print("linear joints moved at most %.9f through G53.1 (sample %d of %d, joint %d at %.9f); ended at %s" + % ((worst,) + (where[0], where[3], where[1], where[2]) + (" ".join("%.9f" % v for v in after[:3]),))) if worst > 1e-6: error("G53.1 moved a linear joint") if not close(tool_axis(after), list(R[:, 2]), 1e-6): From be368ab908bbe6fec083e5b9921fee9c7e26a9c1 Mon Sep 17 00:00:00 2001 From: Luca Toniolo <10792599+grandixximo@users.noreply.github.com> Date: Mon, 7 Sep 2026 16:30:15 +1000 Subject: [PATCH 48/60] 5axiskins: supply the work and tool frames The head carries the tool and nothing turns the work, so the work frame is the machine frame and the tool frame is Rz(C) Ry(-B), the tilt left handed about y as the module's note 10 says. Its third column is the tool axis the forward already uses, from the tip towards the holder, so no native rotation is needed. Without them G53.1, G53.3, G53.6 and G68.3 refused on the bridgemill sim while every other five-axis module took them. tests/kins-frames takes the module and gains two checks: the type the module models must supply frames of its own, since a switchable module's identity type passed on its neighbour's answers; and the W joint runs the tool out along the reported axis reversed, which ties the frame to the forward. Reversing the tilt, reversing the head's turn and dropping the frames each fail. --- src/emc/kinematics/5axiskins.c | 21 +++++++++++++++++++++ tests/kins-frames/checkresult | 2 +- tests/kins-frames/framecheck.c | 27 ++++++++++++++++++++++++++- tests/kins-frames/test.sh | 3 +++ 4 files changed, 51 insertions(+), 2 deletions(-) diff --git a/src/emc/kinematics/5axiskins.c b/src/emc/kinematics/5axiskins.c index cd8b4db7b75..9de19603720 100644 --- a/src/emc/kinematics/5axiskins.c +++ b/src/emc/kinematics/5axiskins.c @@ -202,9 +202,30 @@ static int fiveaxis_jacobian(const kins_params *p, jac); } // fiveaxis_jacobian() +// The head carries the tool and nothing turns the work: the work frame is the +// machine frame and the tool frame Rz(C) Ry(-B), the tilt left handed about y +// (note 10), its third column the tool axis from the tip towards the holder. +static int fiveaxis_tool_frame(const kins_params *p, const double *joints, + PmRotationMatrix *rot, + const KINEMATICS_FORWARD_FLAGS *fflags) +{ + (void)fflags; + const double sb = sin(joints[JB]*TO_RAD), cb = cos(joints[JB]*TO_RAD); + const double sc = sin(joints[JC]*TO_RAD), cc = cos(joints[JC]*TO_RAD); + + rot->x.x = cb * cc; rot->y.x = -sc; rot->z.x = -sb * cc; + rot->x.y = cb * sc; rot->y.y = cc; rot->z.y = -sb * sc; + rot->x.z = sb; rot->y.z = 0; rot->z.z = cb; + + return 0; +} // fiveaxis_tool_frame() + static const kins_ops fiveaxis_ops = { .forward = fiveaxis_forward, .inverse = fiveaxis_inverse, + .work = kinsIdentityFrame, + .tool = fiveaxis_tool_frame, + .native = &TOOL_FRAME_SPINDLE, .jacobian = fiveaxis_jacobian, .primary = 1, }; diff --git a/tests/kins-frames/checkresult b/tests/kins-frames/checkresult index 5fefd687bac..48c09ee0d98 100755 --- a/tests/kins-frames/checkresult +++ b/tests/kins-frames/checkresult @@ -1,3 +1,3 @@ #!/bin/sh -[ "$(grep -c 'frames agree' "$1")" = 5 ] \ +[ "$(grep -c 'frames agree' "$1")" = 6 ] \ && ! grep -q "FAIL" "$1" diff --git a/tests/kins-frames/framecheck.c b/tests/kins-frames/framecheck.c index a2be114cc68..bf284731c53 100644 --- a/tests/kins-frames/framecheck.c +++ b/tests/kins-frames/framecheck.c @@ -51,6 +51,9 @@ RTAPI_MP_INT(ktype, "switchkins type where the module models its own machine"); static int spin = -1; RTAPI_MP_INT(spin, "joint that turns the whole head about the machine's z, -1 for none"); +static int quill = -1; +RTAPI_MP_INT(quill, "joint that extends the tool along its own axis, -1 for none"); + static int r1 = -1, r2 = -1, r3 = -1; RTAPI_MP_INT(r1, "joint number of the first rotary to sweep"); RTAPI_MP_INT(r2, "joint number of the second rotary, -1 for none"); @@ -205,6 +208,16 @@ static void check(const double *j, int own_kinematics) && close3(&tool.z, 0, 0, 1), "the spindle stays square", j); } + if (quill >= 0) { + /* the joint runs the tool out along its own axis, away from the + holder, so the tip moves along the tool axis reversed: the one + tie between the reported frame and the forward transform on a + machine that turns nothing but the tool */ + response(j, quill, &d); + expect(close3(&d, -tool.z.x, -tool.z.y, -tool.z.z), + "the quill runs out along the tool axis", j); + } + if (spin >= 0) { memcpy(t, j, sizeof(t)); t[spin] = j[spin] + TURN; @@ -227,7 +240,7 @@ int rtapi_app_main(void) const int angles = sizeof(angle) / sizeof(angle[0]); double j[EMCMOT_MAX_JOINTS]; int a, b, c, t; - int checked = 0; + int checked = 0, own = 0; if (joints < 1 || joints > EMCMOT_MAX_JOINTS) { rtapi_print_msg(RTAPI_MSG_ERR, "framecheck: joints=%d\n", joints); @@ -245,6 +258,7 @@ int rtapi_app_main(void) memset(j, 0, sizeof(j)); if (!carries_tool) { j[0] = 10; j[1] = 20; j[2] = 30; } + own = 0; /* every kinematics the module offers, not just the one it starts in: the frames a switchable module reports are per type, and the @@ -253,6 +267,7 @@ int rtapi_app_main(void) if (kinematicsSwitchable() && kinematicsSwitch(t)) { break; } if (!supplies_frames(j)) { continue; } checked++; + if (t == ktype) { own = 1; } for (a = 0; a < angles; a++) { if (r1 >= 0) { j[r1] = angle[a]; } @@ -271,6 +286,16 @@ int rtapi_app_main(void) if (!kinematicsSwitchable()) { break; } } + /* the identity type a switchable module carries supplies frames of + its own, so a module that reports none for the machine it models + would otherwise pass on its neighbour's answers */ + if (checked && !own) { + rtapi_print_msg(RTAPI_MSG_ERR, + "framecheck: FAIL the module reports no frames for" + " kinematics type %d, the machine it models\n", ktype); + failures++; + } + if (!checked) { rtapi_print_msg(RTAPI_MSG_ERR, "framecheck: the module reports frames for no type\n"); diff --git a/tests/kins-frames/test.sh b/tests/kins-frames/test.sh index 4bb8e108765..cbc350a2eef 100755 --- a/tests/kins-frames/test.sh +++ b/tests/kins-frames/test.sh @@ -49,5 +49,8 @@ setp xyzbca_trsrn_kins.z-pivot 200 setp xyzbca_trsrn_kins.tool-offset-z 50" \ "joints=6 r1=3 r2=4 r3=5 spin=5 ktype=1" +run "5axiskins coordinates=XYZBCW" "setp 5axiskins.pivot-length 250" \ + "joints=6 carries_tool=1 r1=3 r2=4 spin=4 quill=5" + run "pumakins" "setp pumakins.A2 300" \ "joints=6 carries_tool=1 r1=0 r2=3 r3=4 spin=0" From 619398987eaa50dd0adc26c91587bcd911b07c3d Mon Sep 17 00:00:00 2001 From: Luca Toniolo <10792599+grandixximo@users.noreply.github.com> Date: Tue, 8 Sep 2026 16:04:42 +1000 Subject: [PATCH 49/60] tests: give the tilted work plane a plane that tells the forms apart Every plane in the frame test was one rotation of ninety degrees about a single axis, written four ways. With two angles zero, composing about the frame as it turns and about the fixed axes give the same matrix, so the difference between G68.2 P0 and P1, the whole reason they are separate codes, went unobserved. One plane at 25, 40 and -15 degrees instead, still written four ways, with the points and vectors derived from it; G68.4 and R take odd angles too, and G53 inside the plane lands somewhere that is not a permutation of the axes. Caught now: P0 composing the way P1 does, R composed before the plane rotation, the three-point form taking Y as X cross Z. --- tests/interp/g68-frame/expected | 88 +++++++++++++++++---------------- tests/interp/g68-frame/g68.ngc | 26 +++++----- 2 files changed, 59 insertions(+), 55 deletions(-) diff --git a/tests/interp/g68-frame/expected b/tests/interp/g68-frame/expected index 98c0321fffe..79e5b75b286 100644 --- a/tests/interp/g68-frame/expected +++ b/tests/interp/g68-frame/expected @@ -10,46 +10,48 @@ 10 N..... SET_G5X_OFFSET(2, 100.0000, 200.0000, 300.0000, 0.0000, 0.0000, 0.0000) 11 N..... SET_G92_OFFSET(0.0000, 0.0000, 0.0000, 0.0000, 0.0000, 0.0000) 12 N..... SET_XY_ROTATION(0.0000) - 13 N..... COMMENT("a plane rotated 90 about X: plane Y is world Z, plane Z is world -Y") - 14 N..... SET_G68_FRAME(10.0000, 20.0000, 30.0000, [1.0000, 0.0000, 0.0000, 0.0000, 0.0000, -1.0000, 0.0000, 1.0000, 0.0000], 1) - 15 N..... STRAIGHT_TRAVERSE(1.0000, 2.0000, 3.0000, 0.0000, 0.0000, 0.0000) - 16 N..... COMMENT("the same plane by fixed axis angles about X") - 17 N..... SET_G68_FRAME(10.0000, 20.0000, 30.0000, [1.0000, 0.0000, 0.0000, 0.0000, 0.0000, -1.0000, 0.0000, 1.0000, 0.0000], 1) - 18 N..... STRAIGHT_TRAVERSE(1.0000, 2.0000, 3.0000, 0.0000, 0.0000, 0.0000) - 19 N..... COMMENT("the same plane by three points") - 20 N..... SET_G68_FRAME(10.0000, 20.0000, 30.0000, [1.0000, 0.0000, 0.0000, 0.0000, 0.0000, -1.0000, 0.0000, 1.0000, 0.0000], 1) - 21 N..... STRAIGHT_TRAVERSE(1.0000, 2.0000, 3.0000, 0.0000, 0.0000, 0.0000) - 22 N..... COMMENT("the same plane by two vectors, X nudged off square") - 23 N..... SET_G68_FRAME(10.0000, 20.0000, 30.0000, [1.0000, 0.0000, 0.0000, 0.0000, 0.0000, -1.0000, 0.0000, 1.0000, 0.0000], 1) - 24 N..... STRAIGHT_TRAVERSE(1.0000, 2.0000, 3.0000, 0.0000, 0.0000, 0.0000) - 25 N..... COMMENT("R turns the plane about its own Z") - 26 N..... SET_G68_FRAME(10.0000, 20.0000, 30.0000, [0.0000, -1.0000, 0.0000, 0.0000, 0.0000, -1.0000, 1.0000, 0.0000, 0.0000], 1) - 27 N..... STRAIGHT_TRAVERSE(1.0000, 2.0000, 3.0000, 0.0000, 0.0000, 0.0000) - 28 N..... COMMENT("an arc in the plane") - 29 N..... SET_FEED_RATE(100.0000) - 30 N..... STRAIGHT_FEED(0.0000, 0.0000, 0.0000, 0.0000, 0.0000, 0.0000) - 31 N..... ARC_FEED(2.0000, 0.0000, 1.0000, 0.0000, -1, 0.0000, 0.0000, 0.0000, 0.0000) - 32 N..... COMMENT("G53 inside the plane goes to absolute coordinates") - 33 N..... STRAIGHT_TRAVERSE(-30.0000, 10.0000, 20.0000, 0.0000, 0.0000, 0.0000) - 34 N..... COMMENT("and #5021 reports them") - 35 N..... MESSAGE(" abs 100.000000 200.000000 300.000000 prog -30.000000 10.000000 20.000000") - 36 N..... COMMENT("a probe result comes back in plane coordinates: nothing to run here") - 37 N..... COMMENT("a tool length change moves the program coordinates along the plane axis that is world Z") - 38 N..... USE_TOOL_LENGTH_OFFSET(0.0000 0.0000 7.0000, 0.0000 0.0000 0.0000, 0.0000 0.0000 0.0000) - 39 N..... MESSAGE(" prog -37.000000 10.000000 20.000000") - 40 N..... USE_TOOL_LENGTH_OFFSET(0.0000 0.0000 0.0000, 0.0000 0.0000 0.0000, 0.0000 0.0000 0.0000) - 41 N..... COMMENT("G68.4 composes: a further 90 about the plane's X") - 42 N..... SET_G68_FRAME(10.0000, 20.0000, 30.0000, [0.0000, 0.0000, 1.0000, 0.0000, -1.0000, 0.0000, 1.0000, 0.0000, 0.0000], 1) - 43 N..... STRAIGHT_TRAVERSE(1.0000, 2.0000, 3.0000, 0.0000, 0.0000, 0.0000) - 44 N..... COMMENT("G69 cancels") - 45 N..... SET_G68_FRAME(0.0000, 0.0000, 0.0000, [1.0000, 0.0000, 0.0000, 0.0000, 1.0000, 0.0000, 0.0000, 0.0000, 1.0000], 0) - 46 N..... STRAIGHT_TRAVERSE(1.0000, 2.0000, 3.0000, 0.0000, 0.0000, 0.0000) - 47 N..... SET_G5X_OFFSET(1, 0.0000, 0.0000, 0.0000, 0.0000, 0.0000, 0.0000) - 48 N..... SET_XY_ROTATION(0.0000) - 49 N..... SET_FEED_MODE(0, 0) - 50 N..... SET_FEED_RATE(0.0000) - 51 N..... STOP_SPINDLE_TURNING(0) - 52 N..... SET_SPINDLE_MODE(0 0.0000) - 53 N..... PROGRAM_END() - 54 N..... ON_RESET() - 55 N..... ON_RESET() + 13 N..... COMMENT("one plane, four ways. Three distinct angles, none of them right, so that") + 14 N..... COMMENT("the order the rotations compose in shows in the answer") + 15 N..... COMMENT("P0 turns about the frame as it goes, ZXZ by default") + 16 N..... SET_G68_FRAME(10.0000, 20.0000, 30.0000, [0.9592, -0.0781, 0.2717, 0.2285, 0.7800, -0.5826, -0.1664, 0.6209, 0.7660], 1) + 17 N..... STRAIGHT_TRAVERSE(1.0000, 2.0000, 3.0000, 0.0000, 0.0000, 0.0000) + 18 N..... COMMENT("the same plane about the fixed axes of the system it sits in, XYZ") + 19 N..... SET_G68_FRAME(10.0000, 20.0000, 30.0000, [0.9592, -0.0781, 0.2717, 0.2285, 0.7800, -0.5826, -0.1664, 0.6209, 0.7660], 1) + 20 N..... STRAIGHT_TRAVERSE(1.0000, 2.0000, 3.0000, 0.0000, 0.0000, 0.0000) + 21 N..... COMMENT("the same plane by three points: the first two give +X, the third the +Y side") + 22 N..... SET_G68_FRAME(10.0000, 20.0000, 30.0000, [0.9592, -0.0781, 0.2717, 0.2285, 0.7800, -0.5826, -0.1664, 0.6209, 0.7660], 1) + 23 N..... STRAIGHT_TRAVERSE(1.0000, 2.0000, 3.0000, 0.0000, 0.0000, 0.0000) + 24 N..... COMMENT("the same plane by two vectors, X nudged off square") + 25 N..... SET_G68_FRAME(10.0000, 20.0000, 30.0000, [0.9592, -0.0781, 0.2717, 0.2285, 0.7800, -0.5826, -0.1664, 0.6209, 0.7660], 1) + 26 N..... STRAIGHT_TRAVERSE(1.0000, 2.0000, 3.0000, 0.0000, 0.0000, 0.0000) + 27 N..... COMMENT("R turns the plane about its own Z") + 28 N..... SET_G68_FRAME(10.0000, 20.0000, 30.0000, [0.7134, -0.6459, 0.2717, 0.6561, 0.4797, -0.5826, 0.2460, 0.5939, 0.7660], 1) + 29 N..... STRAIGHT_TRAVERSE(1.0000, 2.0000, 3.0000, 0.0000, 0.0000, 0.0000) + 30 N..... COMMENT("an arc in the plane") + 31 N..... SET_FEED_RATE(100.0000) + 32 N..... STRAIGHT_FEED(0.0000, 0.0000, 0.0000, 0.0000, 0.0000, 0.0000) + 33 N..... ARC_FEED(2.0000, 0.0000, 1.0000, 0.0000, -1, 0.0000, 0.0000, 0.0000, 0.0000) + 34 N..... COMMENT("G53 inside the plane goes to absolute coordinates") + 35 N..... STRAIGHT_TRAVERSE(-27.6365, -20.9503, -14.0466, 0.0000, 0.0000, 0.0000) + 36 N..... COMMENT("and #5021 reports them") + 37 N..... MESSAGE(" abs 100.000000 200.000000 300.000000 prog -27.636497 -20.950346 -14.046603") + 38 N..... COMMENT("a probe result comes back in plane coordinates: nothing to run here") + 39 N..... COMMENT("a tool length change moves the program coordinates along the plane axis that is world Z") + 40 N..... USE_TOOL_LENGTH_OFFSET(0.0000 0.0000 7.0000, 0.0000 0.0000 0.0000, 0.0000 0.0000 0.0000) + 41 N..... MESSAGE(" prog -29.358386 -25.107354 -19.408914") + 42 N..... USE_TOOL_LENGTH_OFFSET(0.0000 0.0000 0.0000, 0.0000 0.0000 0.0000, 0.0000 0.0000 0.0000) + 43 N..... COMMENT("G68.4 composes onto the plane, in its own axes") + 44 N..... SET_G68_FRAME(10.0000, 20.0000, 30.0000, [0.6477, -0.5920, 0.4796, 0.4862, -0.1635, -0.8584, 0.5865, 0.7892, 0.1820], 1) + 45 N..... STRAIGHT_TRAVERSE(1.0000, 2.0000, 3.0000, 0.0000, 0.0000, 0.0000) + 46 N..... COMMENT("G69 cancels") + 47 N..... SET_G68_FRAME(0.0000, 0.0000, 0.0000, [1.0000, 0.0000, 0.0000, 0.0000, 1.0000, 0.0000, 0.0000, 0.0000, 1.0000], 0) + 48 N..... STRAIGHT_TRAVERSE(1.0000, 2.0000, 3.0000, 0.0000, 0.0000, 0.0000) + 49 N..... SET_G5X_OFFSET(1, 0.0000, 0.0000, 0.0000, 0.0000, 0.0000, 0.0000) + 50 N..... SET_XY_ROTATION(0.0000) + 51 N..... SET_FEED_MODE(0, 0) + 52 N..... SET_FEED_RATE(0.0000) + 53 N..... STOP_SPINDLE_TURNING(0) + 54 N..... SET_SPINDLE_MODE(0 0.0000) + 55 N..... PROGRAM_END() + 56 N..... ON_RESET() + 57 N..... ON_RESET() diff --git a/tests/interp/g68-frame/g68.ngc b/tests/interp/g68-frame/g68.ngc index 656cce9b906..544f5a16e7a 100644 --- a/tests/interp/g68-frame/g68.ngc +++ b/tests/interp/g68-frame/g68.ngc @@ -3,24 +3,26 @@ g21 g90 g10 l2 p2 x100 y200 z300 r0 g55 -(a plane rotated 90 about X: plane Y is world Z, plane Z is world -Y) -g68.2 x10 y20 z30 i0 j90 k0 +(one plane, four ways. Three distinct angles, none of them right, so that) +(the order the rotations compose in shows in the answer) +(P0 turns about the frame as it goes, ZXZ by default) +g68.2 x10 y20 z30 i25 j40 k-15 g0 x1 y2 z3 -(the same plane by fixed axis angles about X) -g68.2 p1 q123 x10 y20 z30 i90 j0 k0 +(the same plane about the fixed axes of the system it sits in, XYZ) +g68.2 p1 q123 x10 y20 z30 i39.025043525 j9.576578516 k13.400523974 g0 x1 y2 z3 -(the same plane by three points) +(the same plane by three points: the first two give +X, the third the +Y side) g68.2 p2 q0 x10 y20 z30 g68.2 p2 q1 x0 y0 z0 -g68.2 p2 q2 x5 y0 z0 -g68.2 p2 q3 x0 y0 z5 +g68.2 p2 q2 x4.796086535 y1.142635331 z-0.831828377 +g68.2 p2 q3 x-0.234429999 y2.339990858 z1.862655459 g0 x1 y2 z3 (the same plane by two vectors, X nudged off square) -g68.2 p3 q1 x10 y20 z30 i1 j0 k0.000001 -g68.2 p3 q2 i0 j-1 k0 +g68.2 p3 q1 x10 y20 z30 i0.959217579 j0.228526484 k-0.166364909 +g68.2 p3 q2 i0.271653782 j-0.582563416 k0.766044443 g0 x1 y2 z3 (R turns the plane about its own Z) -g68.2 p1 q123 x10 y20 z30 i90 j0 k0 r90 +g68.2 x10 y20 z30 i25 j40 k-15 r37.5 g0 x1 y2 z3 (an arc in the plane) g1 f100 x0 y0 z0 @@ -34,8 +36,8 @@ g53 g0 x100 y200 z300 g43.1 z7 (debug, prog #5420 #5421 #5422) g49 -(G68.4 composes: a further 90 about the plane's X) -g68.4 p1 q123 i90 j0 k0 +(G68.4 composes onto the plane, in its own axes) +g68.4 p1 q123 i35 j-20 k10 g0 x1 y2 z3 (G69 cancels) g69 From 4a937355c7d7bc0aad199f7023c4112a3747c408 Mon Sep 17 00:00:00 2001 From: Luca Toniolo <10792599+grandixximo@users.noreply.github.com> Date: Tue, 8 Sep 2026 18:07:36 +1000 Subject: [PATCH 50/60] docs: say where the tilted work plane conventions come from The chapter and the G-code reference described the two angle forms but never said whose conventions they are: P0 is Heidenhain's PLANE EULER, P1 its PLANE SPATIAL, the numbering is Fanuc's, and Fanuc's fifth form is not implemented. Two differences from the control they borrow from are stated: P1 and P2 refuse where a control offering SEQ falls back to the nearer pose, and P names a pose only where a rotary turns the tool, so a machine whose rotaries all carry the work has only the nearest form; the G53.1 section read as though a tilting table had a pose to name. --- docs/src/gcode/g-code.adoc | 29 ++++++++++++++------- docs/src/motion/kinematics-conventions.adoc | 28 ++++++++++++++++++++ 2 files changed, 47 insertions(+), 10 deletions(-) diff --git a/docs/src/gcode/g-code.adoc b/docs/src/gcode/g-code.adoc index ddaf3f22d98..a4d1a023e76 100644 --- a/docs/src/gcode/g-code.adoc +++ b/docs/src/gcode/g-code.adoc @@ -1773,19 +1773,23 @@ machines, two on a five-axis one, and often a choice of which joints to use. 'P' picks which of them. Without 'P', or with 'P0', the machine takes the one nearest where its rotaries are standing, which is the shortest move and depends on where that is. 'P1' and 'P2' name the pose instead, so a program -reaches the same one wherever it starts from: on a five axis machine the two -poses lean the head or the table opposite ways, and they differ in the sign -of the secondary rotary, the one whose axis the other carries. 'P1' is the -pose with that rotary positive and 'P2' the pose with it negative. The -interpreter works out which rotary that is by asking the kinematics module, -so nothing is configured for it. This is the choice Heidenhain writes as -`SEQ+` and `SEQ-`. +reaches the same one wherever it starts from: the two poses lean the head +opposite ways, and they differ in the sign of the secondary rotary, the one +whose axis the other carries. 'P1' is the pose with that rotary positive and +'P2' the pose with it negative. The interpreter works out which rotary that +is by asking the kinematics module, so nothing is configured for it. This is +the choice Heidenhain writes as `SEQ+` and `SEQ-`, with one difference: a +control offering `SEQ` falls back to the nearer pose when both lie the same +side of home, where 'P1' and 'P2' refuse and say so. The two poses become one where the tool direction asked for lies along the primary rotary's axis, straight up on a vertical mill, and there every form -gives the same answer. A machine that is not of this shape, a robot among -them, has no such sign to name, and there only the nearest form is -available. +gives the same answer. + +'P' names a pose only where a rotary turns the tool. On a machine whose +rotaries all carry the work, the tilting-table configurations among them, +there is no such rotary and only the nearest form is available; the same +goes for a machine that is not of this shape at all, a robot among them. 'Q' says whether the joints that carry the work, the table, take part. @@ -2182,6 +2186,11 @@ is. Words left out are zero. 'R' turns the plane about its own Z after everything else, in degrees. +The forms are numbered as Fanuc numbers them, and the two angle forms are the +conventions Heidenhain writes as `PLANE EULER` and `PLANE SPATIAL`. Fanuc has +a fifth form, projection angles, that is not implemented. See the +<> chapter. + The blocks of a 'P2' or 'P3' definition have to follow one another; any other block in between is an error. A definition with a plane already active replaces it, with the words in the coordinate system underneath, not in the diff --git a/docs/src/motion/kinematics-conventions.adoc b/docs/src/motion/kinematics-conventions.adoc index 9b22064570e..a0588c001e2 100644 --- a/docs/src/motion/kinematics-conventions.adoc +++ b/docs/src/motion/kinematics-conventions.adoc @@ -637,6 +637,34 @@ Mount orientation is not this:: determinant, rather than as three angles whose ordering convention is written down nowhere. +== Relation to Other Controls + +The tilted work plane borrows its shape from the controls that had one first, +so a program or a post moving either way reads the same. + +The two angle forms are the two those controls define. `G68.2 P0`, three +angles each about an axis of the plane as rotated so far, is what Heidenhain +calls `PLANE EULER`: a precession about Z, a nutation about the X the +precession has already turned, and a rotation about the tilted Z. `G68.2 P1`, +three angles about the axes of the system underneath, is `PLANE SPATIAL`, +whose three angles are each about a non-tilted axis. The numbering of the +forms follows Fanuc's `G68.2`, where P0 is Euler angles, P1 roll, pitch and +yaw, P2 three points and P3 two vectors. Fanuc has a fifth form, projection +angles, that is not implemented. + +Selecting one of the two poses that reach a plane, `P1` and `P2` on +<>, is Heidenhain's `SEQ+` and `SEQ-`, and takes the same +reference: the sign of the rotary measured from its home position. Two +differences are worth knowing. A control offering `SEQ` falls back to the +nearer pose when both lie the same side of home, where these refuse and say +so. And `SEQ` is offered on a machine whose rotaries all carry the work, +keyed to the tilting one, where `P` here names a pose only when a rotary +turns the tool, so on a table-table machine only the nearest form is +available. + +Whether the joints carrying the work take part, `Q0` and `Q1`, is +Heidenhain's `COORD ROT` and `TABLE ROT`. + == References * <>, for the axis nomenclature From 5c85e95f212b2404ecc29c8f19e0e25c75ecb45d Mon Sep 17 00:00:00 2001 From: Luca Toniolo <10792599+grandixximo@users.noreply.github.com> Date: Tue, 8 Sep 2026 20:23:32 +1000 Subject: [PATCH 51/60] interpreter, motion: hold the joints a point-to-point move asks for A point does not name one joint set: a robot wrist reaches it again with the forearm turned half a revolution, and both ends of a point-to-point move read the joints back from the point. The interpreter keeps its seed while it still explains where the machine is, instead of inverting the point every time and reporting joints the machine is not standing in, which had the next G53.7 refused on a joint limit. Motion holds the joints a joint segment ended on while the planner stays there, instead of inverting again the next cycle and undoing the move. tests/ptp-robot drives a wrist joint through zero and back and checks the joints not named stay put. --- src/emc/motion/control.c | 50 +++++++++++++++++++++++++--- src/emc/rs274ngc/interp_workplane.cc | 17 +++++++++- tests/ptp-robot/test-ui.py | 16 +++++++++ 3 files changed, 78 insertions(+), 5 deletions(-) diff --git a/src/emc/motion/control.c b/src/emc/motion/control.c index 35af182d3d3..a609cf1a367 100644 --- a/src/emc/motion/control.c +++ b/src/emc/motion/control.c @@ -929,6 +929,25 @@ static void check_for_faults(void) } } +/* The joints a joint interpolated segment ended on, held while the + planner stays at that point: a module's inverse answers with its own + joint set, which a robot wrist reaches with the forearm turned half a + revolution from the one asked for. The hold ends when the point moves. */ +static int joint_hold_valid = 0; +static double joint_hold[EMCMOT_MAX_JOINTS]; +static EmcPose joint_hold_pose; + +/* whether two machine points are the same, to a hair either way */ +static int same_carte_pos(const EmcPose *a, const EmcPose *b) +{ + const double tol = 1e-9; + + return fabs(a->tran.x - b->tran.x) < tol && fabs(a->tran.y - b->tran.y) < tol + && fabs(a->tran.z - b->tran.z) < tol + && fabs(a->a - b->a) < tol && fabs(a->b - b->b) < tol && fabs(a->c - b->c) < tol + && fabs(a->u - b->u) < tol && fabs(a->v - b->v) < tol && fabs(a->w - b->w) < tol; +} + static void set_operating_mode(void) { int joint_num; @@ -1449,7 +1468,7 @@ static void get_pos_cmds(long period) from the queue: its end joints seed the inverse, since the modules that read their rotary angles from the seed would otherwise get last cycle's */ - tpTakeJointEnd(&emcmotInternal->coord_tp, positions); + int joint_end_fresh = tpTakeJointEnd(&emcmotInternal->coord_tp, positions); /* get new commanded traj pos */ tpGetPos(&emcmotInternal->coord_tp, &emcmotStatus->carte_pos_cmd); @@ -1467,9 +1486,32 @@ static void get_pos_cmds(long period) ext_offset_coord_limit = 0; } - /* OUTPUT KINEMATICS - convert to joints in local array */ - result = kinematicsInverse(&emcmotStatus->carte_pos_cmd, positions, - &iflags, &fflags); + /* OUTPUT KINEMATICS - convert to joints in local array, or + hold the joints a joint interpolated segment ended on while + the planner stays at the point they put the machine on */ + if (joint_end_fresh) { + EmcPose at = emcmotStatus->carte_pos_cmd; + joint_hold_valid = 0; + if (kinematicsForward(positions, &at, &fflags, &iflags) == 0 + && same_carte_pos(&at, &emcmotStatus->carte_pos_cmd)) { + for (joint_num = 0; joint_num < EMCMOT_MAX_JOINTS; joint_num++) { + joint_hold[joint_num] = positions[joint_num]; + } + joint_hold_pose = emcmotStatus->carte_pos_cmd; + joint_hold_valid = 1; + } + } + if (joint_hold_valid + && same_carte_pos(&joint_hold_pose, &emcmotStatus->carte_pos_cmd)) { + for (joint_num = 0; joint_num < EMCMOT_MAX_JOINTS; joint_num++) { + positions[joint_num] = joint_hold[joint_num]; + } + result = 0; + } else { + joint_hold_valid = 0; + result = kinematicsInverse(&emcmotStatus->carte_pos_cmd, positions, + &iflags, &fflags); + } } if(result == 0) { diff --git a/src/emc/rs274ngc/interp_workplane.cc b/src/emc/rs274ngc/interp_workplane.cc index 12f7a1afc08..652fbbaa0f5 100644 --- a/src/emc/rs274ngc/interp_workplane.cc +++ b/src/emc/rs274ngc/interp_workplane.cc @@ -536,17 +536,32 @@ void Interp::machine_pose_to_program(setup_pointer s, const EmcPose *pose, doubl prog[8] = USER_TO_PROGRAM_LEN(pose->w) - s->tool_offset.w - s->w_origin_offset - s->w_axis_offset; } +// whether two machine points are the same, to a hair either way +static bool same_pose(const EmcPose *a, const EmcPose *b) +{ + const double tol = 1e-9; + + return fabs(a->tran.x - b->tran.x) < tol && fabs(a->tran.y - b->tran.y) < tol + && fabs(a->tran.z - b->tran.z) < tol + && fabs(a->a - b->a) < tol && fabs(a->b - b->b) < tol && fabs(a->c - b->c) < tol + && fabs(a->u - b->u) < tol && fabs(a->v - b->v) < tol && fabs(a->w - b->w) < tol; +} + // the joints the machine is at, as far as the interpreter can know ahead of // motion: the seed while it still explains the current point, since a point // does not name one joint set, else the joints the machine stands in int Interp::current_joints(setup_pointer s, void *vctx, double *joints) { KinematicsUserContext *ctx = (KinematicsUserContext *)vctx; - EmcPose pose; + EmcPose pose, seeded; int pass, i; current_machine_pose(s, &pose); for (i = 0; i < EMCMOT_MAX_JOINTS; i++) { joints[i] = s->kins_seed[i]; } + seeded = pose; + if (kinematicsUserForward(ctx, joints, &seeded) == 0 && same_pose(&pose, &seeded)) { + return INTERP_OK; + } for (pass = 0; pass < 8; pass++) { double prev[EMCMOT_MAX_JOINTS], worst = 0.0; for (i = 0; i < EMCMOT_MAX_JOINTS; i++) { prev[i] = joints[i]; } diff --git a/tests/ptp-robot/test-ui.py b/tests/ptp-robot/test-ui.py index 92362c901a7..c013a42f2ff 100755 --- a/tests/ptp-robot/test-ui.py +++ b/tests/ptp-robot/test-ui.py @@ -74,6 +74,22 @@ def refused(cmd, expect): if abs(after[j] - before[j]) > 1e-6: error("G53.7 moved joint %d from %.9f to %.9f" % (j, before[j], after[j])) +# a wrist reaches the same point with the forearm turned half a revolution +# and the wrist joints reversed, so a joint driven through zero must not +# come back as the other set: the joints not named stay where they are +mdi("G53.7 G0 J0=0 J1=0 J2=0 J3=0 J4=0 J5=0") +held = mdi("G53.7 G0 J4=90") +for value in (-10, 90, -45): + now = mdi("G53.7 G0 J4=%d" % value) + print("G53.7 G0 J4=%-4d %s" % (value, " ".join("%.4f" % v for v in now))) + drain() + if abs(now[4] - value) > 1e-6: + error("G53.7 J4=%d left joint 4 at %.6f" % (value, now[4])) + for j in (0, 1, 2, 3, 5): + if abs(now[j] - held[j]) > 1e-6: + error("G53.7 J4=%d moved joint %d from %.6f to %.6f" + % (value, j, held[j], now[j])) + # the letter form is refused whichever letter is used, because X names the # first rotary joint here; the message says so and points at G53.7 refused("G53.5 G0 X10", "joint 0") From 8a89cb764483f433fb62192b94c8c93bf7190139 Mon Sep 17 00:00:00 2001 From: Luca Toniolo <10792599+grandixximo@users.noreply.github.com> Date: Tue, 8 Sep 2026 23:13:58 +1000 Subject: [PATCH 52/60] interpreter: seed the kinematics from the joints the machine stands in The interpreter works out the joints behind a point by inverting it, and had nothing to start the first inverse from but zeros. A module that answers the inverse by iterating cannot be started anywhere: genserkins takes the Jacobian at the seed, and a serial arm with every joint at zero stands in a singular pose, so on a robot every G53.5 and G53.7 was refused with "the kinematics cannot invert the current position". Canon now reports the joint positions, and the interpreter falls back on them when its own seed no longer explains where the machine is. They also name the arm the machine is standing in, which the inverse of a point cannot: a robot reaches the same point again with the elbow the other way up. --- src/emc/nml_intf/canon.hh | 6 ++++++ src/emc/rs274ngc/gcodemodule.cc | 1 + src/emc/rs274ngc/interp_workplane.cc | 12 +++++++++++- src/emc/sai/saicanon.cc | 5 +++++ src/emc/task/emccanon.cc | 11 +++++++++++ 5 files changed, 34 insertions(+), 1 deletion(-) diff --git a/src/emc/nml_intf/canon.hh b/src/emc/nml_intf/canon.hh index 9211790508f..7f25a53b209 100644 --- a/src/emc/nml_intf/canon.hh +++ b/src/emc/nml_intf/canon.hh @@ -1011,6 +1011,12 @@ extern double GET_EXTERNAL_POSITION_V(); // returns the current w-axis position extern double GET_EXTERNAL_POSITION_W(); +// Copies up to max of the joint positions the machine stands in and +// returns how many were written. A point does not name one joint set, so +// an iterative inverse needs somewhere to start. Zero when the caller +// has no machine to ask. +extern int GET_EXTERNAL_JOINT_POSITIONS(double *joints, int max); + // Returns the position of the specified axis at the last probe trip, // in the current work coordinate system. diff --git a/src/emc/rs274ngc/gcodemodule.cc b/src/emc/rs274ngc/gcodemodule.cc index 87f8542811e..8c518674a65 100644 --- a/src/emc/rs274ngc/gcodemodule.cc +++ b/src/emc/rs274ngc/gcodemodule.cc @@ -1042,6 +1042,7 @@ double GET_EXTERNAL_POSITION_C() { return _pos_c; } double GET_EXTERNAL_POSITION_U() { return _pos_u; } double GET_EXTERNAL_POSITION_V() { return _pos_v; } double GET_EXTERNAL_POSITION_W() { return _pos_w; } +int GET_EXTERNAL_JOINT_POSITIONS(double * /*joints*/, int /*max*/) { return 0; } void INIT_CANON() {} void SET_PARAMETER_FILE_NAME(const char *name) diff --git a/src/emc/rs274ngc/interp_workplane.cc b/src/emc/rs274ngc/interp_workplane.cc index 652fbbaa0f5..29253aaafa2 100644 --- a/src/emc/rs274ngc/interp_workplane.cc +++ b/src/emc/rs274ngc/interp_workplane.cc @@ -553,8 +553,9 @@ static bool same_pose(const EmcPose *a, const EmcPose *b) int Interp::current_joints(setup_pointer s, void *vctx, double *joints) { KinematicsUserContext *ctx = (KinematicsUserContext *)vctx; + double standing[EMCMOT_MAX_JOINTS]; EmcPose pose, seeded; - int pass, i; + int pass, i, n; current_machine_pose(s, &pose); for (i = 0; i < EMCMOT_MAX_JOINTS; i++) { joints[i] = s->kins_seed[i]; } @@ -562,6 +563,15 @@ int Interp::current_joints(setup_pointer s, void *vctx, double *joints) if (kinematicsUserForward(ctx, joints, &seeded) == 0 && same_pose(&pose, &seeded)) { return INTERP_OK; } + n = GET_EXTERNAL_JOINT_POSITIONS(standing, EMCMOT_MAX_JOINTS); + for (i = 0; i < n; i++) { joints[i] = standing[i]; } + if (n > 0) { + seeded = pose; + if (kinematicsUserForward(ctx, joints, &seeded) == 0 && same_pose(&pose, &seeded)) { + for (i = 0; i < EMCMOT_MAX_JOINTS; i++) { s->kins_seed[i] = joints[i]; } + return INTERP_OK; + } + } for (pass = 0; pass < 8; pass++) { double prev[EMCMOT_MAX_JOINTS], worst = 0.0; for (i = 0; i < EMCMOT_MAX_JOINTS; i++) { prev[i] = joints[i]; } diff --git a/src/emc/sai/saicanon.cc b/src/emc/sai/saicanon.cc index e552926e298..784d9377658 100644 --- a/src/emc/sai/saicanon.cc +++ b/src/emc/sai/saicanon.cc @@ -923,6 +923,11 @@ double GET_EXTERNAL_POSITION_W() return 0.; } +int GET_EXTERNAL_JOINT_POSITIONS(double * /*joints*/, int /*max*/) +{ + return 0; +} + double GET_EXTERNAL_PROBE_POSITION_U() { return 0.; diff --git a/src/emc/task/emccanon.cc b/src/emc/task/emccanon.cc index a954bbb4bed..fa70ec82036 100644 --- a/src/emc/task/emccanon.cc +++ b/src/emc/task/emccanon.cc @@ -4127,6 +4127,17 @@ double GET_EXTERNAL_POSITION_W(void) return position.w; } +int GET_EXTERNAL_JOINT_POSITIONS(double *joints, int max) +{ + int n = emcStatus->motion.traj.joints; + + if (n > max) { n = max; } + for (int i = 0; i < n; i++) { + joints[i] = emcStatus->motion.joint[i].output; + } + return n; +} + double GET_EXTERNAL_PROBE_POSITION_X(void) { CANON_POSITION position; From d2b0d7901f1dc8b23e30c7475df9998fa4e4617a Mon Sep 17 00:00:00 2001 From: Luca Toniolo <10792599+grandixximo@users.noreply.github.com> Date: Wed, 9 Sep 2026 11:55:19 +1000 Subject: [PATCH 53/60] preview: let the canon report where the joints stand The preview has a machine to ask, through the status buffer it already reads for the tool table and the offsets, so it can answer for the joints as well. Without them every inverse started from zeros, which previewed a point-to-point move from a pose the machine is not standing in, and on a serial arm could not be inverted at all. The one method on the mixin covers AXIS, gremlin and the Qt screens, which all take their canon from it. --- lib/python/rs274/interpret.py | 3 +++ src/emc/rs274ngc/gcodemodule.cc | 26 +++++++++++++++++++++++++- 2 files changed, 28 insertions(+), 1 deletion(-) diff --git a/lib/python/rs274/interpret.py b/lib/python/rs274/interpret.py index 3c83b5502a8..0662adc90bc 100644 --- a/lib/python/rs274/interpret.py +++ b/lib/python/rs274/interpret.py @@ -193,5 +193,8 @@ def get_axis_mask(self): def get_block_delete(self): return self.s.block_delete + def get_external_joint_positions(self): + return tuple(self.s.joint_actual_position[:self.s.joints]) + # vim:ts=8:sts=4:et: diff --git a/src/emc/rs274ngc/gcodemodule.cc b/src/emc/rs274ngc/gcodemodule.cc index 8c518674a65..67807a39c51 100644 --- a/src/emc/rs274ngc/gcodemodule.cc +++ b/src/emc/rs274ngc/gcodemodule.cc @@ -1042,7 +1042,31 @@ double GET_EXTERNAL_POSITION_C() { return _pos_c; } double GET_EXTERNAL_POSITION_U() { return _pos_u; } double GET_EXTERNAL_POSITION_V() { return _pos_v; } double GET_EXTERNAL_POSITION_W() { return _pos_w; } -int GET_EXTERNAL_JOINT_POSITIONS(double * /*joints*/, int /*max*/) { return 0; } + +// Where the machine's joints stand. A point does not name one joint set, +// so an iterative inverse has to start somewhere: a canon that watches the +// status buffer says where. One that cannot answers nothing, and the +// interpreter works from the point alone. +int GET_EXTERNAL_JOINT_POSITIONS(double *joints, int max) { + PyObject *result, *seq; + Py_ssize_t n, i; + + if(interp_error) return 0; + if(!PyObject_HasAttrString(callback, "get_external_joint_positions")) return 0; + result = callmethod(callback, "get_external_joint_positions", ""); + if(result == NULL) { PyErr_Clear(); return 0; } + seq = PySequence_Fast(result, "joint positions"); + if(seq == NULL) { PyErr_Clear(); Py_DECREF(result); return 0; } + n = PySequence_Fast_GET_SIZE(seq); + if(n > max) n = max; + for(i = 0; i < n; i++) { + joints[i] = PyFloat_AsDouble(PySequence_Fast_GET_ITEM(seq, i)); + if(PyErr_Occurred()) { PyErr_Clear(); n = 0; break; } + } + Py_DECREF(seq); + Py_DECREF(result); + return (int)n; +} void INIT_CANON() {} void SET_PARAMETER_FILE_NAME(const char *name) From 6c216a941662861e9b1d2ffdb0656d73bde0e3bf Mon Sep 17 00:00:00 2001 From: Luca Toniolo <10792599+grandixximo@users.noreply.github.com> Date: Wed, 9 Sep 2026 15:56:21 +1000 Subject: [PATCH 54/60] tests: the point-to-point moves and their preview on an iterative module pumakins answers the inverse in closed form and never looks at the joints it is handed, so tests/ptp-robot says nothing about the seed. genserkins takes the Jacobian at those joints: the machine here homes well away from all zeros, so the seed has to come from where the machine stands, and all zeros is the singular pose it cannot come from at all. The same program goes through the interpreter and through the preview, and the two have to agree on where it ends up. Then the arm is parked in the singular pose, where the module reports through the HAL library it prints with: the preview refuses, rather than the process going down. --- tests/ptp-iterative/README | 9 ++ tests/ptp-iterative/checkresult | 3 + tests/ptp-iterative/sim.hal | 16 +++ tests/ptp-iterative/test-ui.py | 175 ++++++++++++++++++++++++++++++++ tests/ptp-iterative/test.ini | 140 +++++++++++++++++++++++++ tests/ptp-iterative/test.ngc | 3 + tests/ptp-iterative/test.sh | 4 + tests/ptp-iterative/tool.tbl | 1 + tests/ptp-robot/skip | 4 - tests/twp-native/skip | 4 - 10 files changed, 351 insertions(+), 8 deletions(-) create mode 100644 tests/ptp-iterative/README create mode 100755 tests/ptp-iterative/checkresult create mode 100644 tests/ptp-iterative/sim.hal create mode 100755 tests/ptp-iterative/test-ui.py create mode 100644 tests/ptp-iterative/test.ini create mode 100644 tests/ptp-iterative/test.ngc create mode 100755 tests/ptp-iterative/test.sh create mode 100644 tests/ptp-iterative/tool.tbl delete mode 100755 tests/ptp-robot/skip delete mode 100755 tests/twp-native/skip diff --git a/tests/ptp-iterative/README b/tests/ptp-iterative/README new file mode 100644 index 00000000000..456921354f4 --- /dev/null +++ b/tests/ptp-iterative/README @@ -0,0 +1,9 @@ +The point-to-point moves on a kinematics module that answers the inverse +by iterating, and the preview of them. + +genserkins takes the Jacobian at the joints it is handed, so the seed +decides whether there is an answer at all: this machine homes to a pose +that is nowhere near all zeros, and all zeros is the singular pose the +inverse cannot start from. The interpreter reads the joints the machine +stands in, and the preview reads them from the status buffer the way a GUI +does, so the same program previews and runs to the same place. diff --git a/tests/ptp-iterative/checkresult b/tests/ptp-iterative/checkresult new file mode 100755 index 00000000000..9d48d3f180e --- /dev/null +++ b/tests/ptp-iterative/checkresult @@ -0,0 +1,3 @@ +#!/bin/sh +# the test script counts its own failures +grep -q "^Exiting with 0 errors" "$1" diff --git a/tests/ptp-iterative/sim.hal b/tests/ptp-iterative/sim.hal new file mode 100644 index 00000000000..e92c60eb526 --- /dev/null +++ b/tests/ptp-iterative/sim.hal @@ -0,0 +1,16 @@ +loadrt [KINS]KINEMATICS +loadrt [EMCMOT]EMCMOT servo_period_nsec=[EMCMOT]SERVO_PERIOD num_joints=[KINS]JOINTS + +addf motion-command-handler servo-thread +addf motion-controller servo-thread + +net J0 joint.0.motor-pos-cmd => joint.0.motor-pos-fb +net J1 joint.1.motor-pos-cmd => joint.1.motor-pos-fb +net J2 joint.2.motor-pos-cmd => joint.2.motor-pos-fb +net J3 joint.3.motor-pos-cmd => joint.3.motor-pos-fb +net J4 joint.4.motor-pos-cmd => joint.4.motor-pos-fb +net J5 joint.5.motor-pos-cmd => joint.5.motor-pos-fb + +net estop-loop iocontrol.0.user-enable-out iocontrol.0.emc-enable-in +net tool-prep-loop iocontrol.0.tool-prepare iocontrol.0.tool-prepared +net tool-change-loop iocontrol.0.tool-change iocontrol.0.tool-changed diff --git a/tests/ptp-iterative/test-ui.py b/tests/ptp-iterative/test-ui.py new file mode 100755 index 00000000000..7fb988a976e --- /dev/null +++ b/tests/ptp-iterative/test-ui.py @@ -0,0 +1,175 @@ +#!/usr/bin/env python3 +# A module that answers the inverse by iterating has to be started +# somewhere, and the pose this machine homes to is not the one it would be +# started from by default. The moves are checked twice: through the +# interpreter, which reads the joints from motion, and through the preview, +# which reads them from the status buffer the way a GUI does. +import gcode +import linuxcnc +import preview_helpers +import os +import sys +import time +from rs274.interpret import StatMixin + +JOINTS = 6 +PROGRAM = "test.ngc" + +c = linuxcnc.command() +s = linuxcnc.stat() +e = linuxcnc.error_channel() + +errors = 0 + + +def error(what): + global errors + errors += 1 + print("*** ERROR %s" % what) + + +def drain(): + while e.poll(): + pass + + +def settled(): + deadline = time.time() + 60 + last = None + while time.time() < deadline: + s.poll() + now = [s.joint_position[i] for i in range(JOINTS)] + if s.inpos and not s.queue and now == last: + return now + last = now + time.sleep(0.05) + error("timed out waiting for the move") + return last + + +def mdi(cmd): + c.mdi(cmd) + c.wait_complete(60) + return settled() + + +class PreviewCanon(StatMixin): + # Stay on the per-event canon protocol: the catch-all below would + # otherwise answer gcode.parse's probe for the move-batch one. + use_move_batches = False + + def __init__(self, stat, parameter): + StatMixin.__init__(self, stat, False) + self.parameter_file = parameter + self.points = [] + + def __getattr__(self, name): + if name.startswith("_"): + raise AttributeError(name) + return lambda *args, **kwargs: None + + def straight_traverse(self, *pos): + self.points.append(pos) + + def straight_feed(self, *pos): + self.points.append(pos) + + +# the canon protocol carries lengths in the interpreter's own units, which +# a GUI turns into the machine's; the angles are already there +def in_machine_units(pos): + s.poll() + scale = (s.linear_units or 1) * 25.4 + return [v * scale for v in pos[:3]] + list(pos[3:6]) + + +def preview(program=PROGRAM): + ini = linuxcnc.ini(os.environ["INI_FILE_NAME"]) + s.poll() + canon = PreviewCanon(s, ini.getstring("RS274NGC", "PARAMETER_FILE")) + codes = preview_helpers.create_unitcode_and_initcode(s, ini) + result, line = gcode.parse(program, canon, *codes) + if result > gcode.MIN_ERROR: + return None, "line %d: %s" % (line, gcode.strerror(result)) + return canon.points, None + + +c.state(linuxcnc.STATE_ESTOP_RESET) +c.state(linuxcnc.STATE_ON) +c.wait_complete(30) +c.home(-1) +c.wait_complete(60) +c.mode(linuxcnc.MODE_MDI) +c.wait_complete(30) +drain() + +home = settled() +print("homed at %s" % " ".join("%.4f" % v for v in home)) +if abs(home[1] + 90) > 1e-6 or abs(home[4] - 90) > 1e-6: + error("the machine did not home to the pose the test is written for") + +# the preview runs first, from the pose the machine stands in, and its last +# point is where the program ends up +points, refused = preview() +if refused: + error("the preview refused %s, %s" % (PROGRAM, refused)) +elif not points: + error("the preview of %s reported no move at all" % PROGRAM) +previewed = points[-1] if points else None + +# the interpreter takes the same program, one line at a time +for line in open(PROGRAM): + line = line.strip() + if not line or line.startswith("m2"): + continue + reached = mdi(line) + print("%-20s %s" % (line, " ".join("%.4f" % v for v in reached))) + drain() + +if abs(reached[0] - 10) > 1e-6 or abs(reached[4] - 80) > 1e-6: + error("the program left joints 0 and 4 at %.6f and %.6f" + % (reached[0], reached[4])) +for j in (1, 2, 3, 5): + if abs(reached[j] - home[j]) > 1e-6: + error("the program moved joint %d from %.6f to %.6f" + % (j, home[j], reached[j])) + +# and both agree on where that is +s.poll() +if previewed: + for name, i, got in zip("XYZABC", range(6), in_machine_units(previewed)): + if abs(got - s.position[i]) > 1e-3: + error("the preview put %s at %.6f, the machine at %.6f" + % (name, got, s.position[i])) + print("preview and machine agree on %s" + % " ".join("%.4f" % v for v in in_machine_units(previewed))) + +# a preview taken now starts where the machine stands, so a program that +# names the joints it is already in asks for no move at all +after, refused = preview() +if refused: + error("the second preview refused %s, %s" % (PROGRAM, refused)) +if after and max(abs(a - b) for a, b in zip(in_machine_units(after[0]), s.position[:6])) > 1e-3: + error("the second preview started at %s, not at %s" + % (["%.4f" % v for v in in_machine_units(after[0])], + ["%.4f" % v for v in s.position[:6]])) + +# a point out of the arm's reach: the module says so through the HAL +# library it prints with, which has to be within reach of the process the +# preview runs in, or the answer is the process going down +# all joints at zero is the pose this arm cannot be inverted from, and the +# module says so through the HAL library it prints with. That library has +# to be within reach of the process the preview runs in: a GUI has it only +# underneath the interpreter it loaded, and out of reach the answer is the +# process going down rather than a refusal. +mdi("g53.7 g0 j0=0 j1=0 j2=0 j3=0 j4=0 j5=0") +out, refused = preview() +if not refused: + error("the preview answered from the pose the arm cannot be inverted from") +elif "invert" not in refused: + error("the preview said %r, which does not mention the inverse" % refused) +else: + print("the preview refused from the singular pose: %s" % refused) + +print("Exiting with %d errors" % errors) +sys.exit(1 if errors else 0) diff --git a/tests/ptp-iterative/test.ini b/tests/ptp-iterative/test.ini new file mode 100644 index 00000000000..bc942288e36 --- /dev/null +++ b/tests/ptp-iterative/test.ini @@ -0,0 +1,140 @@ +[EMC] +VERSION = 1.1 +DEBUG = 0 + +[DISPLAY] +DISPLAY = ./test-ui.py + +[RS274NGC] +RS274NGC_STARTUP_CODE = G17 G21 G40 G49 G54 G64 P0.001 G80 G90 G92.1 G94 G97 G98 +PARAMETER_FILE = sim.var + +[KINS] +KINEMATICS = genserkins +JOINTS = 6 + +[HAL] +HALFILE = sim.hal +# the modified DH parameters of the RV-6SDL, as the melfa-sim config has them +HALCMD = setp genserkins.A-1 85 +HALCMD = setp genserkins.A-2 380 +HALCMD = setp genserkins.A-3 100 +HALCMD = setp genserkins.ALPHA-1 -1.570796326 +HALCMD = setp genserkins.ALPHA-3 -1.570796326 +HALCMD = setp genserkins.ALPHA-4 1.570796326 +HALCMD = setp genserkins.ALPHA-5 -1.570796326 +HALCMD = setp genserkins.D-0 350 +HALCMD = setp genserkins.D-3 425 +HALCMD = setp genserkins.D-5 235 + +[TRAJ] +COORDINATES = XYZABC +LINEAR_UNITS = mm +ANGULAR_UNITS = deg +DEFAULT_LINEAR_VELOCITY = 100 +MAX_LINEAR_VELOCITY = 120 +MAX_LINEAR_ACCELERATION = 700 +DEFAULT_LINEAR_ACCELERATION = 300 +NO_FORCE_HOMING = 1 + +[EMCMOT] +EMCMOT = motmod +SERVO_PERIOD = 1000000 +COMM_TIMEOUT = 4 + +[TASK] +TASK = milltask +CYCLE_TIME = 0.010 + +[EMCIO] +TOOL_TABLE = tool.tbl + +[AXIS_X] +MIN_LIMIT = -5000 +MAX_LIMIT = 5000 +MAX_VELOCITY = 200 +MAX_ACCELERATION = 700 + +[AXIS_Y] +MIN_LIMIT = -5000 +MAX_LIMIT = 5000 +MAX_VELOCITY = 200 +MAX_ACCELERATION = 700 + +[AXIS_Z] +MIN_LIMIT = -5000 +MAX_LIMIT = 5000 +MAX_VELOCITY = 200 +MAX_ACCELERATION = 700 + +[AXIS_A] +MIN_LIMIT = -360 +MAX_LIMIT = 360 +MAX_VELOCITY = 90 +MAX_ACCELERATION = 900 + +[AXIS_B] +MIN_LIMIT = -185 +MAX_LIMIT = 185 +MAX_VELOCITY = 90 +MAX_ACCELERATION = 900 + +[AXIS_C] +MIN_LIMIT = -320 +MAX_LIMIT = 320 +MAX_VELOCITY = 90 +MAX_ACCELERATION = 900 + +[JOINT_0] +TYPE = ANGULAR +HOME = 0 +MAX_VELOCITY = 90 +MAX_ACCELERATION = 900 +MIN_LIMIT = -360 +MAX_LIMIT = 360 +HOME_SEQUENCE = 0 + +[JOINT_1] +TYPE = ANGULAR +HOME = -90 +MAX_VELOCITY = 90 +MAX_ACCELERATION = 900 +MIN_LIMIT = -360 +MAX_LIMIT = 360 +HOME_SEQUENCE = 0 + +[JOINT_2] +TYPE = ANGULAR +HOME = 0 +MAX_VELOCITY = 90 +MAX_ACCELERATION = 900 +MIN_LIMIT = -360 +MAX_LIMIT = 360 +HOME_SEQUENCE = 0 + +[JOINT_3] +TYPE = ANGULAR +HOME = 0 +MAX_VELOCITY = 90 +MAX_ACCELERATION = 900 +MIN_LIMIT = -360 +MAX_LIMIT = 360 +HOME_SEQUENCE = 0 + +[JOINT_4] +TYPE = ANGULAR +HOME = 90 +MAX_VELOCITY = 90 +MAX_ACCELERATION = 900 +MIN_LIMIT = -360 +MAX_LIMIT = 360 +HOME_SEQUENCE = 0 + +[JOINT_5] +TYPE = ANGULAR +HOME = 0 +MAX_VELOCITY = 90 +MAX_ACCELERATION = 900 +MIN_LIMIT = -360 +MAX_LIMIT = 360 +HOME_SEQUENCE = 0 diff --git a/tests/ptp-iterative/test.ngc b/tests/ptp-iterative/test.ngc new file mode 100644 index 00000000000..0afcad5d314 --- /dev/null +++ b/tests/ptp-iterative/test.ngc @@ -0,0 +1,3 @@ +g53.7 g0 j4=80 +g53.7 g0 j0=10 +m2 diff --git a/tests/ptp-iterative/test.sh b/tests/ptp-iterative/test.sh new file mode 100755 index 00000000000..765cf14fed6 --- /dev/null +++ b/tests/ptp-iterative/test.sh @@ -0,0 +1,4 @@ +#!/bin/bash -e +# a failed run leaves the var file behind, and it carries offsets +rm -f sim.var sim.var.bak +linuxcnc -r test.ini diff --git a/tests/ptp-iterative/tool.tbl b/tests/ptp-iterative/tool.tbl new file mode 100644 index 00000000000..2028da29213 --- /dev/null +++ b/tests/ptp-iterative/tool.tbl @@ -0,0 +1 @@ +T1 P1 Z25 D6 diff --git a/tests/ptp-robot/skip b/tests/ptp-robot/skip deleted file mode 100755 index a12f31a77c2..00000000000 --- a/tests/ptp-robot/skip +++ /dev/null @@ -1,4 +0,0 @@ -#!/bin/sh -# Builds a realtime component with halcompile, which needs the build -# tools present. Skip when testing installed packages. -[ -z "$SYSTEM_BUILD" ] diff --git a/tests/twp-native/skip b/tests/twp-native/skip deleted file mode 100755 index a12f31a77c2..00000000000 --- a/tests/twp-native/skip +++ /dev/null @@ -1,4 +0,0 @@ -#!/bin/sh -# Builds a realtime component with halcompile, which needs the build -# tools present. Skip when testing installed packages. -[ -z "$SYSTEM_BUILD" ] From f741f60172a2f91fa11347f88fe9b3e478a11d92 Mon Sep 17 00:00:00 2001 From: Luca Toniolo <10792599+grandixximo@users.noreply.github.com> Date: Wed, 9 Sep 2026 21:08:54 +1000 Subject: [PATCH 55/60] genserkins: hold the inverse to a step the Jacobian is good for The inverse took the whole Newton step the Jacobian asked for, and the Jacobian holds only near the estimate: an endpoint far from the seed asked for many radians, the arm landed somewhere unrelated and converged on a pose reached through whole turns. Seeded at 90 -90 0 0 90 -17.7 and asked for X450 Y-200 Z150 it answered joint 0 at -3263.962, the right pose nine turns out, which the joint limits refuse on a move the arm could make. Cap the step at GENSER_MAX_ANGLE_STEP and scale the whole vector so its direction survives, measured on the rotary links; a servo cycle asks for far less and is untouched. --- src/emc/kinematics/genserfuncs.c | 21 +++++++++++++++++++++ src/emc/kinematics/genserkins.h | 2 ++ 2 files changed, 23 insertions(+) diff --git a/src/emc/kinematics/genserfuncs.c b/src/emc/kinematics/genserfuncs.c index a25ebcc3c71..17889e63160 100644 --- a/src/emc/kinematics/genserfuncs.c +++ b/src/emc/kinematics/genserfuncs.c @@ -632,6 +632,27 @@ static int genser_inverse(const kins_params *p, kins_scratch *s, /* push the Cartesian velocity vector through the inverse Jacobian */ go_matrix_vector_mult(&Jinv, dvw, dj); + /* The Jacobian holds only near the estimate, and a far pose asks + for a step of many radians: the arm lands somewhere unrelated + and converges by way of whole turns its limits refuse. Cap the + step, keeping its direction, and let the iteration walk there. */ + { + double worst = 0.0; + + for (link = 0; link < genser->link_num; link++) { + if (GO_QUANTITY_ANGLE == linkout[link].quantity + && fabs(dj[link]) > worst) { + worst = fabs(dj[link]); + } + } + if (worst > GENSER_MAX_ANGLE_STEP) { + double scale = GENSER_MAX_ANGLE_STEP / worst; + for (link = 0; link < genser->link_num; link++) { + dj[link] *= scale; + } + } + } + //pass through 678 as uvw if (p->max_joints > 6) joints[6] = world->u; if (p->max_joints > 7) joints[7] = world->v; diff --git a/src/emc/kinematics/genserkins.h b/src/emc/kinematics/genserkins.h index c5a2d9526f8..cb92fe605b2 100644 --- a/src/emc/kinematics/genserkins.h +++ b/src/emc/kinematics/genserkins.h @@ -45,6 +45,8 @@ #define GENSER_MAX_JOINTS 6 #define GENSER_DEFAULT_MAX_ITERATIONS 100 +/* the most a rotary joint moves in one pass of the inverse, in radians */ +#define GENSER_MAX_ANGLE_STEP 0.2 #define PI_2 GO_PI_2 From b194a50869a1bfd99f9f366565b29190baf6a08b Mon Sep 17 00:00:00 2001 From: Luca Toniolo <10792599+grandixximo@users.noreply.github.com> Date: Wed, 9 Sep 2026 21:09:03 +1000 Subject: [PATCH 56/60] motion: check a move against the joints the queue leaves behind The joint limit check inverted a move's endpoint from the joints the machine stands in. The reading runs far ahead, so every endpoint of a short program was checked from the starting pose, and an iterating inverse answers nearest whatever it is handed: park a robot at J4=-90, run a program that takes it to J4=90 and asks for a point, and the point is refused because it was inverted from the parked pose with the other wrist. Seed from the end of the queue instead: a joint segment knows its own end and the rest carry the answer the last endpoint came out with; with nothing queued the machine is the reference. The joint interpolated path already worked this way and shares the helper. --- src/emc/motion/command.c | 48 +++++++++++++++++++++++++++++++--------- 1 file changed, 38 insertions(+), 10 deletions(-) diff --git a/src/emc/motion/command.c b/src/emc/motion/command.c index 7270a6abdfd..4b2210520eb 100644 --- a/src/emc/motion/command.c +++ b/src/emc/motion/command.c @@ -140,6 +140,30 @@ static int inverse_settled(EmcPose *pos, double *joints, return 0; } +/* Where the queue leaves the joints, which is the seed an iterative + inverse wants: the reading runs ahead of the machine, so the joints to + hand it are not the ones the machine is standing in. A joint + interpolated segment knows its own end; the rest take the answer the + last endpoint checked came out with. Returns 0, and the joints the + machine stands in, when there is nothing queued to ask. */ +static double planned_joints[EMCMOT_MAX_JOINTS]; +static int planned_joints_ok = 0; + +static int queue_end_joints(double *joints_out) +{ + int j; + + if (tpGetQueueEndJoints(&emcmotInternal->coord_tp, joints_out)) { return 1; } + if (planned_joints_ok && tpQueueDepth(&emcmotInternal->coord_tp) > 0) { + for (j = 0; j < EMCMOT_MAX_JOINTS; j++) { joints_out[j] = planned_joints[j]; } + return 1; + } + for (j = 0; j < EMCMOT_MAX_JOINTS; j++) { + joints_out[j] = (j < ALL_JOINTS) ? joints[j].pos_cmd : 0.0; + } + return 0; +} + /* limits_ok() returns 1 if none of the hard limits are set, 0 if any are set. Called on a linear and circular move. */ STATIC int limits_ok(void) @@ -288,17 +312,15 @@ STATIC int inRange(EmcPose pos, int id, char *move_type) /* Now, check that the endpoint puts the joints within their limits too */ - /* fill in all joints with 0 */ - for (joint_num = 0; joint_num < ALL_JOINTS; joint_num++) { - joint = &joints[joint_num]; - joint_pos[joint_num] = joint->pos_cmd; - } + /* start the inverse from where the queue leaves the joints */ + queue_end_joints(joint_pos); /* now fill in with real values, for joints that are used */ - if (kinematicsInverse(&pos, joint_pos, &iflags, &fflags) != 0) + if (inverse_settled(&pos, joint_pos, &iflags, &fflags) != 0) { reportError(_("%s move on line %d fails kinematicsInverse"), move_type, id); + planned_joints_ok = 0; return 0; } @@ -329,6 +351,15 @@ STATIC int inRange(EmcPose pos, int id, char *move_type) move_type, id, joint_num, joint->min_pos_limit); } } + + /* an endpoint on its way to the queue is where the next one starts + from; a refused one leaves the queue as it was */ + if (in_range) { + for (joint_num = 0; joint_num < EMCMOT_MAX_JOINTS; joint_num++) { + planned_joints[joint_num] = joint_pos[joint_num]; + } + planned_joints_ok = 1; + } return in_range; } @@ -1190,12 +1221,9 @@ void emcmotCommandHandler_locked(void *arg, long servo_period) } /* where the queue ends in joint space */ - if (!tpGetQueueEndJoints(&emcmotInternal->coord_tp, start)) { + if (!queue_end_joints(start)) { EmcPose goal; tpGetGoalPos(&emcmotInternal->coord_tp, &goal); - for (joint_num = 0; joint_num < EMCMOT_MAX_JOINTS; joint_num++) { - start[joint_num] = (joint_num < ALL_JOINTS) ? joints[joint_num].pos_cmd : 0.0; - } if (inverse_settled(&goal, start, &iflags, &fflags) != 0) { reportError(_("joint interpolated move on line %d: the queue end fails kinematicsInverse"), emcmotCommand->id); From d878037b258071f1c29aa79010ca62380bc2c3cc Mon Sep 17 00:00:00 2001 From: Luca Toniolo <10792599+grandixximo@users.noreply.github.com> Date: Thu, 10 Sep 2026 19:03:36 +0800 Subject: [PATCH 57/60] interpreter: add G53.2, solve the tool orientation without moving G53.2 asks where the rotaries have to go for the tool to be normal to the active plane, like G53.1, and moves nothing: it publishes the pose on #<_orient_x> and kin and on #5071 to #5079, with #<_orient_valid> and #5080 saying a pose is held, so the program reaches it with a move of its own, for instance one G0 naming the linear and rotary words together, a single Cartesian move under TCP. Heidenhain's STAY. Reading the parameters before the first G53.2 is an error. Three fixes found on the way: a G68.3 block with axis words and a modal G0 emitted a stray move; SET_G68_FRAME in the preview plugin forgot the metric conversion; and the solver's answers in (-180, 180] are unwrapped onto the turn nearest the present rotary position before the nearest-first ranking, or a rotary crossing 180 swings the long way round. --- docs/src/gcode/g-code.adoc | 25 ++++++++--- docs/src/gcode/overview.adoc | 14 +++++- docs/src/motion/kinematics-conventions.adoc | 4 +- src/emc/rs274ngc/gcodemodule.cc | 1 + src/emc/rs274ngc/interp_array.cc | 3 +- src/emc/rs274ngc/interp_check.cc | 12 ++--- src/emc/rs274ngc/interp_convert.cc | 2 +- src/emc/rs274ngc/interp_internal.cc | 3 +- src/emc/rs274ngc/interp_internal.hh | 4 ++ src/emc/rs274ngc/interp_namedparams.cc | 32 ++++++++++++++ src/emc/rs274ngc/interp_setup.cc | 2 + src/emc/rs274ngc/interp_workplane.cc | 38 +++++++++++++--- tests/remap/introspect/expected | 4 +- tests/twp-native/test-ui.py | 49 +++++++++++++++++++++ 14 files changed, 166 insertions(+), 27 deletions(-) diff --git a/docs/src/gcode/g-code.adoc b/docs/src/gcode/g-code.adoc index a4d1a023e76..0a966aba025 100644 --- a/docs/src/gcode/g-code.adoc +++ b/docs/src/gcode/g-code.adoc @@ -88,7 +88,7 @@ as the 'L number', and so on for any other letter. |<> |Cancel Tool Length Offset |<> |Local Coordinate System Offset |<> |Move in Machine Coordinates -|<> |Orient the Tool to the Tilted Work Plane +|<> |Orient the Tool to the Tilted Work Plane |<> |Point-to-Point Move |<> |Select Coordinate System (1 - 9) |<> |Exact Path Mode @@ -1744,18 +1744,19 @@ It is an error if: * or G53 is used while cutter compensation is on. [[gcode:g53.1]] -== G53.1, G53.3, G53.6 Orient the Tool to the Work Plane(((G53.1 Orient the Tool))) +== G53.1, G53.2, G53.3, G53.6 Orient the Tool to the Work Plane(((G53.1 Orient the Tool))) [source,ngc] ---- G53.1 +G53.2 G53.3 X- Y- Z- G53.6 ---- -Each of these moves the rotary joints so that the tool axis is normal to the -active <>, the plane's Z. They differ in what -happens to the tool tip on the way: +All four solve where the rotary joints have to go so that the tool axis is +normal to the active <>, the plane's Z. They +differ in what happens then: * 'G53.1' moves the rotaries alone. The linear joints stay where they are, and the tool tip swings to wherever that carries it. It is a @@ -1764,6 +1765,16 @@ happens to the tool tip on the way: the rotary words, so the kinematics compensates the linear joints all along. * 'G53.3' moves the rotaries and takes the tool to 'X Y Z', given in the plane, in one point-to-point move. A word left out keeps the present value. +* 'G53.2' moves nothing. It only publishes the solved pose on the named + parameters '#<_orient_x>', '#<_orient_y>', '#<_orient_z>', '#<_orient_a>', + '#<_orient_b>' and '#<_orient_c>', and on the numbered parameters + '#5071' to '#5079' ('X Y Z A B C U V W'), in program units in the plane, + and sets '#<_orient_valid>' and '#5080' to 1. The program can then reach + the pose with a move of its own making, for instance a single 'G0' that + names 'X Y Z' and the rotary words together, so the turn and the travel + are one Cartesian move under the TCP kinematics. This is what Heidenhain + calls `STAY`. Reading '#<_orient_x>' and kin before the first 'G53.2' is + an error; '#<_orient_valid>' and '#5080' say whether they hold a pose. The kinematics module answers where the rotaries have to go, with its tool frame inverse (see the kinematics conventions chapter), so no configuration @@ -1834,8 +1845,8 @@ It is an error if: the way 'P' asks for, or the machine has no pair of poses a tilting joint tells apart. * 'Q' is anything but 0 or 1. -* Axis words are used with 'G53.1' or 'G53.6', or words other than 'X', 'Y' - and 'Z' with 'G53.3'. +* Axis words are used with 'G53.1', 'G53.2' or 'G53.6', or words other than + 'X', 'Y' and 'Z' with 'G53.3'. * Cutter compensation is on. [[gcode:g53.4]] diff --git a/docs/src/gcode/overview.adoc b/docs/src/gcode/overview.adoc index a05f01628c4..177be9b1074 100644 --- a/docs/src/gcode/overview.adoc +++ b/docs/src/gcode/overview.adoc @@ -304,6 +304,11 @@ example '##2' means the value of the parameter whose index is the which the G38 took place. Volatile. * '5070' - <> probe result: 1 if success, 0 if probe failed to close. Used with G38.3 and G38.5. Volatile. +* '5071-5079' - Pose last solved by <> for X, Y, Z, A, + B, C, U, V & W, in program units in the tilted work plane. Same values + as `#<_orient_x>` and kin. Read-only, volatile. +* '5080' - 'G53.2' result: 1 once a 'G53.2' has solved a pose, 0 before. + Same as `#<_orient_valid>`. Read-only, volatile. * '5081-5089' - Tool length offset currently applied to motion for X, Y, Z, A, B, C, U, V & W, in the current program units. Set by `G43`/`G43.1`/`G43.2`, and 0 when `G49` is in effect. These report the @@ -507,6 +512,13 @@ can be added easily without changes to the source code. 'P' number of the last 'G12.1', or 0 after 'G13.1' or when no kinematics has been selected. See <>. +* '#<_orient_valid>', '#<_orient_x>', '#<_orient_y>', '#<_orient_z>', + '#<_orient_a>', '#<_orient_b>', '#<_orient_c>' - The pose 'G53.2' last + solved, in program units in the tilted work plane. '#<_orient_valid>' is + 1 once a 'G53.2' has run; reading the others before that is an error. The + same values are on the numbered parameters '#5071' to '#5080'. See + <>. + * '#<_plane>' - returns the value designating the current plane: [width="20%",options="header"] @@ -964,7 +976,7 @@ The modal groups are shown in the following Table. [width="80%",cols="4,6",options="header"] |=== |Modal Group Meaning | Member Words -|Non-modal codes (Group 0) | G4, G10 G28, G30, G52, G53, G53.1, G53.3, G53.4, G53.5, G53.6, G53.7, G92, G92.1, G92.2, G92.3, +|Non-modal codes (Group 0) | G4, G10 G28, G30, G52, G53, G53.1, G53.2, G53.3, G53.4, G53.5, G53.6, G53.7, G92, G92.1, G92.2, G92.3, |Motion (Group 1) | G0, G1, G2, G3, G33, G38.n, G73, G76, G80, G81 G82, G83, G84, G85, G86, G87, G88, G89 |Plane selection (Group 2) | G17, G18, G19, G17.1, G18.1, G19.1 diff --git a/docs/src/motion/kinematics-conventions.adoc b/docs/src/motion/kinematics-conventions.adoc index a0588c001e2..7b5614c261f 100644 --- a/docs/src/motion/kinematics-conventions.adoc +++ b/docs/src/motion/kinematics-conventions.adoc @@ -340,8 +340,8 @@ refuses a program for naming both directions on a five-axis machine, and neither should this. In tree the G-code side of that is `G68.2`, which defines the plane, and -`G53.1`, `G53.3` and `G53.6`, which ask this inverse where the rotaries go, -with the plane's normal and its X as the request. Their `Q` word is the +`G53.1`, `G53.2`, `G53.3` and `G53.6`, which ask this inverse where the +rotaries go, with the plane's normal and its X as the request. Their `Q` word is the `held` mask: `Q0` holds the joints the work frame survey finds and takes the reported turn as the coordinate rotation it is, `Q1` holds nothing. See the G-code chapter. diff --git a/src/emc/rs274ngc/gcodemodule.cc b/src/emc/rs274ngc/gcodemodule.cc index 67807a39c51..f2289e97919 100644 --- a/src/emc/rs274ngc/gcodemodule.cc +++ b/src/emc/rs274ngc/gcodemodule.cc @@ -718,6 +718,7 @@ void SET_XY_ROTATION(double t) { void SET_G68_FRAME(double x, double y, double z, const double rotation[9], int active) { + if(metric) { x /= 25.4; y /= 25.4; z /= 25.4; } maybe_new_line(); if(interp_error) return; PyObject *result = diff --git a/src/emc/rs274ngc/interp_array.cc b/src/emc/rs274ngc/interp_array.cc index afcb22141b2..71964ea1d3a 100644 --- a/src/emc/rs274ngc/interp_array.cc +++ b/src/emc/rs274ngc/interp_array.cc @@ -96,7 +96,7 @@ const int Interp::gees[] = { /* 460 */ -1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1, /* 480 */ -1,-1,-1,-1,-1,-1,-1,-1,-1,-1, 8,-1,-1,-1,-1,-1,-1,-1,-1,-1, /* 500 */ -1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1, -/* 520 */ 0,-1,-1,-1,-1,-1,-1,-1,-1,-1, 0, 0,-1, 0, 0, 0, 0, 0,-1,-1, +/* 520 */ 0,-1,-1,-1,-1,-1,-1,-1,-1,-1, 0, 0, 0, 0, 0, 0, 0, 0,-1,-1, /* 540 */ 12,-1,-1,-1,-1,-1,-1,-1,-1,-1,12,-1,-1,-1,-1,-1,-1,-1,-1,-1, /* 560 */ 12,-1,-1,-1,-1,-1,-1,-1,-1,-1,12,-1,-1,-1,-1,-1,-1,-1,-1,-1, /* 580 */ 12,-1,-1,-1,-1,-1,-1,-1,-1,-1,12,12,12,12,-1,-1,-1,-1,-1,-1, @@ -228,6 +228,7 @@ const int Interp::required_parameters[] = { const int Interp::readonly_parameters[] = { 5021, 5022, 5023, 5024, 5025, 5026, 5027, 5028, 5029, // machine X Y ... W + 5071, 5072, 5073, 5074, 5075, 5076, 5077, 5078, 5079, 5080, // G53.2 pose X Y ... W, valid 5400, // tool toolno 5401, // tool x offset 5402, // tool y offset diff --git a/src/emc/rs274ngc/interp_check.cc b/src/emc/rs274ngc/interp_check.cc index 261e5060705..4b650bb50f7 100644 --- a/src/emc/rs274ngc/interp_check.cc +++ b/src/emc/rs274ngc/interp_check.cc @@ -109,10 +109,10 @@ int Interp::check_g_codes(block_pointer block, //!< pointer to a block to be c (settings->distance_mode == DISTANCE_MODE::INCREMENTAL))), NCE_CANNOT_USE_G53_INCREMENTAL); } else if (mode0 == G_92) { - } else if (mode0 == G_53_1 || mode0 == G_53_6) { + } else if (mode0 == G_53_1 || mode0 == G_53_2 || mode0 == G_53_6) { CHKS((block->x_flag || block->y_flag || block->z_flag || block->a_flag || block->b_flag || block->c_flag || block->u_flag || block->v_flag || block->w_flag), - _("Cannot use axis words with G53.1 or G53.6")); + _("Cannot use axis words with G53.1, G53.2 or G53.6")); } else if (mode0 == G_53_3) { CHKS((block->a_flag || block->b_flag || block->c_flag || block->u_flag || block->v_flag || block->w_flag), _("Only X, Y and Z words can be used with G53.3")); @@ -360,7 +360,7 @@ int Interp::check_other_codes(block_pointer block) //!< pointer to a block if (block->p_flag) { CHKS(((block->g_modes[GM_MODAL_0] != G_10) && (block->g_modes[GM_MODAL_0] != G_4) && (block->g_modes[GM_CONTROL_MODE] != G_64 && (block->g_modes[GM_MODAL_0] != G_12_1)) && (block->g_modes[GM_WORK_PLANE] == -1) && - (block->g_modes[GM_MODAL_0] != G_53_1) && (block->g_modes[GM_MODAL_0] != G_53_3) && (block->g_modes[GM_MODAL_0] != G_53_6) && + (block->g_modes[GM_MODAL_0] != G_53_1) && (block->g_modes[GM_MODAL_0] != G_53_2) && (block->g_modes[GM_MODAL_0] != G_53_3) && (block->g_modes[GM_MODAL_0] != G_53_6) && (motion != G_76) && (motion != G_82) && (motion != G_86) && (motion != G_88) && (motion != G_89) && (motion != G_5) && (motion != G_5_2) && (motion != G_70) && @@ -372,7 +372,7 @@ int Interp::check_other_codes(block_pointer block) //!< pointer to a block (block->m_modes[5] != 64) && (block->m_modes[5] != 65) && (block->m_modes[5] != 66) && (block->m_modes[7] != 19) && (block->user_m != 1) && (block->o_type != M_98)), - _("P word with no G2 G3 G4 G10 G12.1 G53.1 G53.3 G53.6 G64 G68.2 G5 G5.2 G6, G6.2, G76 G82 G86 G88 G89" + _("P word with no G2 G3 G4 G10 G12.1 G53.1 G53.2 G53.3 G53.6 G64 G68.2 G5 G5.2 G6, G6.2, G76 G82 G86 G88 G89" " or M50 M51 M52 M53 M62 M63 M64 M65 M66 M98 " "or user M code to use it")); int p_value = round_to_int(block->p_number); @@ -390,12 +390,12 @@ int Interp::check_other_codes(block_pointer block) //!< pointer to a block (block->m_modes[5] != 66) && (block->m_modes[5] != 67) && (block->m_modes[5] != 68) && (block->g_modes[GM_MODAL_0] != G_10) && (block->m_modes[6] != 61) && (block->g_modes[GM_CONTROL_MODE] != G_64) && (block->g_modes[GM_WORK_PLANE] == -1) && - (block->g_modes[GM_MODAL_0] != G_53_1) && (block->g_modes[GM_MODAL_0] != G_53_3) && (block->g_modes[GM_MODAL_0] != G_53_6) && + (block->g_modes[GM_MODAL_0] != G_53_1) && (block->g_modes[GM_MODAL_0] != G_53_2) && (block->g_modes[GM_MODAL_0] != G_53_3) && (block->g_modes[GM_MODAL_0] != G_53_6) && (motion != G_70) && (motion != G_71) && (motion != G_71_1) && (motion != G_71_2) && (motion != G_72) && (motion != G_72_1) && (motion != G_72_2) && (block->m_modes[7] != 19), - _("Q word with no G5, G6, G10, G53.1, G53.3, G53.6, G64, G68.2, G73, G76, G83, M19, M66, M67, M68 or user M code that uses it")); + _("Q word with no G5, G6, G10, G53.1, G53.2, G53.3, G53.6, G64, G68.2, G73, G76, G83, M19, M66, M67, M68 or user M code that uses it")); } if (block->r_flag) { diff --git a/src/emc/rs274ngc/interp_convert.cc b/src/emc/rs274ngc/interp_convert.cc index 377e649e113..918de7b95e0 100644 --- a/src/emc/rs274ngc/interp_convert.cc +++ b/src/emc/rs274ngc/interp_convert.cc @@ -4371,7 +4371,7 @@ int Interp::convert_modal_0(int code, //!< G-code, must be from group 0 CHP(convert_nurbs(code, block, settings)); } else if ((code == G_4) || (code == G_53) || (code == G_53_4) || (code == G_53_5) || (code == G_53_7)); // handled elsewhere - else if ((code == G_53_1) || (code == G_53_3) || (code == G_53_6)) { + else if ((code == G_53_1) || (code == G_53_2) || (code == G_53_3) || (code == G_53_6)) { CHP(convert_orient_tool(code, block, settings)); } else if ((code == G_12_1) || (code == G_13_1)) { diff --git a/src/emc/rs274ngc/interp_internal.cc b/src/emc/rs274ngc/interp_internal.cc index 42de545b9b7..0725329be12 100644 --- a/src/emc/rs274ngc/interp_internal.cc +++ b/src/emc/rs274ngc/interp_internal.cc @@ -176,7 +176,8 @@ int Interp::enhance_block(block_pointer block, //!< pointer to a block to be c ((mode0 == G_10) || (mode0 == G_28) || (mode0 == G_30) || (mode0 == G_52) || (mode0 == G_92) || (mode0 == G_53_3)); // a tilted work plane definition takes the axis words the same way - if (block->g_modes[GM_WORK_PLANE] == G_68_2 || block->g_modes[GM_WORK_PLANE] == G_68_4) { + if (block->g_modes[GM_WORK_PLANE] == G_68_2 || block->g_modes[GM_WORK_PLANE] == G_68_3 + || block->g_modes[GM_WORK_PLANE] == G_68_4) { CHKS(polar_flag, _("Polar coordinates cannot define a tilted work plane")); mode_zero_covets_axes = 1; } diff --git a/src/emc/rs274ngc/interp_internal.hh b/src/emc/rs274ngc/interp_internal.hh index 90565ae4342..b8d4c98c90a 100644 --- a/src/emc/rs274ngc/interp_internal.hh +++ b/src/emc/rs274ngc/interp_internal.hh @@ -254,6 +254,7 @@ enum GCodes G_52 = 520, G_53 = 530, G_53_1 = 531, + G_53_2 = 532, G_53_3 = 533, G_53_4 = 534, G_53_5 = 535, @@ -771,6 +772,9 @@ struct setup int g68_seq_p; unsigned g68_seq_have; // bit per Q received double g68_seq_word[4][7]; // per Q: x y z i j k r + // the pose G53.2 last solved, in program words, for #<_orient_a> and kin + bool orient_valid; + double orient_pose[6]; // x y z a b c // the kinematics, for G68.3 and the orientation moves: loaded on first // use through the non-realtime loader, on a HAL component of our own void *kins_ctx; // KinematicsUserContext diff --git a/src/emc/rs274ngc/interp_namedparams.cc b/src/emc/rs274ngc/interp_namedparams.cc index 9ab4bae38b5..cb830d8542b 100644 --- a/src/emc/rs274ngc/interp_namedparams.cc +++ b/src/emc/rs274ngc/interp_namedparams.cc @@ -59,6 +59,13 @@ enum predefined_named_parameters { NP_LINE, NP_MOTION_MODE, NP_KINS_TYPE, + NP_ORIENT_VALID, + NP_ORIENT_X, + NP_ORIENT_Y, + NP_ORIENT_Z, + NP_ORIENT_A, + NP_ORIENT_B, + NP_ORIENT_C, NP_PLANE, NP_CCOMP, NP_METRIC, @@ -546,6 +553,22 @@ int Interp::lookup_named_param(const char *nameBuf, *value = _setup.kins_type; break; + case NP_ORIENT_VALID: // _orient_valid: G53.2 has solved a pose + *value = _setup.orient_valid; + break; + + case NP_ORIENT_X: // _orient_x and kin: the pose G53.2 last solved + case NP_ORIENT_Y: + case NP_ORIENT_Z: + case NP_ORIENT_A: + case NP_ORIENT_B: + case NP_ORIENT_C: + if (!_setup.orient_valid) { + ERS(_("no G53.2 has solved an orientation yet")); + } + *value = _setup.orient_pose[cmd - NP_ORIENT_X]; + break; + case NP_PLANE: // _plane switch(_setup.plane) { case CANON_PLANE::XY: @@ -899,6 +922,15 @@ int Interp::init_named_parameters() // kinematics selected by G12.1 P- / G13.1, 0 when none has been selected init_readonly_param("_kins_type", NP_KINS_TYPE, PA_USE_LOOKUP); + // the pose G53.2 last solved: 1.0 once one has been, and its words + init_readonly_param("_orient_valid", NP_ORIENT_VALID, PA_USE_LOOKUP); + init_readonly_param("_orient_x", NP_ORIENT_X, PA_USE_LOOKUP); + init_readonly_param("_orient_y", NP_ORIENT_Y, PA_USE_LOOKUP); + init_readonly_param("_orient_z", NP_ORIENT_Z, PA_USE_LOOKUP); + init_readonly_param("_orient_a", NP_ORIENT_A, PA_USE_LOOKUP); + init_readonly_param("_orient_b", NP_ORIENT_B, PA_USE_LOOKUP); + init_readonly_param("_orient_c", NP_ORIENT_C, PA_USE_LOOKUP); + // G17/18/19/17.1/18.1/19.1 -> return 170/180/190/171/181/191 init_readonly_param("_plane", NP_PLANE, PA_USE_LOOKUP); diff --git a/src/emc/rs274ngc/interp_setup.cc b/src/emc/rs274ngc/interp_setup.cc index 4a5e3ce866d..dbbccf58ae0 100644 --- a/src/emc/rs274ngc/interp_setup.cc +++ b/src/emc/rs274ngc/interp_setup.cc @@ -112,6 +112,8 @@ setup::setup() : g68_seq_p(0), g68_seq_have(0), g68_seq_word{}, + orient_valid(false), + orient_pose{0.0, 0.0, 0.0, 0.0, 0.0, 0.0}, kins_ctx(nullptr), kins_comp_id(0), kins_module{}, diff --git a/src/emc/rs274ngc/interp_workplane.cc b/src/emc/rs274ngc/interp_workplane.cc index 29253aaafa2..4172d8bf914 100644 --- a/src/emc/rs274ngc/interp_workplane.cc +++ b/src/emc/rs274ngc/interp_workplane.cc @@ -682,11 +682,12 @@ int Interp::convert_work_plane_from_tool(block_pointer block, setup_pointer s) return work_plane_set(s, G_68_3, origin, rotation); } -// G53.1, G53.3 and G53.6: the rotaries to the plane's normal. G53.1 turns -// the rotaries alone, in joint space; G53.6 keeps the tool centre point, a -// Cartesian move; G53.3 goes to X Y Z in the plane. P picks the pose, -// nearest first or by the sign of the tilting joint; Q0 holds the joints that -// carry the work (Heidenhain COORD ROT), Q1 frees them (TABLE ROT). +// G53.1, G53.2, G53.3 and G53.6: the rotaries to the plane's normal. G53.1 +// turns the rotaries alone, in joint space; G53.6 keeps the tool centre point, +// a Cartesian move; G53.3 goes to X Y Z in the plane; G53.2 only publishes the +// pose on #<_orient_x> and kin (Heidenhain STAY). P picks the pose, nearest +// first or by the sign of the tilting joint; Q0 holds the joints that carry +// the work (COORD ROT), Q1 frees them (TABLE ROT). int Interp::convert_orient_tool(int code, block_pointer block, setup_pointer s) { void *vctx; @@ -702,7 +703,7 @@ int Interp::convert_orient_tool(int code, block_pointer block, setup_pointer s) unsigned int held = 0; int p, q, n, i, j, chosen, njoints; const double *sol; - const char *name = (code == G_53_1) ? "G53.1" : (code == G_53_3) ? "G53.3" : "G53.6"; + const char *name = (code == G_53_1) ? "G53.1" : (code == G_53_2) ? "G53.2" : (code == G_53_3) ? "G53.3" : "G53.6"; CHKS((!s->g68_active), _("%s needs a tilted work plane; define one with G68.2 first"), name); CHKS((s->cutter_comp_side != CUTTER_COMP::OFF), @@ -737,6 +738,18 @@ int Interp::convert_orient_tool(int code, block_pointer block, setup_pointer s) CHKS((n < 0), _("%s: the kinematics cannot answer the orientation"), name); CHKS((n == 0), _("%s: the plane's normal cannot be reached by the rotary joints"), name); + // the solver reports each answer in (-180, 180], but the machine stands + // somewhere in turn space: unwrap every angular joint onto the turn + // nearest the present position, or the nearest pose is not the nearest + // move and a free rotary swings the long way round + for (i = 0; i < n; i++) { + for (j = 0; j < njoints; j++) { + double *v = &solutions[i*njoints + j]; + if (!(s->kins_angular_joints & (1 << j))) { continue; } + *v += 360.0 * floor((now[j] - *v) / 360.0 + 0.5); + } + } + // nearest first, by rotary travel in joint units for (i = 0; i < n; i++) { distance[i] = 0.0; @@ -782,6 +795,19 @@ int Interp::convert_orient_tool(int code, block_pointer block, setup_pointer s) } machine_pose_to_program(s, &end_pose, end_prog); + if (code == G_53_2) { + // STAY: solve only, nothing moves. The pose goes to the named + // parameters #<_orient_x> and kin and to #5071-#5080, for the + // program to use in a move of its own making, the way + // Heidenhain's STAY fills Q120-122. The machine state does not + // change. + for (i = 0; i < 6; i++) { s->orient_pose[i] = end_prog[i]; } + s->orient_valid = true; + for (i = 0; i < 9; i++) { s->parameters[5071 + i] = end_prog[i]; } + s->parameters[5080] = 1.0; + return INTERP_OK; + } + write_canon_state_tag(block, s); if (code == G_53_1) { // the rotaries alone: the linear joints are where they are, since diff --git a/tests/remap/introspect/expected b/tests/remap/introspect/expected index 2f33b4bbe08..206bc4d96af 100644 --- a/tests/remap/introspect/expected +++ b/tests/remap/introspect/expected @@ -29,8 +29,8 @@ speed= 3000.0 global parameter set in test.ngc: 47.11 parameter set via test.ini: 3.14159 locals: ['a_new_local'] -globals: ['_a', '_a_global_set_in_test_dot_ngc', '_a_new_global', '_abs_a', '_abs_b', '_abs_c', '_abs_u', '_abs_v', '_abs_w', '_abs_x', '_abs_y', '_abs_z', '_absolute', '_adaptive_feed', '_b', '_c', '_call_level', '_ccomp', '_coord_system', '_current_pocket', '_current_tool', '_feed', '_feed_hold', '_feed_override', '_flood', '_ijk_absolute_mode', '_imperial', '_incremental', '_ini[example]variable', '_inverse_time', '_kins_type', '_lathe_diameter_mode', '_lathe_radius_mode', '_line', '_metric', '_metric_machine', '_mist', '_motion_mode', '_plane', '_remap_level', '_retract_old_z', '_retract_r_plane', '_rpm', '_selected_pocket', '_selected_tool', '_speed_override', '_spindle_css_mode', '_spindle_cw', '_spindle_on', '_spindle_rpm_mode', '_task', '_tool_offset', '_u', '_units_per_minute', '_units_per_rev', '_v', '_value', '_value_returned', '_vmajor', '_vminor', '_w', '_x', '_y', '_z', 'foo'] -params(): ['a_new_local', '_a', '_a_global_set_in_test_dot_ngc', '_a_new_global', '_abs_a', '_abs_b', '_abs_c', '_abs_u', '_abs_v', '_abs_w', '_abs_x', '_abs_y', '_abs_z', '_absolute', '_adaptive_feed', '_b', '_c', '_call_level', '_ccomp', '_coord_system', '_current_pocket', '_current_tool', '_feed', '_feed_hold', '_feed_override', '_flood', '_ijk_absolute_mode', '_imperial', '_incremental', '_ini[example]variable', '_inverse_time', '_kins_type', '_lathe_diameter_mode', '_lathe_radius_mode', '_line', '_metric', '_metric_machine', '_mist', '_motion_mode', '_plane', '_remap_level', '_retract_old_z', '_retract_r_plane', '_rpm', '_selected_pocket', '_selected_tool', '_speed_override', '_spindle_css_mode', '_spindle_cw', '_spindle_on', '_spindle_rpm_mode', '_task', '_tool_offset', '_u', '_units_per_minute', '_units_per_rev', '_v', '_value', '_value_returned', '_vmajor', '_vminor', '_w', '_x', '_y', '_z', 'foo'] +globals: ['_a', '_a_global_set_in_test_dot_ngc', '_a_new_global', '_abs_a', '_abs_b', '_abs_c', '_abs_u', '_abs_v', '_abs_w', '_abs_x', '_abs_y', '_abs_z', '_absolute', '_adaptive_feed', '_b', '_c', '_call_level', '_ccomp', '_coord_system', '_current_pocket', '_current_tool', '_feed', '_feed_hold', '_feed_override', '_flood', '_ijk_absolute_mode', '_imperial', '_incremental', '_ini[example]variable', '_inverse_time', '_kins_type', '_lathe_diameter_mode', '_lathe_radius_mode', '_line', '_metric', '_metric_machine', '_mist', '_motion_mode', '_orient_a', '_orient_b', '_orient_c', '_orient_valid', '_orient_x', '_orient_y', '_orient_z', '_plane', '_remap_level', '_retract_old_z', '_retract_r_plane', '_rpm', '_selected_pocket', '_selected_tool', '_speed_override', '_spindle_css_mode', '_spindle_cw', '_spindle_on', '_spindle_rpm_mode', '_task', '_tool_offset', '_u', '_units_per_minute', '_units_per_rev', '_v', '_value', '_value_returned', '_vmajor', '_vminor', '_w', '_x', '_y', '_z', 'foo'] +params(): ['a_new_local', '_a', '_a_global_set_in_test_dot_ngc', '_a_new_global', '_abs_a', '_abs_b', '_abs_c', '_abs_u', '_abs_v', '_abs_w', '_abs_x', '_abs_y', '_abs_z', '_absolute', '_adaptive_feed', '_b', '_c', '_call_level', '_ccomp', '_coord_system', '_current_pocket', '_current_tool', '_feed', '_feed_hold', '_feed_override', '_flood', '_ijk_absolute_mode', '_imperial', '_incremental', '_ini[example]variable', '_inverse_time', '_kins_type', '_lathe_diameter_mode', '_lathe_radius_mode', '_line', '_metric', '_metric_machine', '_mist', '_motion_mode', '_orient_a', '_orient_b', '_orient_c', '_orient_valid', '_orient_x', '_orient_y', '_orient_z', '_plane', '_remap_level', '_retract_old_z', '_retract_r_plane', '_rpm', '_selected_pocket', '_selected_tool', '_speed_override', '_spindle_css_mode', '_spindle_cw', '_spindle_on', '_spindle_rpm_mode', '_task', '_tool_offset', '_u', '_units_per_minute', '_units_per_rev', '_v', '_value', '_value_returned', '_vmajor', '_vminor', '_w', '_x', '_y', '_z', 'foo'] 14 N..... MESSAGE(" after introspect: return value=2.718280 call_level= 0.000000") 15 N..... SET_G5X_OFFSET(1, 0.0000, 0.0000, 0.0000, 0.0000, 0.0000, 0.0000) 16 N..... SET_XY_ROTATION(0.0000) diff --git a/tests/twp-native/test-ui.py b/tests/twp-native/test-ui.py index c37ebd2579f..96108490911 100755 --- a/tests/twp-native/test-ui.py +++ b/tests/twp-native/test-ui.py @@ -162,6 +162,13 @@ def rot_y(d): return np.array([[math.cos(r), 0, math.sin(r)], [0, 1, 0], [-math.sin(r), 0, math.cos(r)]]) # --- the plane, and G53.1 with the table held --------------------------- +# no G53.2 has run yet, so the pose parameters must refuse to be read +c.mdi("G0 X#<_orient_x>") +c.wait_complete(30) +m = e.poll() +if not m or m[0] not in (linuxcnc.NML_ERROR, linuxcnc.OPERATOR_ERROR): + error("reading #<_orient_x> before the first G53.2 was accepted") +drain() start = mdi("G12.1 P1", "G0 X0 Y0 Z0 A0 B0 C0") show("start", start) R = rot_y(20).dot(rot_x(30)) @@ -271,6 +278,48 @@ def in_plane(): if not close(tool_axis(after), list(R2[:, 2]), 1e-6): error("the tool axis after G53.6 is not the plane normal") +# --- G53.2 solves without moving, the pose lands on the parameters ---- +# same plane as G53.6 above, but from a pose that is not the answer: +# G53.2 must not move, and the pose it publishes must put the tool on the +# plane normal, which is where G53.6 already stands, so the nearest +# solution is the present rotary position +stay = after +after2, samples = sampled("G53.2") +drain() +if not close(after2, stay, 1e-9): + error("G53.2 moved the machine: %s became %s" % (stay, after2)) +# read the pose back through the parameters: a move to the published +# rotary words is a move to the present B and C, with the table held +before3 = mdi("G0 B0 C0") +after3, samples = sampled("G0 B#<_orient_b> C#<_orient_c>") +show("G0 to #<_orient_b/c>", after3) +drain() +if abs(wrap(after3[SECONDARY] - stay[SECONDARY])) > 1e-3 or abs(wrap(after3[PRIMARY] - stay[PRIMARY])) > 1e-3: + error("#<_orient_b> #<_orient_c> held (%.4f, %.4f), G53.6 had reached (%.4f, %.4f)" + % (after3[SECONDARY], after3[PRIMARY], stay[SECONDARY], stay[PRIMARY])) +if not close(tool_axis(after3), list(R2[:, 2]), 1e-6): + error("the tool axis at the pose G53.2 published is not the plane normal") +# the numbered parameters carry the same pose +before4 = mdi("G0 B0 C0") +after4, samples = sampled("G0 B#5075 C#5076") +show("G0 to #5075/#5076", after4) +drain() +if abs(wrap(after4[SECONDARY] - stay[SECONDARY])) > 1e-3 or abs(wrap(after4[PRIMARY] - stay[PRIMARY])) > 1e-3: + error("#5075 #5076 held (%.4f, %.4f), G53.6 had reached (%.4f, %.4f)" + % (after4[SECONDARY], after4[PRIMARY], stay[SECONDARY], stay[PRIMARY])) +c.mdi("#5075 = 0") +c.wait_complete(30) +m = e.poll() +if not m or m[0] not in (linuxcnc.NML_ERROR, linuxcnc.OPERATOR_ERROR): + error("writing #5075 was accepted; the G53.2 pose is not read-only") +drain() +c.mdi("G0 A#<_orient_a>") +c.wait_complete(30) +m = e.poll() +if m and m[0] in (linuxcnc.NML_ERROR, linuxcnc.OPERATOR_ERROR): + error("#<_orient_a> after G53.2: %s" % m[1]) +drain() + # --- G53.3 goes to a point in the plane with the tool oriented ---------- before = mdi("G69") R3 = rot_y(-25).dot(rot_x(35)) From 320806a7b3acdae761916184d55441f606c6cd41 Mon Sep 17 00:00:00 2001 From: Luca Toniolo <10792599+grandixximo@users.noreply.github.com> Date: Thu, 10 Sep 2026 19:03:51 +0800 Subject: [PATCH 58/60] configs: the bridgemill sim shows off the tilted work plane family The old 5axisgui.ngc "drilled" a sphere with W words, W standing in for a tool length: the W axis of 5axiskins never drove a physical joint, the stroke folds into the XYZ slides, and the model's drawn quill hid that by cancelling the very motion that makes a W word work. The quill and its drawing lock go, so a W word shows the slides plunging, and 5axisgui.ngc keeps its name with comments saying what the controller cannot see. The sphere is then drilled two honest ways: g532-fused-orient-move.ngc, where G53.2 solves each plane and one fused G0 turns the head and travels to the hole, and g536-orient-then-move.ngc, where G53.6 reorients about the tip and a separate move travels in the plane. The demos and the M428 remap select the TCP kinematics with G12.1 P0, since on 5axiskins G13.1 lands on the identity. The AXIS GEOMETRY drops W: under TCP a W word never moves the tip, so W in the live plot only ever lied. --- .../axis/vismach/5axis/bridgemill/5axis.ini | 9 +++- .../vismach/5axis/bridgemill/5axisgui.ngc | 15 ++++++ .../sim/axis/vismach/5axis/bridgemill/README | 50 +++++++++++++++++-- .../bridgemill/g532-fused-orient-move.ngc | 50 +++++++++++++++++++ .../bridgemill/g536-orient-then-move.ngc | 49 ++++++++++++++++++ 5 files changed, 166 insertions(+), 7 deletions(-) create mode 100644 configs/sim/axis/vismach/5axis/bridgemill/g532-fused-orient-move.ngc create mode 100644 configs/sim/axis/vismach/5axis/bridgemill/g536-orient-then-move.ngc diff --git a/configs/sim/axis/vismach/5axis/bridgemill/5axis.ini b/configs/sim/axis/vismach/5axis/bridgemill/5axis.ini index 38e706fac22..4c987aba743 100644 --- a/configs/sim/axis/vismach/5axis/bridgemill/5axis.ini +++ b/configs/sim/axis/vismach/5axis/bridgemill/5axis.ini @@ -4,8 +4,13 @@ MACHINE = Sim-5Axis Bridge Mill (xyzbcw) DEBUG = 0 [DISPLAY] - GEOMETRY = XYZCBW - OPEN_FILE = ./5axisgui.ngc +# GEOMETRY tells the AXIS live plot how to guess the tool position from +# the nine axis values: letters translate, ABC rotate the point. W is +# deliberately absent: the plot would add the W value as a Z offset after +# the rotations, while the kinematics spends W along the tool axis, so +# the guess never matches. + GEOMETRY = XYZCB + OPEN_FILE = ./g532-fused-orient-move.ngc INCREMENTS = 10 mm, 1 mm, .1 mm JOG_AXES = XYZC DISPLAY = axis diff --git a/configs/sim/axis/vismach/5axis/bridgemill/5axisgui.ngc b/configs/sim/axis/vismach/5axis/bridgemill/5axisgui.ngc index 7b2c5a8f188..cbc165137d7 100644 --- a/configs/sim/axis/vismach/5axis/bridgemill/5axisgui.ngc +++ b/configs/sim/axis/vismach/5axis/bridgemill/5axisgui.ngc @@ -1,3 +1,17 @@ +; 5axisgui.ngc - the historical demo program, unchanged. +; It drills a sphere pattern with W words. W was never a physical +; quill: no motor is connected to its joint, the kinematics folds +; the word into the XYZ slides, and the head sliding along the tool +; axis is what plunges the rigidly mounted tool. Watch the slides +; in vismach do the stroke. +; +; The quirk, then and now: the controller keeps W as a separate +; world coordinate, so the programmed XYZ point does not move and +; the preview shows nothing of the stroke; only the W DRO tracks it. +; Drilling the controller can see, check and preview is what the +; tilted work plane family is for (g532-fused-orient-move.ngc, +; g536-orient-then-move.ngc). + # = 60 ; sphere radius # = 5 ; safe distance # = -5 @@ -13,6 +27,7 @@ # = [90/#] g49 +g12.1 p0 ; the TCP kinematics, in case a previous run left the identity one active t#m6g43 g53 g0 x0y0z#b0c0 w0 diff --git a/configs/sim/axis/vismach/5axis/bridgemill/README b/configs/sim/axis/vismach/5axis/bridgemill/README index 2adf5599795..4a92c6a2078 100644 --- a/configs/sim/axis/vismach/5axis/bridgemill/README +++ b/configs/sim/axis/vismach/5axis/bridgemill/README @@ -1,6 +1,46 @@ -This is a simulation of an XYZBCWY 5 axis bridge mill. +This is a simulation of an XYZBCWY 5 axis bridge mill with a +tilting head (B, C) and a W axis. -Example demo: +W is not a physical axis of the machine and never was: no motor is +connected to its joint. The kinematics folds a W word into the +XYZ joints, so the head slides along the tool axis and the +rigidly mounted tool goes with it: that is the whole trick, and it +is what vismach shows. There is no quill to draw because there is +no quill. + +The quirk: the controller keeps W as a separate world coordinate, +so a W word leaves the programmed XYZ point untouched and the +preview cannot show the stroke; the W DRO tracks it. It also +means the planner does not see the tool-axis motion, so for +drilling the controller can check and preview, use the tilted work +plane family. + +Because the W joint is virtual, a point-to-point move on it +(g53.5/g53.7 j5=) moves no motor on a real machine while the +controller believes the world moved, the opposite of a W word. +Do not drill that way. + +Demo programs: + + g532-fused-orient-move.ngc -- drill a sphere pattern with tilted work + planes (g68.2), using g53.2 to solve each + orientation without moving (STAY), then + one fused g0 that turns the head and + travels to the hole at the same time + (TCP motion) + g536-orient-then-move.ngc -- the same pattern with g53.6 (Heidenhain + MOVE style): reorient about the fixed + tip, then travel in the tilted plane + 5axisgui.ngc --------------- the historical demo program, unchanged: + it drills the same pattern with W words, + the slides doing the stroke while the + preview stays blind to it + +The tool table provides tool 100 (length 100). Load it with +t100m6g43 so the vismach tool and the kinematics pivot length +(pivotsum: 250 + tool length) match. + +Example MDI: 1) $ linuxcnc 5axis.ini 2) F1 ---------- Estop off @@ -8,12 +48,12 @@ Example demo: CTRL-HOME --- home all F5 ---------- MDI tab 3) orient vismach gui as required - 4) g0w10 ; retract w + 4) g0w10 ; head slides up the tool axis, tip with it 5) g43h100 ; tool offset (100) 6) g0b45 ; tilt 45 deg wrt z 7) g0c30 ; rotate 30 deg in xy - 8) g0w-10 ; simulate drill - 9) g0w10 ; retract drill + 8) g0w-10 ; head slides back, tip plunges 10 along the tool axis + 9) g0w10 ; and back 10) etc Note: Motion for the W coordinate is incorporated diff --git a/configs/sim/axis/vismach/5axis/bridgemill/g532-fused-orient-move.ngc b/configs/sim/axis/vismach/5axis/bridgemill/g532-fused-orient-move.ngc new file mode 100644 index 00000000000..bd193299758 --- /dev/null +++ b/configs/sim/axis/vismach/5axis/bridgemill/g532-fused-orient-move.ngc @@ -0,0 +1,50 @@ +; g532-fused-orient-move.ngc - drill a sphere pattern with tilted work planes: +; g68.2 tilts the plane onto each hole normal, g53.2 solves the head +; orientation without moving (STAY), and one fused g0 turns the head +; and travels to the hole at the same time (TCP motion). See README +; for the other demos. + + # = 60 ; sphere radius + # = 5 ; safe distance +# = -5 + # = 20 ; clearance above the ball for start and stop + # = 8 + # = 16 + # = 100 + # = 1000 + +# = 0 +# = 0 +# = [360/#] +# = [90/#] + +g49 +g12.1 p0 ; the TCP kinematics, in case a previous run left the identity one active +t#m6g43 + +g53 g0 x0 y0 z0 b0 c0 +g10 l20 p0 x0 y0 z[#+#+#] b0 c0 ; ball center at program origin, we park above it +f# +o100 while [# lt #] + # = [[#-1-#]*#] ; top ring first, the way in stays outside the ball + # = 0 +o200 while [# lt #] +o210 if [[# mod 2] eq 0] + # = [# * #] +o210 else + # = [360 - [1+ #] * #] +o210 endif + g68.2 p1 j[90-#] k# ; plane Z is the sphere radius at b, c + g53.2 ; solve the orientation, stay put + g0 x0 y0 z[#+#] b#<_orient_b> c#<_orient_c> ; one TCP turn and travel + g1 z[#+#] + g0 z[#+#] + # = [#+1] +o200 endwhile + # = [#+1] +o100 endwhile + +g69 +g53 g0 z0 ; up, clear of the ball +g53 g0 x0 y0 b0 c0 +m2 diff --git a/configs/sim/axis/vismach/5axis/bridgemill/g536-orient-then-move.ngc b/configs/sim/axis/vismach/5axis/bridgemill/g536-orient-then-move.ngc new file mode 100644 index 00000000000..2354e7e0625 --- /dev/null +++ b/configs/sim/axis/vismach/5axis/bridgemill/g536-orient-then-move.ngc @@ -0,0 +1,49 @@ +; g536-orient-then-move.ngc - same sphere as g532-fused-orient-move.ngc, but with g53.6 +; (Heidenhain MOVE style): reorient about the fixed tip, a TCP move, +; then travel to the next hole in the tilted plane with a separate g0. +; g532-fused-orient-move.ngc fuses the turn and the travel into one move instead. + + # = 60 ; sphere radius + # = 5 ; safe distance +# = -5 + # = 20 ; clearance above the ball for start and stop + # = 8 + # = 16 + # = 100 + # = 1000 + +# = 0 +# = 0 +# = [360/#] +# = [90/#] + +g49 +g12.1 p0 ; the TCP kinematics, in case a previous run left the identity one active +t#m6g43 + +g53 g0 x0 y0 z0 b0 c0 +g10 l20 p0 x0 y0 z[#+#+#] b0 c0 ; ball center at program origin, we park above it +f# +o100 while [# lt #] + # = [[#-1-#]*#] ; top ring first, the way in stays outside the ball + # = 0 +o200 while [# lt #] +o210 if [[# mod 2] eq 0] + # = [# * #] +o210 else + # = [360 - [1+ #] * #] +o210 endif + g68.2 p1 j[90-#] k# ; plane Z is the sphere radius at b, c + g53.6 ; reorient about the tip, a TCP move + g0 x0 y0 z[#+#] + g1 z[#+#] + g0 z[#+#] + # = [#+1] +o200 endwhile + # = [#+1] +o100 endwhile + +g69 +g53 g0 z0 ; up, clear of the ball +g53 g0 x0 y0 b0 c0 +m2 From 31c3f5b447d3bf8af37a5ec42b99ed8fb2f407c9 Mon Sep 17 00:00:00 2001 From: Luca Toniolo <10792599+grandixximo@users.noreply.github.com> Date: Mon, 14 Sep 2026 15:37:46 +1000 Subject: [PATCH 59/60] motion, interp: a tool offset change under a tilt keeps the joints A module that applies the tool length itself reads it from motion, so a G43 with the head tilted moves the programmed point, not the joints; motion, task and the interpreter each did something else. Motion re-reads the point from the joints under the new tool, as after a kinematics switch, takes the external offsets off, moves the planner onto it and latches the joint hold there. The interpreter predicts the same point through the loader, from the joints the machine stands on, or for a G43.4 that switches on the same line from the joints read in the old kinematics before the switch, evaluating with the tool offset the program is under rather than the one motion has reached; the point rides along with the offset on EMC_TRAJ_SET_OFFSET, and motion aborts when its own answer differs. Under the identity, and where the module cannot be evaluated, the interpreter shifts by the offset difference as before and motion aborts only if the point moved. Task issues the offset once the queue has drained, so no queue busting is needed. tests/kins-switch applies G43.4 and a zero length in a tilted pose and after a point-to-point move: the joints hold, the point moves by the tilt term, and a move of nothing from the interpreter's point goes nowhere. --- docs/src/gcode/g-code.adoc | 13 ++++ docs/src/man/man9/kins.9.adoc | 16 +++-- docs/src/motion/switchkins.adoc | 9 ++- lib/python/rs274/interpret.py | 3 + src/emc/motion/command.c | 10 ++- src/emc/motion/control.c | 95 ++++++++++++++++++++++---- src/emc/motion/mot_priv.h | 2 + src/emc/motion/motion.h | 4 ++ src/emc/nml_intf/canon.hh | 9 +++ src/emc/nml_intf/emc.cc | 2 + src/emc/nml_intf/emc.hh | 2 +- src/emc/nml_intf/emc_nml.hh | 6 +- src/emc/rs274ngc/canonmodule.cc | 2 +- src/emc/rs274ngc/gcodemodule.cc | 23 +++++++ src/emc/rs274ngc/interp_convert.cc | 99 ++++++++++++++++++++-------- src/emc/rs274ngc/interp_workplane.cc | 92 +++++++++++++++++++++++--- src/emc/rs274ngc/rs274ngc_interp.hh | 5 ++ src/emc/sai/saicanon.cc | 10 +++ src/emc/task/emccanon.cc | 24 ++++++- src/emc/task/emctaskmain.cc | 9 ++- src/emc/task/taskintf.cc | 6 +- tests/kins-switch/README | 6 +- tests/kins-switch/test-ui.py | 68 ++++++++++++++----- 23 files changed, 435 insertions(+), 80 deletions(-) diff --git a/docs/src/gcode/g-code.adoc b/docs/src/gcode/g-code.adoc index 0a966aba025..e210fa582db 100644 --- a/docs/src/gcode/g-code.adoc +++ b/docs/src/gcode/g-code.adoc @@ -1541,6 +1541,19 @@ in the numbered parameters '5081-5089'. The numbered parameters '5401-5409' instead hold the loaded tool's stored offset, refreshed on tool change ('M6') and 'G10 L1'/'L10'/'L11'. +Some kinematics modules apply the tool length themselves, along the tool +axis rather than along Z (5axiskins, maxkins, the trt modules, genhexkins, +pentakins; see the <> +chapter). 'G43' does not move the machine under those either. With the +tool axis tilted at the time, the programmed coordinates of the point the +tool stands on change by more than the offset difference: the interpreter +works them out through the kinematics, so the next move starts from where +the tool tip actually is. Motion reads the point from the joints the same +way and stops the program if the two disagree. A module that can only be +evaluated in realtime leaves the interpreter with the offset difference +alone; under one of those, change the tool length with the tool axis +untilted, motion stops the program otherwise. + [NOTE] 'G43 H0' is a little special. Its behavior is different on random tool changer machines and nonrandom tool changer machines (see the diff --git a/docs/src/man/man9/kins.9.adoc b/docs/src/man/man9/kins.9.adoc index 79a3a116e21..01cef58cc73 100644 --- a/docs/src/man/man9/kins.9.adoc +++ b/docs/src/man/man9/kins.9.adoc @@ -207,7 +207,9 @@ documentation for more info) *genhexkins.tool-offset*:: TCP offset from platform origin along Z to implement RTCP function. - To avoid joints jump change tool offset only when the platform is not tilted. + Motion hands the module the offset in effect (G43, G49), so the pin needs + no connection. A tool offset change with the platform tilted moves the + programmed point, not the joints. === genserkins - generalized serial kinematics @@ -254,8 +256,8 @@ replacing it. Put a given length in one column or the other, not both. Tool length, applied along the tool rather than along Z, so that the tip stays on the programmed point as B tilts. Motion hands the module the offset in effect (G43, G49), so the pin needs no connection; it is read - only until motion has sent anything. To avoid a joint jump, change the - tool offset only when B is 0. + only until motion has sent anything. A tool offset change with B tilted + moves the programmed point, not the joints. === pentakins - Pentapod Kinematics @@ -291,7 +293,9 @@ The forward kinematics iteration is controlled by HAL pins. *pentakins.tool-offset*:: TCP offset from effector origin along Z to implement RTCP function. - To avoid joints jump change tool offset only when the platform is not tilted. + Motion hands the module the offset in effect (G43, G49), so the pin needs + no connection. A tool offset change with the platform tilted moves the + programmed point, not the joints. === pumakins - kinematics for puma typed robots @@ -416,8 +420,8 @@ expected by it (XYZBCW `->` joints 0..5) Tool length, applied along the tool rather than along Z, so that the tip stays on the programmed point as B and C move. Motion hands the module the offset in effect (G43, G49), so the pin needs no connection; it is read - only until motion has sent anything. To avoid a joint jump, change the - tool offset only when B is 0. A tool length in the W column of the tool + only until motion has sent anything. A tool offset change with B tilted + moves the programmed point, not the joints. A tool length in the W column of the tool table reaches the same place, once a block commands W, and adds to this one rather than replacing it. Put a given length in one column or the other, not both. diff --git a/docs/src/motion/switchkins.adoc b/docs/src/motion/switchkins.adoc index ed4d911f6f0..8cb7fe679b8 100644 --- a/docs/src/motion/switchkins.adoc +++ b/docs/src/motion/switchkins.adoc @@ -606,8 +606,13 @@ exports kinematicsSetTool() as well, through which motion hands it the tool offset in effect whenever that changes. A table entry flagged as the tool is overwritten with it, and the entry's pin only matters until motion has sent anything, so a config need not net -motion.tooloffset.z to the module. A kinstype registered the older -way reads its own pins and is not affected. +motion.tooloffset.z to the module. When the offset changes, motion +keeps the joints where they are and reads the point back from them +under the new offset, so a tool length change with the tool axis +tilted moves the programmed point rather than the machine; the +interpreter reads the point the same way through the non-realtime +loader, and motion stops the program if the two disagree. A kinstype +registered the older way reads its own pins and is not affected. === Module main program diff --git a/lib/python/rs274/interpret.py b/lib/python/rs274/interpret.py index 0662adc90bc..055909ade05 100644 --- a/lib/python/rs274/interpret.py +++ b/lib/python/rs274/interpret.py @@ -196,5 +196,8 @@ def get_block_delete(self): def get_external_joint_positions(self): return tuple(self.s.joint_actual_position[:self.s.joints]) + def get_kinematics_type(self): + return self.s.kinematics_type + # vim:ts=8:sts=4:et: diff --git a/src/emc/motion/command.c b/src/emc/motion/command.c index 4b2210520eb..316e3176e4f 100644 --- a/src/emc/motion/command.c +++ b/src/emc/motion/command.c @@ -2212,10 +2212,16 @@ void emcmotCommandHandler_locked(void *arg, long servo_period) case EMCMOT_SET_OFFSET: rtapi_print_msg(RTAPI_MSG_DBG, "SET_OFFSET"); - emcmotStatus->tool_offset = emcmotCommand->tool_offset; if (kinematicsSetTool) { - kinematicsSetTool(&emcmotStatus->tool_offset); + /* the module applies the offset, so the point the joints + stand on changes with it */ + kinematicsSetTool(&emcmotCommand->tool_offset); + emcmotToolOffsetChanged(&emcmotStatus->tool_offset, + &emcmotCommand->tool_offset, + &emcmotCommand->pos, + emcmotCommand->have_point); } + emcmotStatus->tool_offset = emcmotCommand->tool_offset; break; case EMCMOT_SET_AXIS_POSITION_LIMITS: diff --git a/src/emc/motion/control.c b/src/emc/motion/control.c index a609cf1a367..05ce75a63d5 100644 --- a/src/emc/motion/control.c +++ b/src/emc/motion/control.c @@ -194,6 +194,7 @@ static void output_to_hal(void); static void update_status(void); static void handle_kinematicsSwitch(void); +static int reanchor_pose(const double *joint_pos, EmcPose *at); /*********************************************************************** * PUBLIC FUNCTION CODE * @@ -363,8 +364,6 @@ static void handle_kinematicsSwitch(void) { return; // the kinematics in force is unchanged } - KINEMATICS_FORWARD_FLAGS tmpFFlags = fflags; - KINEMATICS_INVERSE_FLAGS tmpIFlags = iflags; #ifdef SWITCHKINS_DEBUG double beforePose[EMCMOT_MAX_AXIS]; int anum; @@ -376,9 +375,7 @@ static void handle_kinematicsSwitch(void) { solve them is one the machine cannot run in from here: put the old one back, or the inverse would run the joints to wherever the pose we know lands in the new one */ - EmcPose poseKinsSwitch = emcmotStatus->carte_pos_cmd; - if (kinematicsForward(joint_posKinsSwitch, &poseKinsSwitch, - &tmpFFlags, &tmpIFlags)) { + if (reanchor_pose(joint_posKinsSwitch, NULL) != 0) { kinematicsSwitch(switchkins_type); reportError(_("kinematicsForward failed for kinematics type %d," " type %d is still in force"), @@ -386,7 +383,6 @@ static void handle_kinematicsSwitch(void) { SET_MOTION_ERROR_FLAG(1); // abort return; // the kinematics in force and the position are unchanged } - emcmotStatus->carte_pos_cmd = poseKinsSwitch; switchkins_type = requested_type; hal_set_real(emcmot_hal_data->kins_type, (double)switchkins_type); @@ -398,9 +394,29 @@ static void handle_kinematicsSwitch(void) { ,anum,beforePose[anum],*pcmd_p[anum],*pcmd_p[anum]-beforePose[anum]); } #endif +} //handle_kinematicsSwitch() + +/* The point re-read from the joints, for when what the joints mean has + changed without the joints moving: a kinematics switch, or a tool + offset the module applies. The pose they put the tool at is the new + commanded point, the external offsets taken off it, and the planner + is moved onto it. A forward that fails leaves the point alone. + The pose with the external offsets still on it is returned in at. */ +static int reanchor_pose(const double *joint_pos, EmcPose *at) +{ + KINEMATICS_FORWARD_FLAGS tmpFFlags = fflags; + KINEMATICS_INVERSE_FLAGS tmpIFlags = iflags; + EmcPose pose = emcmotStatus->carte_pos_cmd; + + if (kinematicsForward(joint_pos, &pose, &tmpFFlags, &tmpIFlags) != 0) { + return -1; + } + emcmotStatus->carte_pos_cmd = pose; + if (at) { *at = pose; } axis_apply_ext_offsets_to_carte_pos(-1, pcmd_p); tpSetPos(&emcmotInternal->coord_tp, &emcmotStatus->carte_pos_cmd); -} //handle_kinematicsSwitch() + return 0; +} static void process_inputs(void) { @@ -937,17 +953,74 @@ static int joint_hold_valid = 0; static double joint_hold[EMCMOT_MAX_JOINTS]; static EmcPose joint_hold_pose; -/* whether two machine points are the same, to a hair either way */ -static int same_carte_pos(const EmcPose *a, const EmcPose *b) +/* whether two machine points are within tol of each other on every axis */ +static int carte_pos_within(const EmcPose *a, const EmcPose *b, double tol) { - const double tol = 1e-9; - return fabs(a->tran.x - b->tran.x) < tol && fabs(a->tran.y - b->tran.y) < tol && fabs(a->tran.z - b->tran.z) < tol && fabs(a->a - b->a) < tol && fabs(a->b - b->b) < tol && fabs(a->c - b->c) < tol && fabs(a->u - b->u) < tol && fabs(a->v - b->v) < tol && fabs(a->w - b->w) < tol; } +/* whether two machine points are the same, to a hair either way */ +static int same_carte_pos(const EmcPose *a, const EmcPose *b) +{ + return carte_pos_within(a, b, 1e-9); +} + +/* A tool offset the module applies has changed under the point. The + machine stays where it is: the point is re-read from the joints under + the new offset, and the joints are held there, since the inverse of the + re-read point may answer with another joint set. The interpreter + works out the same point ahead of motion and sends it along when it + can evaluate the kinematics; where the two disagree, or where it could + not say and the point moved, the program is not let go on from a point + the interpreter does not have. Nothing to do outside coordinated mode: + the point follows the joints there anyway. */ +void emcmotToolOffsetChanged(const EmcPose *from, const EmcPose *to, + const EmcPose *expected, int have_expected) +{ + const double tol = 1e-4; + double joint_pos[EMCMOT_MAX_JOINTS] = {0,}; + EmcPose was = emcmotStatus->carte_pos_cmd; + EmcPose now; + int joint_num; + + if (same_carte_pos(from, to) || !GET_MOTION_COORD_FLAG()) { return; } + for (joint_num = 0; joint_num < emcmotConfig->numJoints; joint_num++) { + joint_pos[joint_num] = joints[joint_num].coarse_pos; + } + if (reanchor_pose(joint_pos, &now) != 0) { + reportError(_("the kinematics cannot place the tool from the joints" + " after the tool offset change")); + SET_MOTION_ERROR_FLAG(1); + return; + } + for (joint_num = 0; joint_num < EMCMOT_MAX_JOINTS; joint_num++) { + joint_hold[joint_num] = joint_pos[joint_num]; + } + joint_hold_pose = now; + joint_hold_valid = 1; + + if (have_expected) { + if (!carte_pos_within(&emcmotStatus->carte_pos_cmd, expected, tol)) { + reportError(_("the tool offset change put the point at" + " %.4f %.4f %.4f, the interpreter expected" + " %.4f %.4f %.4f"), + emcmotStatus->carte_pos_cmd.tran.x, + emcmotStatus->carte_pos_cmd.tran.y, + emcmotStatus->carte_pos_cmd.tran.z, + expected->tran.x, expected->tran.y, expected->tran.z); + SET_MOTION_ERROR_FLAG(1); + } + } else if (!carte_pos_within(&was, &now, tol)) { + reportError(_("the tool offset change moved the point under a" + " kinematics the interpreter cannot evaluate;" + " change the tool offset with the machine untilted")); + SET_MOTION_ERROR_FLAG(1); + } +} + static void set_operating_mode(void) { int joint_num; diff --git a/src/emc/motion/mot_priv.h b/src/emc/motion/mot_priv.h index a996e183fa6..847fa57aa38 100644 --- a/src/emc/motion/mot_priv.h +++ b/src/emc/motion/mot_priv.h @@ -271,6 +271,8 @@ extern void refresh_jog_limits(emcmot_joint_t *joint,int joint_num); extern void clearHomes(int joint_num); extern void emcmot_config_change(void); +extern void emcmotToolOffsetChanged(const EmcPose *from, const EmcPose *to, + const EmcPose *expected, int have_expected); extern void reportError(const char *fmt, ...) __attribute__((format(printf,1,2))); /* Use the rtapi_print call */ diff --git a/src/emc/motion/motion.h b/src/emc/motion/motion.h index 9525f91d4d6..091e420e10b 100644 --- a/src/emc/motion/motion.h +++ b/src/emc/motion/motion.h @@ -281,6 +281,10 @@ extern "C" { double joint_target[EMCMOT_MAX_JOINTS]; int have_joint_target; double joint_seconds; /* 0 for a rapid, else the time the move is to take */ + + /* SET_OFFSET: pos is where the interpreter expects the point to be + once the offset is on, for motion to check its own answer against */ + int have_point; } emcmot_command_t; /*! \todo FIXME - these packed bits might be replaced with chars diff --git a/src/emc/nml_intf/canon.hh b/src/emc/nml_intf/canon.hh index 7f25a53b209..0e9afe6679e 100644 --- a/src/emc/nml_intf/canon.hh +++ b/src/emc/nml_intf/canon.hh @@ -691,6 +691,11 @@ extern void USE_NO_SPINDLE_FORCE(); extern void SET_TOOL_TABLE_ENTRY(int pocket, int toolno, const EmcPose& offset, double diameter, double frontangle, double backangle, int orientation); extern void USE_TOOL_LENGTH_OFFSET(const EmcPose& offset); +/* The same, with the point the interpreter expects the machine to stand + on once the offset is applied, in program coordinates: where a + kinematics applies the offset itself, motion keeps the joints and + re-reads the point from them, and compares it with this one. */ +extern void USE_TOOL_LENGTH_OFFSET(const EmcPose& offset, const EmcPose& point); extern void CHANGE_TOOL(); @@ -945,6 +950,10 @@ extern int GET_EXTERNAL_KINS_TYPE(); kinematics.h); -1 where it says nothing: no such type, plain kinematics, or no motion controller attached (sai, preview) */ extern int GET_EXTERNAL_KINS_TYPE_FLAGS(int ktype); +/* whether the machine's kinematics is the identity, the joints being the + world, so that no tool offset is applied by the kinematics; true where + no motion controller is attached (sai, a preview without a machine) */ +extern bool GET_EXTERNAL_KINEMATICS_IDENTITY(); // Returns the current motion path-following tolerance extern double GET_EXTERNAL_MOTION_CONTROL_TOLERANCE(); diff --git a/src/emc/nml_intf/emc.cc b/src/emc/nml_intf/emc.cc index cc65795be53..82059df40c6 100644 --- a/src/emc/nml_intf/emc.cc +++ b/src/emc/nml_intf/emc.cc @@ -1607,6 +1607,8 @@ void EMC_TRAJ_SET_OFFSET::update(CMS * cms) { EMC_TRAJ_CMD_MSG::update(cms); EmcPose_update(cms, &offset); + EmcPose_update(cms, &point); + cms->update(have_point); } // cppcheck-suppress duplInheritedMember diff --git a/src/emc/nml_intf/emc.hh b/src/emc/nml_intf/emc.hh index e81ba91d4aa..9da8f5fdfe2 100644 --- a/src/emc/nml_intf/emc.hh +++ b/src/emc/nml_intf/emc.hh @@ -381,7 +381,7 @@ extern int emcTrajCircularMove(const EmcPose& end, const PM_CARTESIAN& center, c normal, int turn, int type, double vel, double ini_maxvel, double acc, double ini_maxjerk); extern int emcTrajSetTermCond(int cond, double tolerance); extern int emcTrajSetSpindleSync(int spindle, double feed_per_revolution, bool wait_for_index); -extern int emcTrajSetOffset(const EmcPose& tool_offset); +extern int emcTrajSetOffset(const EmcPose& tool_offset, const EmcPose *point); extern int emcTrajSetHome(const EmcPose& home); extern int emcTrajClearProbeTrippedFlag(); extern int emcTrajProbe(const EmcPose& pos, int type, double vel, diff --git a/src/emc/nml_intf/emc_nml.hh b/src/emc/nml_intf/emc_nml.hh index adc038f1068..c7feda8a7e6 100644 --- a/src/emc/nml_intf/emc_nml.hh +++ b/src/emc/nml_intf/emc_nml.hh @@ -832,7 +832,7 @@ class EMC_TRAJ_SET_OFFSET:public EMC_TRAJ_CMD_MSG { public: EMC_TRAJ_SET_OFFSET() : EMC_TRAJ_CMD_MSG(EMC_TRAJ_SET_OFFSET_TYPE, sizeof(EMC_TRAJ_SET_OFFSET)), - offset{} + offset{}, point{}, have_point(0) {}; // Sub-class update() calls base-class update() @@ -841,6 +841,10 @@ class EMC_TRAJ_SET_OFFSET:public EMC_TRAJ_CMD_MSG { void update(CMS * cms); EmcPose offset; + // where the interpreter expects the machine to stand once the offset + // is on, for motion to check its own answer against; only when set + EmcPose point; + int have_point; }; class EMC_TRAJ_SET_G5X:public EMC_TRAJ_CMD_MSG { diff --git a/src/emc/rs274ngc/canonmodule.cc b/src/emc/rs274ngc/canonmodule.cc index 002be0c8de9..447001c9efc 100644 --- a/src/emc/rs274ngc/canonmodule.cc +++ b/src/emc/rs274ngc/canonmodule.cc @@ -247,7 +247,7 @@ BOOST_PYTHON_MODULE(emccanon) { def("USE_NO_SPINDLE_FORCE",&USE_NO_SPINDLE_FORCE); // def("USER_DEFINED_FUNCTION_ADD",&USER_DEFINED_FUNCTION_ADD); // def("USE_SPINDLE_FORCE",&USE_SPINDLE_FORCE); - def("USE_TOOL_LENGTH_OFFSET",&USE_TOOL_LENGTH_OFFSET); + def("USE_TOOL_LENGTH_OFFSET",static_cast(&USE_TOOL_LENGTH_OFFSET)); def("WAIT",&WAIT); // from interp_queue.cc diff --git a/src/emc/rs274ngc/gcodemodule.cc b/src/emc/rs274ngc/gcodemodule.cc index f2289e97919..8b6d596b9be 100644 --- a/src/emc/rs274ngc/gcodemodule.cc +++ b/src/emc/rs274ngc/gcodemodule.cc @@ -49,6 +49,7 @@ #include "rs274ngc_interp.hh" #include "nml_intf/interp_return.hh" #include "nml_intf/canon.hh" +#include // KINEMATICS_IDENTITY int _task = 0; // control preview behaviour when remapping @@ -901,6 +902,10 @@ void USE_TOOL_LENGTH_OFFSET(const EmcPose& offset) { Py_XDECREF(result); } +void USE_TOOL_LENGTH_OFFSET(const EmcPose& offset, const EmcPose& /*point*/) { + USE_TOOL_LENGTH_OFFSET(offset); +} + void SET_FEED_REFERENCE(double /*reference*/) { } void SET_CUTTER_RADIUS_COMPENSATION(double /*radius*/) {} void START_CUTTER_RADIUS_COMPENSATION(int /*direction*/) {} @@ -1264,6 +1269,24 @@ void SET_MOTION_CONTROL_MODE(CANON_MOTION_MODE mode) { motion_mode = mode; } CANON_MOTION_MODE GET_EXTERNAL_MOTION_CONTROL_MODE() { return motion_mode; } int GET_EXTERNAL_KINS_TYPE() { return 0; } int GET_EXTERNAL_KINS_TYPE_FLAGS(int ktype) { (void)ktype; return -1; } + +// the kind of transform the machine runs, from a canon that watches the +// status buffer; one that cannot answer has no machine, and the joints +// are the world +bool GET_EXTERNAL_KINEMATICS_IDENTITY() { + PyObject *result; + bool identity = true; + + if(interp_error) return true; + if(!PyObject_HasAttrString(callback, "get_kinematics_type")) return true; + result = callmethod(callback, "get_kinematics_type", ""); + if(result == NULL) { PyErr_Clear(); return true; } + if(PyLong_Check(result)) { + identity = (PyLong_AsLong(result) == KINEMATICS_IDENTITY); + } + Py_DECREF(result); + return identity; +} void SET_NAIVECAM_TOLERANCE(double /*tolerance*/) { } #define RESULT_OK (result == INTERP_OK || result == INTERP_EXECUTE_FINISH) diff --git a/src/emc/rs274ngc/interp_convert.cc b/src/emc/rs274ngc/interp_convert.cc index 918de7b95e0..a983a83bd7d 100644 --- a/src/emc/rs274ngc/interp_convert.cc +++ b/src/emc/rs274ngc/interp_convert.cc @@ -6517,6 +6517,8 @@ int Interp::convert_tool_length_offset(int g_code, //!< g_code being execu { int idx; EmcPose tool_offset; + double standing[EMCMOT_MAX_JOINTS]; + bool have_standing = false; ZERO_EMC_POSE(tool_offset); settings->g43_with_zero_offset = 0; @@ -6528,7 +6530,18 @@ int Interp::convert_tool_length_offset(int g_code, //!< g_code being execu // apply the offset, as if the switch line had run and drained. With // no kinematics attached there is nothing to switch to. CHKS(primary < 0 && kins_type_info_available(), NCE_NO_PRIMARY_KINEMATICS_TYPE); - if (primary >= 0) { switch_kins_type(primary, settings); } + if (primary >= 0 && primary != settings->kins_type) { + // the switch keeps the joints and moves the point, so the point + // the offset is read from is not the one the program is at: take + // the joints, while the kinematics they are known in is in force + void *vctx; + CHP(kins_here(settings, &vctx)); + if (vctx) { + CHP(current_joints(settings, vctx, standing)); + have_standing = true; + } + switch_kins_type(primary, settings); + } settings->kins_by_g43_4 = true; } else if (g_code != G_49) { // the offset in effect is no longer G43.4's, so G49 has no switch to undo @@ -6618,31 +6631,65 @@ int Interp::convert_tool_length_offset(int g_code, //!< g_code being execu } else { ERS("BUG: Code not G43, G43.1, G43.2, G43.4, or G49"); } - USE_TOOL_LENGTH_OFFSET(tool_offset); - - double dx, dy, dz; - - // the tool does not move, so its program coordinates change by the - // offset difference seen from the program: the XY rotation and the - // tilted work plane taken off it - dx = settings->tool_offset.tran.x - tool_offset.tran.x; - dy = settings->tool_offset.tran.y - tool_offset.tran.y; - dz = settings->tool_offset.tran.z - tool_offset.tran.z; - - rotate(&dx, &dy, -settings->rotation_xy); - g68_unrotate(settings, &dx, &dy, &dz); - - settings->current_x += dx; - settings->current_y += dy; - settings->current_z += dz; - settings->AA_current += settings->tool_offset.a - tool_offset.a; - settings->BB_current += settings->tool_offset.b - tool_offset.b; - settings->CC_current += settings->tool_offset.c - tool_offset.c; - settings->u_current += settings->tool_offset.u - tool_offset.u; - settings->v_current += settings->tool_offset.v - tool_offset.v; - settings->w_current += settings->tool_offset.w - tool_offset.w; - - settings->tool_offset = tool_offset; + // The machine does not move, so the program coordinates of the point change. + // A kinematics that applies the offset itself moves the point by more than + // the offset difference, along the tilted tool axis: evaluate it here as + // motion will, and send the point along for motion to check against. + EmcPose point; + bool point_known = false; + CHP(tool_offset_point(settings, &tool_offset, have_standing ? standing : NULL, + &point, &point_known)); + if (point_known) { + double prog[9]; + EmcPose in_program; + + settings->tool_offset = tool_offset; + machine_pose_to_program(settings, &point, prog); + settings->current_x = prog[0]; + settings->current_y = prog[1]; + settings->current_z = prog[2]; + settings->AA_current = prog[3]; + settings->BB_current = prog[4]; + settings->CC_current = prog[5]; + settings->u_current = prog[6]; + settings->v_current = prog[7]; + settings->w_current = prog[8]; + in_program.tran.x = USER_TO_PROGRAM_LEN(point.tran.x); + in_program.tran.y = USER_TO_PROGRAM_LEN(point.tran.y); + in_program.tran.z = USER_TO_PROGRAM_LEN(point.tran.z); + in_program.a = USER_TO_PROGRAM_ANG(point.a); + in_program.b = USER_TO_PROGRAM_ANG(point.b); + in_program.c = USER_TO_PROGRAM_ANG(point.c); + in_program.u = USER_TO_PROGRAM_LEN(point.u); + in_program.v = USER_TO_PROGRAM_LEN(point.v); + in_program.w = USER_TO_PROGRAM_LEN(point.w); + USE_TOOL_LENGTH_OFFSET(tool_offset, in_program); + } else { + double dx, dy, dz; + + USE_TOOL_LENGTH_OFFSET(tool_offset); + + // by the offset difference seen from the program: the XY rotation + // and the tilted work plane taken off it + dx = settings->tool_offset.tran.x - tool_offset.tran.x; + dy = settings->tool_offset.tran.y - tool_offset.tran.y; + dz = settings->tool_offset.tran.z - tool_offset.tran.z; + + rotate(&dx, &dy, -settings->rotation_xy); + g68_unrotate(settings, &dx, &dy, &dz); + + settings->current_x += dx; + settings->current_y += dy; + settings->current_z += dz; + settings->AA_current += settings->tool_offset.a - tool_offset.a; + settings->BB_current += settings->tool_offset.b - tool_offset.b; + settings->CC_current += settings->tool_offset.c - tool_offset.c; + settings->u_current += settings->tool_offset.u - tool_offset.u; + settings->v_current += settings->tool_offset.v - tool_offset.v; + settings->w_current += settings->tool_offset.w - tool_offset.w; + + settings->tool_offset = tool_offset; + } // Update parameters #5081-#5089 to reflect the tool length offset // actually applied to motion (covers G43, G43Hn with n != loaded tool, diff --git a/src/emc/rs274ngc/interp_workplane.cc b/src/emc/rs274ngc/interp_workplane.cc index 4172d8bf914..b754922bc24 100644 --- a/src/emc/rs274ngc/interp_workplane.cc +++ b/src/emc/rs274ngc/interp_workplane.cc @@ -441,11 +441,11 @@ int Interp::convert_work_plane(int g_code, block_pointer block, setup_pointer s) } //---------------------------------------------------------------------- -// The kinematics. G68.3 and the orientation moves need the frames and -// the tool frame inverse of the module motion runs, evaluated here, ahead -// of motion, through the loader in kinematics_userspace/. The loader -// binds its pins to a HAL component, so the interpreter makes one, named -// by its process, the first time it is asked. +// The kinematics. G68.3, the orientation moves and a tool offset change +// need the module motion runs, evaluated here, ahead of motion, through +// the loader in kinematics_userspace/. The loader reads the module's +// pins through HAL, so the interpreter connects as a component, named by +// its process, the first time it is asked. //---------------------------------------------------------------------- #include @@ -456,12 +456,11 @@ int Interp::convert_work_plane(int g_code, block_pointer block, setup_pointer s) #define KINS_CTX(s) ((KinematicsUserContext *)(s)->kins_ctx) -// the loaded module, on the kinematics type the program is in -int Interp::kins_context(setup_pointer s, void **out) +// the module loaded, once +int Interp::kins_load(setup_pointer s) { KinematicsUserContext *ctx; - *out = NULL; if (!s->kins_ctx) { char name[HAL_NAME_LEN + 1]; int comp; @@ -481,15 +480,46 @@ int Interp::kins_context(setup_pointer s, void **out) s->kins_ctx = ctx; for (int i = 0; i < EMCMOT_MAX_JOINTS; i++) { s->kins_seed[i] = 0.0; } } + return INTERP_OK; +} + +// the loaded module, on the kinematics type the program is in +int Interp::kins_context(setup_pointer s, void **out) +{ + KinematicsUserContext *ctx; + + *out = NULL; + CHP(kins_load(s)); ctx = KINS_CTX(s); CHKS((kinematicsUserIsRtOnly(ctx)), _("kinematics module %s cannot be evaluated outside realtime"), s->kins_module); CHKS((kinematicsUserSetType(ctx, s->kins_type) != 0), _("kinematics type %d is not available outside realtime"), s->kins_type); + // with the tool offset the program is under here, not the one motion + // has reached: the interpreter runs ahead of motion + kins_set_tool(ctx, &s->tool_offset); *out = ctx; return INTERP_OK; } +// the offset the module evaluates with, given in program units like the +// interpreter keeps it +void Interp::kins_set_tool(void *vctx, const EmcPose *offset) +{ + EmcPose tool; + + tool.tran.x = PROGRAM_TO_USER_LEN(offset->tran.x); + tool.tran.y = PROGRAM_TO_USER_LEN(offset->tran.y); + tool.tran.z = PROGRAM_TO_USER_LEN(offset->tran.z); + tool.a = PROGRAM_TO_USER_ANG(offset->a); + tool.b = PROGRAM_TO_USER_ANG(offset->b); + tool.c = PROGRAM_TO_USER_ANG(offset->c); + tool.u = PROGRAM_TO_USER_LEN(offset->u); + tool.v = PROGRAM_TO_USER_LEN(offset->v); + tool.w = PROGRAM_TO_USER_LEN(offset->w); + kinematicsUserSetTool((KinematicsUserContext *)vctx, &tool); +} + void Interp::kins_release(setup_pointer s) { if (s->kins_ctx) { @@ -584,6 +614,52 @@ int Interp::current_joints(setup_pointer s, void *vctx, double *joints) return INTERP_OK; } +// The kinematics as far as it can be evaluated here: the context on the +// type the program is in, or NULL where there is nothing to evaluate, +// with no machine attached (sai, a preview without one) or a module that +// runs only in realtime. +int Interp::kins_here(setup_pointer s, void **out) +{ + *out = NULL; + if (GET_EXTERNAL_KINEMATICS_IDENTITY()) { return INTERP_OK; } + CHP(kins_load(s)); + if (kinematicsUserIsRtOnly(KINS_CTX(s))) { return INTERP_OK; } + return kins_context(s, out); +} + +// Where the point goes when the tool offset changes: the joints stay, so it is +// read back from them under the new offset, as motion reads it. known stays +// false under the identity and where nothing can be evaluated; the caller +// then shifts by the offset difference. +int Interp::tool_offset_point(setup_pointer s, const EmcPose *offset, const double *standing, + EmcPose *point, bool *known) +{ + void *vctx; + KinematicsUserContext *ctx; + double joints[EMCMOT_MAX_JOINTS]; + int flags, i; + + *known = false; + if (same_pose(offset, &s->tool_offset)) { return INTERP_OK; } + flags = GET_EXTERNAL_KINS_TYPE_FLAGS(s->kins_type); + if (flags >= 0 && (flags & KINSTYPE_IDENTITY)) { return INTERP_OK; } + CHP(kins_here(s, &vctx)); + ctx = (KinematicsUserContext *)vctx; + if (!ctx || kinematicsUserIsIdentity(ctx)) { return INTERP_OK; } + if (standing) { + for (i = 0; i < EMCMOT_MAX_JOINTS; i++) { joints[i] = standing[i]; } + } else { + CHP(current_joints(s, ctx, joints)); + } + kins_set_tool(ctx, offset); + CHKS((kinematicsUserForward(ctx, joints, point) != 0), + _("the kinematics cannot place the tool from the joints after the tool offset change")); + // the machine stays on these joints + for (i = 0; i < EMCMOT_MAX_JOINTS; i++) { s->kins_seed[i] = joints[i]; } + *known = true; + return INTERP_OK; +} + // a direction of the plane in world coordinates: the plane's rotation // then the XY rotation of the coordinate system it sits on static void plane_axis_in_world(setup_pointer s, int column, double rotation_xy, PmCartesian *out) diff --git a/src/emc/rs274ngc/rs274ngc_interp.hh b/src/emc/rs274ngc/rs274ngc_interp.hh index 795d8d718f2..f60b4078064 100644 --- a/src/emc/rs274ngc/rs274ngc_interp.hh +++ b/src/emc/rs274ngc/rs274ngc_interp.hh @@ -375,7 +375,12 @@ public: int ptp_seconds(block_pointer block, setup_pointer settings, double x, double y, double z, double a, double b, double c, double u, double v, double w, double *seconds); + int kins_load(setup_pointer settings); int kins_context(setup_pointer settings, void **ctx); + int kins_here(setup_pointer settings, void **ctx); + int tool_offset_point(setup_pointer settings, const EmcPose *offset, const double *standing, + EmcPose *point, bool *known); + void kins_set_tool(void *ctx, const EmcPose *offset); void kins_release(setup_pointer settings); void current_machine_pose(setup_pointer settings, EmcPose *pose); void machine_pose_to_program(setup_pointer settings, const EmcPose *pose, double prog[9]); diff --git a/src/emc/sai/saicanon.cc b/src/emc/sai/saicanon.cc index 784d9377658..ad6bb3d4e74 100644 --- a/src/emc/sai/saicanon.cc +++ b/src/emc/sai/saicanon.cc @@ -602,6 +602,11 @@ void USE_TOOL_LENGTH_OFFSET(const EmcPose& offset) offset.tran.x, offset.tran.y, offset.tran.z, offset.a, offset.b, offset.c, offset.u, offset.v, offset.w); } +void USE_TOOL_LENGTH_OFFSET(const EmcPose& offset, const EmcPose& /*point*/) +{ + USE_TOOL_LENGTH_OFFSET(offset); +} + void CHANGE_TOOL() { PRINT("CHANGE_TOOL()\n"); @@ -844,6 +849,11 @@ extern int GET_EXTERNAL_KINS_TYPE_FLAGS(int ktype) return -1; } +extern bool GET_EXTERNAL_KINEMATICS_IDENTITY() +{ + return true; +} + extern void SET_PARAMETER_FILE_NAME(const char *name) { strncpy(_parameter_file_name, name, PARAMETER_FILE_NAME_LENGTH - 1); diff --git a/src/emc/task/emccanon.cc b/src/emc/task/emccanon.cc index fa70ec82036..39228bee0e0 100644 --- a/src/emc/task/emccanon.cc +++ b/src/emc/task/emccanon.cc @@ -3213,7 +3213,7 @@ void SET_TOOL_TABLE_ENTRY(int pocket, int toolno, const EmcPose& offset, double EMC has no tool length offset. To implement it, we save it here, and apply it when necessary */ -void USE_TOOL_LENGTH_OFFSET(const EmcPose& offset) +static void use_tool_length_offset(const EmcPose& offset, const EmcPose *point) { auto set_offset_msg = std::make_unique(); @@ -3242,6 +3242,13 @@ void USE_TOOL_LENGTH_OFFSET(const EmcPose& offset) set_offset_msg->offset.v = TO_EXT_LEN(canon.toolOffset.v); set_offset_msg->offset.w = TO_EXT_LEN(canon.toolOffset.w); + set_offset_msg->have_point = (point != nullptr); + if (point) { + CANON_POSITION at(*point); + from_prog(at); + set_offset_msg->point = to_ext_pose(at); + } + for (int s = 0; s < emcStatus->motion.traj.spindles; s++){ if(canon.spindle[s].css_maximum) { SET_SPINDLE_SPEED(s, canon.spindle[s].speed); @@ -3250,6 +3257,16 @@ void USE_TOOL_LENGTH_OFFSET(const EmcPose& offset) interp_list.append(std::move(set_offset_msg)); } +void USE_TOOL_LENGTH_OFFSET(const EmcPose& offset) +{ + use_tool_length_offset(offset, nullptr); +} + +void USE_TOOL_LENGTH_OFFSET(const EmcPose& offset, const EmcPose& point) +{ + use_tool_length_offset(offset, &point); +} + /* CHANGE_TOOL results from M6 */ void CHANGE_TOOL() { @@ -4223,6 +4240,11 @@ int GET_EXTERNAL_KINS_TYPE_FLAGS(int ktype) return emcStatus->motion.traj.switchkins_flags[ktype]; } +bool GET_EXTERNAL_KINEMATICS_IDENTITY() +{ + return emcStatus->motion.traj.kinematics_type == KINEMATICS_IDENTITY; +} + double GET_EXTERNAL_MOTION_CONTROL_TOLERANCE() { return TO_PROG_LEN(canon.motionTolerance); diff --git a/src/emc/task/emctaskmain.cc b/src/emc/task/emctaskmain.cc index 2709fd0d2cf..28ca94c953e 100644 --- a/src/emc/task/emctaskmain.cc +++ b/src/emc/task/emctaskmain.cc @@ -1908,11 +1908,14 @@ static int emcTaskIssueCommand(NMLmsg * cmd) retval = emcTrajSetSpindleSync(emcTrajSetSpindlesyncMsg->spindle, emcTrajSetSpindlesyncMsg->feed_per_revolution, emcTrajSetSpindlesyncMsg->velocity_mode); break; - case EMC_TRAJ_SET_OFFSET_TYPE: + case EMC_TRAJ_SET_OFFSET_TYPE: { // update tool offset - emcStatus->task.toolOffset = (reinterpret_cast(cmd))->offset; - retval = emcTrajSetOffset(emcStatus->task.toolOffset); + EMC_TRAJ_SET_OFFSET *msg = reinterpret_cast(cmd); + emcStatus->task.toolOffset = msg->offset; + retval = emcTrajSetOffset(emcStatus->task.toolOffset, + msg->have_point ? &msg->point : nullptr); break; + } case EMC_TRAJ_SET_ROTATION_TYPE: emcStatus->task.rotation_xy = (reinterpret_cast(cmd))->rotation; diff --git a/src/emc/task/taskintf.cc b/src/emc/task/taskintf.cc index a5fbeca3aa5..4c34d3078e7 100644 --- a/src/emc/task/taskintf.cc +++ b/src/emc/task/taskintf.cc @@ -1515,10 +1515,14 @@ int emcTrajJointMove(const EmcPose& end, const double *joints, int have_joints, return usrmotWriteEmcmotCommand(&emcmotCommand); } -int emcTrajSetOffset(const EmcPose& tool_offset) +int emcTrajSetOffset(const EmcPose& tool_offset, const EmcPose *point) { emcmotCommand.command = EMCMOT_SET_OFFSET; emcmotCommand.tool_offset = tool_offset; + emcmotCommand.have_point = (point != nullptr); + if (point) { + emcmotCommand.pos = *point; + } return usrmotWriteEmcmotCommand(&emcmotCommand); } diff --git a/tests/kins-switch/README b/tests/kins-switch/README index e426b4d7554..0815a214c3c 100644 --- a/tests/kins-switch/README +++ b/tests/kins-switch/README @@ -8,5 +8,7 @@ the selection reaches the motion controller and the interpreter, that a negative P word and a kinematics the module does not provide are refused, that G13.1 cancels to the identity kinematics the module declares (type 1 here, not 0), that the tool length G43.4 puts in effect reaches the -kinematics with nothing netted to its pin, and that G13.1 in an -ON_ABORT_COMMAND routine does not swallow the rest of the routine. +kinematics with nothing netted to its pin and, the head being tilted, +moves the programmed point rather than the joints, with the interpreter +agreeing on where the point went, and that G13.1 in an ON_ABORT_COMMAND +routine does not swallow the rest of the routine. diff --git a/tests/kins-switch/test-ui.py b/tests/kins-switch/test-ui.py index 09d3b72cd54..68a5776b7e9 100755 --- a/tests/kins-switch/test-ui.py +++ b/tests/kins-switch/test-ui.py @@ -151,37 +151,75 @@ def mdi(cmd): else: print("G43.4 switched to primary with the offset, G49 cancelled both") -# ---- the tool length reaches the kinematics with nothing netted ---------- +# ---- a tool length under a tilt keeps the joints ------------------------ # -# Motion hands the module the offset G43 puts in effect. The head is -# tilted, so the offset moves the joints, by the tilt term of the length: -# the same length along the tool axis instead of along Z. +# Motion hands the module the offset G43 puts in effect, with nothing +# netted. The head is tilted, so the length lies along the tool axis +# instead of along Z: the point the joints stand on moves by the tilt +# term of the length, and the joints stay where they are. The +# interpreter works out the same point ahead of motion, so a move to +# where it thinks the machine is goes nowhere. import math c.mode(linuxcnc.MODE_MDI) c.wait_complete(30) drain() + +def pose(): + s.poll() + return list(s.position[:3]) + +def tool_length_check(what, before_joints, before_pose, after_joints, after_pose, want): + if max(abs(after_joints[j] - before_joints[j]) for j in range(JOINTS)) > 1e-6: + error("%s moved the joints by %s" % (what, + " ".join("%.4f" % (after_joints[j] - before_joints[j]) for j in range(JOINTS)))) + got = [after_pose[j] - before_pose[j] for j in (0, 1, 2)] + if max(abs(g - w) for g, w in zip(got, want)) > 1e-3: + error("%s moved the point by %s, not %s" % (what, + " ".join("%.4f" % v for v in got), " ".join("%.4f" % v for v in want))) + said = [m[1].strip() for m in drain() if m[0] in (linuxcnc.NML_ERROR, linuxcnc.OPERATOR_ERROR)] + if said: + error("%s raised %s" % (what, said)) + # the interpreter's point: a move of nothing from it goes nowhere + mdi("G91") + stayed = mdi("G0 X0 Y0 Z0") + mdi("G90") + if max(abs(stayed[j] - after_joints[j]) for j in range(JOINTS)) > 1e-6: + error("after %s the interpreter has the point elsewhere: a move of nothing moved the joints by %s" + % (what, " ".join("%.4f" % (stayed[j] - after_joints[j]) for j in range(JOINTS)))) + mdi("G12.1 P0") mdi("G0 X10 Y10 Z-5 B-22.5 C45") mdi("G49") +errors_before = errors before = mdi("G0 X10 Y10 Z-5") +before_pose = pose() after = mdi("G43.4 H1") +pose_after = pose() L = 12.5 # tool 1 in tool.tbl b, cc = math.radians(-22.5), math.radians(45) -want = [-L * math.sin(math.pi - b) * math.cos(cc), - -L * math.sin(math.pi - b) * math.sin(cc), - -L * (1 + math.cos(math.pi - b))] -got = [after[j] - before[j] for j in (0, 1, 2)] -if max(abs(g - w) for g, w in zip(got, want)) > 1e-3: - error("G43.4 in the tilted pose moved the joints by %s, not %s" - % (" ".join("%.4f" % v for v in got), " ".join("%.4f" % v for v in want))) -else: - print("G43.4 moved the joints by the tilt term of the tool length") +# the length along the tool axis, less the length along Z the offset stands for +want = [L * math.sin(math.pi - b) * math.cos(cc), + L * math.sin(math.pi - b) * math.sin(cc), + L * (1 + math.cos(math.pi - b))] +tool_length_check("G43.4 in the tilted pose", before, before_pose, after, pose_after, want) # G49 would drop to identity and hold the joints where they are; a zero # offset without a switch takes the length back out back = mdi("G43.1 Z0") -if max(abs(back[j] - before[j]) for j in (0, 1, 2)) > 1e-3: - error("a zero tool length did not take the length back out of the joints") +tool_length_check("a zero tool length", after, pose_after, back, pose(), [-w for w in want]) +if errors == errors_before: + print("a tool length under a tilt moved the point, not the joints") + +# a point-to-point move ends on joints motion holds while the point +# stays; a tool length moves the point, and the joints are held on +errors_before = errors +p2p = mdi("G53.4 G0 X10 Y10 Z-5 B-22.5 C45") +p2p_pose = pose() +after = mdi("G43.1 Z%g" % L) +tool_length_check("a tool length after a point-to-point move", p2p, p2p_pose, after, pose(), want) +if errors == errors_before: + print("a tool length after a point-to-point move keeps the joints too") +mdi("G43.1 Z0") mdi("G49") # ---- a negative kinematics number is refused ----------------------------- From 55c68deed0ca83cf1026d2b9033cd1deb6a93cce Mon Sep 17 00:00:00 2001 From: Luca Toniolo <10792599+grandixximo@users.noreply.github.com> Date: Tue, 15 Sep 2026 15:58:17 +1000 Subject: [PATCH 60/60] interp: G43.5, the tool axis as a vector G43.5 is G43.4 with one thing more: while it is in effect a G0 or G1 line may give the tool axis as I J K in place of rotary words, the form of a five-axis program written for the tool rather than the machine, Fanuc's tool center point control type 2. The rotaries are solved through the module's tool frame inverse, the solver G53.1 uses, split out as orient_solve(): every orienting joint free, the pose nearest the present one, so a path of vectors runs continuous; on the pole of the primary rotary the free joint stays. The pose enters the move as program rotary words through machine_pose_to_program(), so rotary offsets are right by construction and the move interpolates like any other. The vector is read in the coordinate system the line's X Y Z are in, the tilted plane where one is active, the machine's on a G53 line. A direction is never incremental, so G91 applies to the axis words only, and I J K alone turns the tool in place. No configuration names the rotaries: the same program orients a head, a table and a robot. Refused: a vector with a rotary word, a zero vector, the identity kinematics, a vector on a G53.5 or G53.7 line. Left out: Fanuc's interpolation of the vector between lines, which needs the kinematics inside the planner. The semantics follow the G43.5 greatEndian built for xyzab_tdr_kins on the fork, including that the vector is absolute under G91; this is the generic form. tests/kins-switch covers the vector against rotary words, G91, the pole, a rotary offset and the refusals; docs in g-code.adoc. --- docs/src/gcode/g-code.adoc | 69 +++++- docs/src/gcode/overview.adoc | 2 +- docs/src/motion/kinematics-conventions.adoc | 2 +- docs/src/motion/switchkins.adoc | 7 +- src/emc/rs274ngc/interp_array.cc | 2 +- src/emc/rs274ngc/interp_check.cc | 17 +- src/emc/rs274ngc/interp_convert.cc | 22 +- src/emc/rs274ngc/interp_internal.cc | 7 +- src/emc/rs274ngc/interp_internal.hh | 2 + src/emc/rs274ngc/interp_setup.cc | 1 + src/emc/rs274ngc/interp_workplane.cc | 231 +++++++++++++------- src/emc/rs274ngc/interp_write.cc | 9 +- src/emc/rs274ngc/rs274ngc_interp.hh | 3 + tests/kins-switch/README | 8 +- tests/kins-switch/test-ui.py | 72 ++++++ 15 files changed, 348 insertions(+), 106 deletions(-) diff --git a/docs/src/gcode/g-code.adoc b/docs/src/gcode/g-code.adoc index e210fa582db..4e7f6738a7d 100644 --- a/docs/src/gcode/g-code.adoc +++ b/docs/src/gcode/g-code.adoc @@ -85,6 +85,7 @@ as the 'L number', and so on for any other letter. |<> |Dynamic Tool Length Offset |<> |Apply additional Tool Length Offset |<> |Tool Length Offset on Primary Kinematics +|<> |Tool Length Offset with the Tool Axis as a Vector |<> |Cancel Tool Length Offset |<> |Local Coordinate System Offset |<> |Move in Machine Coordinates @@ -1700,13 +1701,79 @@ It is an error if: kinematics, or * any of the 'G43' error conditions holds. +[[gcode:g43.5]] +== G43.5 Tool Length Offset with the Tool Axis as a Vector(((G43.5 Tool Length Offset with the Tool Axis as a Vector))) + +[source,ngc] +---- +G43.5 +G0 X- Y- Z- I- J- K- +G1 X- Y- Z- I- J- K- F- +---- + +* 'H' - tool number (optional) +* 'I J K' - the direction the tool axis is to point along, on a 'G0' or + 'G1' line while 'G43.5' is in effect + +'G43.5' is '<>' with one thing more: while it is in +effect, a 'G0' or 'G1' line may give the direction of the tool axis as +a vector 'I J K' in place of rotary words, and the interpreter works +out where the rotary joints have to go to point the tool that way. +This is the form of a five-axis program written for the tool rather +than for the machine, the one Fanuc calls tool center point control +type 2, and it runs on any machine whose kinematics module supplies its +tool frame (see the <> chapter): the same program orients a tilting head, a +tilting table and a robot. + +The vector points from the tool tip towards the holder, in the +coordinate system the line's 'X Y Z' are in: the active coordinate +system with its rotation, the <> where +one is active, and the machine's on a '<>' line. Its +length does not matter. A word left out is zero, so 'K1' alone is the +tool vertical. The words describe a direction, so they are never +incremental: 'G91' applies to the 'X Y Z' of the line, not to 'I J K', +and 'G91.1' does not apply to them either. A line with 'I J K' and no +axis word turns the tool where it stands. + +Where more than one pose of the rotaries points the tool along the +vector, two on a five-axis machine, the interpreter takes the one +nearest where the rotaries stand, every orienting joint free to take +part, so a path of vectors runs continuous from wherever it starts and +a rotary never swings the long way round. On the pole of the primary +rotary, the tool vertical on a mill, one joint no longer matters to the +direction and it stays where it is. + +The pose the interpreter finds goes into the move as ordinary rotary +words, in program coordinates, so the rotary offsets of the coordinate +system apply, and the move interpolates like any other move with rotary +words: the rotaries turn together with the linear axes between the +poses of consecutive lines. The vector itself is not interpolated +along the way; a program that needs the tool to sweep between two +directions writes the lines in between. + +'G2' and 'G3' keep 'I J K' as the arc centre; the tool direction on an +arc is given with rotary words. 'G43', 'G43.1', 'G43.2' and 'G49' end +the vector form along with the offset they replace; 'G49' cancels the +offset and undoes the kinematics switch, as after 'G43.4'. + +It is an error if: + +* 'I J K' are given together with a rotary word on the same line, +* the vector is zero, +* the kinematics selected is the identity one, or the module supplies + no tool frame or cannot be evaluated by the interpreter, +* the direction cannot be reached by the rotary joints, +* 'I J K' are given on a 'G53.5' or 'G53.7' line, or +* any of the 'G43.4' error conditions holds. + [[gcode:g49]] == G49 Cancel Tool Length Compensation(((G49 Cancel Tool Length Offset))) * 'G49' - cancels tool length compensation 'G49' also switches a switchable kinematics module back to its identity -kinematics when it cancels a 'G43.4', undoing the switch that made. It +kinematics when it cancels a 'G43.4' or 'G43.5', undoing the switch that made. It leaves a kinematics selected by 'G12.1' or 'G13.1' alone, as it does after a plain 'G43', and a module that declares no identity kinematics gets the plain cancel. diff --git a/docs/src/gcode/overview.adoc b/docs/src/gcode/overview.adoc index 177be9b1074..75988002d5d 100644 --- a/docs/src/gcode/overview.adoc +++ b/docs/src/gcode/overview.adoc @@ -985,7 +985,7 @@ The modal groups are shown in the following Table. |Feed Rate Mode (Group 5) | G93, G94, G95 |Units (Group 6) | G20, G21 |Cutter Diameter Compensation (Group 7) | G40, G41, G42, G41.1, G42.1 -|Tool Length Offset (Group 8) | G43, G43.1, G43.2, G43.4, G49 +|Tool Length Offset (Group 8) | G43, G43.1, G43.2, G43.4, G43.5, G49 |Tilted Work Plane (Group 9) | G68.2, G68.3, G68.4, G69 |Canned Cycles Return Mode (Group 10) | G98, G99 |Coordinate System (Group 12) | G54, G55, G56, G57, G58, G59, G59.1, G59.2, G59.3 diff --git a/docs/src/motion/kinematics-conventions.adoc b/docs/src/motion/kinematics-conventions.adoc index 7b5614c261f..6a4cde19d67 100644 --- a/docs/src/motion/kinematics-conventions.adoc +++ b/docs/src/motion/kinematics-conventions.adoc @@ -527,7 +527,7 @@ tool frame to the convention, and the optional Jacobian. A type whose forward iterates from the pose it is handed says so, and the shared code seeds it with the last answer after a switch. A type also says what it IS: `identity` marks the no-transform type `G13.1` cancels to, `primary` the working transform -`G43.4` switches to (see the Switchable Kinematics chapter). A module with +`G43.4` and `G43.5` switch to (see the Switchable Kinematics chapter). A module with several types has one geometry table and one ops table per type, registered with `switchkinsRegisterOps()`; a module with one type describes itself in a `kins_module` and links `kins_single.c`. diff --git a/docs/src/motion/switchkins.adoc b/docs/src/motion/switchkins.adoc index 8cb7fe679b8..6a7c7b09823 100644 --- a/docs/src/motion/switchkins.adoc +++ b/docs/src/motion/switchkins.adoc @@ -225,9 +225,10 @@ for how a module declares its types. For tool length work there are spellings that name the kinematics by what it is rather than by number: 'G43.4' applies the tool length offset and switches to the kinstype the module declares its working -transform, and the 'G49' that cancels it switches back to identity. -See the G-code documentation for 'G43.4' and 'G49', and for 'G12.1' -and 'G13.1', for the full description. +transform, 'G43.5' does the same and lets the lines after it give the +tool axis as a vector, and the 'G49' that cancels either switches back +to identity. See the G-code documentation for 'G43.4', 'G43.5' and +'G49', and for 'G12.1' and 'G13.1', for the full description. === M-code commands diff --git a/src/emc/rs274ngc/interp_array.cc b/src/emc/rs274ngc/interp_array.cc index 71964ea1d3a..456e68b338a 100644 --- a/src/emc/rs274ngc/interp_array.cc +++ b/src/emc/rs274ngc/interp_array.cc @@ -91,7 +91,7 @@ const int Interp::gees[] = { /* 360 */ -1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1, /* 380 */ -1,-1, 1, 1, 1, 1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1, /* 400 */ 7,-1,-1,-1,-1,-1,-1,-1,-1,-1, 7, 7,-1,-1,-1,-1,-1,-1,-1,-1, -/* 420 */ 7, 7,-1,-1,-1,-1,-1,-1,-1,-1, 8, 8, 8,-1, 8,-1,-1,-1,-1,-1, +/* 420 */ 7, 7,-1,-1,-1,-1,-1,-1,-1,-1, 8, 8, 8,-1, 8, 8,-1,-1,-1,-1, /* 440 */ -1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1, /* 460 */ -1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1, /* 480 */ -1,-1,-1,-1,-1,-1,-1,-1,-1,-1, 8,-1,-1,-1,-1,-1,-1,-1,-1,-1, diff --git a/src/emc/rs274ngc/interp_check.cc b/src/emc/rs274ngc/interp_check.cc index 4b650bb50f7..3ab706efb5b 100644 --- a/src/emc/rs274ngc/interp_check.cc +++ b/src/emc/rs274ngc/interp_check.cc @@ -309,11 +309,18 @@ int Interp::check_other_codes(block_pointer block) //!< pointer to a block } if (block->h_flag) { - CHKS((block->g_modes[GM_TOOL_LENGTH_OFFSET] != G_43 && motion != G_76 && block->g_modes[GM_TOOL_LENGTH_OFFSET] != G_43_2 && block->g_modes[GM_TOOL_LENGTH_OFFSET] != G_43_4), - _("H word with no G43, G43.4 or G76 to use it")); + CHKS((block->g_modes[GM_TOOL_LENGTH_OFFSET] != G_43 && motion != G_76 && block->g_modes[GM_TOOL_LENGTH_OFFSET] != G_43_2 && block->g_modes[GM_TOOL_LENGTH_OFFSET] != G_43_4 && block->g_modes[GM_TOOL_LENGTH_OFFSET] != G_43_5), + _("H word with no G43, G43.4, G43.5 or G76 to use it")); } - if (block->i_flag) { /* could still be useless if yz_plane arc */ + // under G43.5 the I J K of a G0 or G1 line are the tool axis: on the + // line that puts G43.5 in effect, and on the lines after it that do + // not change the tool length mode + int tool_vector = (block->g_modes[GM_TOOL_LENGTH_OFFSET] == G_43_5 + || (_setup.tool_vector && block->g_modes[GM_TOOL_LENGTH_OFFSET] == -1)) + && (motion == G_0 || motion == G_1); + + if (block->i_flag && !tool_vector) { /* could still be useless if yz_plane arc */ CHKS(((motion != G_2) && (motion != G_3) && (motion != G_5) && (motion != G_5_1) && (motion != G_6) && (motion != G_6_1) && (motion != G_71) && (motion != G_71_1) && (motion != G_71_2) && @@ -323,7 +330,7 @@ int Interp::check_other_codes(block_pointer block) //!< pointer to a block _("I word with no G2, G3, G5, G5.1, G6, G6.1, G10, G33.1, G68.2, G76, or G87 to use it")); } - if (block->j_flag) { /* could still be useless if xz_plane arc */ + if (block->j_flag && !tool_vector) { /* could still be useless if xz_plane arc */ CHKS(((motion != G_2) && (motion != G_3) && (motion != G_5) && (motion != G_5_1) && (motion != G_6) && (motion != G_6_1) && (motion != G_76) && (motion != G_87) && (block->g_modes[GM_MODAL_0] != G_10) && @@ -338,7 +345,7 @@ int Interp::check_other_codes(block_pointer block) //!< pointer to a block } } - if (block->k_flag) { /* could still be useless if xy_plane arc */ + if (block->k_flag && !tool_vector) { /* could still be useless if xy_plane arc */ CHKS(((motion != G_2) && (motion != G_3) && (motion != G_6_2) && (motion != G_33) && (motion != G_33_1) && (motion != G_76) && (motion != G_87) && (block->g_modes[GM_WORK_PLANE] == -1)), _("K word with no G2, G3, G6.2, G33, G33.1, G68.2, G76, or G87 to use it")); diff --git a/src/emc/rs274ngc/interp_convert.cc b/src/emc/rs274ngc/interp_convert.cc index a983a83bd7d..4937ffa96e8 100644 --- a/src/emc/rs274ngc/interp_convert.cc +++ b/src/emc/rs274ngc/interp_convert.cc @@ -4064,7 +4064,8 @@ int Interp::convert_m(block_pointer block, //!< pointer to a block of RS27 if (FEATURE(RETAIN_G43)) { if (((settings->active_g_codes[9] == G_43) || - (settings->active_g_codes[9] == G_43_4)) && ONCE(STEP_RETAIN_G43)) { + (settings->active_g_codes[9] == G_43_4) || + (settings->active_g_codes[9] == G_43_5)) && ONCE(STEP_RETAIN_G43)) { if(settings->selected_pocket > 0) { struct block_struct g43; init_block(&g43); @@ -5562,13 +5563,22 @@ int Interp::convert_straight(int move, //!< either G_0 or G_1 } settings->motion_mode = move; + // under G43.5 the I J K of a G0 or G1 line are the tool axis, which the + // rotaries are solved for once the line's other words are read + bool tool_vector = settings->tool_vector && (move == G_0 || move == G_1) + && (block->i_flag || block->j_flag || block->k_flag); if (block->g_modes[GM_MODAL_0] == G_53_5 || block->g_modes[GM_MODAL_0] == G_53_7) { // the words name joints, by letter or by number: nothing below applies + CHKS(tool_vector, _("G43.5: a tool vector cannot go with %s, whose words name the joints"), + (block->g_modes[GM_MODAL_0] == G_53_5) ? "G53.5" : "G53.7"); CHP(convert_ptp_joints(block->g_modes[GM_MODAL_0], move, block, settings)); return INTERP_OK; } CHP(find_ends(block, settings, &end_x, &end_y, &end_z, &AA_end, &BB_end, &CC_end, &u_end, &v_end, &w_end)); + if (tool_vector) { + CHP(tool_vector_ends(block, settings, &AA_end, &BB_end, &CC_end)); + } if (move == G_1) { inverse_time_rate_straight(end_x, end_y, end_z, @@ -6524,11 +6534,12 @@ int Interp::convert_tool_length_offset(int g_code, //!< g_code being execu CHKS((settings->cutter_comp_side != CUTTER_COMP::OFF), (_("Cannot change tool offset with cutter radius compensation on"))); - if (g_code == G_43_4) { + if (g_code == G_43_4 || g_code == G_43_5) { int primary = flagged_kins_type(KINSTYPE_PRIMARY); // G43.4 is G43 on the module's working transform: switch first, then // apply the offset, as if the switch line had run and drained. With - // no kinematics attached there is nothing to switch to. + // no kinematics attached there is nothing to switch to. G43.5 is + // the same, and the lines after it may give the tool axis as I J K. CHKS(primary < 0 && kins_type_info_available(), NCE_NO_PRIMARY_KINEMATICS_TYPE); if (primary >= 0 && primary != settings->kins_type) { // the switch keeps the joints and moves the point, so the point @@ -6547,9 +6558,10 @@ int Interp::convert_tool_length_offset(int g_code, //!< g_code being execu // the offset in effect is no longer G43.4's, so G49 has no switch to undo settings->kins_by_g43_4 = false; } + settings->tool_vector = (g_code == G_43_5); if (g_code == G_49) { idx = 0; - } else if (g_code == G_43 || g_code == G_43_4) { + } else if (g_code == G_43 || g_code == G_43_4 || g_code == G_43_5) { logDebug("convert_tool_length_offset h_flag=%d h_number=%d toolchange_flag=%d current_pocket=%d\n", block->h_flag,block->h_number,settings->toolchange_flag,settings->current_pocket); if(block->h_flag) { @@ -6629,7 +6641,7 @@ int Interp::convert_tool_length_offset(int g_code, //!< g_code being execu if(block->w_flag) tool_offset.w += block->w_number; } } else { - ERS("BUG: Code not G43, G43.1, G43.2, G43.4, or G49"); + ERS("BUG: Code not G43, G43.1, G43.2, G43.4, G43.5, or G49"); } // The machine does not move, so the program coordinates of the point change. // A kinematics that applies the offset itself moves the point by more than diff --git a/src/emc/rs274ngc/interp_internal.cc b/src/emc/rs274ngc/interp_internal.cc index 0725329be12..061c221c3dc 100644 --- a/src/emc/rs274ngc/interp_internal.cc +++ b/src/emc/rs274ngc/interp_internal.cc @@ -216,8 +216,11 @@ int Interp::enhance_block(block_pointer block, //!< pointer to a block to be c if (block->g_modes[GM_TOOL_LENGTH_OFFSET] != G_43_1) { block->motion_to_be = settings->motion_mode; } - } else if (!axis_flag && !polar_flag && ijk_flag && (settings->motion_mode == G_2 || settings->motion_mode == G_3)) { - // this is a block like simply "i1" which should be accepted if we're in arc mode + } else if (!axis_flag && !polar_flag && ijk_flag && + (settings->motion_mode == G_2 || settings->motion_mode == G_3 || + (settings->tool_vector && (settings->motion_mode == G_0 || settings->motion_mode == G_1)))) { + // this is a block like simply "i1" which should be accepted if we're in arc mode, + // or a tool vector alone under G43.5, which turns the tool where it stands block->motion_to_be = settings->motion_mode; } CHKS((polar_flag && block->motion_to_be == -1), _("Polar coordinates can only be used for motion")); diff --git a/src/emc/rs274ngc/interp_internal.hh b/src/emc/rs274ngc/interp_internal.hh index b8d4c98c90a..a0ec55d094d 100644 --- a/src/emc/rs274ngc/interp_internal.hh +++ b/src/emc/rs274ngc/interp_internal.hh @@ -248,6 +248,7 @@ enum GCodes G_43_1 = 431, G_43_2 = 432, G_43_4 = 434, + G_43_5 = 435, G_49 = 490, G_50 = 500, G_51 = 510, @@ -797,6 +798,7 @@ struct setup bool kinsSwitch_flag; // flag indicating waiting for kinematics switch done int kins_type; // kinematics selected by G12.1/G13.1 bool kins_by_g43_4; // G43.4 selected the kinematics, for G49 to undo + bool tool_vector; // G43.5: I J K on G0 and G1 give the tool axis bool toolchange_flag; // flag indicating we just had a tool change int input_index; // channel queried bool input_digital; // input queried was digital (false=analog) diff --git a/src/emc/rs274ngc/interp_setup.cc b/src/emc/rs274ngc/interp_setup.cc index dbbccf58ae0..3606c54b9b9 100644 --- a/src/emc/rs274ngc/interp_setup.cc +++ b/src/emc/rs274ngc/interp_setup.cc @@ -135,6 +135,7 @@ setup::setup() : kinsSwitch_flag(0), kins_type(0), kins_by_g43_4(false), + tool_vector(false), toolchange_flag(0), input_index(0), input_digital(0), diff --git a/src/emc/rs274ngc/interp_workplane.cc b/src/emc/rs274ngc/interp_workplane.cc index b754922bc24..aac82513bb6 100644 --- a/src/emc/rs274ngc/interp_workplane.cc +++ b/src/emc/rs274ngc/interp_workplane.cc @@ -660,20 +660,32 @@ int Interp::tool_offset_point(setup_pointer s, const EmcPose *offset, const doub return INTERP_OK; } -// a direction of the plane in world coordinates: the plane's rotation -// then the XY rotation of the coordinate system it sits on -static void plane_axis_in_world(setup_pointer s, int column, double rotation_xy, PmCartesian *out) +// a direction the program gives, in world coordinates: through the +// tilted work plane's rotation where one is active, then the XY rotation +// of the coordinate system it sits on +static void direction_in_world(setup_pointer s, const double v[3], double rotation_xy, PmCartesian *out) { - double x = s->g68_rotation[0][column]; - double y = s->g68_rotation[1][column]; - double z = s->g68_rotation[2][column]; + double x = v[0], y = v[1], z = v[2]; double t = rotation_xy * M_PI / 180.0; + if (s->g68_active) { + x = s->g68_rotation[0][0] * v[0] + s->g68_rotation[0][1] * v[1] + s->g68_rotation[0][2] * v[2]; + y = s->g68_rotation[1][0] * v[0] + s->g68_rotation[1][1] * v[1] + s->g68_rotation[1][2] * v[2]; + z = s->g68_rotation[2][0] * v[0] + s->g68_rotation[2][1] * v[1] + s->g68_rotation[2][2] * v[2]; + } out->x = x * cos(t) - y * sin(t); out->y = x * sin(t) + y * cos(t); out->z = z; } +// a direction of the plane in world coordinates +static void plane_axis_in_world(setup_pointer s, int column, double rotation_xy, PmCartesian *out) +{ + double v[3] = { column == 0 ? 1.0 : 0.0, column == 1 ? 1.0 : 0.0, column == 2 ? 1.0 : 0.0 }; + + direction_in_world(s, v, rotation_xy, out); +} + static void rotate_about(const PmCartesian *axis, double rad, PmCartesian *v) { // Rodrigues, for a unit axis @@ -761,24 +773,16 @@ int Interp::convert_work_plane_from_tool(block_pointer block, setup_pointer s) // G53.1, G53.2, G53.3 and G53.6: the rotaries to the plane's normal. G53.1 // turns the rotaries alone, in joint space; G53.6 keeps the tool centre point, // a Cartesian move; G53.3 goes to X Y Z in the plane; G53.2 only publishes the -// pose on #<_orient_x> and kin (Heidenhain STAY). P picks the pose, nearest -// first or by the sign of the tilting joint; Q0 holds the joints that carry -// the work (COORD ROT), Q1 frees them (TABLE ROT). +// pose on #<_orient_x> and kin (Heidenhain STAY). P and Q are orient_solve()'s. int Interp::convert_orient_tool(int code, block_pointer block, setup_pointer s) { void *vctx; KinematicsUserContext *ctx; - double now[EMCMOT_MAX_JOINTS]; - double solutions[TOOL_FRAME_MAX_SOLUTIONS * EMCMOT_MAX_JOINTS]; - double spin[TOOL_FRAME_MAX_SOLUTIONS]; - double distance[TOOL_FRAME_MAX_SOLUTIONS]; - int order[TOOL_FRAME_MAX_SOLUTIONS], free_dirs[TOOL_FRAME_MAX_SOLUTIONS]; + double now[EMCMOT_MAX_JOINTS], sol[EMCMOT_MAX_JOINTS]; PmCartesian axis, xdir; EmcPose end_pose; double end_prog[9]; - unsigned int held = 0; - int p, q, n, i, j, chosen, njoints; - const double *sol; + int p, q, i; const char *name = (code == G_53_1) ? "G53.1" : (code == G_53_2) ? "G53.2" : (code == G_53_3) ? "G53.3" : "G53.6"; CHKS((!s->g68_active), _("%s needs a tilted work plane; define one with G68.2 first"), name); @@ -795,24 +799,103 @@ int Interp::convert_orient_tool(int code, block_pointer block, setup_pointer s) ctx = (KinematicsUserContext *)vctx; CHKS((kinematicsUserIsIdentity(ctx)), _("%s needs a kinematics type that describes the machine; select it with G12.1 first"), name); - njoints = kinematicsUserGetNumJoints(ctx); CHP(current_joints(s, ctx, now)); plane_axis_in_world(s, 2, s->rotation_xy, &axis); plane_axis_in_world(s, 0, s->rotation_xy, &xdir); + CHP(orient_solve(s, ctx, &axis, &xdir, p, q, now, sol, name)); + + // where that puts the machine, and what the program calls it + end_pose = (EmcPose){}; + current_machine_pose(s, &end_pose); + CHKS((kinematicsUserForward(ctx, sol, &end_pose) != 0), + _("%s: the kinematics cannot place the orientation it found"), name); + for (i = 0; i < EMCMOT_MAX_JOINTS; i++) { s->kins_seed[i] = sol[i]; } + machine_pose_to_program(s, &end_pose, end_prog); + + if (code == G_53_2) { + // STAY: solve only, nothing moves. The pose goes to the named + // parameters #<_orient_x> and kin and to #5071-#5080, for the + // program to use in a move of its own making, the way + // Heidenhain's STAY fills Q120-122. The machine state does not + // change. + for (i = 0; i < 6; i++) { s->orient_pose[i] = end_prog[i]; } + s->orient_valid = true; + for (i = 0; i < 9; i++) { s->parameters[5071 + i] = end_prog[i]; } + s->parameters[5080] = 1.0; + return INTERP_OK; + } + + write_canon_state_tag(block, s); + if (code == G_53_1) { + // the rotaries alone: the linear joints are where they are, since + // the solver left them at the seed, and the tool goes wherever + // that carries it + JOINT_TRAVERSE(block->line_number, sol, 1, + end_prog[0], end_prog[1], end_prog[2], + end_prog[3], end_prog[4], end_prog[5], + end_prog[6], end_prog[7], end_prog[8]); + s->current_x = end_prog[0]; + s->current_y = end_prog[1]; + s->current_z = end_prog[2]; + } else if (code == G_53_6) { + // the tool centre point stays: a Cartesian move of the rotaries + STRAIGHT_TRAVERSE(block->line_number, s->current_x, s->current_y, s->current_z, + end_prog[3], end_prog[4], end_prog[5], + s->u_current, s->v_current, s->w_current); + } else { + double x = block->x_flag ? block->x_number : s->current_x; + double y = block->y_flag ? block->y_number : s->current_y; + double z = block->z_flag ? block->z_number : s->current_z; + + JOINT_TRAVERSE(block->line_number, NULL, 0, x, y, z, + end_prog[3], end_prog[4], end_prog[5], + s->u_current, s->v_current, s->w_current); + s->current_x = x; + s->current_y = y; + s->current_z = z; + } + s->AA_current = end_prog[3]; + s->BB_current = end_prog[4]; + s->CC_current = end_prog[5]; + if (code == G_53_1) { + s->u_current = end_prog[6]; + s->v_current = end_prog[7]; + s->w_current = end_prog[8]; + } + return INTERP_OK; +} + +// The joints that point the tool along axis, and its x along xdir where given, +// from the joints the machine is at: every pose the module reports, unwrapped +// onto the nearest turn and ranked by rotary travel. P picks by rank or by +// the sign of the tilting joint; Q0 holds the joints that carry the work +// (Heidenhain COORD ROT), Q1 frees them (TABLE ROT). +int Interp::orient_solve(setup_pointer s, void *vctx, const PmCartesian *axis, const PmCartesian *xdir, + int p, int q, const double *now, double *joints, const char *name) +{ + KinematicsUserContext *ctx = (KinematicsUserContext *)vctx; + double solutions[TOOL_FRAME_MAX_SOLUTIONS * EMCMOT_MAX_JOINTS]; + double spin[TOOL_FRAME_MAX_SOLUTIONS]; + double distance[TOOL_FRAME_MAX_SOLUTIONS]; + int order[TOOL_FRAME_MAX_SOLUTIONS], free_dirs[TOOL_FRAME_MAX_SOLUTIONS]; + unsigned int held = 0; + int n, i, j, chosen, njoints; + + njoints = kinematicsUserGetNumJoints(ctx); if (q == 0) { if (kinematicsUserWorkJoints(ctx, now, &held) != 0) { held = 0; } } - n = kinematicsUserToolFrameInverse(ctx, &axis, &xdir, now, held, + n = kinematicsUserToolFrameInverse(ctx, axis, xdir, now, held, solutions, TOOL_FRAME_MAX_SOLUTIONS, free_dirs, spin); if (n == 0 && held) { // nothing reachable with the work held still: let it move held = 0; - n = kinematicsUserToolFrameInverse(ctx, &axis, &xdir, now, held, + n = kinematicsUserToolFrameInverse(ctx, axis, xdir, now, held, solutions, TOOL_FRAME_MAX_SOLUTIONS, free_dirs, spin); } CHKS((n < 0), _("%s: the kinematics cannot answer the orientation"), name); - CHKS((n == 0), _("%s: the plane's normal cannot be reached by the rotary joints"), name); + CHKS((n == 0), _("%s: the direction asked for cannot be reached by the rotary joints"), name); // the solver reports each answer in (-180, 180], but the machine stands // somewhere in turn space: unwrap every angular joint onto the turn @@ -857,70 +940,56 @@ int Interp::convert_orient_tool(int code, block_pointer block, setup_pointer s) CHKS((chosen < 0), _("%s P%d: no reachable pose has joint %d %s"), name, p, secondary, (p == 1) ? "positive" : "negative"); } - sol = solutions + chosen * njoints; - - // where that puts the machine, and what the program calls it - end_pose = (EmcPose){}; - current_machine_pose(s, &end_pose); - { - double full[EMCMOT_MAX_JOINTS]; - for (i = 0; i < EMCMOT_MAX_JOINTS; i++) { full[i] = (i < njoints) ? sol[i] : 0.0; } - CHKS((kinematicsUserForward(ctx, full, &end_pose) != 0), - _("%s: the kinematics cannot place the orientation it found"), name); - for (i = 0; i < EMCMOT_MAX_JOINTS; i++) { s->kins_seed[i] = full[i]; } - } - machine_pose_to_program(s, &end_pose, end_prog); - - if (code == G_53_2) { - // STAY: solve only, nothing moves. The pose goes to the named - // parameters #<_orient_x> and kin and to #5071-#5080, for the - // program to use in a move of its own making, the way - // Heidenhain's STAY fills Q120-122. The machine state does not - // change. - for (i = 0; i < 6; i++) { s->orient_pose[i] = end_prog[i]; } - s->orient_valid = true; - for (i = 0; i < 9; i++) { s->parameters[5071 + i] = end_prog[i]; } - s->parameters[5080] = 1.0; - return INTERP_OK; + for (i = 0; i < EMCMOT_MAX_JOINTS; i++) { + joints[i] = (i < njoints) ? solutions[chosen * njoints + i] : 0.0; } + return INTERP_OK; +} - write_canon_state_tag(block, s); - if (code == G_53_1) { - // the rotaries alone: the linear joints are where they are, since - // the solver left them at the seed, and the tool goes wherever - // that carries it - JOINT_TRAVERSE(block->line_number, sol, 1, - end_prog[0], end_prog[1], end_prog[2], - end_prog[3], end_prog[4], end_prog[5], - end_prog[6], end_prog[7], end_prog[8]); - s->current_x = end_prog[0]; - s->current_y = end_prog[1]; - s->current_z = end_prog[2]; - } else if (code == G_53_6) { - // the tool centre point stays: a Cartesian move of the rotaries - STRAIGHT_TRAVERSE(block->line_number, s->current_x, s->current_y, s->current_z, - end_prog[3], end_prog[4], end_prog[5], - s->u_current, s->v_current, s->w_current); +// G43.5: I J K on a G0 or G1 line are the tool axis, tip towards holder, in +// the coordinate system the line's X Y Z are in. The rotaries come from the +// tool frame inverse, every orienting joint free, the nearest pose, as program +// rotary coordinates so a rotary offset is right by construction. +int Interp::tool_vector_ends(block_pointer block, setup_pointer s, double *a, double *b, double *c) +{ + void *vctx; + KinematicsUserContext *ctx; + double now[EMCMOT_MAX_JOINTS], sol[EMCMOT_MAX_JOINTS]; + double v[3], prog[9]; + PmCartesian axis; + EmcPose pose; + int i; + + CHKS((block->a_flag || block->b_flag || block->c_flag), + _("G43.5: a tool vector and rotary words on one line give the orientation twice")); + v[0] = block->i_flag ? block->i_number : 0.0; + v[1] = block->j_flag ? block->j_number : 0.0; + v[2] = block->k_flag ? block->k_number : 0.0; + CHKS((vec_norm(v) < 1e-9), _("G43.5: the tool vector I J K is zero")); + CHP(kins_context(s, &vctx)); + ctx = (KinematicsUserContext *)vctx; + CHKS((kinematicsUserIsIdentity(ctx)), + _("G43.5: a tool vector needs a kinematics type that describes the machine; select it with G12.1 first")); + CHP(current_joints(s, ctx, now)); + if (block->g_modes[GM_MODAL_0] == G_53) { + axis.x = v[0]; + axis.y = v[1]; + axis.z = v[2]; } else { - double x = block->x_flag ? block->x_number : s->current_x; - double y = block->y_flag ? block->y_number : s->current_y; - double z = block->z_flag ? block->z_number : s->current_z; - - JOINT_TRAVERSE(block->line_number, NULL, 0, x, y, z, - end_prog[3], end_prog[4], end_prog[5], - s->u_current, s->v_current, s->w_current); - s->current_x = x; - s->current_y = y; - s->current_z = z; - } - s->AA_current = end_prog[3]; - s->BB_current = end_prog[4]; - s->CC_current = end_prog[5]; - if (code == G_53_1) { - s->u_current = end_prog[6]; - s->v_current = end_prog[7]; - s->w_current = end_prog[8]; + direction_in_world(s, v, s->rotation_xy, &axis); } + CHP(orient_solve(s, ctx, &axis, NULL, 0, 1, now, sol, "G43.5")); + + // where that puts the rotaries, and what the program calls it + pose = (EmcPose){}; + current_machine_pose(s, &pose); + CHKS((kinematicsUserForward(ctx, sol, &pose) != 0), + _("G43.5: the kinematics cannot place the orientation it found")); + for (i = 0; i < EMCMOT_MAX_JOINTS; i++) { s->kins_seed[i] = sol[i]; } + machine_pose_to_program(s, &pose, prog); + *a = prog[3]; + *b = prog[4]; + *c = prog[5]; return INTERP_OK; } diff --git a/src/emc/rs274ngc/interp_write.cc b/src/emc/rs274ngc/interp_write.cc index 54dfe128f98..755c7c8fc0f 100644 --- a/src/emc/rs274ngc/interp_write.cc +++ b/src/emc/rs274ngc/interp_write.cc @@ -111,16 +111,17 @@ int Interp::write_g_codes(block_pointer block, //!< pointer to a block of RS27 7) ? (530 + (10 * settings->origin_index)) : (584 + settings->origin_index); // the kins type, not the label, is the authority: a G43 given on the - // module's primary type shows as G43.4, and the label follows the type - // motion reports after a resync. -1 is "no information" and matches - // every flag, so it is excluded before the bit test. + // module's primary type shows as G43.4, or G43.5 while I J K give the + // tool axis, and the label follows the type motion reports after a + // resync. -1 is "no information" and matches every flag, so it is + // excluded before the bit test. kf = GET_EXTERNAL_KINS_TYPE_FLAGS(settings->kins_type); settings->active_g_codes[9] = (settings->g43_with_zero_offset || settings->tool_offset.tran.x || settings->tool_offset.tran.y || settings->tool_offset.tran.z || settings->tool_offset.a || settings->tool_offset.b || settings->tool_offset.c || settings->tool_offset.u || settings->tool_offset.v || settings->tool_offset.w) ? - ((kf >= 0 && (kf & KINSTYPE_PRIMARY)) ? G_43_4 : G_43) : G_49; + ((kf >= 0 && (kf & KINSTYPE_PRIMARY)) ? (settings->tool_vector ? G_43_5 : G_43_4) : G_43) : G_49; settings->active_g_codes[10] = (settings->retract_mode == RETRACT_MODE::OLD_Z) ? G_98 : G_99; // Three modes: G_64, G_61, G_61_1 or CANON_CONTINUOUS/EXACT_PATH/EXACT_STOP settings->active_g_codes[11] = diff --git a/src/emc/rs274ngc/rs274ngc_interp.hh b/src/emc/rs274ngc/rs274ngc_interp.hh index f60b4078064..55c5d420dba 100644 --- a/src/emc/rs274ngc/rs274ngc_interp.hh +++ b/src/emc/rs274ngc/rs274ngc_interp.hh @@ -371,6 +371,9 @@ public: int work_plane_check_sequence(block_pointer block, setup_pointer settings); int convert_work_plane_from_tool(block_pointer block, setup_pointer settings); int convert_orient_tool(int code, block_pointer block, setup_pointer settings); + int orient_solve(setup_pointer settings, void *ctx, const PmCartesian *axis, const PmCartesian *xdir, + int p, int q, const double *now, double *joints, const char *name); + int tool_vector_ends(block_pointer block, setup_pointer settings, double *a, double *b, double *c); int convert_ptp_joints(int code, int move, block_pointer block, setup_pointer settings); int ptp_seconds(block_pointer block, setup_pointer settings, double x, double y, double z, double a, double b, double c, diff --git a/tests/kins-switch/README b/tests/kins-switch/README index 0815a214c3c..e409e2b3cf9 100644 --- a/tests/kins-switch/README +++ b/tests/kins-switch/README @@ -10,5 +10,9 @@ that G13.1 cancels to the identity kinematics the module declares (type 1 here, not 0), that the tool length G43.4 puts in effect reaches the kinematics with nothing netted to its pin and, the head being tilted, moves the programmed point rather than the joints, with the interpreter -agreeing on where the point went, and that G13.1 in an ON_ABORT_COMMAND -routine does not swallow the rest of the routine. +agreeing on where the point went, that under G43.5 a tool vector I J K +lands on the joints and the point the rotary words reach, is not +incremental, keeps the free joint on the pole and is refused with a +rotary word, as a zero vector, on the identity kinematics and outside +G43.5, and that G13.1 in an ON_ABORT_COMMAND routine does not swallow +the rest of the routine. diff --git a/tests/kins-switch/test-ui.py b/tests/kins-switch/test-ui.py index 68a5776b7e9..bca7225ba6c 100755 --- a/tests/kins-switch/test-ui.py +++ b/tests/kins-switch/test-ui.py @@ -222,6 +222,78 @@ def tool_length_check(what, before_joints, before_pose, after_joints, after_pose mdi("G43.1 Z0") mdi("G49") +# ---- G43.5: the tool axis as a vector ------------------------------------ +# +# Under G43.5 a G0 or G1 line gives the direction of the tool axis as I J K +# and the interpreter finds the rotaries. The head's tool axis at B, C is +# (-sin B cos C, -sin B sin C, cos B), so the vector for the tilted pose +# above has to land on the joints and the point the rotary words reach. + +def refused(cmd, needle): + drain() + c.mdi(cmd) + c.wait_complete(30) + m = e.poll() + if not m or m[0] not in (linuxcnc.NML_ERROR, linuxcnc.OPERATOR_ERROR): + error("%s was accepted" % cmd) + elif needle not in m[1]: + error("%s said %r, nothing about %r" % (cmd, m[1].strip(), needle)) + else: + print("refused as expected: %s" % m[1].strip()) + drain() + +errors_before = errors +mdi("G12.1 P0") +mdi("G0 X0 Y0 Z0 B0 C0") +mdi("G43.5 H1") +s.poll() +if 435 not in s.gcodes: + error("G43.5 is not among the active G-codes %s" % (s.gcodes,)) +by_words = mdi("G0 X10 Y10 Z-5 B-22.5 C45") +by_words_pose = pose() +mdi("G0 X0 Y0 Z0 B0 C0") +vec = (-math.sin(b) * math.cos(cc), -math.sin(b) * math.sin(cc), math.cos(b)) +by_vector = mdi("G0 X10 Y10 Z-5 I%.9f J%.9f K%.9f" % vec) +if max(abs(by_vector[j] - by_words[j]) for j in range(JOINTS)) > 1e-5: + error("the vector put the joints at %s, the rotary words at %s" + % (" ".join("%.4f" % v for v in by_vector), " ".join("%.4f" % v for v in by_words))) +if max(abs(p - q) for p, q in zip(pose(), by_words_pose)) > 1e-5: + error("the vector put the point at %s, the rotary words at %s" + % (" ".join("%.4f" % v for v in pose()), " ".join("%.4f" % v for v in by_words_pose))) +# a direction is not incremental +mdi("G91") +held = mdi("G1 X0 I%.9f J%.9f K%.9f F1000" % vec) +mdi("G90") +if max(abs(held[j] - by_vector[j]) for j in range(JOINTS)) > 1e-6: + error("the same vector under G91 moved the joints by %s" + % " ".join("%.4f" % (held[j] - by_vector[j]) for j in range(JOINTS))) +# the pole: the tool vertical leaves C where it is, and the point stays +pole = mdi("G0 K1") +if abs(pole[3]) > 1e-6 or abs(pole[4] - 45) > 1e-3: + error("the tool vertical put B, C at %.4f, %.4f, not 0, 45" % (pole[3], pole[4])) +if max(abs(p - q) for p, q in zip(pose()[:3], by_words_pose[:3])) > 1e-5: + error("the tool vertical moved the point to %s" % " ".join("%.4f" % v for v in pose())) +# a rotary offset renames the angles, the direction is the same: the same +# joints, called something else by the program +mdi("G10 L2 P1 C30") +mdi("G0 X0 Y0 Z0 B0 C0") +offset = mdi("G0 X10 Y10 Z-5 I%.9f J%.9f K%.9f" % vec) +mdi("G10 L2 P1 C0") +if max(abs(offset[j] - by_words[j]) for j in range(JOINTS)) > 1e-5: + error("under a C offset the vector put the joints at %s, not %s" + % (" ".join("%.4f" % v for v in offset), " ".join("%.4f" % v for v in by_words))) +if errors == errors_before: + print("G43.5 turned the tool along the vector, onto the joints the rotary words reach") +refused("G0 X0 K1 B5", "twice") +refused("G0 X0 I0 J0 K0", "zero") +mdi("G13.1") +refused("G0 X0 K1", "G12.1 first") +mdi("G43.4 H1") +refused("G0 X0 K1", "K word with no") +mdi("G43.5 H1") +mdi("G49") +refused("G0 X0 K1", "K word with no") + # ---- a negative kinematics number is refused ----------------------------- drain()