5 axis - SRT switchable kinematics

Hi,
In the process of changing from Mach3 /4 to Linux cnc. Have 90% of linux cnc, mesa etc all ready to go
 
Now looking for the best starting point for 5 axis Kinematics, 
Bridge style 5 axis machine, Y table, X, Z, C, A on the bridge,
C rotation about Z, +/- 360 degrees and A aixs - after the C axis, (can change to tilt about the X axis +-120deg.
C axis to the spindle (in the Z axis center line) has a small offset of around 30mm.

I had been working through XYZBC-srt from Rudy du Preez's paper, and edited for our setup, but looking at the new Kinematics in 2.9, I think I have a bit more to learn for the updates. and on how to switch the TCP kinematics on and off as needed.

looking for a start point for a switchable system, able to use tool center point control?
The machine is currently running Mach3 and I have customized the PP in fusion to handle the 5 axis head offsets,
however doing the TCP conversion in the post process is not ideal, hence the move to LinucCNC.

Thanks

Just got sim xyzac-trt running, looks perfect - just edit to SRT machine and add in the required offsets for the machine

Thanks

I have almost everything working - one issue remaining when using the TCP kinematics with the GUI DRO's that i can not solve.

I have set up another 3 axis router with linuxcnc this week - the machine works well.

For the 5 axis - I have used 5axiskins.c from the bridgemill sim example and added code for the spindle offset. - Code below.

This works correctly in the vismach simulator. ( i had an issue after zeroing an axis with the DRO updates)

On the real machine config, the axis moves generated by the kinematics appear to be fighting the gui / control command position.

using trivkins the machine config works correctly - all dro data and motion are correct.

when I switch to 5axiskins.c ( bridge kinematics )
The first issue - the DRO does not update when the machine is doing the home reference. Servo's move - can see the feedback in HAL show on the Mesa card,  x-pos-fb, y-pos-fb pins etc.
after completing the home reference - DRO works with normal MDI commands

second issue.
moving B and C axis under TCP kinematics:
The x,y,z servos move with the TCP kins correction data as expected. HAL data looks correct but the DRO does not update correctly.
the x-axis DRO moves while Y and Z do not.
X DRO then jumps back to the original position with axis.0 follow error. 
for example, I move the B axis, X-axis adjusts for a while then jumps back to the original value - The servo does not follow the jump.
the Y and Z axis DRO flicker but keep the original value,
but the y axis and z axis servo are moving with the TCP correction

looking for any ideas for anything I might have missed in the configurations.

Config is mostly clean from pconf - edit for 5 axis machine and 7i95mesa
added pins for the x,y offsets in the updated kins code.

running Debian 11, linuxcnc master branch

Thanks again for help.
Andrew

Modified 5akiskins.c
/********************************************************************
* Description: 5axiskins.c
*   kinematics for XYZBC 5 axis bridge mill
*
*   Derived from a work by Fred Proctor & Will Shackleford
*
* Author:
* License: GPL Version 2
* System: Linux
*
* 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)
*  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)
*     wrt z axis
*  4) W axis: tool motion. Negative values increase tool radial
*     motion example: drilling into body at b,c angles
*  5) W axis motion is incorporated into the motion of the
*     joints used for X,Y,Z positioning and no motor or
*     hal pin connections are required for the joint specified
*     as JW.  However, a joint must be configured for W to
*     support display of the W axis letter value for
*     complicated reasons. (motion/control.c computes joint
*     positions only for the number of configured kinematic
*     joints (NO_OF_KINS_JOINTS) and the joint positions
*     are needed to display axis letters via inverse
*     kinematics.
*  6) If no coordinates module parameter is supplied, kins
*     will use the required coordinates XYZBCW mapped
*     to joints 0..5 in sequence.
*  7) Multiple joints may be assigned to an axis letter
*     with the module coordinates parameter
*  8) If a coordinates module parameter is supplied,
*     the kins will map coordinate letters in sequence
*     to joint numbers beginning with joint 0.
*  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.
********************************************************************/

// non-required coordinates (A,U,V) can be set by using
// the module coordinates parameter
#define REQUIRED_COORDINATES "XYZBCW"

#define DEFAULT_PIVOT_LENGTH 250;

#define DEFAULT_X_OFFSET 0;
#define DEFAULT_Y_OFFSET 0;

#include "motion.h"
#include "hal.h"
#include "rtapi.h"
#include "rtapi_math.h"
#include "rtapi_string.h"
#include "rtapi_ctype.h"
#include "kinematics.h"
#include "posemath.h"
#include "switchkins.h"

struct haldata {
    hal_float_t *pivot_length;
    hal_float_t *x_offset;  
    hal_float_t *y_offset;
} *haldata;
static int fiveaxis_max_joints;

static PmCartesian s2r(double r, double t, double p) {
    // s2r: spherical coordinates to cartesian coordinates
    // r       = length of vector
    // p=phi   = angle of vector wrt z axis
    // t=theta = angle of vector projected onto xy plane
    //           (projection length in xy plane is r*sin(p)
    PmCartesian c;
    t = TO_RAD*t; p = TO_RAD*p; // degrees to radians

    c.x = r * sin(p) * cos(t);
    c.y = r * sin(p) * sin(t);
    c.z = r * cos(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)
{
    PmCartesian r = s2r(*(haldata->pivot_length) + joints[JW],
                        joints[JC],
                        180.0 - joints[JB]);

    // AJ Get x and y offset and radians of C axis angle
    double c_x = *(haldata->x_offset);
    double c_y = *(haldata->y_offset);
    double c_r = TO_RAD*joints[JC];

    // Note: 'principal' joints are used
    pos->tran.x = joints[JX] + r.x + c_x*cos(c_r) + c_y*sin(c_r) - c_x; // AJ add correction for C axis offsets
    pos->tran.y = joints[JY] + r.y + c_x*sin(c_r) + c_y*cos(c_r) - c_y; // AJ add correction for C axis offsets
    pos->tran.z = joints[JZ] + *(haldata->pivot_length) + r.z;
    pos->b      = joints[JB];
    pos->c      = joints[JC];
    pos->w      = joints[JW];

    // optional letters (specify with coordinates module parameter)
    pos->a = (JA != -1)? joints[JA] : 0;
    pos->u = (JU != -1)? joints[JU] : 0;
    pos->v = (JV != -1)? joints[JV] : 0;

    return 0;
} //fiveaxis_KinematicsForward()

static int fiveaxis_KinematicsInverse(const EmcPose * pos,
                                      double *joints,
                                      const KINEMATICS_INVERSE_FLAGS * iflags,
                                      KINEMATICS_FORWARD_FLAGS * fflags)
{
    PmCartesian r = s2r(*(haldata->pivot_length) + pos->w,
                        pos->c,
                        180.0 - pos->b);

    // AJ Get x and y offset and radians of C axis angle
    double c_x = *(haldata->x_offset);
    double c_y = *(haldata->y_offset);
    double c_r = TO_RAD*pos->c;

    EmcPose P;  // computed position
    P.tran.x = pos->tran.x  - r.x 
                            - c_x*cos(c_r) // AJ - C axis X offset 
                            + c_y*sin(c_r) // AJ - C axis Y offset
                            + c_x; // AJ - offset - for setup tool at workpiece 0 and axis includes offset to 0
    P.tran.y = pos->tran.y  - r.y 
                            - c_x*sin(c_r) // AJ -  C axis x offset
                            - c_y*cos(c_r) // AJ - C axis y offset
                            + c_y; // AJ - offset - for setup tool at workpiece 0 and axis includes offset to 0
    P.tran.z = pos->tran.z - *(haldata->pivot_length) - r.z;
    P.b = pos->b;
    P.c = pos->c;
    P.w = pos->w;

    // optional letters (specify with coordinates module parameter)
    P.a = (JA != -1)? pos->a : 0;
    P.u = (JU != -1)? pos->u : 0;
    P.v = (JV != -1)? pos->v : 0;

    // 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()

int fiveaxis_KinematicsSetup(const  int   comp_id,
                             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);
    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; jno<EMCMOT_MAX_JOINTS; jno++) {
        if (axis_idx_for_jno[jno] == 0) {if (JX == -1) JX=jno;}
        if (axis_idx_for_jno[jno] == 1) {if (JY == -1) JY=jno;}
        if (axis_idx_for_jno[jno] == 2) {if (JZ == -1) JZ=jno;}
        if (axis_idx_for_jno[jno] == 3) {if (JA == -1) JA=jno;}
        if (axis_idx_for_jno[jno] == 4) {if (JB == -1) JB=jno;}
        if (axis_idx_for_jno[jno] == 5) {if (JC == -1) JC=jno;}
        if (axis_idx_for_jno[jno] == 6) {if (JU == -1) JU=jno;}
        if (axis_idx_for_jno[jno] == 7) {if (JV == -1) JV=jno;}
        if (axis_idx_for_jno[jno] == 8) {if (JW == -1) JW=jno;}
    }

    haldata = hal_malloc(sizeof(struct haldata));

    result = hal_pin_float_newf(HAL_IN,&;(haldata->pivot_length),    comp_id,
                                "%s.pivot-length",kp->halprefix);
    result += hal_pin_float_newf(HAL_IN,&;(haldata->x_offset),       comp_id,    // AJ - get x offset
                                "%s.x-offset",kp->halprefix);
    result += hal_pin_float_newf(HAL_IN,&;(haldata->y_offset),       comp_id,    // AJ - get Y offset
                                "%s.y-offset",kp->halprefix);
    if(result < 0) goto error;

    // *haldata->pivot_length = DEFAULT_PIVOT_LENGTH;
    // *haldata->x_offset = DEFAULT_X_OFFSET;
    // *haldata->y_offset = DEFAULT_Y_OFFSET;

    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",*haldata->pivot_length);
    rtapi_print("  default x-offset = %.3f\n",*haldata->x_offset); // AJ
    rtapi_print("  default y-offset = %.3f\n",*haldata->y_offset); // AJ

    return 0;

error:
    return -1;
} // fiveaxis_KinematicsSetup()

int switchkinsSetup(kparms* kp,
                    KS* kset0, KS* kset1, KS* kset2,
                    KF* kfwd0, KF* kfwd1, KF* kfwd2,
                    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;

    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;
    } else {
        rtapi_print("\n!!! switchkins-type 0 is %s\n",kp->kinsname);
        *kset0 = fiveaxis_KinematicsSetup;
        *kfwd0 = fiveaxis_KinematicsForward;
        *kinv0 = fiveaxis_KinematicsInverse;

        *kset1 = identityKinematicsSetup;
        *kfwd1 = identityKinematicsForward;
        *kinv1 = identityKinematicsInverse;
    }
    *kset2 = userkKinematicsSetup;
    *kfwd2 = userkKinematicsForward;
    *kinv2 = userkKinematicsInverse;

    return 0;
} // switchkinsSetup()
 

Found the problems.

a small error in the forward kinematics maths, mixed one + / -

the second was speed mismatch on the axis velocities with the bench test setup.

ready to install on the real machine next...