I just switched over to the development branch from 2.7.8 and the traj planner is much better. However, I may have found an issue.
Within my kins modules i am leveraging the pos->tran.z value and when a 'Z' tool offset is established the pos->tran.z value does not include the offset even after a G0 Zx command.
Code:
/********************************************************************
* Description: XZABkins.c
* Derived from trivkins for 3 axis Cartesian machine
* Established for 4-axis lathe (XZAB) where A rotates around X in Z-Yplane and B rotates around Y in Z-Xplane
*
* Derived from a work by Fred Proctor & Will Shackleford
*
* License: GPL Version 2
*
* Copyright (c) 2009 All rights reserved.
*
********************************************************************/
#include "motion.h"
#include "posemath.h"
#include "hal.h"
#include "rtapi.h"
#include "rtapi.h" /* RTAPI realtime OS API */
#include "rtapi_app.h" /* RTAPI realtime module decls */
#include "rtapi_math.h"
#include "rtapi_string.h"
struct haldata {
hal_float_t *Tool_offset;
hal_s32_t *Tool_number;
hal_float_t *J0;
hal_float_t *J1;
hal_float_t *J3;
hal_float_t *Px;
hal_float_t *Pz;
hal_float_t *Pb;
hal_float_t *FwdRad;
hal_float_t *InvRad;
} *haldata;
hal_float_t Last_Tool_offset = 0.00;
hal_s32_t Last_Tool_number = 0;
int kinematicsForward(const double *joints,
EmcPose * pos,
const KINEMATICS_FORWARD_FLAGS * fflags,
KINEMATICS_INVERSE_FLAGS * iflags)
{
double b_rad;
double dT = *(haldata->Tool_offset);
long lTN = *(haldata->Tool_number);
double b_adj = 0.00;
if (lTN == 100 && dT != 0.00) {
b_adj = 90.00;
}
b_rad = (joints[3] + b_adj)*TO_RAD; /* B-axis offset */
pos->tran.x = joints[0] - (dT * sin(b_rad));
pos->tran.z = joints[1] - (dT * cos(b_rad)) + dT;
pos->a = joints[2];
pos->b = joints[3] + b_adj ;
*(haldata->Px) = pos->tran.x;
*(haldata->Pz) = pos->tran.z;
*(haldata->Pb) = pos->b;
*(haldata->InvRad) = b_rad;
return 0;
}
int kinematicsInverse(const EmcPose * pos,
double *joints,
const KINEMATICS_INVERSE_FLAGS * iflags,
KINEMATICS_FORWARD_FLAGS * fflags)
{
double b_rad;
double dT = *(haldata->Tool_offset);
long lTN = *(haldata->Tool_number);
double b_adj = 0.00;
if (lTN == 100 && dT != 0.00) {
b_adj = 90.00;
}
b_rad = (pos->b)*TO_RAD; /* B-axis offset */
joints[0] = pos->tran.x + (dT * sin(b_rad));
joints[1] = (pos->tran.z - dT) + (dT * cos(b_rad)) ;
joints[2] = pos->a;
joints[3] = pos->b - b_adj;
*(haldata->J0) = joints[0];
*(haldata->J1) = joints[1];
*(haldata->J3) = joints[3];
*(haldata->FwdRad) = b_rad;
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);
}
static KINEMATICS_TYPE ktype = -1;
KINEMATICS_TYPE kinematicsType()
{
return ktype;
}
static char *kinstype = "b"; // use KINEMATICS_BOTH
RTAPI_MP_STRING(kinstype, "Kinematics Type (Identity,Both)");
EXPORT_SYMBOL(kinematicsType);
EXPORT_SYMBOL(kinematicsForward);
EXPORT_SYMBOL(kinematicsInverse);
MODULE_LICENSE("GPL");
int comp_id;
int rtapi_app_main(void) {
int res = 0;
comp_id = hal_init("XZABkins");
if(comp_id < 0) return comp_id;
haldata = hal_malloc(sizeof(struct haldata));
switch (*kinstype) {
case 'b': case 'B': ktype = KINEMATICS_BOTH; break;
case 'f': case 'F': ktype = KINEMATICS_FORWARD_ONLY; break;
case 'i': case 'I': ktype = KINEMATICS_INVERSE_ONLY; break;
case '1': default: ktype = KINEMATICS_IDENTITY;
}
if((res = hal_pin_float_new("XZABkins.Tool-offset", HAL_IN, &(haldata->Tool_offset),
comp_id)) < 0) goto error;
if((res = hal_pin_s32_new("XZABkins.Tool-number", HAL_IN, &(haldata->Tool_number),
comp_id)) < 0) goto error;
if((res = hal_pin_float_new("XZABkins.Joint0", HAL_OUT, &(haldata->J0),
comp_id)) < 0) goto error;
if((res = hal_pin_float_new("XZABkins.Joint1", HAL_OUT, &(haldata->J1),
comp_id)) < 0) goto error;
if((res = hal_pin_float_new("XZABkins.Joint3", HAL_OUT, &(haldata->J3),
comp_id)) < 0) goto error;
if((res = hal_pin_float_new("XZABkins.PosX", HAL_OUT, &(haldata->Px),
comp_id)) < 0) goto error;
if((res = hal_pin_float_new("XZABkins.PosZ", HAL_OUT, &(haldata->Pz),
comp_id)) < 0) goto error;
if((res = hal_pin_float_new("XZABkins.PosB", HAL_OUT, &(haldata->Pb),
comp_id)) < 0) goto error;
if((res = hal_pin_float_new("XZABkins.FwdRad", HAL_OUT, &(haldata->FwdRad),
comp_id)) < 0) goto error;
if((res = hal_pin_float_new("XZABkins.InvRad", HAL_OUT, &(haldata->InvRad),
comp_id)) < 0) goto error;
hal_ready(comp_id);
return 0;
error:
hal_exit(comp_id);
return res;
}
void rtapi_app_exit(void) { hal_exit(comp_id); }
