From ccf64755b80815b04205e22b0587cdd71b90b010 Mon Sep 17 00:00:00 2001 From: Andrew Kyrychenko Date: Sat, 20 Dec 2014 00:32:39 +0200 Subject: [PATCH] Add hal pins for joints coordinates and diagnostics Pins added for base and platform joints coordinates, iterations control and diagnostics. Cleaned from unused code. Signed-off-by: Andrew Kyrychenko --- src/emc/kinematics/genhexkins.c | 682 ++++++++++++--------------------------- src/emc/kinematics/genhexkins.h | 365 ++++----------------- 2 files changed, 264 insertions(+), 783 deletions(-) diff --git a/src/emc/kinematics/genhexkins.c b/src/emc/kinematics/genhexkins.c index 5f9ed78..834b13c 100644 --- a/src/emc/kinematics/genhexkins.c +++ b/src/emc/kinematics/genhexkins.c @@ -1,17 +1,16 @@ /******************************************************************** * Description: genhexkins.c +* * Kinematics for a generalised hexapod machine * * Derived from a work by R. Brian Register * -* Author: +* Adapting Author: Andrew Kyrychenko * License: GPL Version 2 * System: Linux * -* Copyright (c) 2004 All rights reserved. -* -* Last change: -******************************************************************* +* Last change: 2014.11.27 +********************************************************************* These are the forward and inverse kinematic functions for a class of machines referred to as "Stewart Platforms". @@ -25,14 +24,21 @@ stored in b[i] is the position of the end of the i'th strut attached to the base, in base (world) coordinates. - To have these functions solve the kinematics of a particular - platform configuration, adjust the values stored in arrays "a[i]" and - "b[i]". The values stored in these arrays are defined in the header - file genhex.h. The kinematicsInverse function solves the inverse - kinematics using a closed form algorithm. The inverse kinematics - problem is given the pose of the platform and returns the strut - lengths. For this problem there is only one solution that is always - returned correctly. + The default values for base and platform joints positions are defined + in the header file genhexkins.h. The actual values for a particular + machine can be adjusted by hal pins + + genhexkins.base.N.x + genhexkins.base.N.y + genhexkins.base.N.z + genhexkins.platform.N.x + genhexkins.platform.N.y + genhexkins.platform.N.z + + The kinematicsInverse function solves the inverse kinematics using + a closed form algorithm. The inverse kinematics problem is given + the pose of the platform and returns the strut lengths. For this + problem there is only one solution that is always returned correctly. The kinematicsForward function solves the forward kinematics using an iterative algorithm. Due to the iterative nature of this algorithm @@ -47,13 +53,48 @@ solution will be returned. Assuming there is a solution "near" the initial value, the function will always return one correct solution out of the multiple possible solutions. - -----------------------------------------------------------------------------*/ + + There are diagnostic hal pins to control forward kinematics iterations: + + genhexkins.convergence-criterion (IO) - minimum error value that ends + iterations with converged solution; + + genhexkins.last-iterations (RO) - number of iterations spent for the + last forward kinematics solution; + + genhexkins.limit-iterations (IO) - limit of iterations, if exceeded + stops iterations with no convergence; + + genhexkins.max-error (IO) - large error value, if exceeded stops + iterations with no convergence; + + genhexkins.max-iterations (RO) - maximum number of iterations spent for + a converged solution during current session; + + + ----------------------------------------------------------------------------*/ #include "rtapi_math.h" #include "posemath.h" #include "genhexkins.h" #include "kinematics.h" /* these decls, KINEMATICS_FORWARD_FLAGS */ +#include "hal.h" +struct haldata { + hal_float_t *basex[6]; + hal_float_t *basey[6]; + hal_float_t *basez[6]; + hal_float_t *platformx[6]; + hal_float_t *platformy[6]; + hal_float_t *platformz[6]; + hal_u32_t *iter; + hal_u32_t *max_iter; + hal_u32_t *iter_limit; + hal_float_t *max_error; + hal_float_t *conv_criterion; +} *haldata = 0; + + /******************************* MatInvert() ***************************/ /*----------------------------------------------------------------------------- @@ -155,70 +196,24 @@ static void MatMult(double J[][6], const double x[], double Ans[]) } } -/* define position of base strut ends in base (world) coordinate system */ -static PmCartesian b[6] = {{BASE_0_X, BASE_0_Y, BASE_0_Z}, - {BASE_1_X, BASE_1_Y, BASE_1_Z}, - {BASE_2_X, BASE_2_Y, BASE_2_Z}, - {BASE_3_X, BASE_3_Y, BASE_3_Z}, - {BASE_4_X, BASE_4_Y, BASE_4_Z}, - {BASE_5_X, BASE_5_Y, BASE_5_Z}}; - -static PmCartesian a[6] = {{PLATFORM_0_X, PLATFORM_0_Y, PLATFORM_0_Z}, - {PLATFORM_1_X, PLATFORM_1_Y, PLATFORM_1_Z}, - {PLATFORM_2_X, PLATFORM_2_Y, PLATFORM_2_Z}, - {PLATFORM_3_X, PLATFORM_3_Y, PLATFORM_3_Z}, - {PLATFORM_4_X, PLATFORM_4_Y, PLATFORM_4_Z}, - {PLATFORM_5_X, PLATFORM_5_Y, PLATFORM_5_Z}}; - -const char * kinematicsGetName(void) -{ - return "genhex"; -} - -int kinematicsSetParameters(const double *params) -{ - int t; - PmCartesian base[6]; - PmCartesian platform[6]; - - for(t=0; t<6; t++) - { - base[t].x=params[t*3+0]; - base[t].y=params[t*3+1]; - base[t].z=params[t*3+2]; - } - for(t=0; t<6; t++) - { - platform[t].x=params[18+t*3+0]; - platform[t].y=params[18+t*3+1]; - platform[t].z=params[18+t*3+2]; - } - - return genhexSetParams(base,platform); -} - +/* declare arrays for base and platform coordinates */ +static PmCartesian b[NUM_STRUTS]; +static PmCartesian a[NUM_STRUTS]; -int genhexSetParams(const PmCartesian base[], const PmCartesian platform[]) -{ - int t; +/***************************genhexkins_init*******************************/ - for (t = 0; t < 6; t++) { - b[t] = base[t]; - a[t] = platform[t]; - } - - return 0; -} - -int genhexGetParams(PmCartesian base[], PmCartesian platform[]) -{ - int t; - - for (t = 0; t < 6; t++) { - base[t] = b[t]; - platform[t] = b[t]; - } +int genhexkins_init(void) { + int t; + /* set the base and platform coordinates from hal pin values */ + for (t = 0; t < NUM_STRUTS; t++) { + b[t].x = *haldata->basex[t]; + b[t].y = *haldata->basey[t]; + b[t].z = *haldata->basez[t]; + a[t].x = *haldata->platformx[t]; + a[t].y = *haldata->platformy[t]; + a[t].z = *haldata->platformz[t]; + } return 0; } @@ -234,6 +229,8 @@ static int JInvMat(const EmcPose * pos, PmCartesian InvKinStrutVect,InvKinStrutVectUnit; PmCartesian RMatrix_a_cross_Strut; + genhexkins_init(); + /* assign a, b, c to roll, pitch, yaw angles and convert to rot matrix */ rpy.r = pos->a * PM_PI / 180.0; rpy.p = pos->b * PM_PI / 180.0; @@ -328,12 +325,12 @@ int jacobianForward(const double * joints, flags are set to indicate their value appropriate to the world coordinates passed in. */ -static int iteration = 0; /* global so we can report it */ int kinematicsForward(const double * joints, EmcPose * pos, const KINEMATICS_FORWARD_FLAGS * fflags, KINEMATICS_INVERSE_FLAGS * iflags) { + PmCartesian aw; PmCartesian InvKinStrutVect,InvKinStrutVectUnit; PmCartesian q_trans, RMatrix_a, RMatrix_a_cross_Strut; @@ -350,17 +347,9 @@ int kinematicsForward(const double * joints, int iterate = 1; int i; int retval = 0; + int iteration = 0; -#define HIGH_CONV_CRITERION (1e-12) -#define MEDIUM_CONV_CRITERION (1e-5) -#define LOW_CONV_CRITERION (1e-3) -#define MEDIUM_CONV_ITERATIONS 50 -#define LOW_CONV_ITERATIONS 100 -#define FAIL_CONV_ITERATIONS 150 -#define LARGE_CONV_ERROR 10000 - double conv_criterion = HIGH_CONV_CRITERION; - - iteration = 0; + genhexkins_init(); /* abort on obvious problems, like joints <= 0 */ /* FIXME-- should check against triangle inequality, so that joints @@ -387,33 +376,17 @@ int kinematicsForward(const double * joints, /* Enter Newton-Raphson iterative method */ while (iterate) { /* check for large error and return error flag if no convergence */ - if ((conv_err > +LARGE_CONV_ERROR) || - (conv_err < -LARGE_CONV_ERROR)) { + if ((conv_err > +(*haldata->max_error)) || + (conv_err < -(*haldata->max_error))) { /* we can't converge */ return -2; }; iteration++; -#if 0 - /* if forward kinematics are having a difficult time converging - ease the restrictions on the convergence criterion */ - if (iteration == MEDIUM_CONV_ITERATIONS) { - conv_criterion = MEDIUM_CONV_CRITERION; - retval = -3; /* this means if we eventually converge, - the result is sloppy */ - } - - if (iteration == LOW_CONV_ITERATIONS) { - conv_criterion = LOW_CONV_CRITERION; - retval = -4; /* this means if we eventually converge, - the result is even sloppier */ - } -#endif - /* check iteration to see if the kinematics can reach the convergence criterion and return error flag if it can't */ - if (iteration > FAIL_CONV_ITERATIONS) { + if (iteration > *haldata->iter_limit) { /* we can't converge */ return -5; } @@ -469,7 +442,7 @@ int kinematicsForward(const double * joints, /* enter loop to determine if a strut needs another iteration */ iterate = 0; /*assume iteration is done */ for (i = 0; i < NUM_STRUTS; i++) { - if (fabs(StrutLengthDiff[i]) > conv_criterion) { + if (fabs(StrutLengthDiff[i]) > *haldata->conv_criterion) { iterate = 1; } } @@ -485,13 +458,14 @@ int kinematicsForward(const double * joints, pos->tran.y = q_trans.y; pos->tran.z = q_trans.z; + *haldata->iter = iteration; + + if (iteration > *haldata->max_iter) + *haldata->max_iter = iteration; + return retval; } -int genhexKinematicsForwardIterations(void) -{ - return iteration; -} /************************ kinematicsInverse() ********************************/ @@ -500,11 +474,14 @@ int kinematicsInverse(const EmcPose * pos, const KINEMATICS_INVERSE_FLAGS * iflags, KINEMATICS_FORWARD_FLAGS * fflags) { + PmCartesian aw, temp; PmRotationMatrix RMatrix; PmRpy rpy; int i; + genhexkins_init(); + /* define Rotation Matrix */ rpy.r = pos->a * PM_PI / 180.0; rpy.p = pos->b * PM_PI / 180.0; @@ -526,369 +503,15 @@ int kinematicsInverse(const EmcPose * pos, return 0; } -/* - kinematicsHome() is implemented by taking the desired world coordinates, - which are passed as an arg, and running the inverse kinematics to get - the resulting joints. The flags are set to zero. -*/ -int kinematicsHome(EmcPose * world, - double * joint, - KINEMATICS_FORWARD_FLAGS * fflags, - KINEMATICS_INVERSE_FLAGS * iflags) -{ - *fflags = 0; - *iflags = 0; - - return kinematicsInverse(world, joint, iflags, fflags); -} KINEMATICS_TYPE kinematicsType() { -#if 1 return KINEMATICS_BOTH; -#else - return KINEMATICS_INVERSE_ONLY; -#endif } -#ifdef MAIN - -#include -/* FIXME-- this works for Unix only */ -#include /* struct timeval */ -#include /* gettimeofday() */ -static double timestamp() -{ - struct timeval tp; - - if (0 != gettimeofday(&tp, NULL)) { - return 0.0; - } - return ((double) tp.tv_sec) + ((double) tp.tv_usec) / 1000000.0; -} - -int main(int argc, char *argv[]) -{ -#define BUFFERLEN 256 - char buffer[BUFFERLEN]; - int inverse = 1; - int jacobian = 0; - EmcPose pos = {0.0, 0.0, 0.0, 0.0, 0.0, 0.0}; - EmcPose vel = {0.0, 0.0, 0.0, 0.0, 0.0, 0.0}; - double joints[6] = {0.0}; - double jointvels[6] = {0.0}; - KINEMATICS_INVERSE_FLAGS iflags = 0; - KINEMATICS_FORWARD_FLAGS fflags = 0; - int t; - int retval; -#define ITERATIONS 100000 - double start, end; - - /* syntax is a.out {i|f # # # # # #} */ - if (argc == 8) { - if (argv[1][0] == 'f') { - /* joints passed, so do interations on forward kins for timing */ - for (t = 0; t < 6; t++) { - if (1 != sscanf(argv[t + 2], "%lf", &joints[t])) { - fprintf(stderr, "bad value: %s\n", argv[t + 2]); - return 1; - } - } - inverse = 0; - } - else if (argv[1][0] == 'i') { - /* world coords passed, so do iterations on inverse kins for timing */ - if (1 != sscanf(argv[2], "%lf", &pos.tran.x)) { - fprintf(stderr, "bad value: %s\n", argv[2]); - return 1; - } - if (1 != sscanf(argv[3], "%lf", &pos.tran.y)) { - fprintf(stderr, "bad value: %s\n", argv[3]); - return 1; - } - if (1 != sscanf(argv[4], "%lf", &pos.tran.z)) { - fprintf(stderr, "bad value: %s\n", argv[4]); - return 1; - } - if (1 != sscanf(argv[5], "%lf", &pos.a)) { - fprintf(stderr, "bad value: %s\n", argv[5]); - return 1; - } - if (1 != sscanf(argv[6], "%lf", &pos.b)) { - fprintf(stderr, "bad value: %s\n", argv[6]); - return 1; - } - if (1 != sscanf(argv[7], "%lf", &pos.c)) { - fprintf(stderr, "bad value: %s\n", argv[7]); - return 1; - } - inverse = 1; - } - else { - fprintf(stderr, "syntax: %s {i|f # # # # # #}\n", argv[0]); - return 1; - } - - /* need an initial estimate for the forward kins, so ask for it */ - if (inverse == 0) { - do { - printf("initial estimate for Cartesian position, xyzrpw: "); - fflush(stdout); - if (NULL == fgets(buffer, BUFFERLEN, stdin)) { - return 0; - } - } while (6 != sscanf(buffer, "%lf %lf %lf %lf %lf %lf", - &pos.tran.x, - &pos.tran.y, - &pos.tran.z, - &pos.a, - &pos.b, - &pos.c)); - } - - start = timestamp(); - for (t = 0; t < ITERATIONS; t++) { - if (inverse) { - retval = kinematicsInverse(&pos, joints, &iflags, &fflags); - if (0 != retval) { - printf("inv kins error %d\n", retval); - break; - } - } - else { - retval = kinematicsForward(joints, &pos, &fflags, &iflags); - if (0 != retval) { - printf("fwd kins error %d\n", retval); - break; - } - } - } - end = timestamp(); - - printf("calculation time: %f secs\n", - (end - start) / ((double) ITERATIONS)); - return 0; - } /* end of if args for timestamping */ - - /* else we're interactive */ - while (! feof(stdin)) { - if (inverse) { - if (jacobian) { - printf("jinv> "); - } - else { - printf("inv> "); - } - } - else { - if (jacobian) { - printf("jfwd> "); - } - else { - printf("fwd> "); - } - } - fflush(stdout); - - if (NULL == fgets(buffer, BUFFERLEN, stdin)) { - break; - } - - if (buffer[0] == 'i') { - inverse = 1; - continue; - } - else if (buffer[0] == 'f') { - inverse = 0; - continue; - } - else if (buffer[0] == 'j') { - jacobian = ! jacobian; - continue; - } - else if (buffer[0] == 'q') { - break; - } - - if (inverse) { - if (jacobian) { - if (12 != sscanf(buffer, "%lf %lf %lf %lf %lf %lf %lf %lf %lf %lf %lf %lf", - &pos.tran.x, - &pos.tran.y, - &pos.tran.z, - &pos.a, - &pos.b, - &pos.c, - &vel.tran.x, - &vel.tran.y, - &vel.tran.z, - &vel.a, - &vel.b, - &vel.c)) { - printf("?\n"); - } - else { - retval = jacobianInverse(&pos, &vel, joints, jointvels); - printf("%f %f %f %f %f %f\n", - jointvels[0], - jointvels[1], - jointvels[2], - jointvels[3], - jointvels[4], - jointvels[5]); - if (0 != retval) { - printf("inv Jacobian error %d\n", retval); - } - else { - retval = jacobianForward(joints, jointvels, &pos, &vel); - printf("%f %f %f %f %f %f\n", - vel.tran.x, - vel.tran.y, - vel.tran.z, - vel.a, - vel.b, - vel.c); - if (0 != retval) { - printf("fwd kins error %d\n", retval); - } - } - } - } - else { - if (6 != sscanf(buffer, "%lf %lf %lf %lf %lf %lf", - &pos.tran.x, - &pos.tran.y, - &pos.tran.z, - &pos.a, - &pos.b, - &pos.c)) { - printf("?\n"); - } - else { - retval = kinematicsInverse(&pos, joints, &iflags, &fflags); - printf("%f %f %f %f %f %f\n", - joints[0], - joints[1], - joints[2], - joints[3], - joints[4], - joints[5]); - if (0 != retval) { - printf("inv kins error %d\n", retval); - } - else { - retval = kinematicsForward(joints, &pos, &fflags, &iflags); - printf("%f %f %f %f %f %f\n", - pos.tran.x, - pos.tran.y, - pos.tran.z, - pos.a, - pos.b, - pos.c); - if (0 != retval) { - printf("fwd kins error %d\n", retval); - } - } - } - } - } - else { - if (jacobian) { - if (12 != sscanf(buffer, "%lf %lf %lf %lf %lf %lf %lf %lf %lf %lf %lf %lf", - &joints[0], - &joints[1], - &joints[2], - &joints[3], - &joints[4], - &joints[5], - &jointvels[0], - &jointvels[1], - &jointvels[2], - &jointvels[3], - &jointvels[4], - &jointvels[5])) { - printf("?\n"); - } - else { - retval = jacobianForward(joints, jointvels, &pos, &vel); - printf("%f %f %f %f %f %f\n", - vel.tran.x, - vel.tran.y, - vel.tran.z, - vel.a, - vel.b, - vel.c); - if (0 != retval) { - printf("fwd kins error %d\n", retval); - } - else { - retval = jacobianInverse(&pos, &vel, joints, jointvels); - printf("%f %f %f %f %f %f\n", - jointvels[0], - jointvels[1], - jointvels[2], - jointvels[3], - jointvels[4], - jointvels[5]); - if (0 != retval) { - printf("inv kins error %d\n", retval); - } - } - } - } - else { - if (6 != sscanf(buffer, "%lf %lf %lf %lf %lf %lf", - &joints[0], - &joints[1], - &joints[2], - &joints[3], - &joints[4], - &joints[5])) { - printf("?\n"); - } - else { - retval = kinematicsForward(joints, &pos, &fflags, &iflags); - printf("%f %f %f %f %f %f\n", - pos.tran.x, - pos.tran.y, - pos.tran.z, - pos.a, - pos.b, - pos.c); - if (0 != retval) { - printf("fwd kins error %d\n", retval); - } - else { - retval = kinematicsInverse(&pos, joints, &iflags, &fflags); - printf("%f %f %f %f %f %f\n", - joints[0], - joints[1], - joints[2], - joints[3], - joints[4], - joints[5]); - if (0 != retval) { - printf("inv kins error %d\n", retval); - } - } - } - } - } - } - - return 0; - -#undef ITERATIONS -#undef BUFFERLEN -} - -#endif /* MAIN */ - -#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); @@ -896,17 +519,130 @@ EXPORT_SYMBOL(kinematicsInverse); MODULE_LICENSE("GPL"); +int comp_id; +int rtapi_app_main(void) +{ + int res = 0, i; -int comp_id; -int rtapi_app_main(void) { comp_id = hal_init("genhexkins"); - if(comp_id > 0) { - hal_ready(comp_id); - return 0; - } - return comp_id; + if (comp_id < 0) + return comp_id; + + haldata = hal_malloc(sizeof(struct haldata)); + if (!haldata) + goto error; + + for (i = 0; i < 6; i++) { + if ((res = + hal_pin_float_newf(HAL_IO, &haldata->basex[i], comp_id, + "genhexkins.base.%d.x", i)) < 0) + goto error; + *haldata->basex[i] = 0; + if ((res = + hal_pin_float_newf(HAL_IO, &haldata->basey[i], comp_id, + "genhexkins.base.%d.y", i)) < 0) + goto error; + *haldata->basey[i] = 0; + if ((res = + hal_pin_float_newf(HAL_IO, &haldata->basez[i], comp_id, + "genhexkins.base.%d.z", i)) < 0) + goto error; + *haldata->basez[i] = 0; + if ((res = + hal_pin_float_newf(HAL_IO, &haldata->platformx[i], comp_id, + "genhexkins.platform.%d.x", i)) < 0) + goto error; + *haldata->platformx[i] = 0; + if ((res = + hal_pin_float_newf(HAL_IO, &haldata->platformy[i], comp_id, + "genhexkins.platform.%d.y", i)) < 0) + goto error; + *haldata->platformy[i] = 0; + if ((res = + hal_pin_float_newf(HAL_IO, &haldata->platformz[i], comp_id, + "genhexkins.platform.%d.z", i)) < 0) + goto error; + *haldata->platformz[i] = 0; + } + if ((res = + hal_pin_u32_newf(HAL_OUT, &haldata->iter, comp_id, + "genhexkins.last-iterations")) < 0) + goto error; + *haldata->iter = 0; + + if ((res = + hal_pin_u32_newf(HAL_OUT, &haldata->max_iter, comp_id, + "genhexkins.max-iterations")) < 0) + goto error; + *haldata->max_iter = 0; + + if ((res = + hal_pin_float_newf(HAL_IO, &haldata->max_error, comp_id, + "genhexkins.max-error")) < 0) + goto error; + *haldata->max_error = 100; + + if ((res = + hal_pin_float_newf(HAL_IO, &haldata->conv_criterion, comp_id, + "genhexkins.convergence-criterion")) < 0) + goto error; + *haldata->conv_criterion = 1e-9; + + if ((res = + hal_pin_u32_newf(HAL_IO, &haldata->iter_limit, comp_id, + "genhexkins.limit-iterations")) < 0) + goto error; + *haldata->iter_limit = 120; + + *haldata->basex[0] = DEFAULT_BASE_0_X; + *haldata->basey[0] = DEFAULT_BASE_0_Y; + *haldata->basez[0] = DEFAULT_BASE_0_Z; + *haldata->basex[1] = DEFAULT_BASE_1_X; + *haldata->basey[1] = DEFAULT_BASE_1_Y; + *haldata->basez[1] = DEFAULT_BASE_1_Z; + *haldata->basex[2] = DEFAULT_BASE_2_X; + *haldata->basey[2] = DEFAULT_BASE_2_Y; + *haldata->basez[2] = DEFAULT_BASE_2_Z; + *haldata->basex[3] = DEFAULT_BASE_3_X; + *haldata->basey[3] = DEFAULT_BASE_3_Y; + *haldata->basez[3] = DEFAULT_BASE_3_Z; + *haldata->basex[4] = DEFAULT_BASE_4_X; + *haldata->basey[4] = DEFAULT_BASE_4_Y; + *haldata->basez[4] = DEFAULT_BASE_4_Z; + *haldata->basex[5] = DEFAULT_BASE_5_X; + *haldata->basey[5] = DEFAULT_BASE_5_Y; + *haldata->basez[5] = DEFAULT_BASE_5_Z; + + *haldata->platformx[0] = DEFAULT_PLATFORM_0_X; + *haldata->platformy[0] = DEFAULT_PLATFORM_0_Y; + *haldata->platformz[0] = DEFAULT_PLATFORM_0_Z; + *haldata->platformx[1] = DEFAULT_PLATFORM_1_X; + *haldata->platformy[1] = DEFAULT_PLATFORM_1_Y; + *haldata->platformz[1] = DEFAULT_PLATFORM_1_Z; + *haldata->platformx[2] = DEFAULT_PLATFORM_2_X; + *haldata->platformy[2] = DEFAULT_PLATFORM_2_Y; + *haldata->platformz[2] = DEFAULT_PLATFORM_2_Z; + *haldata->platformx[3] = DEFAULT_PLATFORM_3_X; + *haldata->platformy[3] = DEFAULT_PLATFORM_3_Y; + *haldata->platformz[3] = DEFAULT_PLATFORM_3_Z; + *haldata->platformx[4] = DEFAULT_PLATFORM_4_X; + *haldata->platformy[4] = DEFAULT_PLATFORM_4_Y; + *haldata->platformz[4] = DEFAULT_PLATFORM_4_Z; + *haldata->platformx[5] = DEFAULT_PLATFORM_5_X; + *haldata->platformy[5] = DEFAULT_PLATFORM_5_Y; + *haldata->platformz[5] = DEFAULT_PLATFORM_5_Z; + + hal_ready(comp_id); + return 0; + + error: + hal_exit(comp_id); + return res; } -void rtapi_app_exit(void) { hal_exit(comp_id); } -#endif + +void rtapi_app_exit(void) +{ + hal_exit(comp_id); +} \ No newline at end of file diff --git a/src/emc/kinematics/genhexkins.h b/src/emc/kinematics/genhexkins.h index dce1a59..e49bd6e 100644 --- a/src/emc/kinematics/genhexkins.h +++ b/src/emc/kinematics/genhexkins.h @@ -1,322 +1,67 @@ -/******************************************************************** +/******************************************************************* * Description: genhexkins.h +* * Kinematics for a generalised hexapod machine * * Derived from a work by R. Brian Register * -* Author: +* Adapting Author: Andrew Kyrychenko * License: GPL Version 2 * System: Linux -* -* Copyright (c) 2004 All rights reserved. * -* Last change: -******************************************************************* - - This is the header file to accompany genhexkins.c. This header file is used - to configure genhexkins.c to solve the kinematics for a particular Stewart - Platform configuration. +* Last change: 2014.11.27 +******************************************************************** - Defined are the parameters necessary to configure the functions to solve - several different Stewart Platform configurations. To choose a particular - configuration #define the configuration you are interested in and - comment out any others. + This is the header file to accompany genhexkins.c. This header file + is used to configure genhexkins.c to solve the kinematics for a default + Stewart Platform configuration. */ -#include "posemath.h" /* PmCartesian */ - -/* genhexSetParams lets you set the Cartesian coords of the base and platform, - overriding the defaults set in the subsequent #defines */ -extern int genhexSetParams(const PmCartesian base[], const PmCartesian platform[]); -extern int genhexGetParams(PmCartesian base[], PmCartesian platform[]); -extern int genhexKinematicsForwardIterations(void); - -#define MINI_TETRA - -#define NUM_STRUTS 6 - -#ifdef INGERSOLL_HEXAPOD - -/* Define position of base strut ends in base (world) coordinates for */ -/* Ingersoll Hexapod */ -#define BASE_0_X 1.7580 -#define BASE_1_X 1.6021 -#define BASE_2_X -1.7580 -#define BASE_3_X -1.6021 -#define BASE_4_X 0.0 -#define BASE_5_X 0.0 - -#define BASE_0_Y 2.8 -#define BASE_1_Y 3.07 -#define BASE_2_Y 2.8 -#define BASE_3_Y 3.07 -#define BASE_4_Y 2.8 -#define BASE_5_Y 3.07 - -#define BASE_0_Z -1.015 -#define BASE_1_Z -0.925 -#define BASE_2_Z -1.015 -#define BASE_3_Z -0.925 -#define BASE_4_Z 2.03 -#define BASE_5_Z 1.85 - -/* Define position of platform strut end in platform coordinate system */ -/* for Ingersoll Hexapod */ -#define PLATFORM_0_X 0.225 -#define PLATFORM_1_X 0.1125 -#define PLATFORM_2_X -0.1125 -#define PLATFORM_3_X -0.225 -#define PLATFORM_4_X -0.1125 -#define PLATFORM_5_X 0.1125 - -#define PLATFORM_0_Y 0.0 -#define PLATFORM_1_Y 0.1949 -#define PLATFORM_2_Y 0.1949 -#define PLATFORM_3_Y 0.0 -#define PLATFORM_4_Y -0.1949 -#define PLATFORM_5_Y -0.1949 - -#define PLATFORM_0_Z -0.228 -#define PLATFORM_1_Z -0.228 -#define PLATFORM_2_Z -0.228 -#define PLATFORM_3_Z -0.228 -#define PLATFORM_4_Z -0.228 -#define PLATFORM_5_Z -0.228 - -#endif - -#ifdef UF_HEXAPOD - -/* Define position of base strut ends in base (world) coordinates for */ -/* UF Hexapod */ -#define BASE_0_X -16.00 -#define BASE_1_X -10.00 -#define BASE_2_X 16.00 -#define BASE_3_X 13.00 -#define BASE_4_X 0.00 -#define BASE_5_X -3.00 - -#define BASE_0_Y -9.24 -#define BASE_1_Y -9.24 -#define BASE_2_Y -9.24 -#define BASE_3_Y -4.04 -#define BASE_4_Y 18.48 -#define BASE_5_Y 13.28 - -#define BASE_0_Z 18.00 -#define BASE_1_Z 18.00 -#define BASE_2_Z 18.00 -#define BASE_3_Z 18.00 -#define BASE_4_Z 18.00 -#define BASE_5_Z 18.00 - -/* Define position of platform strut end in platform coordinate system */ -/* for UF Hexapod */ -#define PLATFORM_0_X -9.00 -#define PLATFORM_1_X 0.00 -#define PLATFORM_2_X 3.00 -#define PLATFORM_3_X 12.00 -#define PLATFORM_4_X 6.00 -#define PLATFORM_5_X -12.00 - -#define PLATFORM_0_Y 1.73 -#define PLATFORM_1_Y -13.86 -#define PLATFORM_2_Y -8.66 -#define PLATFORM_3_Y 6.93 -#define PLATFORM_4_Y 6.93 -#define PLATFORM_5_Y 6.93 - -#define PLATFORM_0_Z 0.00 -#define PLATFORM_1_Z 0.00 -#define PLATFORM_2_Z 0.00 -#define PLATFORM_3_Z 0.00 -#define PLATFORM_4_Z 0.00 -#define PLATFORM_5_Z 0.00 - -#endif - -#ifdef MINI_TETRA - -/* Define position of base strut ends in base (world) coordinates for */ -/* mini tetra */ -#define BASE_0_X -22.950 -#define BASE_0_Y 13.250 - -#define BASE_1_X 22.950 -#define BASE_1_Y 13.250 - -#define BASE_2_X 22.950 -#define BASE_2_Y 13.250 - -#define BASE_3_X 0 -#define BASE_3_Y -26.5 - -#define BASE_4_X 0 -#define BASE_4_Y -26.5 - -#define BASE_5_X -22.950 -#define BASE_5_Y 13.250 - -#define BASE_0_Z 0 -#define BASE_1_Z 0 -#define BASE_2_Z 0 -#define BASE_3_Z 0 -#define BASE_4_Z 0 -#define BASE_5_Z 0 - -/* Define position of platform strut end in platform coordinate system */ -/* for mini tetra */ -#define PLATFORM_0_X -1 -#define PLATFORM_0_Y 11.5 - -#define PLATFORM_1_X 1 -#define PLATFORM_1_Y 11.5 - -#define PLATFORM_2_X 10.459 -#define PLATFORM_2_Y -4.884 - -#define PLATFORM_3_X 9.459 -#define PLATFORM_3_Y -6.616 - -#define PLATFORM_4_X -9.459 -#define PLATFORM_4_Y -6.616 - -#define PLATFORM_5_X -10.459 -#define PLATFORM_5_Y -4.884 - -#define PLATFORM_0_Z 0 -#define PLATFORM_1_Z 0 -#define PLATFORM_2_Z 0 -#define PLATFORM_3_Z 0 -#define PLATFORM_4_Z 0 -#define PLATFORM_5_Z 0 - -#endif - -#ifdef OCTA_TETRA - -/* Define position of base strut ends in base (world) coordinates for - minitetra with octahedral outside frame. Let A be side length of - base triangle. Here A = 39 inches. */ -#define BASE_0_X -19.5 /* -A/2 */ -#define BASE_0_Y 11.258 /* A cos 30 * 1/3 */ - -#define BASE_1_X 19.5 /* A/2 */ -#define BASE_1_Y 11.258 /* A cos 30 * 1/3 */ - -#define BASE_2_X 19.5 /* same as BASE_1_X */ -#define BASE_2_Y 11.258 /* same as BASE_1_Y */ - -#define BASE_3_X 0 /* 0 */ -#define BASE_3_Y -22.517 /* -A cos 30 * 2/3 */ - -#define BASE_4_X 0 /* same as BASE_3_X */ -#define BASE_4_Y -22.517 /* same as BASE_3_Y */ - -#define BASE_5_X -19.5 /* same as BASE_0_X */ -#define BASE_5_Y 11.258 /* same as BASE_0_Y */ - -#define BASE_0_Z 0 /* all 0 */ -#define BASE_1_Z 0 -#define BASE_2_Z 0 -#define BASE_3_Z 0 -#define BASE_4_Z 0 -#define BASE_5_Z 0 - -/* Define position of platform strut end in platform coordinate system - for minitetra with octahedral outside frame */ -#define PLATFORM_0_X -1 -#define PLATFORM_0_Y 11.5 - -#define PLATFORM_1_X 1 -#define PLATFORM_1_Y 11.5 - -#define PLATFORM_2_X 10.459 -#define PLATFORM_2_Y -4.884 - -#define PLATFORM_3_X 9.459 -#define PLATFORM_3_Y -6.616 - -#define PLATFORM_4_X -9.459 -#define PLATFORM_4_Y -6.616 - -#define PLATFORM_5_X -10.459 -#define PLATFORM_5_Y -4.884 - -#define PLATFORM_0_Z 0 -#define PLATFORM_1_Z 0 -#define PLATFORM_2_Z 0 -#define PLATFORM_3_Z 0 -#define PLATFORM_4_Z 0 -#define PLATFORM_5_Z 0 - -#endif - -#ifdef GEN_TETRA - -/* - GEN_TETRA uses 4 parameters for the side and corner lengths of the - base and platform. Set these and recompile, and you don't have to - figure out all the vertex coordinates yourself. -*/ - -#define BASE_LENGTH 40.0 -#define BASE_CORNER 1.75 -#define PLATFORM_LENGTH 18.9 -#define PLATFORM_CORNER 0.5 -#define Q 0.2886751346 /* 1/3 cos 30 */ - -#define BASE_0_X (0.5 * BASE_CORNER) -#define BASE_0_Y (Q * (2.0 * BASE_LENGTH + BASE_CORNER)) - -#define BASE_1_X (0.5 * (BASE_LENGTH + BASE_CORNER)) -#define BASE_1_Y (Q * (BASE_CORNER - BASE_LENGTH)) - -#define BASE_2_X (0.5 * BASE_LENGTH) -#define BASE_2_Y (-Q * (BASE_LENGTH + 2.0 * BASE_CORNER)) - -#define BASE_3_X -BASE_2_X -#define BASE_3_Y BASE_2_Y - -#define BASE_4_X -BASE_1_X -#define BASE_4_Y BASE_1_Y - -#define BASE_5_X -BASE_0_X -#define BASE_5_Y BASE_0_Y - -#define BASE_0_Z 0 /* all 0 */ -#define BASE_1_Z 0 -#define BASE_2_Z 0 -#define BASE_3_Z 0 -#define BASE_4_Z 0 -#define BASE_5_Z 0 - -/* Define position of platform strut end in platform coordinate system - for minitetra with octahedral outside frame */ -#define PLATFORM_0_X (0.5 * PLATFORM_LENGTH) -#define PLATFORM_0_Y (Q * (PLATFORM_LENGTH + 2.0 * PLATFORM_CORNER)) - -#define PLATFORM_1_X (0.5 * (PLATFORM_LENGTH + PLATFORM_CORNER)) -#define PLATFORM_1_Y (Q * (PLATFORM_LENGTH - PLATFORM_CORNER)) - -#define PLATFORM_2_X (0.5 * PLATFORM_CORNER) -#define PLATFORM_2_Y (-Q * (2.0 * PLATFORM_LENGTH + PLATFORM_CORNER)) - -#define PLATFORM_3_X -PLATFORM_2_X -#define PLATFORM_3_Y PLATFORM_2_Y - -#define PLATFORM_4_X -PLATFORM_1_X -#define PLATFORM_4_Y PLATFORM_1_Y - -#define PLATFORM_5_X -PLATFORM_0_X -#define PLATFORM_5_Y PLATFORM_0_Y - -#define PLATFORM_0_Z 0 -#define PLATFORM_1_Z 0 -#define PLATFORM_2_Z 0 -#define PLATFORM_3_Z 0 -#define PLATFORM_4_Z 0 -#define PLATFORM_5_Z 0 - -#endif +#define NUM_STRUTS 6 // number of struts, only 6 supported for now + +/* Default position of base strut ends in base (world) coordinates */ + +#define DEFAULT_BASE_0_X -22.950 +#define DEFAULT_BASE_1_X 22.950 +#define DEFAULT_BASE_2_X 22.950 +#define DEFAULT_BASE_3_X 0.000 +#define DEFAULT_BASE_4_X 0.000 +#define DEFAULT_BASE_5_X -22.950 + +#define DEFAULT_BASE_0_Y 13.250 +#define DEFAULT_BASE_1_Y 13.250 +#define DEFAULT_BASE_2_Y 13.250 +#define DEFAULT_BASE_3_Y -26.500 +#define DEFAULT_BASE_4_Y -26.500 +#define DEFAULT_BASE_5_Y 13.250 + +#define DEFAULT_BASE_0_Z 0.000 +#define DEFAULT_BASE_1_Z 0.000 +#define DEFAULT_BASE_2_Z 0.000 +#define DEFAULT_BASE_3_Z 0.000 +#define DEFAULT_BASE_4_Z 0.000 +#define DEFAULT_BASE_5_Z 0.000 + +/* Default position of platform strut end in platform coordinate system */ + +#define DEFAULT_PLATFORM_0_X -1.000 +#define DEFAULT_PLATFORM_1_X 1.000 +#define DEFAULT_PLATFORM_2_X 10.459 +#define DEFAULT_PLATFORM_3_X 9.459 +#define DEFAULT_PLATFORM_4_X -9.459 +#define DEFAULT_PLATFORM_5_X -10.459 + +#define DEFAULT_PLATFORM_0_Y 11.5 +#define DEFAULT_PLATFORM_1_Y 11.5 +#define DEFAULT_PLATFORM_2_Y -4.884 +#define DEFAULT_PLATFORM_3_Y -6.616 +#define DEFAULT_PLATFORM_4_Y -6.616 +#define DEFAULT_PLATFORM_5_Y -4.884 + +#define DEFAULT_PLATFORM_0_Z 0.000 +#define DEFAULT_PLATFORM_1_Z 0.000 +#define DEFAULT_PLATFORM_2_Z 0.000 +#define DEFAULT_PLATFORM_3_Z 0.000 +#define DEFAULT_PLATFORM_4_Z 0.000 +#define DEFAULT_PLATFORM_5_Z 0.000 \ No newline at end of file -- 1.7.10.4