Hello, i want to be able to move the Coordinate system of the Workpiece around the Rotational axis by using custom M Codes (M122 turns it on and M123 turns it off). I already have a theoretically working Kinematics with the help of someone from youtube (theoretically because it wont find the pin i assigned to change the kinematics from TCP to Trivial but when i set the normal state to 0 (Trivial) in the Kinematics the tooltip does not follow anymore), where i can switch between Trivial Kinematics and Tool Center point, where the tooltip follows the same spot in the Work Coordinate system when i rotate the rotational axis. I now want to do the same with PLANE SPATIAL Function where it only calculates the the new position of the work Coordinate system if i command for example A 45 C90. It must not rotate the coordinate system, only move around the Rotational axes so XYZ moves will always be true XYZ moves in the Machine. I made a Picture to Explain it easier. The red Line is the X axis and also the A Rotational Axis, the Blue is Z and also C rotational axis. Are both Kinematics Forward and Inverse needed for this function?
Thanks for any help -- website: http://www.machinekit.io blog: http://blog.machinekit.io github: https://github.com/machinekit --- You received this message because you are subscribed to the Google Groups "Machinekit" group. To unsubscribe from this group and stop receiving emails from it, send an email to machinekit+unsubscr...@googlegroups.com. Visit this group at https://groups.google.com/group/machinekit. For more options, visit https://groups.google.com/d/optout.
/******************************************************************** * Description: XYZACkinsTEST.c * Kinematics for 5 axis mill named XYZAC. * This mill has a tilting table (A axis) and horizontal rotary * mounted to the table (C axis). * with rotary axis offsets * * Author: Rudy du Preez * License: GPL Version 2 * ********************************************************************/ #include "kinematics.h" /* these decls */ #include "posemath.h" #include "hal.h" #include "rtapi.h" #include "rtapi_math.h" #define VTVERSION VTKINEMATICS_VERSION1 struct haldata { hal_float_t *PLANE_enable; // neu hal_float_t *enable; // neu hal_float_t *X_offset; // neu hal_float_t *Y_offset; hal_float_t *Z_offset; hal_float_t *Tool_offset; } *haldata; static int kinematicsForward(const double *joints, EmcPose * pos, const KINEMATICS_FORWARD_FLAGS * fflags, KINEMATICS_INVERSE_FLAGS * iflags) { //TCP-Funktion double dx = *(haldata->X_offset); // neu double dy = *(haldata->Y_offset); double dz = *(haldata->Z_offset); double dT = *(haldata->Tool_offset); double a_rad = joints[3] * M_PI/180; double c_rad = joints[5] * M_PI/180; dz = dz + dT; if (*(haldata->enable)==1.0){ pos->tran.x = rtapi_cos(c_rad) * (joints[0] - dx) + rtapi_sin(c_rad) * rtapi_cos(a_rad) * (joints[1] - dy) + rtapi_sin(c_rad) * rtapi_sin(a_rad) * (joints[2] - dz) + rtapi_sin(c_rad) * dy; pos->tran.y = -rtapi_sin(c_rad) * (joints[0] - dx) + rtapi_cos(c_rad) * rtapi_cos(a_rad) * (joints[1] - dy) + rtapi_cos(c_rad) * rtapi_sin(a_rad) * (joints[2] - dz) + rtapi_cos(c_rad) * dy; pos->tran.z = -rtapi_sin(a_rad) * (joints[1] - dy) + rtapi_cos(a_rad) * (joints[2] - dz) + dz; }else{ pos->tran.x = joints[0]; pos->tran.y = joints[1]; pos->tran.z = joints[2]; } //pos->tran.x = rtapi_cos(c_rad) * (joints[0]) + //rtapi_sin(c_rad) * rtapi_cos(a_rad) * (joints[1] - dy) + //rtapi_sin(c_rad) * rtapi_sin(a_rad) * (joints[2] - dz) + //rtapi_sin(c_rad) * dy; //pos->tran.y = -rtapi_sin(c_rad) * (joints[0]) + //rtapi_cos(c_rad) * rtapi_cos(a_rad) * (joints[1] - dy) + //rtapi_cos(c_rad) * rtapi_sin(a_rad) * (joints[2] - dz) + //rtapi_cos(c_rad) * dy; //pos->tran.z = -rtapi_sin(a_rad) * (joints[1] - dy) + //rtapi_cos(a_rad) * (joints[2] - dz) + dz; pos->a = joints[3]; pos->b = joints[4]; pos->c = joints[5]; return 0; } static int kinematicsInverse(const EmcPose * pos, double *joints, const KINEMATICS_INVERSE_FLAGS * iflags, KINEMATICS_FORWARD_FLAGS * fflags) { //TCP-Funktion double dx = *(haldata->X_offset); // neu double dy = *(haldata->Y_offset); double dz = *(haldata->Z_offset); double dT = *(haldata->Tool_offset); double c_rad = pos->c * M_PI/180; double a_rad = pos->a * M_PI/180; dz = dz + dT; if (*(haldata->enable)==1.0){ joints[0] = rtapi_cos(c_rad) * pos->tran.x - rtapi_sin(c_rad) * pos->tran.y; joints[1] = rtapi_sin(c_rad) * rtapi_cos(a_rad) * pos->tran.x + rtapi_cos(c_rad) * rtapi_cos(a_rad) * pos->tran.y - rtapi_sin(a_rad) * pos->tran.z - rtapi_cos(a_rad) * dy + rtapi_sin(a_rad) * dz + dy; joints[2] = rtapi_sin(c_rad) * rtapi_sin(a_rad) * pos->tran.x + rtapi_cos(c_rad) * rtapi_sin(a_rad) * pos->tran.y + rtapi_cos(a_rad) * pos->tran.z - rtapi_sin(a_rad) * dy - rtapi_cos(a_rad) * dz + dz; }else{ joints[0] = pos->tran.x; joints[1] = pos->tran.y; joints[2] = pos->tran.z; } //joints[0] = rtapi_cos(c_rad) * pos->tran.x - //rtapi_sin(c_rad) * pos->tran.y; //joints[1] = rtapi_sin(c_rad) * rtapi_cos(a_rad) * pos->tran.x + //rtapi_cos(c_rad) * rtapi_cos(a_rad) * pos->tran.y - //rtapi_sin(a_rad) * pos->tran.z - //rtapi_cos(a_rad) * dy + rtapi_sin(a_rad) * dz + dy; //joints[2] = rtapi_sin(c_rad) * rtapi_sin(a_rad) * pos->tran.x + //rtapi_cos(c_rad) * rtapi_sin(a_rad) * pos->tran.y + //rtapi_cos(a_rad) * pos->tran.z - //rtapi_sin(a_rad) * dy - rtapi_cos(a_rad) * dz + dz; joints[3] = pos->a; joints[4] = pos->b; joints[5] = pos->c; 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 kinematicsType() { return KINEMATICS_BOTH; } #include "rtapi.h" /* RTAPI realtime OS API */ #include "rtapi_app.h" /* RTAPI realtime module decls */ #include "hal.h" MODULE_LICENSE("GPL"); static vtkins_t vtk = { .kinematicsForward = kinematicsForward, .kinematicsInverse = kinematicsInverse, // .kinematicsHome = kinematicsHome, .kinematicsType = kinematicsType }; static int comp_id, vtable_id; static const char *name = "XYZACkinsTEST"; int rtapi_app_main(void) { int res = 0; comp_id = hal_init(name); if (comp_id < 0) return comp_id; haldata = hal_malloc(sizeof(struct haldata)); if (((res = hal_pin_newf(HAL_FLOAT, HAL_IO, (void **) &(haldata->enable), comp_id, "%s.XYZACkinsTEST.enable", name)) < 0) || ((res = hal_pin_newf(HAL_FLOAT, HAL_IN, (void **) &(haldata->Tool_offset), comp_id, "%s.Tool-offset", name)) < 0) || ((res = hal_pin_newf(HAL_FLOAT, HAL_IN, (void **) &(haldata->X_offset), comp_id, "%s.X-offset", name)) < 0) || ((res = hal_pin_newf(HAL_FLOAT, HAL_IN, (void **) &(haldata->Y_offset), comp_id, "%s.Y-offset", name)) < 0) || ((res = hal_pin_newf(HAL_FLOAT, HAL_IN, (void **) &(haldata->Z_offset), comp_id, "%s.Z-offset", name)) < 0)) goto error; *(haldata->enable) = 0.0; *(haldata->X_offset) = 0.0; *(haldata->Y_offset) = 0.0; *(haldata->Z_offset) = 0.0; vtable_id = hal_export_vtable(name, VTVERSION, &vtk, comp_id); if (vtable_id < 0) { rtapi_print_msg(RTAPI_MSG_ERR, "%s: ERROR: hal_export_vtable(%s,%d,%p) failed: %d\n", name, name, VTVERSION, &vtk, vtable_id ); return -ENOENT; } hal_ready(comp_id); return 0; error: hal_exit(comp_id); return res; } void rtapi_app_exit(void) { hal_exit(comp_id); }