introduce "conventional-directions" HAL pin.

when set to true, rotational axes direction will
follow the conventions as documented in the manual

Author: DerAndere1

configs/sim/axis/vismach/5axis/table-rotary-tilting/xyzac-trt.ini

@@ -69,6 +69,7 @@ HALCMD = sets :z-offset     10
 HALCMD = setp xyzac-trt-kins.x-rot-point 0
 HALCMD = setp xyzac-trt-kins.y-rot-point 0
 HALCMD = setp xyzac-trt-kins.z-rot-point 0
+HALCMD = setp xyzac-trt-kins.conventional-directions 0
 
 [HALUI]
 # NOTE: kinstype==0 is identity kins because sparm=identityfirst

configs/sim/axis/vismach/5axis/table-rotary-tilting/xyzac-trt.txt

@@ -28,3 +28,10 @@ Hal Input pins:
 X, Y and Z rot-point pins represent the
 offsets of the center of rotation of the C axis
 relative to the machine absolute zero
+
+Hal Input pins:
+  xyzac-trt-kins.conventional-directions
+
+Pin conventional-directions is false by default. If true,
+axis directions follow the conventions as defined
+at https://linuxcnc.org/docs/html/gcode/machining-center.html#_rotational_axes

configs/sim/axis/vismach/5axis/table-rotary-tilting/xyzbc-trt.ini

@@ -69,6 +69,7 @@ HALCMD = sets :z-offset     -15
 HALCMD = setp xyzbc-trt-kins.x-rot-point 0
 HALCMD = setp xyzbc-trt-kins.y-rot-point 0
 HALCMD = setp xyzbc-trt-kins.z-rot-point 0
+HALCMD = setp xyzbc-trt-kins.conventional-directions 0
 
 [HALUI]
 # NOTE: kinstype==0 is identity kins because sparm=identityfirst

configs/sim/axis/vismach/5axis/table-rotary-tilting/xyzbc-trt.txt

@@ -21,10 +21,17 @@ of rotation of the B axis relative to the center
 of rotation of the C axis.
 
 Hal Input pins:
-  xyzac-trt-kins.x-rot-point
-  xyzac-trt-kins.y-rot-point
-  xyzac-trt-kins.z-rot-point
+  xyzbc-trt-kins.x-rot-point
+  xyzbc-trt-kins.y-rot-point
+  xyzbc-trt-kins.z-rot-point
 
 X, Y and Z rot-point pins represent the
 offsets of the center of rotation of the C axis
 relative to the machine absolute zero
+
+Hal Input pins:
+  xyzbc-trt-kins.conventional-directions
+
+Pin conventional-directions is false by default. If true,
+axis directions follow the conventions as defined
+at https://linuxcnc.org/docs/html/gcode/machining-center.html#_rotational_axes

src/emc/kinematics/maxkins.c

@@ -32,6 +32,7 @@
 
 struct haldata {
     hal_float_t *pivot_length;
+    hal_bit_t *conventional_directions; //default is false
 } *haldata;
 
 int kinematicsForward(const double *joints,
@@ -41,23 +42,26 @@ int kinematicsForward(const double *joints,
 {
     (void)fflags;
     (void)iflags;
+
+    const real_t con = *(haldata->conventional_directions) ? 1.0 : -1.0;
+
     // B correction
-    double zb = (*(haldata->pivot_length) + joints[8]) * cos(d2r(joints[4]));
-    double xb = (*(haldata->pivot_length) + joints[8]) * sin(d2r(joints[4]));
+    const double zb = (*(haldata->pivot_length) + joints[8]) * cos(d2r(joints[4]));
+    const double xb = (*(haldata->pivot_length) + joints[8]) * sin(d2r(joints[4]));
 
     // C correction
-    double xyr = hypot(joints[0], joints[1]);
-    double xytheta = atan2(joints[1], joints[0]) + d2r(joints[5]);
+    const double xyr = hypot(joints[0], joints[1]);
+    const double xytheta = atan2(joints[1], joints[0]) + d2r(joints[5]);
 
     // U correction
-    double zv = joints[6] * sin(d2r(joints[4]));
-    double xv = joints[6] * cos(d2r(joints[4]));
+    const double zv = joints[6] * sin(d2r(joints[4]));
+    const double xv = joints[6] * cos(d2r(joints[4]));
 
     // V correction is always in joint 1 only
 
-    pos->tran.x = xyr * cos(xytheta) + xb - xv;
+    pos->tran.x = xyr * cos(xytheta) - (con * xb) - xv;
     pos->tran.y = xyr * sin(xytheta) - joints[7];
-    pos->tran.z = joints[2] - zb + zv + *(haldata->pivot_length);
+    pos->tran.z = joints[2] - zb - (con * zv) + *(haldata->pivot_length);
 
     pos->a = joints[3];
     pos->b = joints[4];
@@ -76,23 +80,26 @@ int kinematicsInverse(const EmcPose * pos,
 {
     (void)iflags;
     (void)fflags;
+
+    const real_t con = *(haldata->conventional_directions) ? 1.0 : -1.0;
+
     // B correction
-    double zb = (*(haldata->pivot_length) + pos->w) * cos(d2r(pos->b));
-    double xb = (*(haldata->pivot_length) + pos->w) * sin(d2r(pos->b));
+    const double zb = (*(haldata->pivot_length) + pos->w) * cos(d2r(pos->b));
+    const double xb = (*(haldata->pivot_length) + pos->w) * sin(d2r(pos->b));
 
     // C correction
-    double xyr = hypot(pos->tran.x, pos->tran.y);
-    double xytheta = atan2(pos->tran.y, pos->tran.x) - d2r(pos->c);
+    const double xyr = hypot(pos->tran.x, pos->tran.y);
+    const double xytheta = atan2(pos->tran.y, pos->tran.x) - d2r(pos->c);
 
     // U correction
-    double zv = pos->u * sin(d2r(pos->b));
-    double xv = pos->u * cos(d2r(pos->b));
+    const double zv = pos->u * sin(d2r(pos->b));
+    const double xv = pos->u * cos(d2r(pos->b));
 
     // V correction is always in joint 1 only
 
-    joints[0] = xyr * cos(xytheta) - xb + xv;
+    joints[0] = xyr * cos(xytheta) + (con * xb) + xv;
     joints[1] = xyr * sin(xytheta) + pos->v;
-    joints[2] = pos->tran.z + zb + zv - *(haldata->pivot_length);
+    joints[2] = pos->tran.z + zb - (con * zv) - *(haldata->pivot_length);
 
     joints[3] = pos->a;
     joints[4] = pos->b;
@@ -127,10 +134,13 @@ int rtapi_app_main(void) {
     haldata = hal_malloc(sizeof(struct haldata));
 
     result = hal_pin_float_new("maxkins.pivot-length", HAL_IO, &(haldata->pivot_length), comp_id);
+
+    result += hal_pin_bit_new("maxkins.conventional-directions", HAL_IN, &(haldata->conventional_directions), comp_id);
+
     if(result < 0) goto error;
 
     *(haldata->pivot_length) = 0.666;
-
+    *(haldata->conventional_directions) = 0; // default is unconventional
     hal_ready(comp_id);
     return 0;
 

src/emc/kinematics/trtfuncs.c

@@ -60,6 +60,7 @@ struct haldata {
     hal_float_t *y_offset;
     hal_float_t *z_offset;
     hal_float_t *tool_offset;
+    hal_bit_t *conventional_directions; // default: false
 } *haldata;
 
 
@@ -140,6 +141,8 @@ int trtKinematicsSetup(const int   comp_id,
                  "%s.z-offset",kp->halprefix);
     res += hal_pin_float_newf(HAL_IN, &(haldata->tool_offset), comp_id,
                  "%s.tool-offset",kp->halprefix);
+    res += hal_pin_bit_newf(HAL_IN, &(haldata->conventional_directions), comp_id,
+                 "%s.conventional-directions", kp->halprefix);
     if (res) {goto error;}
     return 0;
 
@@ -155,31 +158,31 @@ int xyzacKinematicsForward(const double *joints,
 {
     (void)fflags;
     (void)iflags;
-    double x_rot_point = *(haldata->x_rot_point);
-    double y_rot_point = *(haldata->y_rot_point);
-    double z_rot_point = *(haldata->z_rot_point);
-    double          dt = *(haldata->tool_offset);
-    double          dy = *(haldata->y_offset);
-    double          dz = *(haldata->z_offset);
-    double       a_rad = joints[JA]*TO_RAD;
-    double       c_rad = joints[JC]*TO_RAD;
-
-    dz = dz + dt;
-
-    pos->tran.x = + cos(c_rad)              * (joints[JX]      - x_rot_point)
-                  + sin(c_rad) * cos(a_rad) * (joints[JY] - dy - y_rot_point)
-                  + sin(c_rad) * sin(a_rad) * (joints[JZ] - dz - z_rot_point)
-                  + sin(c_rad) * dy
+    const double x_rot_point = *(haldata->x_rot_point);
+    const double y_rot_point = *(haldata->y_rot_point);
+    const double z_rot_point = *(haldata->z_rot_point);
+    const double          dt = *(haldata->tool_offset);
+    const double          dy = *(haldata->y_offset);
+    const double          dz = *(haldata->z_offset) + dt;
+    const double       a_rad = joints[JA]*TO_RAD;
+    const double       c_rad = joints[JC]*TO_RAD;
+
+    const real_t con = *(haldata->conventional_directions) ? 1.0 : -1.0;
+
+    pos->tran.x = +       cos(c_rad)              * (joints[JX]      - x_rot_point)
+                  - con * sin(c_rad) * cos(a_rad) * (joints[JY] - dy - y_rot_point)
+                  +       sin(c_rad) * sin(a_rad) * (joints[JZ] - dz - z_rot_point)
+                  - con * sin(c_rad) * dy
                   + x_rot_point;
 
-    pos->tran.y = - sin(c_rad)              * (joints[JX]      - x_rot_point)
-                  + cos(c_rad) * cos(a_rad) * (joints[JY] - dy - y_rot_point)
-                  + cos(c_rad) * sin(a_rad) * (joints[JZ] - dz - z_rot_point)
-                  + cos(c_rad) * dy
+    pos->tran.y = + con * sin(c_rad)              * (joints[JX]      - x_rot_point)
+                  +       cos(c_rad) * cos(a_rad) * (joints[JY] - dy - y_rot_point)
+                  - con * cos(c_rad) * sin(a_rad) * (joints[JZ] - dz - z_rot_point)
+                  +       cos(c_rad) * dy
                   + y_rot_point;
 
     pos->tran.z = + 0
-                  - sin(a_rad) * (joints[JY] - dy - y_rot_point)
+                  + con * sin(a_rad) * (joints[JY] - dy - y_rot_point)
                   + cos(a_rad) * (joints[JZ] - dz - z_rot_point)
                   + dz
                   + z_rot_point;
@@ -203,36 +206,36 @@ int xyzacKinematicsInverse(const EmcPose * pos,
 {
     (void)iflags;
     (void)fflags;
-    double x_rot_point = *(haldata->x_rot_point);
-    double y_rot_point = *(haldata->y_rot_point);
-    double z_rot_point = *(haldata->z_rot_point);
-    double         dy  = *(haldata->y_offset);
-    double         dz  = *(haldata->z_offset);
-    double         dt  = *(haldata->tool_offset);
-    double      a_rad  = pos->a*TO_RAD;
-    double      c_rad  = pos->c*TO_RAD;
+    const double x_rot_point = *(haldata->x_rot_point);
+    const double y_rot_point = *(haldata->y_rot_point);
+    const double z_rot_point = *(haldata->z_rot_point);
+    const double         dy  = *(haldata->y_offset);
+    const double         dt  = *(haldata->tool_offset);
+    const double         dz  = *(haldata->z_offset) + dt;
+    const double      a_rad  = pos->a*TO_RAD;
+    const double      c_rad  = pos->c*TO_RAD;
 
-    EmcPose P; // computed position
+    const real_t con = *(haldata->conventional_directions) ? 1.0 : -1.0;
 
-    dz = dz + dt;
+    EmcPose P; // computed position
 
-    P.tran.x   = + cos(c_rad)              * (pos->tran.x - x_rot_point)
-                 - sin(c_rad)              * (pos->tran.y - y_rot_point)
+    P.tran.x   = +       cos(c_rad)              * (pos->tran.x - x_rot_point)
+                 + con * sin(c_rad)              * (pos->tran.y - y_rot_point)
                  + x_rot_point;
 
-    P.tran.y   = + sin(c_rad) * cos(a_rad) * (pos->tran.x - x_rot_point)
-                 + cos(c_rad) * cos(a_rad) * (pos->tran.y - y_rot_point)
-                 -              sin(a_rad) * (pos->tran.z - z_rot_point)
-                 -              cos(a_rad) * dy
-                 +              sin(a_rad) * dz
+    P.tran.y   = - con * sin(c_rad) * cos(a_rad) * (pos->tran.x - x_rot_point)
+                 +       cos(c_rad) * cos(a_rad) * (pos->tran.y - y_rot_point)
+                 + con *              sin(a_rad) * (pos->tran.z - z_rot_point)
+                 -                    cos(a_rad) * dy
+                 - con *              sin(a_rad) * dz
                  + dy
                  + y_rot_point;
 
-    P.tran.z   = + sin(c_rad) * sin(a_rad) * (pos->tran.x - x_rot_point)
-                 + cos(c_rad) * sin(a_rad) * (pos->tran.y - y_rot_point)
-                 +              cos(a_rad) * (pos->tran.z - z_rot_point)
-                 -              sin(a_rad) * dy
-                 -              cos(a_rad) * dz
+    P.tran.z   = +       sin(c_rad) * sin(a_rad) * (pos->tran.x - x_rot_point)
+                 - con * cos(c_rad) * sin(a_rad) * (pos->tran.y - y_rot_point)
+                 +                    cos(a_rad) * (pos->tran.z - z_rot_point)
+                 + con *              sin(a_rad) * dy
+                 -                    cos(a_rad) * dz
                  + dz
                  + z_rot_point;
 
@@ -264,31 +267,31 @@ int xyzbcKinematicsForward(const double *joints,
     (void)fflags;
     (void)iflags;
     // Note: 'principal' joints are used
-    double x_rot_point = *(haldata->x_rot_point);
-    double y_rot_point = *(haldata->y_rot_point);
-    double z_rot_point = *(haldata->z_rot_point);
-    double          dx = *(haldata->x_offset);
-    double          dz = *(haldata->z_offset);
-    double          dt = *(haldata->tool_offset);
-                    dz = dz + dt;
-    double       b_rad = joints[JB]*TO_RAD;
-    double       c_rad = joints[JC]*TO_RAD;
-
-
-    pos->tran.x =   cos(c_rad) * cos(b_rad) * (joints[JX] - dx - x_rot_point)
-                  + sin(c_rad) *              (joints[JY]      - y_rot_point)
-                  - cos(c_rad) * sin(b_rad) * (joints[JZ] - dz - z_rot_point)
-                  + cos(c_rad) * dx
+    const double x_rot_point = *(haldata->x_rot_point);
+    const double y_rot_point = *(haldata->y_rot_point);
+    const double z_rot_point = *(haldata->z_rot_point);
+    const double          dx = *(haldata->x_offset);
+    const double          dt = *(haldata->tool_offset);
+    const double          dz = *(haldata->z_offset) + dt;
+    const double       b_rad = joints[JB]*TO_RAD;
+    const double       c_rad = joints[JC]*TO_RAD;
+
+    const real_t con = *(haldata->conventional_directions) ? 1.0 : -1.0;
+
+    pos->tran.x =         cos(c_rad) * cos(b_rad) * (joints[JX] - dx - x_rot_point)
+                  - con * sin(c_rad) *              (joints[JY]      - y_rot_point)
+                  + con * cos(c_rad) * sin(b_rad) * (joints[JZ] - dz - z_rot_point)
+                  +       cos(c_rad) * dx
                   + x_rot_point;
 
-    pos->tran.y = - sin(c_rad) * cos(b_rad) * (joints[JX] - dx - x_rot_point)
-                  + cos(c_rad) *              (joints[JY]      - y_rot_point)
-                  + sin(c_rad) * sin(b_rad) * (joints[JZ] - dz - z_rot_point)
-                  - sin(c_rad) * dx
+    pos->tran.y = + con * sin(c_rad) * cos(b_rad) * (joints[JX] - dx - x_rot_point)
+                  +       cos(c_rad) *              (joints[JY]      - y_rot_point)
+                  +       sin(c_rad) * sin(b_rad) * (joints[JZ] - dz - z_rot_point)
+                  + con * sin(c_rad) * dx
                   + y_rot_point;
 
-    pos->tran.z =   sin(b_rad) * (joints[JX] - dx - x_rot_point)
-                  + cos(b_rad) * (joints[JZ] - dz - z_rot_point)
+    pos->tran.z = - con * sin(b_rad) * (joints[JX] - dx - x_rot_point)
+                  +       cos(b_rad) * (joints[JZ] - dz - z_rot_point)
                   + dz
                   + z_rot_point;
 
@@ -311,33 +314,34 @@ int xyzbcKinematicsInverse(const EmcPose * pos,
 {
     (void)iflags;
     (void)fflags;
-    double x_rot_point = *(haldata->x_rot_point);
-    double y_rot_point = *(haldata->y_rot_point);
-    double z_rot_point = *(haldata->z_rot_point);
-    double          dx = *(haldata->x_offset);
-    double          dz = *(haldata->z_offset);
-    double          dt = *(haldata->tool_offset);
-                    dz = dz + dt;
-    double       b_rad = pos->b*TO_RAD;
-    double       c_rad = pos->c*TO_RAD;
-    double         dpx = -cos(b_rad)*dx - sin(b_rad)*dz + dx;
-    double         dpz =  sin(b_rad)*dx - cos(b_rad)*dz + dz;
+    const double x_rot_point = *(haldata->x_rot_point);
+    const double y_rot_point = *(haldata->y_rot_point);
+    const double z_rot_point = *(haldata->z_rot_point);
+    const double          dx = *(haldata->x_offset);
+    const double          dt = *(haldata->tool_offset);
+    const double          dz = *(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;
+
+    const real_t con = *(haldata->conventional_directions) ? 1.0 : -1.0;
 
     EmcPose P; // computed position
 
-    P.tran.x   = + cos(c_rad) * cos(b_rad) * (pos->tran.x - x_rot_point)
-                 - sin(c_rad) * cos(b_rad) * (pos->tran.y - y_rot_point)
-                 + sin(b_rad) *              (pos->tran.z - z_rot_point)
+    P.tran.x   = +       cos(c_rad) * cos(b_rad) * (pos->tran.x - x_rot_point)
+                 + con * sin(c_rad) * cos(b_rad) * (pos->tran.y - y_rot_point)
+                 - con *              sin(b_rad) * (pos->tran.z - z_rot_point)
                  + dpx
                  + x_rot_point;
 
-    P.tran.y   = + sin(c_rad) * (pos->tran.x - x_rot_point)
-                 + cos(c_rad) * (pos->tran.y - y_rot_point)
+    P.tran.y   = - con * sin(c_rad) * (pos->tran.x - x_rot_point)
+                 +       cos(c_rad) * (pos->tran.y - y_rot_point)
                  + y_rot_point;
 
-    P.tran.z   = - cos(c_rad) * sin(b_rad) * (pos->tran.x - x_rot_point)
-                 + sin(c_rad) * sin(b_rad) * (pos->tran.y - y_rot_point)
-                 + cos(b_rad) *              (pos->tran.z - z_rot_point)
+    P.tran.z   = + con * cos(c_rad) * sin(b_rad) * (pos->tran.x - x_rot_point)
+                 +       sin(c_rad) * sin(b_rad) * (pos->tran.y - y_rot_point)
+                 +                    cos(b_rad) * (pos->tran.z - z_rot_point)
                  + dpz
                  + z_rot_point;