rotarydeltakins-common.h
1 2 // Copyright 2013 Chris Radek <chris@timeguy.com> 3 // 4 // This program is free software; you can redistribute it and/or modify 5 // it under the terms of the GNU General Public License as published by 6 // the Free Software Foundation; either version 2 of the License, or 7 // (at your option) any later version. 8 // 9 // This program is distributed in the hope that it will be useful, 10 // but WITHOUT ANY WARRANTY; without even the implied warranty of 11 // MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 12 // GNU General Public License for more details. 13 // 14 // You should have received a copy of the GNU General Public License 15 // along with this program; if not, write to the Free Software 16 // Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA. 17 18 19 /* 20 Based on work by "mzavatsky" at: 21 http://forums.trossenrobotics.com/tutorials/introduction-129/delta-robot-kinematics-3276/ 22 23 "You can freely use this code in your applications." 24 25 which was based on: 26 27 Descriptive Geometric Kinematic Analysis of Clavel's "Delta" Robot 28 P.J. Zsombor-Murray, McGill University 29 30 "... the purpose of this article: to provide a clear kinematic 31 analysis useful to those who may wish to program and employ nice 32 little three legged robots ..." 33 34 35 The platform is on "top", the origin is in the center of the plane 36 containing the three hip joints. Z points upward, so Z coordinates 37 are always negative. Thighs always point outward, straight out 38 (knee at Z=0) is considerd zero degrees for the angular hip joint. 39 Positive rotation is knee-downward, so if you rotate all knees 40 positive, the Z coordinate will get more negative. 41 42 Joint zero is the one whose thigh swings in the YZ plane. 43 */ 44 45 #ifndef LINUXCNCROTARYDELTAKINS_COMMON_H 46 #define LINUXCNCROTARYDELTAKINS_COMMON_H 47 48 #include "emcpos.h" 49 // distance from origin to a hip joint 50 static double platformradius; 51 52 // thigh connects the hip to the knee 53 static double thighlength; 54 55 // shin (the parallelogram) connects the knee to the foot 56 static double shinlength; 57 58 // distance from center of foot (controlled point) to an ankle joint 59 static double footradius; 60 61 #ifndef sq 62 #define sq(a) ((a)*(a)) 63 #endif 64 #ifndef D2R 65 #define D2R(d) ((d)*M_PI/180.) 66 #endif 67 68 static void set_geometry(double pfr, double tl, double sl, double fr) { 69 platformradius = pfr; 70 thighlength = tl; 71 shinlength = sl; 72 footradius = fr; 73 } 74 75 // Given three hip joint angles, find the controlled point 76 static int kinematics_forward(const double *joints, EmcPose *pos) { 77 double 78 j0 = joints[0], 79 j1 = joints[1], 80 j2 = joints[2], 81 y1, z1, // x1 is 0 82 x2, y2, z2, x3, y3, z3, 83 a1, b1, a2, b2, 84 w1, w2, w3, 85 denom, 86 a, b, c, d; 87 88 j0 = D2R(j0); 89 j1 = D2R(j1); 90 j2 = D2R(j2); 91 92 y1 = -(platformradius - footradius + thighlength * cos(j0)); 93 z1 = -thighlength * sin(j0); 94 95 y2 = (platformradius - footradius + thighlength * cos(j1)) * 0.5; 96 x2 = y2 * sqrt(3); 97 z2 = -thighlength * sin(j1); 98 99 y3 = (platformradius - footradius + thighlength * cos(j2)) * 0.5; 100 x3 = -y3 * sqrt(3); 101 z3 = -thighlength * sin(j2); 102 103 denom = x3 * (y2 - y1) - x2 * (y3 - y1); 104 105 w1 = sq(y1) + sq(z1); 106 w2 = sq(x2) + sq(y2) + sq(z2); 107 w3 = sq(x3) + sq(y3) + sq(z3); 108 109 a1 = (z2-z1) * (y3-y1) - (z3-z1) * (y2-y1); 110 b1 = -((w2-w1) * (y3-y1) - (w3-w1) * (y2-y1)) / 2.0; 111 112 a2 = -(z2 - z1) * x3 + (z3 - z1) * x2; 113 b2 = ((w2 - w1) * x3 - (w3 - w1) * x2) / 2.0; 114 115 // a*z^2 + b*z + c = 0 116 a = sq(a1) + sq(a2) + sq(denom); 117 b = 2 * (a1 * b1 + a2 * (b2 - y1 * denom) - z1 * sq(denom)); 118 c = (b2 - y1 * denom) * (b2 - y1 * denom) + 119 sq(b1) + sq(denom) * (sq(z1) - sq(shinlength)); 120 121 d = sq(b) - 4 * a * c; 122 if (d < 0) return -1; 123 124 pos->tran.z = (-b - sqrt(d)) / (2 * a); 125 pos->tran.x = (a1 * pos->tran.z + b1) / denom; 126 pos->tran.y = (a2 * pos->tran.z + b2) / denom; 127 128 pos->a = joints[3]; 129 pos->b = joints[4]; 130 pos->c = joints[5]; 131 pos->u = joints[6]; 132 pos->v = joints[7]; 133 pos->w = joints[8]; 134 135 return 0; 136 } 137 138 139 // Given controlled point, find joint zero's angle 140 // (J0 is the easy one in the ZY plane) 141 static int inverse_j0(double x, double y, double z, double *theta) { 142 double a, b, d, knee_y, knee_z; 143 144 a = 0.5 * (sq(x) + sq(y - footradius) + sq(z) + sq(thighlength) - 145 sq(shinlength) - sq(platformradius)) / z; 146 b = (footradius - platformradius - y) / z; 147 148 d = sq(thighlength) * (sq(b) + 1) - sq(a - b * platformradius); 149 if (d < 0) return -1; 150 151 knee_y = (platformradius + a*b + sqrt(d)) / (sq(b) + 1); 152 knee_z = b * knee_y - a; 153 154 *theta = atan2(knee_z, knee_y - platformradius); 155 *theta *= 180.0/M_PI; 156 return 0; 157 } 158 159 static void rotate(double *x, double *y, double theta) { 160 double xx, yy; 161 xx = *x, yy = *y; 162 *x = xx * cos(theta) - yy * sin(theta); 163 *y = xx * sin(theta) + yy * cos(theta); 164 } 165 166 static int kinematics_inverse(const EmcPose *pos, double *joints) { 167 double xr, yr; 168 if(inverse_j0(pos->tran.x, pos->tran.y, pos->tran.z, &joints[0])) return -1; 169 170 // now use symmetry property to get the other two just as easily... 171 xr = pos->tran.x; yr = pos->tran.y; 172 rotate(&xr, &yr, -2*M_PI/3); 173 if(inverse_j0(xr, yr, pos->tran.z, &joints[1])) return -1; 174 175 xr = pos->tran.x; yr = pos->tran.y; 176 rotate(&xr, &yr, 2*M_PI/3); 177 if(inverse_j0(xr, yr, pos->tran.z, &joints[2])) return -1; 178 179 joints[3] = pos->a; 180 joints[4] = pos->b; 181 joints[5] = pos->c; 182 joints[6] = pos->u; 183 joints[7] = pos->v; 184 joints[8] = pos->w; 185 186 return 0; 187 } 188 189 #define RDELTA_PFR 10.0 190 #define RDELTA_TL 10.0 191 #define RDELTA_SL 14.0 192 #define RDELTA_FR 6.0 193 194 #endif