/* Copyright 2019 Dewey Garrett This program is free software; you can redistribute it and/or modify it under the terms of the GNU General Public License as published by the Free Software Foundation; either version 2 of the License, or (at your option) any later version. This program is distributed in the hope that it will be useful, but WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License for more details. You should have received a copy of the GNU General Public License along with this program; if not, write to the Free Software Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA. */ /* switchkins.c provide functions for switchable kins modules: * rtapi_app() * rtapi_exit() * kinematicsType() * kinematicsForward() * kinematicsInverse() * kinematicsSwitch() * kinematicsSwitchable() * Using modules must supply function: switchkinsSetup() */ #include "motion.h" #include "hal.h" #include "rtapi_app.h" #include "kinematics.h" #include "switchkins.h" //********************************************************************* // kinematic functions (default=0 for err detection): static kparms kp; // kinematics parms (common all types) static KF kfwd0 = 0; // 0==switchkins_type kinematics forward static KF kfwd1 = 0; // 1 static KF kfwd2 = 0; // 2 static KI kinv0 = 0; // 0==switchkins_type kinematics inverse static KI kinv1 = 0; // 1 static KI kinv2 = 0; // 2 static hal_u32_t switchkins_type; static struct swdata { hal_bit_t *kinstype_is_0; hal_bit_t *kinstype_is_1; hal_bit_t *kinstype_is_2; hal_float_t *gui_x; hal_float_t *gui_y; hal_float_t *gui_z; hal_float_t *gui_a; hal_float_t *gui_b; hal_float_t *gui_c; } *swdata; // Note: parallel kinematics (like genhexkins) often // use iterative method for Forward algorithm // and require an initial EmcPose. // If fwd_iterates_mask is set // then save/use the lastpose static int fwd_iterates[SWITCHKINS_MAX_TYPES] = {0}; static bool use_lastpose[SWITCHKINS_MAX_TYPES] = {0}; static EmcPose lastpose[SWITCHKINS_MAX_TYPES]; static void save_lastpose(int ktype, EmcPose* pos) { lastpose[ktype].tran.x = pos->tran.x; lastpose[ktype].tran.y = pos->tran.y; lastpose[ktype].tran.z = pos->tran.z; lastpose[ktype].a = pos->a; lastpose[ktype].b = pos->b; lastpose[ktype].c = pos->c; lastpose[ktype].u = pos->u; lastpose[ktype].v = pos->v; lastpose[ktype].w = pos->w; } // save_lastpose() static void get_lastpose(int ktype, EmcPose* pos) { pos->tran.x = lastpose[ktype].tran.x; pos->tran.y = lastpose[ktype].tran.y; pos->tran.z = lastpose[ktype].tran.z; pos->a = lastpose[ktype].a; pos->b = lastpose[ktype].b; pos->c = lastpose[ktype].c; pos->u = lastpose[ktype].u; pos->v = lastpose[ktype].v; pos->w = lastpose[ktype].w; } // get_lastpose() static int gui_forward_kins(const double *joints) { // the hexapod vismach gui uses these hal pins to // display platform position/orientation in both // genhexkins and identity kinematic types // (similar needs for many parallel kinemtic machines) int res; KINEMATICS_FORWARD_FLAGS fflags; KINEMATICS_INVERSE_FLAGS iflags; switch (kp.gui_kinstype) { case 0: res = kfwd0(joints, &lastpose[0], &fflags, &iflags);break; case 1: res = kfwd1(joints, &lastpose[1], &fflags, &iflags);break; case 2: res = kfwd2(joints, &lastpose[2], &fflags, &iflags);break; default: rtapi_print_msg(RTAPI_MSG_ERR, "gui_forward_kins BAD gui_kinstype <%d>\n", kp.gui_kinstype); return -1; } *swdata->gui_x = lastpose[kp.gui_kinstype].tran.x; *swdata->gui_y = lastpose[kp.gui_kinstype].tran.y; *swdata->gui_z = lastpose[kp.gui_kinstype].tran.z; *swdata->gui_a = lastpose[kp.gui_kinstype].a; *swdata->gui_b = lastpose[kp.gui_kinstype].b; *swdata->gui_c = lastpose[kp.gui_kinstype].c; return res; } // gui_forward_kins //********************************************************************* int kinematicsSwitchable() {return 1;} int kinematicsSwitch(int new_switchkins_type) { int k; for (k=0; k< SWITCHKINS_MAX_TYPES; k++) { use_lastpose[k] = 0;} switchkins_type = new_switchkins_type; switch (switchkins_type) { case 0: rtapi_print_msg(RTAPI_MSG_INFO, "kinematicsSwitch:TYPE0\n"); *swdata->kinstype_is_0 = 1; *swdata->kinstype_is_1 = 0; *swdata->kinstype_is_2 = 0; break; case 1: rtapi_print_msg(RTAPI_MSG_INFO, "kinematicsSwitch:TYPE1\n"); *swdata->kinstype_is_0 = 0; *swdata->kinstype_is_1 = 1; *swdata->kinstype_is_2 = 0; break; case 2: rtapi_print_msg(RTAPI_MSG_INFO, "kinematicsSwitch:TYPE2\n"); *swdata->kinstype_is_0 = 0; *swdata->kinstype_is_1 = 0; *swdata->kinstype_is_2 = 1; break; default: rtapi_print_msg(RTAPI_MSG_ERR, "kinematicsSwitch:BAD VALUE <%d>\n", switchkins_type); *swdata->kinstype_is_1 = 0; *swdata->kinstype_is_0 = 0; *swdata->kinstype_is_2 = 0; return -1; // FAIL } if (fwd_iterates[switchkins_type]) { use_lastpose[switchkins_type] = 1; // restarting a kins types } return 0; // 0==> no error } // kinematicsSwitch() int kinematicsForward(const double *joint, EmcPose * pos, const KINEMATICS_FORWARD_FLAGS * fflags, KINEMATICS_INVERSE_FLAGS * iflags) { int r; if (fwd_iterates[switchkins_type] && use_lastpose[switchkins_type]) { // initialize iterative forward kins (ok for identity too) get_lastpose(switchkins_type,pos); use_lastpose[switchkins_type] = 0; } switch (switchkins_type) { case 0: r = kfwd0(joint, pos, fflags, iflags); break; case 1: r = kfwd1(joint, pos, fflags, iflags); break; case 2: r = kfwd2(joint, pos, fflags, iflags); break; default: rtapi_print_msg(RTAPI_MSG_ERR, "switchkins: Forward BAD switchkins_type \n", switchkins_type); return -1; } if (fwd_iterates[switchkins_type]) {save_lastpose(switchkins_type,pos);} if (r) return r; // gui.* pins created only if gui_kinstype>=0 // consider alternate implementations for gui_forward_kins(): // a) always call and use -1 to select default 0 type if (kp.gui_kinstype >=0) { // create gui pins for a vismach gui using the // kins type specified by kp.gui_kinstype; // currently the skgui pins are only needed for // the hexagui vismach program (as it needs // world coords for switchkin-types r = gui_forward_kins(joint); } return r; } // kinematicsForward() int kinematicsInverse(const EmcPose * pos, double *joint, const KINEMATICS_INVERSE_FLAGS * iflags, KINEMATICS_FORWARD_FLAGS * fflags) { int r; switch (switchkins_type) { case 0: r = kinv0(pos, joint, iflags, fflags); break; case 1: r = kinv1(pos, joint, iflags, fflags); break; case 2: r = kinv2(pos, joint, iflags, fflags); break; default: rtapi_print_msg(RTAPI_MSG_ERR, "switchkins: Inverse BAD switchkins_type \n", switchkins_type); return -1; } return r; } // kinematicsInverse() KINEMATICS_TYPE kinematicsType() { return KINEMATICS_BOTH; } //********************************************************************* static char *coordinates; RTAPI_MP_STRING(coordinates, "Axes-to-joints-ordering"); static char *sparm; RTAPI_MP_STRING(sparm, "switchkins module-specific parameter"); EXPORT_SYMBOL(kinematicsSwitchable); EXPORT_SYMBOL(kinematicsSwitch); EXPORT_SYMBOL(kinematicsType); EXPORT_SYMBOL(kinematicsForward); EXPORT_SYMBOL(kinematicsInverse); MODULE_LICENSE("GPL"); static int comp_id; //********************************************************************* int rtapi_app_main(void) { int i,res; char* emsg="other"; // defaults prior to switchkinsSetup() call kp.kinsname = NULL; kp.halprefix = NULL; kp.required_coordinates = ""; kp.max_joints = 0; // Setup must supply kp.allow_duplicates = 0; kp.fwd_iterates_mask = 0; kp.gui_kinstype = -1; // negative means: not used kp.sparm = sparm; // module parm passed to kins KS ksetup0 = 0; KS ksetup1 = 0; KS ksetup2 = 0; res = switchkinsSetup(&kp, &ksetup0, &ksetup1, &ksetup2, &kfwd0, &kfwd1, &kfwd2, &kinv0, &kinv1, &kinv2); if (res) {emsg="switchkinsSetp FAIL"; goto error;} for (i=0; i < SWITCHKINS_MAX_TYPES; i++) { if (kp.fwd_iterates_mask & (1< EMCMOT_MAX_JOINTS) { emsg = "bogus max_joints"; goto error; } if (kp.gui_kinstype >= SWITCHKINS_MAX_TYPES) { emsg = "bogus gui_kinstype"; goto error; } if (!ksetup0 || !ksetup1 || !ksetup2) { emsg = "Missing setup function"; goto error; } if (!kfwd0 || !kfwd1 || !kfwd2) { emsg = "Missing fwd functionn"; goto error; } if (!kinv0 || !kinv1 || !kinv2) { emsg = "Missing inv function"; goto error; } comp_id = hal_init(kp.kinsname); if(comp_id < 0) goto error; swdata = hal_malloc(sizeof(struct swdata)); if (!swdata) goto error; res += hal_pin_bit_new("kinstype.is-0", HAL_OUT, &(swdata->kinstype_is_0), comp_id); res += hal_pin_bit_new("kinstype.is-1", HAL_OUT, &(swdata->kinstype_is_1), comp_id); res += hal_pin_bit_new("kinstype.is-2", HAL_OUT, &(swdata->kinstype_is_2), comp_id); if (kp.gui_kinstype >=0) { res += hal_pin_float_newf(HAL_IN, &swdata->gui_x, comp_id, "skgui.x"); res += hal_pin_float_newf(HAL_IN, &swdata->gui_y, comp_id, "skgui.y"); res += hal_pin_float_newf(HAL_IN, &swdata->gui_z, comp_id, "skgui.z"); res += hal_pin_float_newf(HAL_IN, &swdata->gui_a, comp_id, "skgui.a"); res += hal_pin_float_newf(HAL_IN, &swdata->gui_b, comp_id, "skgui.b"); res += hal_pin_float_newf(HAL_IN, &swdata->gui_c, comp_id, "skgui.c"); if (res) {emsg = "hal pin create fail";goto error;} } switchkins_type = 0; // startup with default type kinematicsSwitch(switchkins_type); if (!coordinates) {coordinates = kp.required_coordinates;} ksetup0(comp_id,coordinates,&kp); ksetup1(comp_id,coordinates,&kp); ksetup2(comp_id,coordinates,&kp); hal_ready(comp_id); return 0; error: rtapi_print_msg(RTAPI_MSG_ERR, "\nSwitchkins FAIL %s:<%s>\n",kp.kinsname,emsg); hal_exit(comp_id); return -1; } // rtapi_app_main() void rtapi_app_exit(void) { hal_exit(comp_id); }