Help for Linearangular Axis in Kinematics

   1/********************************************************************
   2 * Description: 5axiskins.c
   3 *   Trivial kinematics for 3 axis Cartesian machine
   4 *
   5 *   Derived from a work by Fred Proctor & Will Shackleford
   6 *
   7 * Author:
   8 * License: GPL Version 2
   9 * System: Linux
  10 *    
  11 * Copyright (c) 2007 Chris Radek
  12 *
  13 * Last change:
  14 ********************************************************************/
  15 
  16 #include "kinematics.h"         /* these decls */
  17 #include "posemath.h"
  18 #include "hal.h"
  19 #include "rtapi_math.h"
  20 
  21 #define d2r(d) ((d)*PM_PI/180.0)
  22 #define r2d(r) ((r)*180.0/PM_PI)
  23 
  24 struct haldata {
  25     hal_float_t *pivot_length;
  26 } *haldata;
  27 
  28 static PmCartesian s2r(double r, double t, double p) {
  29     PmCartesian c;
  30     t = d2r(t), p = d2r(p);
  31 
  32     c.x = r * sin(p) * cos(t);
  33     c.y = r * sin(p) * sin(t);
  34     c.z = r * cos(p);
  35 
  36     return c;
  37 }
  38 
  39 int kinematicsForward(const double *joints,
  40                       EmcPose * pos,
  41                       const KINEMATICS_FORWARD_FLAGS * fflags,
  42                       KINEMATICS_INVERSE_FLAGS * iflags)
  43 {
  44     PmCartesian r = s2r(*(haldata->pivot_length) + joints[8], joints[5], 180.0 - joints[4]);
  45 
  46     pos->tran.x = joints[0] + r.x;
  47     pos->tran.y = joints[1] + r.y;
  48     pos->tran.z = joints[2] + *(haldata->pivot_length) + r.z;
  49     pos->a = joints[3];
  50     pos->b = joints[4];
  51     pos->c = joints[5];
  52     pos->u = joints[6];
  53     pos->v = joints[7];
  54     pos->w = joints[8];
  55 
  56     return 0;
  57 }
  58 
  59 int kinematicsInverse(const EmcPose * pos,
  60                       double *joints,
  61                       const KINEMATICS_INVERSE_FLAGS * iflags,
  62                       KINEMATICS_FORWARD_FLAGS * fflags)
  63 {
  64 
  65     PmCartesian r = s2r(*(haldata->pivot_length) + pos->w, pos->c, 180.0 - pos->b);
  66 
  67     joints[0] = pos->tran.x - r.x;
  68     joints[1] = pos->tran.y - r.y;
  69     joints[2] = pos->tran.z - *(haldata->pivot_length) - r.z;
  70     joints[3] = pos->a;
  71     joints[4] = pos->b;
  72     joints[5] = pos->c;
  73     joints[6] = pos->u;
  74     joints[7] = pos->v;
  75     joints[8] = pos->w;
  76 
  77     return 0;
  78 }
  79 
  80 /* implemented for these kinematics as giving joints preference */
  81 int kinematicsHome(EmcPose * world,
  82                    double *joint,
  83                    KINEMATICS_FORWARD_FLAGS * fflags,
  84                    KINEMATICS_INVERSE_FLAGS * iflags)
  85 {
  86     *fflags = 0;
  87     *iflags = 0;
  88 
  89     return kinematicsForward(joint, world, fflags, iflags);
  90 }
  91 
  92 KINEMATICS_TYPE kinematicsType()
  93 {
  94     return KINEMATICS_BOTH;
  95 }
  96 
  97 #include "rtapi.h"              /* RTAPI realtime OS API */
  98 #include "rtapi_app.h"          /* RTAPI realtime module decls */
  99 #include "hal.h"
 100 
 101 EXPORT_SYMBOL(kinematicsType);
 102 EXPORT_SYMBOL(kinematicsForward);
 103 EXPORT_SYMBOL(kinematicsInverse);
 104 MODULE_LICENSE("GPL");
 105 
 106 int comp_id;
 107 int rtapi_app_main(void) {
 108     int result;
 109     comp_id = hal_init("5axiskins");
 110     if(comp_id < 0) return comp_id;
 111 
 112     haldata = hal_malloc(sizeof(struct haldata));
 113 
 114     result = hal_pin_float_new("5axiskins.pivot-length", HAL_IO, &(haldata->pivot_length), comp_id);
 115     if(result < 0) goto error;
 116 
 117     *(haldata->pivot_length) = 250.0;
 118 
 119     hal_ready(comp_id);
 120     return 0;
 121 
 122 error:
 123     hal_exit(comp_id);
 124     return result;
 125 }
 126 
 127 void rtapi_app_exit(void) { hal_exit(comp_id); }