custom kinematics causes joint following errors depending on speed

More
09 Dec 2016 00:07 #83818 by alexfloca
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); }

Please Log in or Create an account to join the conversation.

More
09 Dec 2016 00:24 #83819 by alexfloca
just tested using rotatekins and not my custom kinematics module, very same results, same critical feedrate where following error occures.

Please Log in or Create an account to join the conversation.

More
09 Dec 2016 01:28 #83820 by PCW
can you post your hal and ini files?

Please Log in or Create an account to join the conversation.

More
09 Dec 2016 06:31 #83828 by alexfloca

File Attachment:

File Name: 5axemm.hal
File Size:5 KB

File Attachment:

File Name: 5axemm.ini
File Size:3 KB


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
Attachments:

Please Log in or Create an account to join the conversation.

More
09 Dec 2016 18:01 #83833 by PCW
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

Please Log in or Create an account to join the conversation.

More
10 Dec 2016 01:38 #83871 by andypugh
I suspect that the problem is due to you not having a [TRAJ]DEFAULT_ACCELERATION

linuxcnc.org/docs/2.7/html/config/ini-config.html#_traj_section

Also, your [TRAJ]MAX_VELOCITY seems to be a bad match to the axis velocities.

(I realise that it is not at all clear how these settings all interact. And I am not in a position to fix that, I don't know either)

Please Log in or Create an account to join the conversation.

More
11 Dec 2016 21:59 #83927 by alexfloca
Hi Andy,

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

thanks for the hint!
Alex

Please Log in or Create an account to join the conversation.

Time to create page: 0.161 seconds
Powered by Kunena Forum