custom kinematics causes joint following errors depending on speed

I have developed a custom kinematics module for a 5 axis router with a rotating-tilting head and rotary axis position error compensation.
Every thing works just fine if I jog the machine up to approx 1.400 mm/min in world mode. In joint mode, I can jog any axis up to the maximum speed of 15000 mm/min. As soon as I increase the jog speed in world mode over the 1400 mm/min, I get a following error in the axis I jog.
From the linuxcnc point of view we are talking about a basic stepper configuration using the parport driver.
I tried optimizing the trigonometry calculations with no luck, no increase in jog speed. I even tried commenting out the whole trigonometric calculations (having this custom kinematics module act similar to trivkins) with no luck, same maximum jog speed. The only thing that works is increasing the MIN_FERROR and FERROR for every axis in the .ini file.

Any ideeas where this limitation/error can come from?

I am using linuxcnc 2.7.8, kinematics as follows:


#include "kinematics.h" /* these decls */
#include "posemath.h"
#include "hal.h"
#include "rtapi.h"
#include "rtapi_math.h"

#define d2r(d) ((d)*PM_PI/180.0)
#define r2d(r) ((r)*180.0/PM_PI)

struct haldata {
hal_float_t *tool_length;
hal_float_t *pivot_length;

} *haldata;

static PmCartesian s2r(double r, double t, double p ) {
PmCartesian c;

double dx = 0.65;
double dy = 0.05;
double dxdx = 0.4225;
double dydy = 0.0025;

t = d2r(t), p = d2r(p);
double cosp = cos(p);
double sinp = sin(p);
double dydx = 0.0769230769;

double t1 = sqrt(dxdx*cosp*cosp + dydy);
double t2 = t + atan(dydx/cosp); //dy/dx

c.x = t1*cos(t2) - r * sinp*cos(t);
c.y = t1*sin(t2) - r * sinp*sin(t);
c.z = -(dx*sinp+r*cosp);



return c;
}

int kinematicsForward(const double *joints,
EmcPose * pos,
const KINEMATICS_FORWARD_FLAGS * fflags,
KINEMATICS_INVERSE_FLAGS * iflags)
{
PmCartesian r = s2r(*(haldata->pivot_length)+*(haldata->tool_length), joints[5], joints[4]);

pos->tran.x = joints[0] + r.x;
pos->tran.y = joints[1] + r.y;
pos->tran.z = joints[2] + r.z;
pos->a = joints[3];
pos->b = joints[4];
pos->c = joints[5];
pos->u = joints[6];
pos->v = joints[7];
pos->w = joints[8];

return 0;
}

int kinematicsInverse(const EmcPose * pos,
double *joints,
const KINEMATICS_INVERSE_FLAGS * iflags,
KINEMATICS_FORWARD_FLAGS * fflags)
{

PmCartesian r = s2r(*(haldata->pivot_length)+*(haldata->tool_length), pos->c, pos->b);

joints[0] = pos->tran.x - r.x;
joints[1] = pos->tran.y - r.y;
joints[2] = pos->tran.z - r.z;
joints[3] = pos->a;
joints[4] = pos->b;
joints[5] = pos->c;
joints[6] = pos->u;
joints[7] = pos->v;
joints[8] = pos->w;

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;

return kinematicsForward(joint, world, fflags, iflags);
}

KINEMATICS_TYPE kinematicsType()
{
return KINEMATICS_BOTH;
}

#include "rtapi.h" /* RTAPI realtime OS API */
#include "rtapi_app.h" /* RTAPI realtime module decls */
#include "hal.h"

EXPORT_SYMBOL(kinematicsType);
EXPORT_SYMBOL(kinematicsForward);
EXPORT_SYMBOL(kinematicsInverse);
MODULE_LICENSE("GPL");

int comp_id;
int rtapi_app_main(void) {
int result;
comp_id = hal_init("5axecomp");
if(comp_id < 0) return comp_id;

haldata = hal_malloc(sizeof(struct haldata));

result = hal_pin_float_new("5axe.tooloffset.z", HAL_IN, &(haldata->tool_length), comp_id);
if(result < 0) goto error;
result = hal_pin_float_new("5axe.pivot-length", HAL_IO, &(haldata->pivot_length), comp_id);
if(result < 0) goto error;

*(haldata->pivot_length) = 161.0;

hal_ready(comp_id);
return 0;

error:
hal_exit(comp_id);
return result;
}

void rtapi_app_exit(void) { hal_exit(comp_id); }

just tested using rotatekins and not my custom kinematics module, very same results, same critical feedrate where following error occures.

can you post your hal and ini files?

those are the hal and ini files for my test configuration, stripped down to a minimum, no limit switches as no physical machine is connected to this test configuration

With a quick look, I dont see anything obviously wrong there

If you widen your ferror limits and plot ferror in halscope when you jog
it may give a clue to why things are going wrong

Hi Andy,

[TRAJ]DEFAULT_ACCELERATION did a huge difference,now everything works just fine as it was supposed to

thanks for the hint!
Alex