From 0bb08ca793806613f447e95607a115ee06578226 Mon Sep 17 00:00:00 2001 From: ari86m Date: Sun, 9 Aug 2015 22:12:47 -0400 Subject: [PATCH] Added _2_Joint_Leg_Path_Test_Inverse_Kinematic Confirmed walking, but slow. --- _2_Joint_Leg_Path_Test_Inverse_Kinematic | 341 +++++++++++++++++++++++ 1 file changed, 341 insertions(+) create mode 100644 _2_Joint_Leg_Path_Test_Inverse_Kinematic diff --git a/_2_Joint_Leg_Path_Test_Inverse_Kinematic b/_2_Joint_Leg_Path_Test_Inverse_Kinematic new file mode 100644 index 0000000..b0547be --- /dev/null +++ b/_2_Joint_Leg_Path_Test_Inverse_Kinematic @@ -0,0 +1,341 @@ +/* _2_Joint_Leg_Path_Test_Inverse_Kinematic +//-------1---------2---------3---------4---------5---------6---------7-- +// WARNING: declaration int i, j, or k must always be local scope + +V3 07/11/15 by AM + added 3DOF zero point calibration for all joints (set wrist joints like 2DOF) +v6 07/15/15 by AM + added gamma1 values - still need *(-1) to control direction of solution for R side & frt/bck +v7 07/16/15 by AM + changed gamma to approximation so it will work with the gamma offset for corner legs + verified all legs move with y range +/-25 & z range 20 to -120 (z range must be large) +v8 07/16/15 by AM + changed to move all legs at once by calling move all legs to one point which calls move one leg to one point + verified all legs move but slowly, steps forward & falls on belly, steps forward & falls on belly +v9 07/18/15 by AM + changed constants to integer + TODO: combine solution synergies + wristOffset 90 to 100 +v10 08/09/15 by AM + Verified robot moved as expected, but slowly, one leg at a time + Added descriptions to array "a" which contains points to move the leg tips to. +*/ + +#include // servo library +//#include // math library + +// Initialize Global Variables +int pLed = 13; // LED pin +Servo servo[3][2][3]; // servo control object, Arms +// array of 18 servos for 3 joints on 6 arms +// initialize servo array indexes +int frt = 0; // front row, head end +int mid = 1; // middle row +int bck = 2; // back row, tail end +//---------- +int lft = 0; // left side, as viewed from above +int rgt = 1; // right side, as viewed from above +//---------- +int sdr = 0; // shoulder joint +int elb = 1; // elbow joint +int rst = 2; // wrist joint if installed + +// +// +const int numPoints = 6; +const int numLegTypes = 2; // 2 types of legs moving: 1st (y,z) = FL, MR, BL & 2nd (y,z) = FR, ML, BR +/**************************************/ +// Array "a" contains the points to move thru in robot (y,z) coordinates. X coordinate is only a result. +// The left side of the array contains the points for the left side of the robot & the right, the right side. +// The array contains points to move to for the left & right sides. Typically, the R leading side contents +// is the same numbers as the L leading side but out of phase. Compare point 1 to point 4. +// (y,z) = FL, ML, BL & (y,z) = FR, MR, BR +// 1st row: number of elements in the array +// 2nd row: a neutral point (future use) for {Ly, Lz, Ry, Rz} +// 3rd row: actual points start in the format: {Ly, Lz, Ry, Rz} +const int a[numPoints + 2][2 * numLegTypes] = { + {numPoints + 2, 0, 0, 0}, // 1st element is # array rows + {0, -120, 0, -120}, // neutral point - not used yet - move to this when all action stops + {-50, -130, 30, 00}, // point 1 + {-50, -130, 30, -130}, // point 2 + {-50, 00, 30, -130}, // etc. (must match numPoints above) + {30, 00, -50, -130}, + {30, -130, -50, -130}, + {30, -130, -50, 00} +}; + +// Geometric definitions of robot leg used in Inverse Kinematics +// Setup for 3 DOF leg to mimic 2DOF leg motion +// Actual 2 DOF leg should change L8 +const int L1 = 60; // leg link length 1 from 1st to 2nd axis +const int L2 = 25; // affective leg link length 2 in direction of link 2 +const int L8 = 150; //***~130 if 2DOF leg*** effective leg link length 2 in direction 90 degrees to link 2 +const int CornerLegOffset = 30; // degrees. corner leg neutral position distance from zero position +const int WristOffset = 100; // degrees. 3DOF leg neutral position (freeze here to simulate 2DOF leg) + +/* ********* Enter zero position joint values here *************************** + / \ While robot is on its back, find joint angles that straighted + /Head \ all the leg joints like so: + ---I---I---o o---I---I--- -Shoulder: all legs parallel like painting to the left, + ---I---I---o o---I---I--- -Elbow: parallel with table or floor (while on, pull toward floor) + ---I---I---o Tail o---I---I--- -Wrist: if installed make wrist servo parallel with floor (claw will curl upward a little) + \ / + V + Coordinates for robot when resting on belly: + Y + | + o-> X Z is up +*/ +// Zero Point Array for initial calibration +const int zpa[3][2][3] = { // zero values array (for common reference from robot to robot) + {{58, 60, 70}, {130, 55, 50}}, // Front L & R values for sdr,elb,rst + {{88, 48, 70}, {90, 65, 55}}, // Middle L & R values for sdr,elb,rst + {{117, 50, 70}, {67, 50, 65}} // Back L & R values for sdr,elb,rst +}; + +int npa[3][2][3]; // neutral values array (for common reference from robot to robot) + +// FUNCTION: setup +void setup() { + servo[frt][lft][sdr].attach(40); + servo[frt][lft][elb].attach(42); + servo[frt][rgt][sdr].attach(36); + servo[frt][rgt][elb].attach(38); + + servo[mid][lft][sdr].attach(46); + servo[mid][lft][elb].attach(48); + servo[mid][rgt][sdr].attach(30); + servo[mid][rgt][elb].attach(32); + + servo[bck][lft][sdr].attach(50); + servo[bck][lft][elb].attach(52); + servo[bck][rgt][sdr].attach(26); + servo[bck][rgt][elb].attach(28); + + servo[frt][lft][rst].attach(2); + servo[frt][rgt][rst].attach(3); + servo[mid][lft][rst].attach(4); + servo[mid][rgt][rst].attach(5); + servo[bck][lft][rst].attach(7); + servo[bck][rgt][rst].attach(8); + + Serial.begin(9600); // initialize serial communications + pinMode(pLed, OUTPUT); // initialize pin 13 as an output + + // initialize values to zero position & load neutral point array + for (int i = 0; i < 3; i++) { // rows of legs (front, middle, back) + for (int j = 0; j < 2; j++) { // sides of robot (left, right) + for (int k = 0; k < 3; k++) { // joints per leg (shoulder, elbow, (optional wrist)) + servo[i][j][k].write(zpa[i][j][k] + 10); // set all servos to zero value + delay(300); + Serial.print("Zero position values for each joint in setup\t"); // debug assignment values + Serial.print(i); Serial.print("\t"); // debug assignment values + Serial.print(j); Serial.print("\t"); + Serial.print(k); Serial.print("\t"); + Serial.println(zpa[i][j][k]); + npa[i][j][k] = zpa[i][j][k]; // populate neutral position array + if (k == 2) { + npa[i][j][k] += WristOffset; // set wrist joint neutral point for 3DOF legs + } + } + } + } + // set shoulder joint neutral position for 3DOF legs + npa[frt][lft][sdr] += CornerLegOffset; + npa[frt][rgt][sdr] -= CornerLegOffset; + npa[bck][lft][sdr] -= CornerLegOffset; + npa[bck][rgt][sdr] += CornerLegOffset; + + // set all joints to neutral values & exercise each + for (int i = 0; i < 3; i++) { // rows of legs (front, middle, back) + for (int j = 0; j < 2; j++) { // sides of robot (left, right) + for (int k = 0; k < 3; k++) { // joints per leg (shoulder, elbow, (optional wrist)) + servo[i][j][k].write(npa[i][j][k] - 20); // set all servos to zero value + delay(100); + servo[i][j][k].write(npa[i][j][k] + 20); // set all servos to zero value + delay(100); + servo[i][j][k].write(npa[i][j][k]); // set all servos to zero value + delay(100); + Serial.print("Neutral position values for each joint\t"); // debug assignment values + Serial.print(i); Serial.print("\t"); // debug assignment values + Serial.print(j); Serial.print("\t"); + Serial.print(k); Serial.print("\t"); + Serial.println(npa[i][j][k]); + } + } + } + delay(4000); // wait for neutral position visual assessment + + +} // end setup + +// FUNCTION: loop +void loop() { + int s = 100; // speed (placeholder to be used later) + for (int i = 2; i < a[0][0]; i++) { + moveAllLegsToPoint(a, i, s); + //delay(100); + } + + //moveOneLeg(frt, lft, a, 50); // this function call needs an array a[points+2][2] for (y,z) values + //delay(100); + +} // end loop + +void moveAllLegsToPoint(const int a[][4], int point, int s) { // strange error pointed elsewhere when this array referred as a[][2*numLegTypes] + //2 types of legs moving: 1st (y,z) = FL, MR, BL & 2nd (y,z) = FR, ML, BR + moveOneLegToPoint(frt, lft, a[point][0], a[point][1], s); +// delay(100); + moveOneLegToPoint(mid, rgt, a[point][0], a[point][1], s); +// delay(100); + moveOneLegToPoint(bck, lft, a[point][0], a[point][1], s); +// delay(100); + moveOneLegToPoint(frt, rgt, a[point][2], a[point][3], s); +// delay(100); + moveOneLegToPoint(mid, lft, a[point][2], a[point][3], s); +// delay(100); + moveOneLegToPoint(bck, rgt, a[point][2], a[point][3], s); +// delay(100); +} + +void moveOneLegToPoint(int legRow, int legSide, int yIn, int zIn, int s) { + + int y = 0; int z = 1; //array references to robot axis y (forward) & z (up) + // variables gamma0 & alpha0 are initial calcs in radians to be added to neutral position + float gamma, gamma0, gamma1; //shoulder joint degrees (adjusted to servo angle) + float alpha, alpha0, alpha1, alpha2; //alpha = elbow joint in degrees (adjusted to servo angle) & alpha1 & alpha2 are intermediate values + float realnum1, realnum2; // value2 to check to make sure solution exists + // robot physical variables to be moved outside...stored here temporarily + float x7; // hypotenuse of L2 & L8 + float x_dep; // dependent value of x, just for reference + if ((legRow == frt) && (legSide == lft)) { + gamma1 = CornerLegOffset; + } + else if ((legRow == bck) && (legSide == rgt)) { + gamma1 = CornerLegOffset; + } + else if ((legRow == frt) && (legSide == rgt)) { + gamma1 = -CornerLegOffset; + } + else if ((legRow == bck) && (legSide == lft)) { + gamma1 = -CornerLegOffset; + } + + // ****** 2D inverse kinematics calculations start here ********** + x7 = sqrt(L2 * L2 + L8 * L8); //calculate effective hypotenuse of link 2 + // Calculate alpha = elbow joint to push down to commanded robot -z value + alpha2 = atan2(L8, L2); //constant - based on Link 2 geometry + alpha1 = asin(zIn / x7); + alpha0 = alpha1 + alpha2; + alpha = -alpha0 * 180.0 / PI + npa[legRow][legSide][elb]; //move added to neutral position + + // calculate gamma0 = shoulder offset from neutral position (R side inversed) + if (legSide == lft) { + gamma0 = yIn / (L1 + x7 * cos(alpha1)); //L legs have inverse y as robot + } + else { + gamma0 = -yIn / (L1 + x7 * cos(alpha1)); // Right legs have same +y as robot + } + gamma = gamma0 * 180.0 / PI + npa[legRow][legSide][sdr]; //move added to neutral position + + // print some values to check...before running + Serial.print("Leg row & column\t"); Serial.print(legRow); Serial.print("\t"); Serial.print(legSide); + Serial.print("\tcommands: y =\t"); Serial.print(yIn); Serial.print("\t& z =\t"); Serial.println(zIn); + Serial.print("Gamma0 = \t"); Serial.print(gamma0); Serial.print("\tAlpha0 = \t"); Serial.println(alpha0); + Serial.print("Gamma = \t"); Serial.print(gamma); Serial.print("\tAlpha = \t"); Serial.println(alpha); +/* // check that solutions are real values, so check that no sqrt(-1) or asin(>1) or acos(>1) + realnum1 = L1 + x7 * cos(alpha1); + //realnum2 = abs(yIn+npa[LegRow][LegSide][0]; // 888888888888888888888 need to add gamma1 check too + if ((abs(zIn) > x7) || (abs(yIn) > realnum1)) { + Serial.println("WARNING: No known solution may cause erratic motions."); + } + else { + Serial.println("Solution Found"); + } +*/ + // finally make the moves for this leg + servo[legRow][legSide][sdr].write(gamma); + servo[legRow][legSide][elb].write(alpha); + + +} // end moveAllLegsToPoint + + + + + + + + + + + +// the following MoveOneLeg function moves one leg thru all the points in an array + +// move leg at speed s thru points to end of array a +// inputs: which leg (Front, Middle, Back & Left, Right), 2D move array & speed (speed not used yet) +void moveOneLeg(int LegRow, int LegSide, int a[][2], int s) { + int i; //array row reference (which point) + int y = 0; int z = 1; //array references to robot axis y (forward) & z (up) + // variables gamma0 & alpha0 are initial calcs in radians to be added to neutral position + float gamma, gamma0, gamma1; //shoulder joint degrees (adjusted to servo angle) + float alpha, alpha0, alpha1, alpha2; //alpha = elbow joint in degrees (adjusted to servo angle) & alpha1 & alpha2 are intermediate values + float realnum1, realnum2; // value2 to check to make sure solution exists + // robot physical variables to be moved outside...stored here temporarily + float x7; // hypotenuse of L2 & L8 + float x_dep; // dependent value of x, just for reference + int points = a[0][0]; //array size is stored in 0,0 + if ((LegRow == frt) && (LegSide == lft)) { + gamma1 = CornerLegOffset; + } + else if ((LegRow == bck) && (LegSide == rgt)) { + gamma1 = CornerLegOffset; + } + else if ((LegRow == frt) && (LegSide == rgt)) { + gamma1 = -CornerLegOffset; + } + else if ((LegRow == bck) && (LegSide == lft)) { + gamma1 = -CornerLegOffset; + } + for (i = 2; i < points; i++) { //start piont reference on 3rd row (1st row contains total number of rows, 2nd row contains neutral point) + + // ****** 2D inverse kinematics calculations start here ********** + x7 = sqrt(L2 * L2 + L8 * L8); //calculate effective hypotenuse of link 2 + // Calculate alpha = elbow joint to push down to commanded robot z value + alpha2 = atan2(L8, L2); //constant - based on Link 2 geometry + alpha1 = asin(a[i][z] / x7); + alpha0 = alpha1 + alpha2; + alpha = -alpha0 * 180.0 / PI + npa[LegRow][LegSide][elb]; //move added to neutral position + + // calculate gamma0 = shoulder offset from neutral position (L side inversed) + if (LegSide == lft) { + gamma0 = a[i][y] / (L1 + x7 * cos(alpha1)); //L legs have inverse y as robot + } + else { + gamma0 = -a[i][y] / (L1 + x7 * cos(alpha1)); // Right legs have same +y as robot + } + gamma = gamma0 * 180.0 / PI + npa[LegRow][LegSide][sdr]; //move added to neutral position + + // print some values to check...before running + Serial.print("Array values row "); Serial.print(i); Serial.print(" are y = "); + Serial.print(a[i][0]); Serial.print(" & z = "); Serial.println(a[i][1]); + Serial.print("Gamma0 = \t"); Serial.print(gamma0); Serial.print("\tAlpha0 = \t"); Serial.println(alpha0); + Serial.print("Gamma = \t"); Serial.print(gamma); Serial.print("\tAlpha = \t"); Serial.println(alpha); + // check that solutions are real values, so check that no sqrt(-1) or asin(>1) or acos(>1) + realnum1 = L1 + x7 * cos(alpha1); + //realnum2 = abs(a[i][y]+npa[LegRow][LegSide][0]; // 888888888888888888888 need to add gamma1 check too + if ((abs(a[i][z]) > x7) || (abs(a[i][y]) > realnum1)) { + Serial.println("WARNING: No known solution may cause erratic motions."); + } + else { + Serial.println("Solution Found"); + } + + // finally make the moves + servo[LegRow][LegSide][0].write(gamma); + servo[LegRow][LegSide][1].write(alpha); + delay(100); // ******************************** change to timer related + + } + return; +} // end moveOneLeg