Skip to content

Commit

Permalink
Added _2_Joint_Leg_Path_Test_Inverse_Kinematic
Browse files Browse the repository at this point in the history 
Confirmed walking, but slow.
  • Loading branch information
@ari86m
ari86m committed Aug 10, 2015
1 parent f8ea574 commit 0bb08ca
Showing 1 changed file with 341 additions and 0 deletions.
341 changes: 341 additions & 0 deletions _2_Joint_Leg_Path_Test_Inverse_Kinematic
View file
@@ -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.h> // servo library
//#include <math.h> // 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

0 comments on commit 0bb08ca

Please sign in to comment.