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); }
