tripodkins.c
1 /******************************************************************** 2 * Description: tripodkins.c 3 * Kinematics for 3 axis Tripod machine 4 * 5 * Derived from a work by Fred Proctor 6 * 7 * Author: 8 * License: GPL Version 2 9 * System: Linux 10 * 11 * Copyright (c) 2004 All rights reserved. 12 * 13 * Last change: 14 ********************************************************************/ 15 16 /* 17 These kinematics are for a tripod with point vertices. 18 19 Vertices A, B, and C are the base, and vertex D is the controlled point. 20 Three tripod strut lengths AD, BD, and CD are the joints that move 21 point D around. 22 23 Point A is the origin, with coordinates (0, 0, 0). Point B lies on the 24 x axis, with coordinates (Bx, 0, 0). Point C lies in the xy plane, with 25 coordinates (Cx, Cy, 0). Point D has coordinates (Dx, Dy, Dz). 26 27 The controlled Cartesian values are Dx, Dy, and Dz. A frame attached to 28 D, say with x parallel to AD and y in the plane ABD, would change its 29 orientation as the strut lengths changed. The orientation of this frame 30 relative to the world frame is not computed. 31 32 With respect to the kinematics functions, 33 34 pos->tran.x = Dx 35 pos->tran.y = Dy 36 pos->tran.z = Dz 37 pos->a,b,c = 0 38 39 joints[0] = AD 40 joints[1] = BD 41 joints[2] = CD 42 43 The inverse kinematics have no singularities. Any values for Dx, Dy, and 44 Dz will yield numerical results. Of course, these may be beyond the 45 strut length limits, but there are no singular effects like infinite speed. 46 47 The forward kinematics has a singularity due to the triangle inequalities 48 for triangles ABD, BCD, and CAD. When any of these approach the limit, 49 Dz is zero and D lies in the base plane. 50 51 The forward kinematics flags, referred to in kinematicsForward and 52 set in kinematicsInverse, let the forward kinematics select between 53 the positive and negative values of Dz for given strut values. 54 Dz > 0 is "above", Dz < 0 is "below". Dz = 0 is the singularity. 55 56 fflags == 0 selects Dz > 0, 57 fflags != 0 selects Dz < 0. 58 59 The inverse kinematics flags let the inverse kinematics select between 60 multiple valid solutions of strut lengths for given Cartesian values 61 for D. There are no multiple solutions: D constrains the strut lengths 62 completely. So, the inverse flags are ignored. 63 */ 64 65 #include "kinematics.h" /* these decls */ 66 #include "rtapi_math.h" 67 68 /* ident tag */ 69 #ifndef __GNUC__ 70 #ifndef __attribute__ 71 #define __attribute__(x) 72 #endif 73 #endif 74 75 #include "hal.h" 76 struct haldata { 77 hal_float_t *bx, *cx, *cy; 78 } *haldata = 0; 79 80 #define Bx (*(haldata->bx)) 81 #define Cx (*(haldata->cx)) 82 #define Cy (*(haldata->cy)) 83 84 #define sq(x) ((x)*(x)) 85 86 /* 87 forward kinematics takes three strut lengths and computes Dx, Dy, and Dz 88 pos->tran.x,y,z, respectively. The forward flag is used to resolve 89 D above/below the xy plane. The inverse flags are not set since there 90 are no ambiguities going from world to joint coordinates. 91 92 The forward kins are derived as follows: 93 94 1. Let x, y, z be Dx, Dy, Dz to save pixels. Cartesian displacement from 95 D to A, B, and C gives 96 97 AD^2 = x^2 + y^2 + z^2 98 BD^2 = (x - Bx)^2 + y^2 + z^2 99 CD^2 = (x - Cx)^2 + (y - Cy)^2 + z^2 100 101 This yields 102 103 I. P = x^2 + y^2 + z^2 104 II. Q = x^2 + y^2 + z^2 + sx 105 III. R = x^2 + y^2 + z^2 + tx + uy 106 107 Where 108 109 P = AD^2, 110 Q = BD^2 - Bx^2 111 R = CD^2 - Cx^2 - Cy^2 112 s = -2Bx 113 t = -2Cx 114 u = -2Cy 115 116 II - I gives Q - P = sx, so x = (Q - P)/s, s != 0. The constraint on s 117 means that Bx != 0, or points A and B can't be the same. 118 119 III - II gives R - Q = (t - s)x + uy, so y = (R - Q - (t - s)x)/u, u != 0. 120 The constraint on u means that Cy != 0, or points A B C can't be collinear. 121 122 Substituting x, y into I gives z = sqrt(P - x^2 - y^2), which has two 123 solutions. Positive means the tripod is above the xy plane, negative 124 means below. 125 */ 126 int kinematicsForward(const double * joints, 127 EmcPose * pos, 128 const KINEMATICS_FORWARD_FLAGS * fflags, 129 KINEMATICS_INVERSE_FLAGS * iflags) 130 { 131 #define AD (joints[0]) 132 #define BD (joints[1]) 133 #define CD (joints[2]) 134 #define Dx (pos->tran.x) 135 #define Dy (pos->tran.y) 136 #define Dz (pos->tran.z) 137 double P, Q, R; 138 double s, t, u; 139 140 P = sq(AD); 141 Q = sq(BD) - sq(Bx); 142 R = sq(CD) - sq(Cx) - sq(Cy); 143 s = -2.0 * Bx; 144 t = -2.0 * Cx; 145 u = -2.0 * Cy; 146 147 if (s == 0.0) { 148 /* points A and B coincident. Fix Bx, #defined up top. */ 149 return -1; 150 } 151 Dx = (Q - P) / s; 152 153 if (u == 0.0) { 154 /* points A B C are colinear. Fix Cy, #defined up top. */ 155 return -1; 156 } 157 Dy = (R - Q - (t - s) * Dx) / u; 158 Dz = P - sq(Dx) - sq(Dy); 159 if (Dz < 0.0) { 160 /* triangle inequality violated */ 161 return -1; 162 } 163 Dz = sqrt(Dz); 164 if (*fflags) { 165 Dz = -Dz; 166 } 167 168 pos->a = 0.0; 169 pos->b = 0.0; 170 pos->c = 0.0; 171 172 return 0; 173 174 #undef AD 175 #undef BD 176 #undef CD 177 #undef Dx 178 #undef Dy 179 #undef Dz 180 } 181 182 int kinematicsInverse(const EmcPose * pos, 183 double * joints, 184 const KINEMATICS_INVERSE_FLAGS * iflags, 185 KINEMATICS_FORWARD_FLAGS * fflags) 186 { 187 #define AD (joints[0]) 188 #define BD (joints[1]) 189 #define CD (joints[2]) 190 #define Dx (pos->tran.x) 191 #define Dy (pos->tran.y) 192 #define Dz (pos->tran.z) 193 194 AD = sqrt(sq(Dx) + sq(Dy) + sq(Dz)); 195 BD = sqrt(sq(Dx - Bx) + sq(Dy) + sq(Dz)); 196 CD = sqrt(sq(Dx - Cx) + sq(Dy - Cy) + sq(Dz)); 197 198 *fflags = 0; 199 if (Dz < 0.0) { 200 *fflags = 1; 201 } 202 203 return 0; 204 205 #undef AD 206 #undef BD 207 #undef CD 208 #undef Dx 209 #undef Dy 210 #undef Dz 211 } 212 213 KINEMATICS_TYPE kinematicsType() 214 { 215 return KINEMATICS_BOTH; 216 } 217 218 #ifdef MAIN 219 220 #include <stdio.h> 221 #include <string.h> 222 223 /* 224 Interactive testing of kins. 225 226 Syntax: a.out <Bx> <Cx> <Cy> 227 */ 228 int main(int argc, char *argv[]) 229 { 230 #ifndef BUFFERLEN 231 #define BUFFERLEN 256 232 #endif 233 char buffer[BUFFERLEN]; 234 char cmd[BUFFERLEN]; 235 EmcPose pos, vel; 236 double joints[3], jointvels[3]; 237 char inverse; 238 char flags; 239 KINEMATICS_FORWARD_FLAGS fflags; 240 241 inverse = 0; /* forwards, by default */ 242 flags = 0; /* didn't provide flags */ 243 fflags = 0; /* above xy plane, by default */ 244 if (argc != 4 || 245 1 != sscanf(argv[1], "%lf", &Bx) || 246 1 != sscanf(argv[2], "%lf", &Cx) || 247 1 != sscanf(argv[3], "%lf", &Cy)) { 248 fprintf(stderr, "syntax: %s Bx Cx Cy\n", argv[0]); 249 return 1; 250 } 251 252 while (! feof(stdin)) { 253 if (inverse) { 254 printf("inv> "); 255 } 256 else { 257 printf("fwd> "); 258 } 259 fflush(stdout); 260 261 if (NULL == fgets(buffer, BUFFERLEN, stdin)) { 262 break; 263 } 264 if (1 != sscanf(buffer, "%s", cmd)) { 265 continue; 266 } 267 268 if (! strcmp(cmd, "quit")) { 269 break; 270 } 271 if (! strcmp(cmd, "i")) { 272 inverse = 1; 273 continue; 274 } 275 if (! strcmp(cmd, "f")) { 276 inverse = 0; 277 continue; 278 } 279 if (! strcmp(cmd, "ff")) { 280 if (1 != sscanf(buffer, "%*s %d", &fflags)) { 281 printf("need forward flag\n"); 282 } 283 continue; 284 } 285 286 if (inverse) { /* inverse kins */ 287 if (3 != sscanf(buffer, "%lf %lf %lf", 288 &pos.tran.x, 289 &pos.tran.y, 290 &pos.tran.z)) { 291 printf("need X Y Z\n"); 292 continue; 293 } 294 if (0 != kinematicsInverse(&pos, joints, NULL, &fflags)) { 295 printf("inverse kin error\n"); 296 } 297 else { 298 printf("%f\t%f\t%f\n", joints[0], joints[1], joints[2]); 299 if (0 != kinematicsForward(joints, &pos, &fflags, NULL)) { 300 printf("forward kin error\n"); 301 } 302 else { 303 printf("%f\t%f\t%f\n", pos.tran.x, pos.tran.y, pos.tran.z); 304 } 305 } 306 } 307 else { /* forward kins */ 308 if (flags) { 309 if (4 != sscanf(buffer, "%lf %lf %lf %d", 310 &joints[0], 311 &joints[1], 312 &joints[2], 313 &fflags)) { 314 printf("need 3 strut values and flag\n"); 315 continue; 316 } 317 } 318 else { 319 if (3 != sscanf(buffer, "%lf %lf %lf", 320 &joints[0], 321 &joints[1], 322 &joints[2])) { 323 printf("need 3 strut values\n"); 324 continue; 325 } 326 } 327 if (0 != kinematicsForward(joints, &pos, &fflags, NULL)) { 328 printf("forward kin error\n"); 329 } 330 else { 331 printf("%f\t%f\t%f\n", pos.tran.x, pos.tran.y, pos.tran.z); 332 if (0 != kinematicsInverse(&pos, joints, NULL, &fflags)) { 333 printf("inverse kin error\n"); 334 } 335 else { 336 printf("%f\t%f\t%f\n", joints[0], joints[1], joints[2]); 337 } 338 } 339 } 340 } /* end while (! feof(stdin)) */ 341 342 return 0; 343 } 344 345 #endif /* MAIN */ 346 347 #include "rtapi.h" /* RTAPI realtime OS API */ 348 #include "rtapi_app.h" /* RTAPI realtime module decls */ 349 #include "hal.h" 350 351 EXPORT_SYMBOL(kinematicsType); 352 EXPORT_SYMBOL(kinematicsForward); 353 EXPORT_SYMBOL(kinematicsInverse); 354 355 MODULE_LICENSE("GPL"); 356 357 358 359 int comp_id; 360 int rtapi_app_main(void) { 361 int res = 0; 362 363 comp_id = hal_init("tripodkins"); 364 if(comp_id < 0) return comp_id; 365 366 haldata = hal_malloc(sizeof(struct haldata)); 367 if(!haldata) goto error; 368 369 if((res = hal_pin_float_new("tripodkins.Bx", HAL_IO, &(haldata->bx), comp_id)) < 0) goto error; 370 if((res = hal_pin_float_new("tripodkins.Cx", HAL_IO, &(haldata->cx), comp_id)) < 0) goto error; 371 if((res = hal_pin_float_new("tripodkins.Cy", HAL_IO, &(haldata->cy), comp_id)) < 0) goto error; 372 373 Bx = Cx = Cy = 1.0; 374 hal_ready(comp_id); 375 return 0; 376 377 error: 378 hal_exit(comp_id); 379 return res; 380 } 381 382 void rtapi_app_exit(void) { hal_exit(comp_id); }