From b716d8a3f6b8eedc0c086719df1aa0bc01029af7 Mon Sep 17 00:00:00 2001 From: Andrew Kyrychenko Date: Tue, 23 Dec 2014 23:12:30 +0200 Subject: [PATCH 1/2] genhexkins: add hal pins for joints coordinates Parameters added for base and platform joints coordinates. Pins added to control forward iterations. Cleaned from unused code. Manpage updated. Signed-off-by: Andrew Kyrychenko --- docs/man/man9/kins.9 | 34 +- src/emc/kinematics/genhexkins.c | 719 ++++++++++++--------------------------- src/emc/kinematics/genhexkins.h | 364 +++----------------- 3 files changed, 309 insertions(+), 808 deletions(-) diff --git a/docs/man/man9/kins.9 b/docs/man/man9/kins.9 index 1f933ca..aacd40b 100644 --- a/docs/man/man9/kins.9 +++ b/docs/man/man9/kins.9 @@ -4,7 +4,7 @@ .TP \\$1 .. -.TH KINS "9" "2007-01-20" "LinuxCNC Documentation" "HAL Component" +.TH KINS "9" "2014-12-22" "LinuxCNC Documentation" "HAL Component" .SH NAME kins \- kinematics definitions for LinuxCNC .SH SYNOPSIS @@ -43,7 +43,37 @@ locations (the motors), giving three degrees of freedom in position (XYZ) The location of the three motors is (0,0), (Bx,0), and (Cx,Cy) .SS genhexkins \- Hexapod Kinematics Gives six degrees of freedom in position and orientation (XYZABC). The -location of the motors is defined at compile time. +location of base and platform joints is defined by hal parameters. The +forward kinematics iteration is controlled by hal pins. +.TP +.B genhexkins.base.\fIN\fB.x +.TQ +.B genhexkins.base.\fIN\fB.y +.TQ +.B genhexkins.base.\fIN\fB.z +.TQ +.B genhexkins.platform.\fIN\fB.x +.TQ +.B genhexkins.platform.\fIN\fB.y +.TQ +.B genhexkins.platform.\fIN\fB.z +Parameters describing the \fIN\fRth joint's coordinates. +.TQ +.B genhexkins.convergence-criterion +Minimum error value that ends iterations with converged solution. +.TQ +.B genhexkins.limit-iterations +Limit of iterations, if exceeded iterations stop with no convergence. +.TQ +.B genhexkins.max-error +Maximum error value, if exceeded iterations stop with no convergence. +.TQ +.B genhexkins.last-iterations +Number of iterations spent for the last forward kinematics solution. +.TQ +.B genhexkins.max-iterations +Maximum number of iterations spent for a converged solution during current +session. .SS maxkins \- 5-axis kinematics example Kinematics for Chris Radek's tabletop 5 axis mill named 'max' with tilting head (B axis) and horizintal rotary mounted to the table (C axis). Provides diff --git a/src/emc/kinematics/genhexkins.c b/src/emc/kinematics/genhexkins.c index 5f9ed78..aea1604 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.12.22 +********************************************************************* 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 parameters: + + 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,12 +53,46 @@ 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. - -----------------------------------------------------------------------------*/ + + Hal pins to control and observe forward kinematics iterations: + + genhexkins.convergence-criterion - minimum error value that ends + iterations with converged solution; + + genhexkins.limit-iterations - limit of iterations, if exceeded + iterations stop with no convergence; + + genhexkins.max-error - maximum error value, if exceeded iterations + stop with no convergence; + + genhexkins.last-iterations - number of iterations spent for the + last forward kinematics solution; + + genhexkins.max-iterations - 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[NUM_STRUTS]; + hal_float_t basey[NUM_STRUTS]; + hal_float_t basez[NUM_STRUTS]; + hal_float_t platformx[NUM_STRUTS]; + hal_float_t platformy[NUM_STRUTS]; + hal_float_t platformz[NUM_STRUTS]; + hal_u32_t *last_iter; + hal_u32_t *max_iter; + hal_u32_t *iter_limit; + hal_float_t *max_error; + hal_float_t *conv_criterion; +} *haldata; + /******************************* MatInvert() ***************************/ @@ -135,7 +175,7 @@ static int MatInvert(double J[][NUM_STRUTS], double InvJ[][NUM_STRUTS]) } } - return 0; /* FIXME-- check divisors for 0 above */ + return 0; /* FIXME-- check divisors for 0 above */ } /******************************** MatMult() *********************************/ @@ -155,77 +195,31 @@ 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]; +/************************genhexkins_read_hal_pins**************************/ -int genhexSetParams(const PmCartesian base[], const PmCartesian platform[]) -{ - int t; +int genhexkins_read_hal_pins(void) { + int t; - 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]; - } - - return 0; + /* 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; } /**************************** jacobianInverse() ***************************/ static int JInvMat(const EmcPose * pos, - double InverseJacobian[][NUM_STRUTS]) + double InverseJacobian[][NUM_STRUTS]) { int i; PmRpy rpy; @@ -234,6 +228,8 @@ static int JInvMat(const EmcPose * pos, PmCartesian InvKinStrutVect,InvKinStrutVectUnit; PmCartesian RMatrix_a_cross_Strut; + genhexkins_read_hal_pins(); + /* 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; @@ -266,9 +262,9 @@ static int JInvMat(const EmcPose * pos, } int jacobianInverse(const EmcPose * pos, - const EmcPose * vel, - const double * joints, - double * jointvels) + const EmcPose * vel, + const double * joints, + double * jointvels) { double InverseJacobian[NUM_STRUTS][NUM_STRUTS]; double velmatrix[6]; @@ -278,12 +274,12 @@ int jacobianInverse(const EmcPose * pos, } /* Multiply Jinv[] by vel[] to get jointvels */ - velmatrix[0] = vel->tran.x; /* dx/dt */ - velmatrix[1] = vel->tran.y; /* dy/dt */ - velmatrix[2] = vel->tran.z; /* dz/dt */ - velmatrix[3] = vel->a; /* droll/dt */ - velmatrix[4] = vel->b; /* dpitch/dt */ - velmatrix[5] = vel->c; /* dyaw/dt */ + velmatrix[0] = vel->tran.x; /* dx/dt */ + velmatrix[1] = vel->tran.y; /* dy/dt */ + velmatrix[2] = vel->tran.z; /* dz/dt */ + velmatrix[3] = vel->a; /* droll/dt */ + velmatrix[4] = vel->b; /* dpitch/dt */ + velmatrix[5] = vel->c; /* dyaw/dt */ MatMult(InverseJacobian, velmatrix, jointvels); return 0; @@ -294,9 +290,9 @@ int jacobianInverse(const EmcPose * pos, /* FIXME-- could use a better implementation than computing the inverse and then inverting it */ int jacobianForward(const double * joints, - const double * jointvels, - const EmcPose * pos, - EmcPose * vel) + const double * jointvels, + const EmcPose * pos, + EmcPose * vel) { double InverseJacobian[NUM_STRUTS][NUM_STRUTS]; double Jacobian[NUM_STRUTS][NUM_STRUTS]; @@ -328,12 +324,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; @@ -349,18 +345,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_read_hal_pins(); /* abort on obvious problems, like joints <= 0 */ /* FIXME-- should check against triangle inequality, so that joints @@ -387,33 +374,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; } @@ -429,7 +400,7 @@ int kinematicsForward(const double * joints, pmCartCartAdd(&q_trans, &RMatrix_a, &aw); pmCartCartSub(&aw, &b[i], &InvKinStrutVect); if (0 != pmCartUnit(&InvKinStrutVect, &InvKinStrutVectUnit)) { - return -1; + return -1; } pmCartMag(&InvKinStrutVect, &InvKinStrutLength); StrutLengthDiff[i] = InvKinStrutLength - joints[i]; @@ -467,32 +438,33 @@ int kinematicsForward(const double * joints, } /* enter loop to determine if a strut needs another iteration */ - iterate = 0; /*assume iteration is done */ + iterate = 0; /*assume iteration is done */ for (i = 0; i < NUM_STRUTS; i++) { - if (fabs(StrutLengthDiff[i]) > conv_criterion) { - iterate = 1; + if (fabs(StrutLengthDiff[i]) > *haldata->conv_criterion) { + iterate = 1; } } } /* exit Newton-Raphson Iterative loop */ /* assign r,p,w to a,b,c */ - pos->a = q_RPY.r * 180.0 / PM_PI; - pos->b = q_RPY.p * 180.0 / PM_PI; + pos->a = q_RPY.r * 180.0 / PM_PI; + pos->b = q_RPY.p * 180.0 / PM_PI; pos->c = q_RPY.y * 180.0 / PM_PI; - + /* assign q_trans to pos */ pos->tran.x = q_trans.x; pos->tran.y = q_trans.y; pos->tran.z = q_trans.z; - return retval; -} + *haldata->last_iter = iteration; -int genhexKinematicsForwardIterations(void) -{ - return iteration; + if (iteration > *haldata->max_iter){ + *haldata->max_iter = iteration; + } + return 0; } + /************************ kinematicsInverse() ********************************/ int kinematicsInverse(const EmcPose * pos, @@ -500,11 +472,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_read_hal_pins(); + /* define Rotation Matrix */ rpy.r = pos->a * PM_PI / 180.0; rpy.p = pos->b * PM_PI / 180.0; @@ -526,369 +501,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" +#include "rtapi.h" /* RTAPI realtime OS API */ +#include "rtapi_app.h" /* RTAPI realtime module decls */ EXPORT_SYMBOL(kinematicsType); EXPORT_SYMBOL(kinematicsForward); @@ -896,17 +517,121 @@ 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; - } + 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_param_float_newf(HAL_RW, &(haldata->basex[i]), comp_id, + "genhexkins.base.%d.x", i)) < 0) + goto error; + + if ((res = hal_param_float_newf(HAL_RW, &haldata->basey[i], comp_id, + "genhexkins.base.%d.y", i)) < 0) + goto error; + + if ((res = hal_param_float_newf(HAL_RW, &haldata->basez[i], comp_id, + "genhexkins.base.%d.z", i)) < 0) + goto error; + + if ((res = hal_param_float_newf(HAL_RW, &haldata->platformx[i], comp_id, + "genhexkins.platform.%d.x", i)) < 0) + goto error; + + if ((res = hal_param_float_newf(HAL_RW, &haldata->platformy[i], comp_id, + "genhexkins.platform.%d.y", i)) < 0) + goto error; + + if ((res = hal_param_float_newf(HAL_RW, &haldata->platformz[i], comp_id, + "genhexkins.platform.%d.z", i)) < 0) + goto error; + } + + if ((res = hal_pin_u32_newf(HAL_OUT, &haldata->last_iter, comp_id, + "genhexkins.last-iterations")) < 0) + goto error; + *haldata->last_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); +} diff --git a/src/emc/kinematics/genhexkins.h b/src/emc/kinematics/genhexkins.h index dce1a59..78bcc6b 100644 --- a/src/emc/kinematics/genhexkins.h +++ b/src/emc/kinematics/genhexkins.h @@ -1,322 +1,68 @@ -/******************************************************************** +/******************************************************************* * 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.12.22 +******************************************************************** - 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 +#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.500 +#define DEFAULT_PLATFORM_1_Y 11.500 +#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 -#endif -- 1.7.10.4