src/modules/robot/arm_solutions/RotaryDeltaSolution.cpp

   1 #include "RotaryDeltaSolution.h"
   2 #include "ActuatorCoordinates.h"
   3 #include "checksumm.h"
   4 #include "ConfigValue.h"
   5 #include "ConfigCache.h"
   6 #include "libs/Kernel.h"
   7 #include "libs/nuts_bolts.h"
   8 #include "libs/Config.h"
   9 #include "libs/utils.h"
  10 #include "StreamOutputPool.h"
  11 #include <fastmath.h>
  12
  13 #define delta_e_checksum                CHECKSUM("delta_e")
  14 #define delta_f_checksum                CHECKSUM("delta_f")
  15 #define delta_re_checksum               CHECKSUM("delta_re")
  16 #define delta_rf_checksum               CHECKSUM("delta_rf")
  17 #define delta_z_offset_checksum         CHECKSUM("delta_z_offset")
  18
  19 #define delta_ee_offs_checksum          CHECKSUM("delta_ee_offs")
  20 #define tool_offset_checksum            CHECKSUM("delta_tool_offset")
  21
  22 #define delta_mirror_xy_checksum        CHECKSUM("delta_mirror_xy")
  23
  24 const static float pi     = 3.14159265358979323846;    // PI
  25 const static float two_pi = 2 * pi;
  26 const static float sin120 = 0.86602540378443864676372317075294; //sqrt3/2.0
  27 const static float cos120 = -0.5;
  28 const static float tan60  = 1.7320508075688772935274463415059; //sqrt3;
  29 const static float sin30  = 0.5;
  30 const static float tan30  = 0.57735026918962576450914878050196; //1/sqrt3
  31
  32 RotaryDeltaSolution::RotaryDeltaSolution(Config *config)
  33 {
  34     // End effector length
  35     delta_e = config->value(delta_e_checksum)->by_default(131.636F)->as_number();
  36
  37     // Base length
  38     delta_f = config->value(delta_f_checksum)->by_default(190.526F)->as_number();
  39
  40     // Carbon rod length
  41     delta_re = config->value(delta_re_checksum)->by_default(270.000F)->as_number();
  42
  43     // Servo horn length
  44     delta_rf = config->value(delta_rf_checksum)->by_default(90.000F)->as_number();
  45
  46     // Distance from delta 8mm rod/pulley to table/bed,
  47     // NOTE: For OpenPnP, set the zero to be about 25mm above the bed..
  48     delta_z_offset = config->value(delta_z_offset_checksum)->by_default(290.700F)->as_number();
  49
  50     // Ball joint plane to bottom of end effector surface
  51     delta_ee_offs = config->value(delta_ee_offs_checksum)->by_default(15.000F)->as_number();
  52
  53     // Distance between end effector ball joint plane and tip of tool (PnP)
  54     tool_offset = config->value(tool_offset_checksum)->by_default(30.500F)->as_number();
  55
  56     // mirror the XY axis
  57     mirror_xy= config->value(delta_mirror_xy_checksum)->by_default(true)->as_bool();
  58
  59     debug_flag= false;
  60     init();
  61 }
  62
  63 // inverse kinematics
  64 // helper functions, calculates angle theta1 (for YZ-pane)
  65 int RotaryDeltaSolution::delta_calcAngleYZ(float x0, float y0, float z0, float &theta)
  66 {
  67     float y1 = -0.5F * tan30 * delta_f; // f/2 * tan 30
  68     y0      -=  0.5F * tan30 * delta_e; // shift center to edge
  69     // z = a + b*y
  70     float a = (x0 * x0 + y0 * y0 + z0 * z0 + delta_rf * delta_rf - delta_re * delta_re - y1 * y1) / (2.0F * z0);
  71     float b = (y1 - y0) / z0;
  72
  73     float d = -(a + b * y1) * (a + b * y1) + delta_rf * (b * b * delta_rf + delta_rf); // discriminant
  74     if (d < 0.0F) return -1;                                            // non-existing point
  75
  76     float yj = (y1 - a * b - sqrtf(d)) / (b * b + 1.0F);               // choosing outer point
  77     float zj = a + b * yj;
  78
  79     theta = 180.0F * atanf(-zj / (y1 - yj)) / pi + ((yj > y1) ? 180.0F : 0.0F);
  80     return 0;
  81 }
  82
  83 // forward kinematics: (theta1, theta2, theta3) -> (x0, y0, z0)
  84 // returned status: 0=OK, -1=non-existing position
  85 int RotaryDeltaSolution::delta_calcForward(float theta1, float theta2, float theta3, float &x0, float &y0, float &z0)
  86 {
  87     float t = (delta_f - delta_e) * tan30 / 2.0F;
  88     float degrees_to_radians = pi / 180.0F;
  89
  90     theta1 *= degrees_to_radians;
  91     theta2 *= degrees_to_radians;
  92     theta3 *= degrees_to_radians;
  93
  94     float y1 = -(t + delta_rf * cosf(theta1));
  95     float z1 = -delta_rf * sinf(theta1);
  96
  97     float y2 = (t + delta_rf * cosf(theta2)) * sin30;
  98     float x2 = y2 * tan60;
  99     float z2 = -delta_rf * sinf(theta2);
 100
 101     float y3 = (t + delta_rf * cosf(theta3)) * sin30;
 102     float x3 = -y3 * tan60;
 103     float z3 = -delta_rf * sinf(theta3);
 104
 105     float dnm = (y2 - y1) * x3 - (y3 - y1) * x2;
 106
 107     float w1 = y1 * y1 + z1 * z1;
 108     float w2 = x2 * x2 + y2 * y2 + z2 * z2;
 109     float w3 = x3 * x3 + y3 * y3 + z3 * z3;
 110
 111     // x = (a1*z + b1)/dnm
 112     float a1 = (z2 - z1) * (y3 - y1) - (z3 - z1) * (y2 - y1);
 113     float b1 = -((w2 - w1) * (y3 - y1) - (w3 - w1) * (y2 - y1)) / 2.0F;
 114
 115     // y = (a2*z + b2)/dnm;
 116     float a2 = -(z2 - z1) * x3 + (z3 - z1) * x2;
 117     float b2 = ((w2 - w1) * x3 - (w3 - w1) * x2) / 2.0F;
 118
 119     // a*z^2 + b*z + c = 0
 120     float a = a1 * a1 + a2 * a2 + dnm * dnm;
 121     float b = 2.0F * (a1 * b1 + a2 * (b2 - y1 * dnm) - z1 * dnm * dnm);
 122     float c = (b2 - y1 * dnm) * (b2 - y1 * dnm) + b1 * b1 + dnm * dnm * (z1 * z1 - delta_re * delta_re);
 123
 124     // discriminant
 125     float d = b * b - (float)4.0F * a * c;
 126     if (d < 0.0F) return -1; // non-existing point
 127
 128     z0 = -(float)0.5F * (b + sqrtf(d)) / a;
 129     x0 = (a1 * z0 + b1) / dnm;
 130     y0 = (a2 * z0 + b2) / dnm;
 131
 132     z0 -= z_calc_offset;
 133     return 0;
 134 }
 135
 136
 137 void RotaryDeltaSolution::init()
 138 {
 139     //these are calculated here and not in the config() as these variables can be fine tuned by the user.
 140     z_calc_offset  = -(delta_z_offset - tool_offset - delta_ee_offs);
 141 }
 142
 143 void RotaryDeltaSolution::cartesian_to_actuator(const float cartesian_mm[], ActuatorCoordinates &actuator_mm )
 144 {
 145     //We need to translate the Cartesian coordinates in mm to the actuator position required in mm so the stepper motor  functions
 146     float alpha_theta = 0.0F;
 147     float beta_theta  = 0.0F;
 148     float gamma_theta = 0.0F;
 149
 150     //Code from Trossen Robotics tutorial, has X in front Y to the right and Z to the left
 151     // firepick is X at the back and negates X0 X0
 152     // selected by a config option
 153     float x0 = cartesian_mm[X_AXIS];
 154     float y0 = cartesian_mm[Y_AXIS];
 155     if(mirror_xy) {
 156         x0= -x0;
 157         y0= -y0;
 158     }
 159
 160     float z_with_offset = cartesian_mm[Z_AXIS] + z_calc_offset; //The delta calculation below places zero at the top.  Subtract the Z offset to make zero at the bottom.
 161
 162     int status =              delta_calcAngleYZ(x0,                    y0,                  z_with_offset, alpha_theta);
 163     if (status == 0) status = delta_calcAngleYZ(x0 * cos120 + y0 * sin120, y0 * cos120 - x0 * sin120, z_with_offset, beta_theta); // rotate co-ordinates to +120 deg
 164     if (status == 0) status = delta_calcAngleYZ(x0 * cos120 - y0 * sin120, y0 * cos120 + x0 * sin120, z_with_offset, gamma_theta); // rotate co-ordinates to -120 deg
 165
 166     if (status == -1) { //something went wrong,
 167         //force to actuator FPD home position as we know this is a valid position
 168         actuator_mm[ALPHA_STEPPER] = 0;
 169         actuator_mm[BETA_STEPPER ] = 0;
 170         actuator_mm[GAMMA_STEPPER] = 0;
 171
 172         //DEBUG CODE, uncomment the following to help determine what may be happening if you are trying to adapt this to your own different rotary delta.
 173         if(debug_flag) {
 174             THEKERNEL->streams->printf("//ERROR: Delta calculation fail!  Unable to move to:\n");
 175             THEKERNEL->streams->printf("//    x= %f\n", cartesian_mm[X_AXIS]);
 176             THEKERNEL->streams->printf("//    y= %f\n", cartesian_mm[Y_AXIS]);
 177             THEKERNEL->streams->printf("//    z= %f\n", cartesian_mm[Z_AXIS]);
 178             THEKERNEL->streams->printf("// CalcZ= %f\n", z_calc_offset);
 179             THEKERNEL->streams->printf("// Offz= %f\n", z_with_offset);
 180         }
 181     } else {
 182         actuator_mm[ALPHA_STEPPER] = alpha_theta;
 183         actuator_mm[BETA_STEPPER ] = beta_theta;
 184         actuator_mm[GAMMA_STEPPER] = gamma_theta;
 185
 186         if(debug_flag) {
 187             THEKERNEL->streams->printf("//cartesian x= %f\n\r", cartesian_mm[X_AXIS]);
 188             THEKERNEL->streams->printf("// y= %f\n\r", cartesian_mm[Y_AXIS]);
 189             THEKERNEL->streams->printf("// z= %f\n\r", cartesian_mm[Z_AXIS]);
 190             THEKERNEL->streams->printf("// Offz= %f\n\r", z_with_offset);
 191             THEKERNEL->streams->printf("// actuator x= %f\n\r", actuator_mm[X_AXIS]);
 192             THEKERNEL->streams->printf("// y= %f\n\r", actuator_mm[Y_AXIS]);
 193             THEKERNEL->streams->printf("// z= %f\n\r", actuator_mm[Z_AXIS]);
 194         }
 195     }
 196
 197 }
 198
 199 void RotaryDeltaSolution::actuator_to_cartesian(const ActuatorCoordinates &actuator_mm, float cartesian_mm[] )
 200 {
 201     float x, y, z;
 202     //Use forward kinematics
 203     delta_calcForward(actuator_mm[ALPHA_STEPPER], actuator_mm[BETA_STEPPER ], actuator_mm[GAMMA_STEPPER], x, y, z);
 204     if(mirror_xy) {
 205         cartesian_mm[X_AXIS]= -x;
 206         cartesian_mm[Y_AXIS]= -y;
 207         cartesian_mm[Z_AXIS]= z;
 208     }else{
 209         cartesian_mm[X_AXIS]= x;
 210         cartesian_mm[Y_AXIS]= y;
 211         cartesian_mm[Z_AXIS]= z;
 212     }
 213 }
 214
 215 bool RotaryDeltaSolution::set_optional(const arm_options_t &options)
 216 {
 217
 218     for(auto &i : options) {
 219         switch(i.first) {
 220             case 'A': delta_e           = i.second; break;
 221             case 'B': delta_f           = i.second; break;
 222             case 'C': delta_re          = i.second; break;
 223             case 'D': delta_rf          = i.second; break;
 224             case 'E': delta_z_offset    = i.second; break;
 225             case 'I': delta_ee_offs     = i.second; break;
 226             case 'H': tool_offset       = i.second; break;
 227             case 'W': debug_flag        = i.second != 0; break;
 228         }
 229     }
 230     init();
 231     return true;
 232 }
 233
 234 bool RotaryDeltaSolution::get_optional(arm_options_t &options, bool force_all)
 235 {
 236     options['A'] = this->delta_e;
 237     options['B'] = this->delta_f;
 238     options['C'] = this->delta_re;
 239     options['D'] = this->delta_rf;
 240     options['E'] = this->delta_z_offset;
 241     options['I'] = this->delta_ee_offs;
 242     options['H'] = this->tool_offset;
 243
 244     return true;
 245 };
 246