/********************************************************************************** * Description: deltakins.c * Kinematics for 3 axis Delta machine + 1 rotating axis * Derived from a work of mzavatsky at Trossen Robotics * at forums.trossenrobotics.com * Author: Jozsef Papp / hg5bsd * and: István Ábel at kvarc.extra.hu * License: coding and fitting it into EMC2 from our part is GPL alike * but we won't place the code itself under GPL because we * don't know if this would hurt or not the delta robot * inventors rights, if you are sure that it doesn't hurt, * then you are free to place it under GPL ( in this case place * your name in the comment lines, and keep original comments ) * System: realtime Linux, EMC2 * Copyright (c) 2010 All rights reserved. * Last change: 2014.05.14.21.08. * modified by cncbasher compiles correctly in 2.5 and dev 2013.06.20 ************************************************************************************ * modified by gfxx compiles correctly in 2.5 and dev 2014.05.10 * modified By k1 for add new rotating axis and jog for axis x-y-z-a-b * modified 2.0 by k1 for use with new JA 2016 (no jog with kins) ************************************************************************************/ #include "kinematics.h" /* these decls */ #include "rtapi_math.h" /* robot geometry /Ref - mzavatsky: Delta robot kinematics/ e = side length of end effector triangle, middle arm - "re" f = side length of base triangle, middle drive joints - "rf" re = length of end effector arm rf = length of drive arm sample: e = 115.0; f = 457.3; re = 232.0; rf = 112.0; */ #ifdef RTAPI #include "rtapi.h" /* RTAPI realtime OS API */ #include "rtapi_app.h" /* RTAPI realtime module decls */ #include "hal.h" struct haldata { hal_float_t *e, *f, *re, *rf, *encval, *encdx, *encdy, *encdz, *xincr, *yincr, *zincr, *aincr, *bincr; hal_bit_t *resenc, *isteleop, *ismanual, *ishomed, *jx, *jy, *jz, *ja, *jb; } *haldata = 0; #define delta_e (*(haldata->e)) #define delta_f (*(haldata->f)) #define delta_re (*(haldata->re)) #define delta_rf (*(haldata->rf)) #define delta_encval (*(haldata->encval)) #define delta_encdx (*(haldata->encdx)) #define delta_encdy (*(haldata->encdy)) #define delta_encdz (*(haldata->encdz)) #define delta_xincr (*(haldata->xincr)) #define delta_yincr (*(haldata->yincr)) #define delta_zincr (*(haldata->zincr)) #define delta_aincr (*(haldata->aincr)) #define delta_bincr (*(haldata->bincr)) #define delta_resenc (*(haldata->resenc)) #define delta_isteleop (*(haldata->isteleop)) #define delta_ismanual (*(haldata->ismanual)) #define delta_ishomed (*(haldata->ishomed)) #define delta_jx (*(haldata->jx)) #define delta_jy (*(haldata->jy)) #define delta_jz (*(haldata->jz)) #define delta_ja (*(haldata->ja)) #define delta_jb (*(haldata->jb)) #else double delta_e, delta_f, delta_re, delta_rf, delta_encval, delta_encdx, delta_encdy, delta_encdz; #endif /* trigonometric constants */ const double sqrt3 = 1.7320508075688772935274463415059; /* sqrt(3.0);*/ #ifndef PI #define PI 3.14159265358979323846 #endif const double sin120 = 0.86602540378443864676372317075294; /* (sqrt3)/2;*/ const double cos120 = -0.5; const double tan60 = 1.7320508075688772935274463415059; /* sqrt3;*/ const double sin30 = 0.5; const double tan30 = 0.57735026918962576450914878050196; /* 1/(sqrt3);*/ double mx = 0.0; double my = 0.0; double mz = 0.0; double ma = 0.0; double mb = 0.0; double mxenc = 0.0; double myenc = 0.0; double mzenc = 0.0; double maenc = 0.0; double mbenc = 0.0; double m1enc = 0.0; double m2encx = 0.0; double m2ency = 0.0; double m2encz = 0.0; double kincr = 0.0; /* forward kinematics: (joints[0], joints[1], joints[2]) -> (world->tran.x, world->tran.y, world->tran.z) // returned status: 0=OK, -1=non-existing position*/ int kinematicsForward( const double *joints, EmcPose *world, const KINEMATICS_FORWARD_FLAGS *fflags, KINEMATICS_INVERSE_FLAGS *iflags) { double t, theta1, theta2, theta3, x2, x3, y1, y2, y3, z1, z2, z3, w1, w2, w3, a, b, c, d, a1, a2, b1, b2, dnm; t = (delta_f-delta_e)*tan30/2; /* float dtr = pi/(float)180.0; TO_RAD */ theta1 = joints[0] * TO_RAD; theta2 = joints[1] * TO_RAD; theta3 = joints[2] * TO_RAD; y1 = -(t + delta_rf*cos(theta1)); z1 = -delta_rf*sin(theta1); y2 = (t + delta_rf*cos(theta2))*sin30; x2 = y2*tan60; z2 = -delta_rf*sin(theta2); y3 = (t + delta_rf*cos(theta3))*sin30; x3 = -y3*tan60; z3 = -delta_rf*sin(theta3); dnm = (y2-y1)*x3-(y3-y1)*x2; w1 = y1*y1 + z1*z1; w2 = x2*x2 + y2*y2 + z2*z2; w3 = x3*x3 + y3*y3 + z3*z3; /* x = (a1*z + b1)/dnm*/ a1 = (z2-z1)*(y3-y1)-(z3-z1)*(y2-y1); b1 = -((w2-w1)*(y3-y1)-(w3-w1)*(y2-y1))/2.0; /* y = (a2*z + b2)/dnm;*/ a2 = -(z2-z1)*x3+(z3-z1)*x2; b2 = ((w2-w1)*x3 - (w3-w1)*x2)/2.0; /* a*z^2 + b*z + c = 0*/ a = a1*a1 + a2*a2 + dnm*dnm; b = 2*(a1*b1 + a2*(b2-y1*dnm) - z1*dnm*dnm); c = (b2-y1*dnm)*(b2-y1*dnm) + b1*b1 + dnm*dnm*(z1*z1 - delta_re*delta_re); /* discriminant*/ d = b*b - (double)4.0*a*c; if (d < 0) return -1; /* non-existing point*/ world->tran.z = -(double)0.5*(b+sqrt(d))/a; world->tran.x = (a1*world->tran.z + b1)/dnm; world->tran.y = (a2*world->tran.z + b2)/dnm; world->a = joints[3]; world->b = joints[4]; world->c = joints[5]; world->u = joints[6]; world->v = joints[7]; world->w = joints[8]; return 0; } /* inverse kinematics helper functions, calculates angle theta1 (for YZ-pane)*/ int delta_calcAngleYZ( double x0, double y0, double z0, double *theta ) { double y1, a, b, d, yj, zj; y1= -0.5 * 0.57735 * delta_f; // f/2 * tg 30 y0 -= 0.5 * 0.57735 * delta_e; // shift center to edge /* z = a + b*y*/ a = (x0*x0 + y0*y0 + z0*z0 +delta_rf*delta_rf - delta_re*delta_re - y1*y1)/(2*z0); b = (y1-y0)/z0; /* discriminant*/ d = -(a+b*y1)*(a+b*y1)+delta_rf*(b*b*delta_rf+delta_rf); if (d < 0) return -1; /* non-existing point*/ yj = (y1 - a*b - sqrt(d))/(b*b + 1); /* choosing outer point*/ zj = a + b*yj; *theta = TO_DEG * atan2(-zj,(y1 - yj)) + ((yj>y1)?180.0:0.0); return 0; } /* inverse kinematics: (world->tran.x, world->tran.y, world->tran.z) -> (joints[0], joints[1], joints[2]) returned status: 0=OK, -1=non-existing position*/ int kinematicsInverse(const EmcPose *world, double *joints, const KINEMATICS_INVERSE_FLAGS *iflags, KINEMATICS_FORWARD_FLAGS *fflags) { double x0, y0, z0, theta1, theta2, theta3; int status; x0 = world->tran.x; y0 = world->tran.y; z0 = world->tran.z; joints[3] = world->a; joints[4] = world->b; joints[5] = world->c; joints[6] = world->u; joints[7] = world->v; joints[8] = world->w; theta1 = theta2 = theta3 = 0; status = delta_calcAngleYZ(x0, y0, z0, &theta1); if (status == 0) joints[0] = theta1; if (status == 0) status = delta_calcAngleYZ(x0*cos120 + y0*sin120,y0*cos120-x0*sin120, z0, &theta2); /*rotate coords to +120 deg*/ if (status == 0) joints[1] = theta2; if (status == 0) status = delta_calcAngleYZ(x0*cos120 - y0*sin120,y0*cos120+x0*sin120, z0, &theta3); /*rotate coords to -120 deg*/ if (status == 0) joints[2] = theta3; return status; } /* 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; /* return KINEMATICS_INVERSE_ONLY;*/ } #ifdef RTAPI #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 res = 0; comp_id = hal_init("delta_AAB_inv"); if(comp_id < 0) return comp_id; haldata = hal_malloc(sizeof(struct haldata)); if(!haldata) goto error; if((res = hal_pin_float_new("delta_AAB_inv.e", HAL_IO, &(haldata->e),comp_id)) != 0) goto error; if((res = hal_pin_float_new("delta_AAB_inv.f", HAL_IO, &(haldata->f),comp_id)) != 0) goto error; if((res = hal_pin_float_new("delta_AAB_inv.re", HAL_IO, &(haldata->re),comp_id)) != 0) goto error; if((res = hal_pin_float_new("delta_AAB_inv.rf", HAL_IO, &(haldata->rf),comp_id)) != 0) goto error; if((res = hal_pin_float_new("delta_AAB_inv.encval", HAL_IN, &(haldata->encval),comp_id)) != 0) goto error; if((res = hal_pin_float_new("delta_AAB_inv.encdx", HAL_OUT, &(haldata->encdx),comp_id)) != 0) goto error; if((res = hal_pin_float_new("delta_AAB_inv.encdy", HAL_OUT, &(haldata->encdy),comp_id)) != 0) goto error; if((res = hal_pin_float_new("delta_AAB_inv.encdz", HAL_OUT, &(haldata->encdz),comp_id)) != 0) goto error; if((res = hal_pin_float_new("delta_AAB_inv.xincr", HAL_OUT, &(haldata->xincr),comp_id)) != 0) goto error; if((res = hal_pin_float_new("delta_AAB_inv.yincr", HAL_OUT, &(haldata->yincr),comp_id)) != 0) goto error; if((res = hal_pin_float_new("delta_AAB_inv.zincr", HAL_OUT, &(haldata->zincr),comp_id)) != 0) goto error; if((res = hal_pin_float_new("delta_AAB_inv.aincr", HAL_OUT, &(haldata->aincr),comp_id)) != 0) goto error; if((res = hal_pin_float_new("delta_AAB_inv.bincr", HAL_OUT, &(haldata->bincr),comp_id)) != 0) goto error; if((res = hal_pin_bit_new("delta_AAB_inv.isteleop", HAL_IN, &(haldata->isteleop),comp_id)) != 0) goto error; if((res = hal_pin_bit_new("delta_AAB_inv.ismanual", HAL_IN, &(haldata->ismanual),comp_id)) != 0) goto error; if((res = hal_pin_bit_new("delta_AAB_inv.ishomed", HAL_IN, &(haldata->ishomed),comp_id)) != 0) goto error; if((res = hal_pin_bit_new("delta_AAB_inv.jx", HAL_IN, &(haldata->jx),comp_id)) != 0) goto error; if((res = hal_pin_bit_new("delta_AAB_inv.jy", HAL_IN, &(haldata->jy),comp_id)) != 0) goto error; if((res = hal_pin_bit_new("delta_AAB_inv.jz", HAL_IN, &(haldata->jz),comp_id)) != 0) goto error; if((res = hal_pin_bit_new("delta_AAB_inv.ja", HAL_IN, &(haldata->ja),comp_id)) != 0) goto error; if((res = hal_pin_bit_new("delta_AAB_inv.jb", HAL_IN, &(haldata->jb),comp_id)) != 0) goto error; if((res = hal_pin_bit_new("delta_AAB_inv.resenc", HAL_OUT, &(haldata->resenc),comp_id)) != 0) goto error; delta_e = delta_f = delta_re = delta_rf = 1.0; hal_ready(comp_id); return 0; error: hal_exit(comp_id); return res; } void rtapi_app_exit(void) { hal_exit(comp_id); } #endif