So i worked the whole day with resources from the forum and ChatGPT to create this Kinematic. ChatGPT checked the math forwards and inverse and it seems to check out. I try to set up vismach asap and check it for myself.
component xyzcb_tdr_kins "Switchable kinematics for 5 axis machine with rotary table C and B and virtual Y Axis";
description
"""
This is a switchable kinematics module for a 5-axis milling configuration
using 3 cartesian linear joints (XYZ) and 2 rotary table joints (CB).
The module contains two kinematic models:
type0 (default) is a trivial XYZCB configuration with joints 0..4 mapped to
axes XYZCB respectively.
type1 is a XYZCB configuration with tool center point (TCP) compensation.
Additionally a virtual Y Axis was added for a Mazak Integrex 200Y
""";
pin out s32 dummy=0"one pin needed to satisfy halcompile requirement";
license "GPL";
author "Jochen Rohm";
;;
#include "rtapi_math.h"
#include "kinematics.h"
static struct haldata {
// Declare hal pin pointers used for XYZCB_tdr kinematics:
hal_float_t *tool_offset_z;
hal_float_t *x_offset;
hal_float_t *z_offset;
hal_float_t *x_rot_point;
hal_float_t *y_rot_point;
hal_float_t *z_rot_point;
hal_float_t *angleXX;
//Declare hal pin pointers used for switchable kinematics
hal_bit_t *kinstype_is_0;
hal_bit_t *kinstype_is_1;
} *haldata;
static int XYZCB_tdr_setup(void) {
#define HAL_PREFIX "xyzcb_tdr_kins"
int res=0;
// inherit comp_id from rtapi_main()
if (comp_id < 0) goto error;
// set unready to allow creation of pins
if (hal_set_unready(comp_id)) goto error;
haldata = hal_malloc(sizeof(struct haldata));
if (!haldata) goto error;
// hal pins required for XYZCB_tdr kinematics:
res += hal_pin_float_newf(HAL_IN ,&(haldata->tool_offset_z) ,comp_id,"%s.tool-offset-z" ,HAL_PREFIX);
res += hal_pin_float_newf(HAL_IN ,&(haldata->x_offset) ,comp_id,"%s.x-offset" ,HAL_PREFIX);
res += hal_pin_float_newf(HAL_IN ,&(haldata->z_offset) ,comp_id,"%s.z-offset" ,HAL_PREFIX);
res += hal_pin_float_newf(HAL_IN ,&haldata->x_rot_point ,comp_id,"%s.x-rot-point" ,HAL_PREFIX);
res += hal_pin_float_newf(HAL_IN ,&haldata->y_rot_point ,comp_id,"%s.y-rot-point" ,HAL_PREFIX);
res += hal_pin_float_newf(HAL_IN ,&haldata->z_rot_point ,comp_id,"%s.z-rot-point" ,HAL_PREFIX);
// hal pins required for switchable kinematics:
res += hal_pin_bit_new("kinstype.is-0", HAL_OUT, &(haldata->kinstype_is_0), comp_id);
res += hal_pin_bit_new("kinstype.is-1", HAL_OUT, &(haldata->kinstype_is_1), comp_id);
// define default kinematics at startup for switchable kinematics
*haldata->kinstype_is_0 = 1; //default at startup -> identity kinematics
*haldata->kinstype_is_1 = 0; //-> XYZCB TCP
if (res) goto error;
hal_ready(comp_id);
rtapi_print("*** %s setup ok\n",__FILE__);
return 0;
error:
rtapi_print("\n!!! %s setup failed res=%d\n\n",__FILE__,res);
return -1;
#undef HAL_PREFIX
}
EXPORT_SYMBOL(kinematicsType);
EXPORT_SYMBOL(kinematicsSwitchable);
EXPORT_SYMBOL(kinematicsSwitch);
EXPORT_SYMBOL(kinematicsInverse);
EXPORT_SYMBOL(kinematicsForward);
static hal_u32_t switchkins_type;
int kinematicsSwitchable() {return 1;}
int kinematicsSwitch(int new_switchkins_type)
{
switchkins_type = new_switchkins_type;
rtapi_print("kinematicsSwitch(): type=%d\n",switchkins_type);
// create case structure for switchable kinematics
switch (switchkins_type) {
case 0: rtapi_print_msg(RTAPI_MSG_INFO,
"kinematicsSwitch:TYPE0\n");
*haldata->kinstype_is_0 = 1;
*haldata->kinstype_is_1 = 0;
break;
case 1: rtapi_print_msg(RTAPI_MSG_INFO,
"kinematicsSwitch:TYPE1\n");
*haldata->kinstype_is_0 = 0;
*haldata->kinstype_is_1 = 1;
break;
default: rtapi_print_msg(RTAPI_MSG_ERR,
"kinematicsSwitch:BAD VALUE <%d>\n",
switchkins_type);
*haldata->kinstype_is_1 = 0;
*haldata->kinstype_is_0 = 0;
return -1; // FAIL
}
return 0; // ok
}
KINEMATICS_TYPE kinematicsType()
{
static bool is_setup=0;
if (!is_setup) XYZCB_tdr_setup();
return KINEMATICS_BOTH; // set as required
// Note: If kinematics are identity, using KINEMATICS_BOTH
// may be used in order to allow a gui to display
// joint values in preview prior to homing
} // kinematicsType()
int kinematicsForward(const double *j,
EmcPose * pos,
const KINEMATICS_FORWARD_FLAGS * fflags,
KINEMATICS_INVERSE_FLAGS * iflags)
{
(void)fflags;
(void)iflags;
double x_rot_point = *(haldata->x_rot_point);
double y_rot_point = *(haldata->y_rot_point);
double z_rot_point = *(haldata->z_rot_point);
double dz = *(haldata->z_offset);
double dt = *(haldata->tool_offset_z);
// substitutions as used in mathematical documentation
// including degree -> radians angle conversion
double sc = sin(j[3]*TO_RAD);
double cc = cos(j[3]*TO_RAD);
double sb = sin(j[4]*TO_RAD);
double cb = cos(j[4]*TO_RAD);
// used to be consistent with math in the documentation
double px = 0;
double py = 0;
double pz = 0;
#define DEFAULT_ANGLE (60.0)
// define forward kinematic models using case structure for
// for switchable kinematics
switch (switchkins_type) {
case 0: // ====================== IDENTITY kinematics FORWARD ====================
double angle = *(haldata->angleXX) * PM_PI / 180.0;
pos->tran.x = j[0] + j[1] * cos(angle)
pos->tran.y = - j[1] * sin(angle);
pos->tran.z = j[2];
pos->c = j[3];
pos->b = j[4];
break;
case 1: // ========================= TCP kinematics FORWARD ======================
px = j[0] - x_rot_point + j[1] * cos(angle);
py = - (j[1] - y_rot_point) * sin(angle);
pz = j[2] - z_rot_point - dt;
pos->tran.x = cb * (cc * px - sc * py) + sb * pz + x_rot_point;
pos->tran.y = sc * px + cc * py + y_rot_point;
pos->tran.z = -sb * (cc * px - sc * py) + cb * pz + z_rot_point + dz + dt;
pos->c = j[3];
pos->b = j[4];
break;
}
// unused coordinates:
pos->a = 0;
pos->u = 0;
pos->v = 0;
pos->w = 0;
return 0;
} // kinematicsForward()
int kinematicsInverse(const EmcPose * pos,
double *j,
const KINEMATICS_INVERSE_FLAGS * iflags,
KINEMATICS_FORWARD_FLAGS * fflags)
{
(void)iflags;
(void)fflags;
double x_rot_point = *(haldata->x_rot_point);
double y_rot_point = *(haldata->y_rot_point);
double z_rot_point = *(haldata->z_rot_point);
double dx = *(haldata->x_offset);
double dz = *(haldata->z_offset);
double dt = *(haldata->tool_offset_z);
// substitutions as used in mathematical documentation
// including degree -> radians angle conversion
double sc = sin(pos->c*TO_RAD);
double cc = cos(pos->c*TO_RAD);
double sb = sin(pos->b*TO_RAD);
double cb = cos(pos->b*TO_RAD);
// used to be consistent with math in the documentation
double qx = 0;
double qy = 0;
double qz = 0;
switch (switchkins_type) {
case 0:// ====================== IDENTITY kinematics INVERSE =====================
double angle = *(haldata->angleXX) * PM_PI / 180.0;
j[0] = pos->tran.x + pos->tran.y / tan(angle);
j[1] = - pos->tran.y / sin(angle);
j[2] = pos->tran.z;
j[3] = pos->c;
j[4] = pos->b;
break;
case 1: // ========================= TCP kinematics INVERSE ======================
qx = pos->tran.x - x_rot_point - dx;
qy = pos->tran.y - y_rot_point;
qz = pos->tran.z - z_rot_point - dz - dt;
// Hier integrieren wir die virtuelle Achse (pos->tran.y) wie im Identity-Fall:
double qx_virtual = qx + qy / tan(angle);
double qy_virtual = -qy / sin(angle);
j[0] = cc * (cb * qx_virtual - sb * qz) + sc * qy_virtual + cb * dx - sb * dz + x_rot_point;
j[1] = -sc * (cb * qx_virtual - sb * qz) + cc * qy_virtual + y_rot_point;
j[2] = sb * qx_virtual + cb * qz + sb * dx + cb * dz + z_rot_point + dt;
j[3] = pos->c;
j[4] = pos->b;
break;
}
return 0;
} // kinematicsInverse()
