Delta Robot example
Dependencies: BufferedSerial Eigen
Fork of TCPSocket_Example by
kinematics.cpp@4:778bc352c47f, 2018-10-07 (annotated)
- Committer:
- je310
- Date:
- Sun Oct 07 19:40:12 2018 +0000
- Revision:
- 4:778bc352c47f
- Child:
- 5:01e1e68309ae
can work smoothly, though voltage reading not working;
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
je310 | 4:778bc352c47f | 1 | |
je310 | 4:778bc352c47f | 2 | #include "kinematics.h" |
je310 | 4:778bc352c47f | 3 | //this implementation has been taken from http://forums.trossenrobotics.com/tutorials/introduction-129/delta-robot-kinematics-3276/ |
je310 | 4:778bc352c47f | 4 | // robot geometry |
je310 | 4:778bc352c47f | 5 | // (look at pics above for explanation) |
je310 | 4:778bc352c47f | 6 | extern BufferedSerial buffered_pc; |
je310 | 4:778bc352c47f | 7 | Kinematics::Kinematics(Axis* A_, Axis* B_, Axis* C_,calVals calibration_) |
je310 | 4:778bc352c47f | 8 | { |
je310 | 4:778bc352c47f | 9 | e = calibration_.e; |
je310 | 4:778bc352c47f | 10 | f = calibration_.f; |
je310 | 4:778bc352c47f | 11 | re = calibration_.re; |
je310 | 4:778bc352c47f | 12 | rf = calibration_.rf; |
je310 | 4:778bc352c47f | 13 | A = A_; |
je310 | 4:778bc352c47f | 14 | B = B_; |
je310 | 4:778bc352c47f | 15 | C = C_; |
je310 | 4:778bc352c47f | 16 | DeltaKinematics<float>::DeltaGeometricDim dim; |
je310 | 4:778bc352c47f | 17 | dim.sb = f; |
je310 | 4:778bc352c47f | 18 | dim.sp = e; |
je310 | 4:778bc352c47f | 19 | dim.L = rf; |
je310 | 4:778bc352c47f | 20 | dim.l = re; |
je310 | 4:778bc352c47f | 21 | dim.h = 60; |
je310 | 4:778bc352c47f | 22 | dim.max_neg_angle = -20; |
je310 | 4:778bc352c47f | 23 | dim.min_parallelogram_angle = 30; |
je310 | 4:778bc352c47f | 24 | |
je310 | 4:778bc352c47f | 25 | DK = new DeltaKinematics<float>(dim); |
je310 | 4:778bc352c47f | 26 | |
je310 | 4:778bc352c47f | 27 | } |
je310 | 4:778bc352c47f | 28 | |
je310 | 4:778bc352c47f | 29 | |
je310 | 4:778bc352c47f | 30 | // forward kinematics: (theta1, theta2, theta3) -> (x0, y0, z0) |
je310 | 4:778bc352c47f | 31 | // returned status: 0=OK, -1=non-existing position |
je310 | 4:778bc352c47f | 32 | int Kinematics::delta_calcForward(float theta1, float theta2, float theta3, float &x0, float &y0, float &z0) |
je310 | 4:778bc352c47f | 33 | { |
je310 | 4:778bc352c47f | 34 | float t = (f-e)*tan30/2; |
je310 | 4:778bc352c47f | 35 | float dtr = pi/(float)180.0; |
je310 | 4:778bc352c47f | 36 | |
je310 | 4:778bc352c47f | 37 | theta1 *= dtr; |
je310 | 4:778bc352c47f | 38 | theta2 *= dtr; |
je310 | 4:778bc352c47f | 39 | theta3 *= dtr; |
je310 | 4:778bc352c47f | 40 | |
je310 | 4:778bc352c47f | 41 | float y1 = -(t + rf*cos(theta1)); |
je310 | 4:778bc352c47f | 42 | float z1 = -rf*sin(theta1); |
je310 | 4:778bc352c47f | 43 | |
je310 | 4:778bc352c47f | 44 | float y2 = (t + rf*cos(theta2))*sin30; |
je310 | 4:778bc352c47f | 45 | float x2 = y2*tan60; |
je310 | 4:778bc352c47f | 46 | float z2 = -rf*sin(theta2); |
je310 | 4:778bc352c47f | 47 | |
je310 | 4:778bc352c47f | 48 | float y3 = (t + rf*cos(theta3))*sin30; |
je310 | 4:778bc352c47f | 49 | float x3 = -y3*tan60; |
je310 | 4:778bc352c47f | 50 | float z3 = -rf*sin(theta3); |
je310 | 4:778bc352c47f | 51 | |
je310 | 4:778bc352c47f | 52 | float dnm = (y2-y1)*x3-(y3-y1)*x2; |
je310 | 4:778bc352c47f | 53 | |
je310 | 4:778bc352c47f | 54 | float w1 = y1*y1 + z1*z1; |
je310 | 4:778bc352c47f | 55 | float w2 = x2*x2 + y2*y2 + z2*z2; |
je310 | 4:778bc352c47f | 56 | float w3 = x3*x3 + y3*y3 + z3*z3; |
je310 | 4:778bc352c47f | 57 | |
je310 | 4:778bc352c47f | 58 | // x = (a1*z + b1)/dnm |
je310 | 4:778bc352c47f | 59 | float a1 = (z2-z1)*(y3-y1)-(z3-z1)*(y2-y1); |
je310 | 4:778bc352c47f | 60 | float b1 = -((w2-w1)*(y3-y1)-(w3-w1)*(y2-y1))/2.0; |
je310 | 4:778bc352c47f | 61 | |
je310 | 4:778bc352c47f | 62 | // y = (a2*z + b2)/dnm; |
je310 | 4:778bc352c47f | 63 | float a2 = -(z2-z1)*x3+(z3-z1)*x2; |
je310 | 4:778bc352c47f | 64 | float b2 = ((w2-w1)*x3 - (w3-w1)*x2)/2.0; |
je310 | 4:778bc352c47f | 65 | |
je310 | 4:778bc352c47f | 66 | // a*z^2 + b*z + c = 0 |
je310 | 4:778bc352c47f | 67 | float a = a1*a1 + a2*a2 + dnm*dnm; |
je310 | 4:778bc352c47f | 68 | float b = 2*(a1*b1 + a2*(b2-y1*dnm) - z1*dnm*dnm); |
je310 | 4:778bc352c47f | 69 | float c = (b2-y1*dnm)*(b2-y1*dnm) + b1*b1 + dnm*dnm*(z1*z1 - re*re); |
je310 | 4:778bc352c47f | 70 | |
je310 | 4:778bc352c47f | 71 | // discriminant |
je310 | 4:778bc352c47f | 72 | float d = b*b - (float)4.0*a*c; |
je310 | 4:778bc352c47f | 73 | if (d < 0) return -1; // non-existing point |
je310 | 4:778bc352c47f | 74 | |
je310 | 4:778bc352c47f | 75 | z0 = -(float)0.5*(b+sqrt(d))/a; |
je310 | 4:778bc352c47f | 76 | x0 = (a1*z0 + b1)/dnm; |
je310 | 4:778bc352c47f | 77 | y0 = (a2*z0 + b2)/dnm; |
je310 | 4:778bc352c47f | 78 | return 0; |
je310 | 4:778bc352c47f | 79 | } |
je310 | 4:778bc352c47f | 80 | |
je310 | 4:778bc352c47f | 81 | // inverse kinematics |
je310 | 4:778bc352c47f | 82 | // helper functions, calculates angle theta1 (for YZ-pane) |
je310 | 4:778bc352c47f | 83 | int Kinematics::delta_calcAngleYZ(float x0, float y0, float z0, float &theta) |
je310 | 4:778bc352c47f | 84 | { |
je310 | 4:778bc352c47f | 85 | float y1 = -0.5 * 0.57735 * f; // f/2 * tg 30 |
je310 | 4:778bc352c47f | 86 | y0 -= 0.5 * 0.57735 * e; // shift center to edge |
je310 | 4:778bc352c47f | 87 | // z = a + b*y |
je310 | 4:778bc352c47f | 88 | float a = (x0*x0 + y0*y0 + z0*z0 +rf*rf - re*re - y1*y1)/(2*z0); |
je310 | 4:778bc352c47f | 89 | float b = (y1-y0)/z0; |
je310 | 4:778bc352c47f | 90 | // discriminant |
je310 | 4:778bc352c47f | 91 | float d = -(a+b*y1)*(a+b*y1)+rf*(b*b*rf+rf); |
je310 | 4:778bc352c47f | 92 | if (d < 0) return -1; // non-existing point |
je310 | 4:778bc352c47f | 93 | float yj = (y1 - a*b - sqrt(d))/(b*b + 1); // choosing outer point |
je310 | 4:778bc352c47f | 94 | float zj = a + b*yj; |
je310 | 4:778bc352c47f | 95 | theta = 180.0*atan(-zj/(y1 - yj))/pi + ((yj>y1)?180.0:0.0); |
je310 | 4:778bc352c47f | 96 | return 0; |
je310 | 4:778bc352c47f | 97 | } |
je310 | 4:778bc352c47f | 98 | |
je310 | 4:778bc352c47f | 99 | // inverse kinematics: (x0, y0, z0) -> (theta1, theta2, theta3) |
je310 | 4:778bc352c47f | 100 | // returned status: 0=OK, -1=non-existing position |
je310 | 4:778bc352c47f | 101 | int Kinematics::delta_calcInverse(float x0, float y0, float z0, float &theta1, float &theta2, float &theta3) |
je310 | 4:778bc352c47f | 102 | { |
je310 | 4:778bc352c47f | 103 | theta1 = theta2 = theta3 = 0; |
je310 | 4:778bc352c47f | 104 | int status = delta_calcAngleYZ(x0, y0, z0, theta1); |
je310 | 4:778bc352c47f | 105 | if (status == 0) status = delta_calcAngleYZ(x0*cos120 + y0*sin120, y0*cos120-x0*sin120, z0, theta2); // rotate coords to +120 deg |
je310 | 4:778bc352c47f | 106 | if (status == 0) status = delta_calcAngleYZ(x0*cos120 - y0*sin120, y0*cos120+x0*sin120, z0, theta3); // rotate coords to -120 deg |
je310 | 4:778bc352c47f | 107 | return status; |
je310 | 4:778bc352c47f | 108 | } |
je310 | 4:778bc352c47f | 109 | |
je310 | 4:778bc352c47f | 110 | void Kinematics::activateMotors() |
je310 | 4:778bc352c47f | 111 | { |
je310 | 4:778bc352c47f | 112 | C->setMaxVel(15001); // for safety of the linkages, override to go fast! |
je310 | 4:778bc352c47f | 113 | Thread::wait(100); |
je310 | 4:778bc352c47f | 114 | A->setMaxVel(15001); |
je310 | 4:778bc352c47f | 115 | Thread::wait(100); |
je310 | 4:778bc352c47f | 116 | B->setMaxVel(15001); |
je310 | 4:778bc352c47f | 117 | Thread::wait(100); |
je310 | 4:778bc352c47f | 118 | C->odrive->serial_.printf("w axis0.controller.config.vel_limit %f\n",15002); |
je310 | 4:778bc352c47f | 119 | B->odrive->serial_.printf("w axis0.controller.config.vel_limit %f\n",15002); |
je310 | 4:778bc352c47f | 120 | A->odrive->serial_.printf("w axis0.controller.config.vel_limit %f\n",15002); |
je310 | 4:778bc352c47f | 121 | |
je310 | 4:778bc352c47f | 122 | int requested_state; |
je310 | 4:778bc352c47f | 123 | requested_state = ODrive::AXIS_STATE_CLOSED_LOOP_CONTROL; |
je310 | 4:778bc352c47f | 124 | A->runState(requested_state); |
je310 | 4:778bc352c47f | 125 | B->runState(requested_state); |
je310 | 4:778bc352c47f | 126 | C->runState(requested_state); |
je310 | 4:778bc352c47f | 127 | } |
je310 | 4:778bc352c47f | 128 | |
je310 | 4:778bc352c47f | 129 | void Kinematics::homeMotors() |
je310 | 4:778bc352c47f | 130 | { |
je310 | 4:778bc352c47f | 131 | Thread::wait(1000); |
je310 | 4:778bc352c47f | 132 | A->homeAxis(); |
je310 | 4:778bc352c47f | 133 | Thread::wait(1000); |
je310 | 4:778bc352c47f | 134 | B->homeAxis(); |
je310 | 4:778bc352c47f | 135 | Thread::wait(1000); |
je310 | 4:778bc352c47f | 136 | C->homeAxis(); |
je310 | 4:778bc352c47f | 137 | } |
je310 | 4:778bc352c47f | 138 | |
je310 | 4:778bc352c47f | 139 | int Kinematics::goToPos(float x, float y, float z) |
je310 | 4:778bc352c47f | 140 | { |
je310 | 4:778bc352c47f | 141 | //int error = delta_calcInverse(x,y,z,a,b,c); |
je310 | 4:778bc352c47f | 142 | DeltaKinematics<float>::DeltaVector DV = {x,y,z,0,0,0}; |
je310 | 4:778bc352c47f | 143 | int error = DK->CalculateIpk(&DV, 1); |
je310 | 4:778bc352c47f | 144 | //buffered_pc.printf("angles: a:%f , b:%f , c%f\r\n",DV.phi1,DV.phi2,DV.phi3); |
je310 | 4:778bc352c47f | 145 | DV.phi1 = (DV.phi1/180)*pi; |
je310 | 4:778bc352c47f | 146 | DV.phi2 = (DV.phi2/180)*pi; |
je310 | 4:778bc352c47f | 147 | DV.phi3 = (DV.phi3/180)*pi; |
je310 | 4:778bc352c47f | 148 | if(error == 0 ) { |
je310 | 4:778bc352c47f | 149 | A->goAngle(DV.phi1); |
je310 | 4:778bc352c47f | 150 | B->goAngle(DV.phi2); |
je310 | 4:778bc352c47f | 151 | C->goAngle(DV.phi3); |
je310 | 4:778bc352c47f | 152 | } |
je310 | 4:778bc352c47f | 153 | return error; |
je310 | 4:778bc352c47f | 154 | } |
je310 | 4:778bc352c47f | 155 | |
je310 | 4:778bc352c47f | 156 | void Kinematics::goToAngles(float a, float b, float c) |
je310 | 4:778bc352c47f | 157 | { |
je310 | 4:778bc352c47f | 158 | A->goAngle(a); |
je310 | 4:778bc352c47f | 159 | B->goAngle(b); |
je310 | 4:778bc352c47f | 160 | C->goAngle(c); |
je310 | 4:778bc352c47f | 161 | } |