src/modules/robot/arm_solutions/JohannKosselSolution.cpp

   1 #include "JohannKosselSolution.h"
   2 #include <fastmath.h>
   3 #include "checksumm.h"
   4 #include "ConfigValue.h"
   5
   6 #include "Vector3.h"
   7
   8 #define SQ(x) powf(x, 2)
   9 #define ROUND(x, y) (roundf(x * 1e ## y) / 1e ## y)
  10
  11 JohannKosselSolution::JohannKosselSolution(Config* config)
  12 {
  13     // arm_length is the length of the arm from hinge to hinge
  14     arm_length         = config->value(arm_length_checksum)->by_default(250.0f)->as_number();
  15     // arm_radius is the horizontal distance from hinge to hinge when the effector is centered
  16     arm_radius         = config->value(arm_radius_checksum)->by_default(124.0f)->as_number();
  17
  18     init();
  19 }
  20
  21 void JohannKosselSolution::init() {
  22     arm_length_squared = SQ(arm_length);
  23
  24     // Effective X/Y positions of the three vertical towers.
  25     float DELTA_RADIUS = arm_radius;
  26
  27     float SIN_60   = 0.8660254037844386F;
  28     float COS_60   = 0.5F;
  29
  30     DELTA_TOWER1_X = -SIN_60 * DELTA_RADIUS; // front left tower
  31     DELTA_TOWER1_Y = -COS_60 * DELTA_RADIUS;
  32
  33     DELTA_TOWER2_X =  SIN_60 * DELTA_RADIUS; // front right tower
  34     DELTA_TOWER2_Y = -COS_60 * DELTA_RADIUS;
  35
  36     DELTA_TOWER3_X = 0.0F; // back middle tower
  37     DELTA_TOWER3_Y = DELTA_RADIUS;
  38 }
  39
  40 void JohannKosselSolution::cartesian_to_actuator( float cartesian_mm[], float actuator_mm[] )
  41 {
  42     actuator_mm[ALPHA_STEPPER] = sqrtf(this->arm_length_squared
  43                                 - SQ(DELTA_TOWER1_X - cartesian_mm[X_AXIS])
  44                                 - SQ(DELTA_TOWER1_Y - cartesian_mm[Y_AXIS])
  45                                 ) + cartesian_mm[Z_AXIS];
  46     actuator_mm[BETA_STEPPER ] = sqrtf(this->arm_length_squared
  47                                 - SQ(DELTA_TOWER2_X - cartesian_mm[X_AXIS])
  48                                 - SQ(DELTA_TOWER2_Y - cartesian_mm[Y_AXIS])
  49                                 ) + cartesian_mm[Z_AXIS];
  50     actuator_mm[GAMMA_STEPPER] = sqrtf(this->arm_length_squared
  51                                 - SQ(DELTA_TOWER3_X - cartesian_mm[X_AXIS])
  52                                 - SQ(DELTA_TOWER3_Y - cartesian_mm[Y_AXIS])
  53                                 ) + cartesian_mm[Z_AXIS];
  54 }
  55
  56 void JohannKosselSolution::actuator_to_cartesian( float actuator_mm[], float cartesian_mm[] )
  57 {
  58     // from http://en.wikipedia.org/wiki/Circumscribed_circle#Barycentric_coordinates_from_cross-_and_dot-products
  59     // based on https://github.com/ambrop72/aprinter/blob/2de69a/aprinter/printer/DeltaTransform.h#L81
  60     Vector3 tower1( DELTA_TOWER1_X, DELTA_TOWER1_Y, actuator_mm[0] );
  61     Vector3 tower2( DELTA_TOWER2_X, DELTA_TOWER2_Y, actuator_mm[1] );
  62     Vector3 tower3( DELTA_TOWER3_X, DELTA_TOWER3_Y, actuator_mm[2] );
  63
  64     Vector3 s12 = tower1.sub(tower2);
  65     Vector3 s23 = tower2.sub(tower3);
  66     Vector3 s13 = tower1.sub(tower3);
  67
  68     Vector3 normal = s12.cross(s23);
  69
  70     float magsq_s12 = s12.magsq();
  71     float magsq_s23 = s23.magsq();
  72     float magsq_s13 = s13.magsq();
  73
  74     float inv_nmag_sq = 1.0F / normal.magsq();
  75     float q = 0.5F * inv_nmag_sq;
  76
  77     float a = q * magsq_s23 * s12.dot(s13);
  78     float b = q * magsq_s13 * s12.dot(s23) * -1.0F; // negate because we use s12 instead of s21
  79     float c = q * magsq_s12 * s13.dot(s23);
  80
  81     Vector3 circumcenter( DELTA_TOWER1_X * a + DELTA_TOWER2_X * b + DELTA_TOWER3_X * c,
  82                           DELTA_TOWER1_Y * a + DELTA_TOWER2_Y * b + DELTA_TOWER3_Y * c,
  83                           actuator_mm[0] * a + actuator_mm[1] * b + actuator_mm[2] * c );
  84
  85     float r_sq = 0.5F * q * magsq_s12 * magsq_s23 * magsq_s13;
  86     float dist = sqrtf(inv_nmag_sq * (arm_length_squared - r_sq));
  87
  88     Vector3 cartesian = circumcenter.sub(normal.mul(dist));
  89
  90     cartesian_mm[0] = ROUND(cartesian[0], 4);
  91     cartesian_mm[1] = ROUND(cartesian[1], 4);
  92     cartesian_mm[2] = ROUND(cartesian[2], 4);
  93 }
  94
  95 bool JohannKosselSolution::set_optional(const arm_options_t& options) {
  96
  97     arm_options_t::const_iterator i;
  98
  99     i= options.find('L');
 100     if(i != options.end()) {
 101         arm_length= i->second;
 102
 103     }
 104     i= options.find('R');
 105     if(i != options.end()) {
 106         arm_radius= i->second;
 107     }
 108     init();
 109     return true;
 110 }
 111
 112 bool JohannKosselSolution::get_optional(arm_options_t& options) {
 113     options['L']= this->arm_length;
 114     options['R']= this->arm_radius;
 115     return true;
 116 };