genhexkins.c
1 /******************************************************************** 2 * Description: genhexkins.c 3 * 4 * Kinematics for a generalised hexapod machine 5 * 6 * Derived from a work by R. Brian Register 7 * 8 * Adapting Author: Andrew Kyrychenko 9 * License: GPL Version 2 10 * System: Linux 11 * 12 * Copyright (c) 2004 All rights reserved. 13 ********************************************************************* 14 15 These are the forward and inverse kinematic functions for a class of 16 machines referred to as "Stewart Platforms". 17 18 The functions are general enough to be configured for any platform 19 configuration. In the functions "kinematicsForward" and 20 "kinematicsInverse" are arrays "a[i]" and "b[i]". The values stored 21 in these arrays correspond to the positions of the ends of the i'th 22 strut. The value stored in a[i] is the position of the end of the i'th 23 strut attatched to the platform, in platform coordinates. The value 24 stored in b[i] is the position of the end of the i'th strut attached 25 to the base, in base (world) coordinates. 26 27 The default values for base and platform joints positions are defined 28 in the header file genhexkins.h. The actual values for a particular 29 machine can be adjusted by hal parameters: 30 31 genhexkins.base.N.x 32 genhexkins.base.N.y 33 genhexkins.base.N.z - base joint coordinates. 34 35 genhexkins.platform.N.x 36 genhexkins.platform.N.y 37 genhexkins.platform.N.z - platform joint coordinates. 38 39 genhexkins.spindle-offset - added to Z coordinates of all joints to 40 change the machine origin. Facilitates 41 adjusting spindle position. 42 43 genhexkins.tool-offset - tool length offset (TCP offset along Z), 44 implements RTCP function when connected to 45 motion.tooloffset.Z. 46 47 To avoid joints jump change tool offset (G43, G49) only when the 48 platform is not tilted (A = B = 0). 49 50 Some hexapods use non-captive screw actuators and universal (cardanic) 51 joints, thus the strut lengths depend on orientation of joints axes. 52 Strut length correction is implemented to compensate for this. 53 The calculations use orientation (unit vectors) of base and platform 54 joint axes and the lead of actuator screws: 55 56 genhexkins.base-n.N.x 57 genhexkins.base-n.N.y 58 genhexkins.base-n.N.z - unit vectors of base joint axes; 59 60 genhexkins.platform-n.N.x 61 genhexkins.platform-n.N.y 62 genhexkins.platform-n.N.z - unit vectors of platform joint axes 63 in platform CS. 64 genhexkins.screw-lead - lead of strut actuator screw, positive for 65 right-hand thread. Default is 0 (strut length 66 correction disabled). 67 genhexkins.correction.N - pins showing current values of strut length 68 correction. 69 70 The kinematicsInverse function solves the inverse kinematics using 71 a closed form algorithm. The inverse kinematics problem is given 72 the pose of the platform and returns the strut lengths. For this 73 problem there is only one solution that is always returned correctly. 74 75 The kinematicsForward function solves the forward kinematics using 76 an iterative algorithm. Due to the iterative nature of this algorithm 77 the kinematicsForward function requires an initial value to begin the 78 iterative routine and then converges to the "nearest" solution. The 79 forward kinematics problem is given the strut lengths and returns the 80 pose of the platform. For this problem there arein multiple 81 solutions. The kinematicsForward function will return only one of 82 these solutions which will be the solution nearest to the initial 83 value given. It is possible that there are no solutions "near" the 84 given initial value and the iteration will not converge and no 85 solution will be returned. Assuming there is a solution "near" the 86 initial value, the function will always return one correct solution 87 out of the multiple possible solutions. 88 89 Hal pins to control and observe forward kinematics iterations: 90 91 genhexkins.convergence-criterion - minimum error value that ends 92 iterations with converged solution; 93 94 genhexkins.limit-iterations - limit of iterations, if exceeded 95 iterations stop with no convergence; 96 97 genhexkins.max-error - maximum error value, if exceeded iterations 98 stop with no convergence; 99 100 genhexkins.last-iterations - number of iterations spent for the 101 last forward kinematics solution; 102 103 genhexkins.max-iterations - maximum number of iterations spent for 104 a converged solution during current session. 105 106 ----------------------------------------------------------------------------*/ 107 108 #include "rtapi_math.h" 109 #include "posemath.h" 110 #include "genhexkins.h" 111 #include "kinematics.h" /* these decls, KINEMATICS_FORWARD_FLAGS */ 112 #include "hal.h" 113 114 struct haldata { 115 hal_float_t basex[NUM_STRUTS]; 116 hal_float_t basey[NUM_STRUTS]; 117 hal_float_t basez[NUM_STRUTS]; 118 hal_float_t platformx[NUM_STRUTS]; 119 hal_float_t platformy[NUM_STRUTS]; 120 hal_float_t platformz[NUM_STRUTS]; 121 hal_float_t basenx[NUM_STRUTS]; 122 hal_float_t baseny[NUM_STRUTS]; 123 hal_float_t basenz[NUM_STRUTS]; 124 hal_float_t platformnx[NUM_STRUTS]; 125 hal_float_t platformny[NUM_STRUTS]; 126 hal_float_t platformnz[NUM_STRUTS]; 127 hal_float_t *correction[NUM_STRUTS]; 128 hal_float_t screw_lead; 129 hal_u32_t *last_iter; 130 hal_u32_t *max_iter; 131 hal_u32_t iter_limit; 132 hal_float_t max_error; 133 hal_float_t conv_criterion; 134 hal_float_t *tool_offset; 135 hal_float_t spindle_offset; 136 } *haldata; 137 138 139 /******************************* MatInvert() ***************************/ 140 141 /*----------------------------------------------------------------------------- 142 This is a function that inverts a 6x6 matrix. 143 -----------------------------------------------------------------------------*/ 144 145 static int MatInvert(double J[][NUM_STRUTS], double InvJ[][NUM_STRUTS]) 146 { 147 double JAug[NUM_STRUTS][12], m, temp; 148 int j, k, n; 149 150 /* This function determines the inverse of a 6x6 matrix using 151 Gauss-Jordan elimination */ 152 153 /* Augment the Identity matrix to the Jacobian matrix */ 154 155 for (j=0; j<=5; ++j){ 156 for (k=0; k<=5; ++k){ /* Assign J matrix to first 6 columns of AugJ */ 157 JAug[j][k] = J[j][k]; 158 } 159 for(k=6; k<=11; ++k){ /* Assign I matrix to last six columns of AugJ */ 160 if (k-6 == j){ 161 JAug[j][k]=1; 162 } 163 else{ 164 JAug[j][k]=0; 165 } 166 } 167 } 168 169 /* Perform Gauss elimination */ 170 for (k=0; k<=4; ++k){ /* Pivot */ 171 if ((JAug[k][k]< 0.01) && (JAug[k][k] > -0.01)){ 172 for (j=k+1;j<=5; ++j){ 173 if ((JAug[j][k]>0.01) || (JAug[j][k]<-0.01)){ 174 for (n=0; n<=11;++n){ 175 temp = JAug[k][n]; 176 JAug[k][n] = JAug[j][n]; 177 JAug[j][n] = temp; 178 } 179 break; 180 } 181 } 182 } 183 for (j=k+1; j<=5; ++j){ /* Pivot */ 184 m = -JAug[j][k] / JAug[k][k]; 185 for (n=0; n<=11; ++n){ 186 JAug[j][n]=JAug[j][n] + m*JAug[k][n]; /* (Row j) + m * (Row k) */ 187 if ((JAug[j][n] < 0.000001) && (JAug[j][n] > -0.000001)){ 188 JAug[j][n] = 0; 189 } 190 } 191 } 192 } 193 194 /* Normalization of Diagonal Terms */ 195 for (j=0; j<=5; ++j){ 196 m=1/JAug[j][j]; 197 for(k=0; k<=11; ++k){ 198 JAug[j][k] = m * JAug[j][k]; 199 } 200 } 201 202 /* Perform Gauss Jordan Steps */ 203 for (k=5; k>=0; --k){ 204 for(j=k-1; j>=0; --j){ 205 m = -JAug[j][k]/JAug[k][k]; 206 for (n=0; n<=11; ++n){ 207 JAug[j][n] = JAug[j][n] + m * JAug[k][n]; 208 } 209 } 210 } 211 212 /* Assign last 6 columns of JAug to InvJ */ 213 for (j=0; j<=5; ++j){ 214 for (k=0; k<=5; ++k){ 215 InvJ[j][k] = JAug[j][k+6]; 216 217 } 218 } 219 220 return 0; /* FIXME-- check divisors for 0 above */ 221 } 222 223 /******************************** MatMult() *********************************/ 224 225 /*--------------------------------------------------------------------------- 226 This function simply multiplies a 6x6 matrix by a 1x6 vector 227 ---------------------------------------------------------------------------*/ 228 229 static void MatMult(double J[][6], const double x[], double Ans[]) 230 { 231 int j, k; 232 for (j=0; j<=5; ++j){ 233 Ans[j] = 0; 234 for (k=0; k<=5; ++k){ 235 Ans[j] = J[j][k]*x[k]+Ans[j]; 236 } 237 } 238 } 239 240 /* declare arrays for base and platform coordinates */ 241 static PmCartesian b[NUM_STRUTS]; 242 static PmCartesian a[NUM_STRUTS]; 243 244 /* declare base and platform joint axes vectors */ 245 246 static PmCartesian nb1[NUM_STRUTS]; 247 static PmCartesian na0[NUM_STRUTS]; 248 249 /************************genhexkins_read_hal_pins**************************/ 250 251 int genhexkins_read_hal_pins(void) { 252 int t; 253 254 /* set the base and platform coordinates from hal pin values */ 255 for (t = 0; t < NUM_STRUTS; t++) { 256 b[t].x = haldata->basex[t]; 257 b[t].y = haldata->basey[t]; 258 b[t].z = haldata->basez[t] + haldata->spindle_offset + *haldata->tool_offset; 259 a[t].x = haldata->platformx[t]; 260 a[t].y = haldata->platformy[t]; 261 a[t].z = haldata->platformz[t] + haldata->spindle_offset + *haldata->tool_offset; 262 263 nb1[t].x = haldata->basenx[t]; 264 nb1[t].y = haldata->baseny[t]; 265 nb1[t].z = haldata->basenz[t]; 266 na0[t].x = haldata->platformnx[t]; 267 na0[t].y = haldata->platformny[t]; 268 na0[t].z = haldata->platformnz[t]; 269 270 } 271 return 0; 272 } 273 274 /***************************StrutLengthCorrection***************************/ 275 276 int StrutLengthCorrection(const PmCartesian * StrutVectUnit, 277 const PmRotationMatrix * RMatrix, 278 const int strut_number, 279 double * correction) 280 { 281 PmCartesian nb2, nb3, na1, na2; 282 double dotprod; 283 284 /* define base joints axis vectors */ 285 pmCartCartCross(&nb1[strut_number], StrutVectUnit, &nb2); 286 pmCartCartCross(StrutVectUnit, &nb2, &nb3); 287 pmCartUnitEq(&nb3); 288 289 /* define platform joints axis vectors */ 290 pmMatCartMult(RMatrix, &na0[strut_number], &na1); 291 pmCartCartCross(&na1, StrutVectUnit, &na2); 292 pmCartUnitEq(&na2); 293 294 /* define dot product */ 295 pmCartCartDot(&nb3, &na2, &dotprod); 296 297 *correction = haldata->screw_lead * asin(dotprod) / PM_2_PI; 298 299 return 0; 300 } 301 302 303 /**************************** kinematicsForward() ***************************/ 304 305 int kinematicsForward(const double * joints, 306 EmcPose * pos, 307 const KINEMATICS_FORWARD_FLAGS * fflags, 308 KINEMATICS_INVERSE_FLAGS * iflags) 309 { 310 311 PmCartesian aw; 312 PmCartesian InvKinStrutVect,InvKinStrutVectUnit; 313 PmCartesian q_trans, RMatrix_a, RMatrix_a_cross_Strut; 314 315 double Jacobian[NUM_STRUTS][NUM_STRUTS]; 316 double InverseJacobian[NUM_STRUTS][NUM_STRUTS]; 317 double InvKinStrutLength, StrutLengthDiff[NUM_STRUTS]; 318 double delta[NUM_STRUTS]; 319 double conv_err = 1.0; 320 double corr; 321 322 PmRotationMatrix RMatrix; 323 PmRpy q_RPY; 324 325 int iterate = 1; 326 int i; 327 int iteration = 0; 328 329 genhexkins_read_hal_pins(); 330 331 /* abort on obvious problems, like joints <= 0 */ 332 /* FIXME-- should check against triangle inequality, so that joints 333 are never too short to span shared base and platform sides */ 334 if (joints[0] <= 0.0 || 335 joints[1] <= 0.0 || 336 joints[2] <= 0.0 || 337 joints[3] <= 0.0 || 338 joints[4] <= 0.0 || 339 joints[5] <= 0.0) { 340 return -1; 341 } 342 343 /* assign a,b,c to roll, pitch, yaw angles */ 344 q_RPY.r = pos->a * PM_PI / 180.0; 345 q_RPY.p = pos->b * PM_PI / 180.0; 346 q_RPY.y = pos->c * PM_PI / 180.0; 347 348 /* Assign translation values in pos to q_trans */ 349 q_trans.x = pos->tran.x; 350 q_trans.y = pos->tran.y; 351 q_trans.z = pos->tran.z; 352 353 /* Enter Newton-Raphson iterative method */ 354 while (iterate) { 355 /* check for large error and return error flag if no convergence */ 356 if ((conv_err > +(haldata->max_error)) || 357 (conv_err < -(haldata->max_error))) { 358 /* we can't converge */ 359 return -2; 360 }; 361 362 iteration++; 363 364 /* check iteration to see if the kinematics can reach the 365 convergence criterion and return error flag if it can't */ 366 if (iteration > haldata->iter_limit) { 367 /* we can't converge */ 368 return -5; 369 } 370 371 /* Convert q_RPY to Rotation Matrix */ 372 pmRpyMatConvert(&q_RPY, &RMatrix); 373 374 /* compute StrutLengthDiff[] by running inverse kins on Cartesian 375 estimate to get joint estimate, subtract joints to get joint deltas, 376 and compute inv J while we're at it */ 377 for (i = 0; i < NUM_STRUTS; i++) { 378 pmMatCartMult(&RMatrix, &a[i], &RMatrix_a); 379 pmCartCartAdd(&q_trans, &RMatrix_a, &aw); 380 pmCartCartSub(&aw, &b[i], &InvKinStrutVect); 381 if (0 != pmCartUnit(&InvKinStrutVect, &InvKinStrutVectUnit)) { 382 return -1; 383 } 384 pmCartMag(&InvKinStrutVect, &InvKinStrutLength); 385 386 if (haldata->screw_lead != 0.0) { 387 /* enable strut length correction */ 388 StrutLengthCorrection(&InvKinStrutVectUnit, &RMatrix, i, &corr); 389 /* define corrected joint lengths */ 390 InvKinStrutLength += corr; 391 } 392 393 StrutLengthDiff[i] = InvKinStrutLength - joints[i]; 394 395 /* Determine RMatrix_a_cross_strut */ 396 pmCartCartCross(&RMatrix_a, &InvKinStrutVectUnit, &RMatrix_a_cross_Strut); 397 398 /* Build Inverse Jacobian Matrix */ 399 InverseJacobian[i][0] = InvKinStrutVectUnit.x; 400 InverseJacobian[i][1] = InvKinStrutVectUnit.y; 401 InverseJacobian[i][2] = InvKinStrutVectUnit.z; 402 InverseJacobian[i][3] = RMatrix_a_cross_Strut.x; 403 InverseJacobian[i][4] = RMatrix_a_cross_Strut.y; 404 InverseJacobian[i][5] = RMatrix_a_cross_Strut.z; 405 } 406 407 /* invert Inverse Jacobian */ 408 MatInvert(InverseJacobian, Jacobian); 409 410 /* multiply Jacobian by LegLengthDiff */ 411 MatMult(Jacobian, StrutLengthDiff, delta); 412 413 /* subtract delta from last iterations pos values */ 414 q_trans.x -= delta[0]; 415 q_trans.y -= delta[1]; 416 q_trans.z -= delta[2]; 417 q_RPY.r -= delta[3]; 418 q_RPY.p -= delta[4]; 419 q_RPY.y -= delta[5]; 420 421 /* determine value of conv_error (used to determine if no convergence) */ 422 conv_err = 0.0; 423 for (i = 0; i < NUM_STRUTS; i++) { 424 conv_err += fabs(StrutLengthDiff[i]); 425 } 426 427 /* enter loop to determine if a strut needs another iteration */ 428 iterate = 0; /*assume iteration is done */ 429 for (i = 0; i < NUM_STRUTS; i++) { 430 if (fabs(StrutLengthDiff[i]) > haldata->conv_criterion) { 431 iterate = 1; 432 } 433 } 434 } /* exit Newton-Raphson Iterative loop */ 435 436 /* assign r,p,y to a,b,c */ 437 pos->a = q_RPY.r * 180.0 / PM_PI; 438 pos->b = q_RPY.p * 180.0 / PM_PI; 439 pos->c = q_RPY.y * 180.0 / PM_PI; 440 441 /* assign q_trans to pos */ 442 pos->tran.x = q_trans.x; 443 pos->tran.y = q_trans.y; 444 pos->tran.z = q_trans.z; 445 446 *haldata->last_iter = iteration; 447 448 if (iteration > *haldata->max_iter){ 449 *haldata->max_iter = iteration; 450 } 451 return 0; 452 } 453 454 455 /************************ kinematicsInverse() ********************************/ 456 /* the inverse kinematics take world coordinates and determine joint values, 457 given the inverse kinematics flags to resolve any ambiguities. The forward 458 flags are set to indicate their value appropriate to the world coordinates 459 passed in. */ 460 461 int kinematicsInverse(const EmcPose * pos, 462 double * joints, 463 const KINEMATICS_INVERSE_FLAGS * iflags, 464 KINEMATICS_FORWARD_FLAGS * fflags) 465 { 466 467 PmCartesian aw, temp; 468 PmCartesian InvKinStrutVect, InvKinStrutVectUnit; 469 PmRotationMatrix RMatrix; 470 PmRpy rpy; 471 int i; 472 double InvKinStrutLength, corr; 473 474 genhexkins_read_hal_pins(); 475 476 /* define Rotation Matrix */ 477 rpy.r = pos->a * PM_PI / 180.0; 478 rpy.p = pos->b * PM_PI / 180.0; 479 rpy.y = pos->c * PM_PI / 180.0; 480 pmRpyMatConvert(&rpy, &RMatrix); 481 482 /* enter for loop to calculate joints (strut lengths) */ 483 for (i = 0; i < NUM_STRUTS; i++) { 484 /* convert location of platform strut end from platform 485 to world coordinates */ 486 pmMatCartMult(&RMatrix, &a[i], &temp); 487 pmCartCartAdd(&pos->tran, &temp, &aw); 488 489 /* define strut lengths */ 490 pmCartCartSub(&aw, &b[i], &InvKinStrutVect); 491 pmCartMag(&InvKinStrutVect, &InvKinStrutLength); 492 493 if (haldata->screw_lead != 0.0) { 494 /* enable strut length correction */ 495 /* define unit strut vector */ 496 if (0 != pmCartUnit(&InvKinStrutVect, &InvKinStrutVectUnit)) { 497 return -1; 498 } 499 /* define correction value and corrected joint lengths */ 500 StrutLengthCorrection(&InvKinStrutVectUnit, &RMatrix, i, &corr); 501 *haldata->correction[i] = corr; 502 InvKinStrutLength += corr; 503 } 504 505 joints[i] = InvKinStrutLength; 506 } 507 508 return 0; 509 } 510 511 512 KINEMATICS_TYPE kinematicsType() 513 { 514 return KINEMATICS_BOTH; 515 } 516 517 518 #include "rtapi.h" /* RTAPI realtime OS API */ 519 #include "rtapi_app.h" /* RTAPI realtime module decls */ 520 521 EXPORT_SYMBOL(kinematicsType); 522 EXPORT_SYMBOL(kinematicsForward); 523 EXPORT_SYMBOL(kinematicsInverse); 524 525 MODULE_LICENSE("GPL"); 526 527 int comp_id; 528 529 int rtapi_app_main(void) 530 { 531 int res = 0, i; 532 533 comp_id = hal_init("genhexkins"); 534 if (comp_id < 0) 535 return comp_id; 536 537 haldata = hal_malloc(sizeof(struct haldata)); 538 if (!haldata) 539 goto error; 540 541 542 for (i = 0; i < 6; i++) { 543 544 if ((res = hal_param_float_newf(HAL_RW, &(haldata->basex[i]), comp_id, 545 "genhexkins.base.%d.x", i)) < 0) 546 goto error; 547 548 if ((res = hal_param_float_newf(HAL_RW, &haldata->basey[i], comp_id, 549 "genhexkins.base.%d.y", i)) < 0) 550 goto error; 551 552 if ((res = hal_param_float_newf(HAL_RW, &haldata->basez[i], comp_id, 553 "genhexkins.base.%d.z", i)) < 0) 554 goto error; 555 556 if ((res = hal_param_float_newf(HAL_RW, &haldata->platformx[i], comp_id, 557 "genhexkins.platform.%d.x", i)) < 0) 558 goto error; 559 560 if ((res = hal_param_float_newf(HAL_RW, &haldata->platformy[i], comp_id, 561 "genhexkins.platform.%d.y", i)) < 0) 562 goto error; 563 564 if ((res = hal_param_float_newf(HAL_RW, &haldata->platformz[i], comp_id, 565 "genhexkins.platform.%d.z", i)) < 0) 566 goto error; 567 568 if ((res = hal_param_float_newf(HAL_RW, &haldata->basenx[i], comp_id, 569 "genhexkins.base-n.%d.x", i)) < 0) 570 goto error; 571 572 if ((res = hal_param_float_newf(HAL_RW, &haldata->baseny[i], comp_id, 573 "genhexkins.base-n.%d.y", i)) < 0) 574 goto error; 575 576 if ((res = hal_param_float_newf(HAL_RW, &haldata->basenz[i], comp_id, 577 "genhexkins.base-n.%d.z", i)) < 0) 578 goto error; 579 580 if ((res = hal_param_float_newf(HAL_RW, &haldata->platformnx[i], comp_id, 581 "genhexkins.platform-n.%d.x", i)) < 0) 582 goto error; 583 584 if ((res = hal_param_float_newf(HAL_RW, &haldata->platformny[i], comp_id, 585 "genhexkins.platform-n.%d.y", i)) < 0) 586 goto error; 587 588 if ((res = hal_param_float_newf(HAL_RW, &haldata->platformnz[i], comp_id, 589 "genhexkins.platform-n.%d.z", i)) < 0) 590 goto error; 591 592 if ((res = hal_pin_float_newf(HAL_OUT, &haldata->correction[i], comp_id, 593 "genhexkins.correction.%d", i)) < 0) 594 goto error; 595 *haldata->correction[i] = 0.0; 596 597 } 598 599 if ((res = hal_pin_u32_newf(HAL_OUT, &haldata->last_iter, comp_id, 600 "genhexkins.last-iterations")) < 0) 601 goto error; 602 *haldata->last_iter = 0; 603 604 if ((res = hal_pin_u32_newf(HAL_OUT, &haldata->max_iter, comp_id, 605 "genhexkins.max-iterations")) < 0) 606 goto error; 607 *haldata->max_iter = 0; 608 609 if ((res = hal_param_float_newf(HAL_RW, &haldata->max_error, comp_id, 610 "genhexkins.max-error")) < 0) 611 goto error; 612 haldata->max_error = 500.0; 613 614 if ((res = hal_param_float_newf(HAL_RW, &haldata->conv_criterion, comp_id, 615 "genhexkins.convergence-criterion")) < 0) 616 goto error; 617 haldata->conv_criterion = 1e-9; 618 619 if ((res = hal_param_u32_newf(HAL_RW, &haldata->iter_limit, comp_id, 620 "genhexkins.limit-iterations")) < 0) 621 goto error; 622 haldata->iter_limit = 120; 623 624 if ((res = hal_pin_float_newf(HAL_IN, &haldata->tool_offset, comp_id, 625 "genhexkins.tool-offset")) < 0) 626 goto error; 627 *haldata->tool_offset = 0.0; 628 629 if ((res = hal_param_float_newf(HAL_RW, &haldata->spindle_offset, comp_id, 630 "genhexkins.spindle-offset")) < 0) 631 goto error; 632 haldata->spindle_offset = 0.0; 633 634 if ((res = hal_param_float_newf(HAL_RW, &haldata->screw_lead, comp_id, 635 "genhexkins.screw-lead")) < 0) 636 goto error; 637 haldata->screw_lead = DEFAULT_SCREW_LEAD; 638 639 haldata->basex[0] = DEFAULT_BASE_0_X; 640 haldata->basey[0] = DEFAULT_BASE_0_Y; 641 haldata->basez[0] = DEFAULT_BASE_0_Z; 642 haldata->basex[1] = DEFAULT_BASE_1_X; 643 haldata->basey[1] = DEFAULT_BASE_1_Y; 644 haldata->basez[1] = DEFAULT_BASE_1_Z; 645 haldata->basex[2] = DEFAULT_BASE_2_X; 646 haldata->basey[2] = DEFAULT_BASE_2_Y; 647 haldata->basez[2] = DEFAULT_BASE_2_Z; 648 haldata->basex[3] = DEFAULT_BASE_3_X; 649 haldata->basey[3] = DEFAULT_BASE_3_Y; 650 haldata->basez[3] = DEFAULT_BASE_3_Z; 651 haldata->basex[4] = DEFAULT_BASE_4_X; 652 haldata->basey[4] = DEFAULT_BASE_4_Y; 653 haldata->basez[4] = DEFAULT_BASE_4_Z; 654 haldata->basex[5] = DEFAULT_BASE_5_X; 655 haldata->basey[5] = DEFAULT_BASE_5_Y; 656 haldata->basez[5] = DEFAULT_BASE_5_Z; 657 658 haldata->platformx[0] = DEFAULT_PLATFORM_0_X; 659 haldata->platformy[0] = DEFAULT_PLATFORM_0_Y; 660 haldata->platformz[0] = DEFAULT_PLATFORM_0_Z; 661 haldata->platformx[1] = DEFAULT_PLATFORM_1_X; 662 haldata->platformy[1] = DEFAULT_PLATFORM_1_Y; 663 haldata->platformz[1] = DEFAULT_PLATFORM_1_Z; 664 haldata->platformx[2] = DEFAULT_PLATFORM_2_X; 665 haldata->platformy[2] = DEFAULT_PLATFORM_2_Y; 666 haldata->platformz[2] = DEFAULT_PLATFORM_2_Z; 667 haldata->platformx[3] = DEFAULT_PLATFORM_3_X; 668 haldata->platformy[3] = DEFAULT_PLATFORM_3_Y; 669 haldata->platformz[3] = DEFAULT_PLATFORM_3_Z; 670 haldata->platformx[4] = DEFAULT_PLATFORM_4_X; 671 haldata->platformy[4] = DEFAULT_PLATFORM_4_Y; 672 haldata->platformz[4] = DEFAULT_PLATFORM_4_Z; 673 haldata->platformx[5] = DEFAULT_PLATFORM_5_X; 674 haldata->platformy[5] = DEFAULT_PLATFORM_5_Y; 675 haldata->platformz[5] = DEFAULT_PLATFORM_5_Z; 676 677 haldata->basenx[0] = DEFAULT_BASE_0_NX; 678 haldata->baseny[0] = DEFAULT_BASE_0_NY; 679 haldata->basenz[0] = DEFAULT_BASE_0_NZ; 680 haldata->basenx[1] = DEFAULT_BASE_1_NX; 681 haldata->baseny[1] = DEFAULT_BASE_1_NY; 682 haldata->basenz[1] = DEFAULT_BASE_1_NZ; 683 haldata->basenx[2] = DEFAULT_BASE_2_NX; 684 haldata->baseny[2] = DEFAULT_BASE_2_NY; 685 haldata->basenz[2] = DEFAULT_BASE_2_NZ; 686 haldata->basenx[3] = DEFAULT_BASE_3_NX; 687 haldata->baseny[3] = DEFAULT_BASE_3_NY; 688 haldata->basenz[3] = DEFAULT_BASE_3_NZ; 689 haldata->basenx[4] = DEFAULT_BASE_4_NX; 690 haldata->baseny[4] = DEFAULT_BASE_4_NY; 691 haldata->basenz[4] = DEFAULT_BASE_4_NZ; 692 haldata->basenx[5] = DEFAULT_BASE_5_NX; 693 haldata->baseny[5] = DEFAULT_BASE_5_NY; 694 haldata->basenz[5] = DEFAULT_BASE_5_NZ; 695 696 haldata->platformnx[0] = DEFAULT_PLATFORM_0_NX; 697 haldata->platformny[0] = DEFAULT_PLATFORM_0_NY; 698 haldata->platformnz[0] = DEFAULT_PLATFORM_0_NZ; 699 haldata->platformnx[1] = DEFAULT_PLATFORM_1_NX; 700 haldata->platformny[1] = DEFAULT_PLATFORM_1_NY; 701 haldata->platformnz[1] = DEFAULT_PLATFORM_1_NZ; 702 haldata->platformnx[2] = DEFAULT_PLATFORM_2_NX; 703 haldata->platformny[2] = DEFAULT_PLATFORM_2_NY; 704 haldata->platformnz[2] = DEFAULT_PLATFORM_2_NZ; 705 haldata->platformnx[3] = DEFAULT_PLATFORM_3_NX; 706 haldata->platformny[3] = DEFAULT_PLATFORM_3_NY; 707 haldata->platformnz[3] = DEFAULT_PLATFORM_3_NZ; 708 haldata->platformnx[4] = DEFAULT_PLATFORM_4_NX; 709 haldata->platformny[4] = DEFAULT_PLATFORM_4_NY; 710 haldata->platformnz[4] = DEFAULT_PLATFORM_4_NZ; 711 haldata->platformnx[5] = DEFAULT_PLATFORM_5_NX; 712 haldata->platformny[5] = DEFAULT_PLATFORM_5_NY; 713 haldata->platformnz[5] = DEFAULT_PLATFORM_5_NZ; 714 715 hal_ready(comp_id); 716 return 0; 717 718 error: 719 hal_exit(comp_id); 720 return res; 721 } 722 723 724 void rtapi_app_exit(void) 725 { 726 hal_exit(comp_id); 727 }