openDogV3

1
      void kinematics (int leg, float xIn, float yIn, float zIn, float roll, float pitch, float yawIn, int interOn, int dur) {
2
      
3
      // leg 1  : front left
4
      // leg 2  : front right
5
      // leg 3  : back left
6
      // leg 4  : back right
7
      
8
      // moving the foot sideways on the end plane
9
      float hipOffset = 108;     // distance from the hip pivot to the centre of the leg
10
      float lengthY;
11
      float hipAngle1a;
12
      float hipAngle1b;
13
      float hipAngle1;
14
      float hipAngle1Degrees;
15
      float hipHyp;
16
      
17
      // moving the foot forwards or backwardes in the side plane
18
      float shoulderAngle2;
19
      float shoulderAngle2Degrees;
20
      float z2;
21
      
22
      // side plane of individual leg only
23
      #define shinLength 200     
24
      #define thighLength 200
25
      float z3;
26
      float shoulderAngle1;
27
      float shoulderAngle1Degrees;
28
      float shoulderAngle1a;   
29
      float shoulderAngle1b;
30
      float shoulderAngle1c;
31
      float shoulderAngle1d;
32
      float kneeAngle;  
33
      float kneeAngleDegrees; 
34
      
35
      // *** ROTATION AXIS
36
      
37
      // roll axis
38
      #define bodyWidth 59         // half the distance between the hip  pivots (the front)
39
      float legDiffRoll;            // differnece in height for each leg
40
      float bodyDiffRoll;           // how much shorter the 'virtual body' gets
41
      float footDisplacementRoll;   // where the foot actually is
42
      float footDisplacementAngleRoll; // smaller angle
43
      float footWholeAngleRoll;     // whole leg angle
44
      float hipRollAngle;       // angle for hip when roll axis is in use
45
      float rollAngle;          // angle in RADIANS that the body rolls
46
      float zz1a;               // hypotenuse of final triangle
47
      float zz1;                // new height for leg to pass onto the next bit of code
48
      float yy1;                // new position for leg to move sideways
49
      
50
      // pitch axis
51
      #define bodyLength 272          // half the distance between shoulder pivots  (the side)
52
      float legDiffPitch;            // differnece in height for each leg
53
      float bodyDiffPitch;           // how much shorter the 'virtual body' gets
54
      float footDisplacementPitch;   // where the foot actually is
55
      float footDisplacementAnglePitch; // smaller angle
56
      float footWholeAnglePitch;     // whole leg angle
57
      float shoulderPitchAngle;      // angle for hip when roll axis is in use
58
      float pitchAngle;              // angle in RADIANS that the body rolls
59
      float zz2a;                    // hypotenuse of final triangle
60
      float zz2;                     // new height for the leg to pass onto the next bit of code
61
      float xx1;                     // new position to move the leg fowwards/backwards
62
      
63
      // yaw axis 
64
      
65
      float yawAngle;                 // angle in RADIANs for rotation in yaw
66
      float existingAngle;            // existing angle of leg from centre
67
      float radius;                   // radius of leg from centre of robot based on x and y from sticks
68
      float demandYaw;                // demand yaw postion - existing yaw plus the stick yaw 
69
      float xx3;                      // new X coordinate based on demand angle 
70
      float yy3;                      // new Y coordinate based on demand angle
71
      
72
      float x;
73
      float y;
74
      float z;
75
      float yaw;
76
      
77
      
78
      // ** INTERPOLATION **
79
      // use Interpolated values if Interpolation is on
80
      if (interOn == 1) {     
81
      
82
            if (leg == 1) {        // front right
83
                z = interpFRZ.go(zIn,dur);
84
                x = interpFRX.go(xIn,dur);
85
                y = interpFRY.go(yIn,dur);
86
                yaw = interpFRT.go(yawIn,dur);
87
            }
88
          
89
            else if (leg == 2) {    // front left
90
                z = interpFLZ.go(zIn,dur);
91
                x = interpFLX.go(xIn,dur);
92
                y = interpFLY.go(yIn,dur); 
93
                yaw = interpFLT.go(yawIn,dur);
94
             
95
            }
96
          
97
            else if (leg == 4) {   // back right
98
                z = interpBRZ.go(zIn,dur);
99
                x = interpBRX.go(xIn,dur);
100
                y = interpBRY.go(yIn,dur); 
101
                yaw = interpBRT.go(yawIn,dur); 
102
            }
103
          
104
            else if (leg == 3) {    // back left
105
                z = interpBLZ.go(zIn,dur);
106
                x = interpBLX.go(xIn,dur);
107
                y = interpBLY.go(yIn,dur); 
108
                yaw = interpBLT.go(yawIn,dur);
109
            }  
110
      } 
111
      
112
      
113
      // wait for filters to settle before using Interpolated values
114
      // set a timer for filter to settle    
115
      if (interpFlag == 0) {           
116
                z = zIn;        // in the meantime use raw values
117
                x = xIn;
118
                y = yIn;
119
                yaw = yawIn;
120
            if (currentMillis - previousInterpMillis >= 300) {
121
                interpFlag = 1;
122
            } 
123
       }
124
       // once time has settled and Interpolation is off then use the original values
125
       else if (interpFlag == 1 && interOn == 0 ) {
126
                z = zIn;
127
                x = xIn;
128
                y = yIn;
129
                yaw = yawIn;
130
       }
131
      
132
       // **** START INVERSE KINEMATICS CALCS ****
133
      
134
       //yy3 = y;
135
       //zz2 = z;
136
       //xx3 = x;
137

138
           // ** YAW AXIS **
139

140
      // convert degrees to radians for the calcs
141
      yawAngle = (PI/180) * yaw;
142
      
143
      // put in offsets from robot's parameters so we can work out the radius of the foot from the robot's centre
144
      if (leg == 1) {         // front left leg
145
         y = y - (bodyWidth+hipOffset); 
146
         x = x - bodyLength;      
147
      }
148
      else if (leg == 2) {    // front right leg
149
         y = y + (bodyWidth+hipOffset);
150
         x = x - bodyLength; 
151
      }
152
      else if (leg == 3) {    // back left leg
153
         y = y - (bodyWidth+hipOffset); 
154
         x = x + bodyLength;
155
      }
156
      else if (leg == 4) {    // back left leg
157
         y = y + (bodyWidth+hipOffset); 
158
         x = x + bodyLength;
159
      }
160
      
161
      //calc existing angle of leg from cetre
162
      existingAngle = atan(y/x);   
163
      
164
      // calc radius from centre
165
      radius = y/sin(existingAngle);
166
      
167
      //calc demand yaw angle
168
      demandYaw = existingAngle + yawAngle;
169
      
170
      // calc new X and Y based on demand yaw angle
171
      xx3 = radius * cos(demandYaw);           // calc new X and Y based on new yaw angle
172
      yy3 = radius * sin(demandYaw);
173
      
174
      // remove the offsets so we pivot around 0/0 x/y
175
      if (leg == 1) {         // front left leg
176
         yy3 = yy3 + (bodyWidth+hipOffset); 
177
         xx3 = xx3 + bodyLength;      
178
      }
179
      else if (leg == 2) {    // front right leg
180
         yy3 = yy3 - (bodyWidth+hipOffset);
181
         xx3 = xx3 + bodyLength; 
182
      }
183
      else if (leg == 3) {    // back left leg
184
         yy3 = yy3 + (bodyWidth+hipOffset); 
185
         xx3 = xx3 - bodyLength;
186
      }
187
      else if (leg == 4) {    // back left leg
188
         yy3 = yy3 - (bodyWidth+hipOffset); 
189
         xx3 = xx3 - bodyLength;
190
      }       
191

192
      // ** PITCH AXIS ***   
193
      
194
      if (leg == 1 || leg == 2) {
195
      pitch = pitch *-1;
196
      xx3 = xx3*-1;
197
      }
198
      
199
      // convert pitch to degrees
200
      pitchAngle = (PI/180) * pitch;
201
      
202
      //calc top triangle sides
203
      legDiffPitch = sin(pitchAngle) * bodyLength;
204
      bodyDiffPitch = cos(pitchAngle) * bodyLength;
205
      
206
      // calc actual height from the ground for each side
207
      legDiffPitch = z - legDiffPitch;
208
      
209
      // calc foot displacement
210
      footDisplacementPitch = ((bodyDiffPitch - bodyLength)*-1)+xx3;
211
      
212
      //calc smaller displacement angle
213
      footDisplacementAnglePitch = atan(footDisplacementPitch/legDiffPitch);
214
      
215
      //calc distance from the ground at the displacement angle (the hypotenuse of the final triangle)
216
      zz2a = legDiffPitch/cos(footDisplacementAnglePitch);
217
      
218
      // calc the whole angle for the leg
219
      footWholeAnglePitch = footDisplacementAnglePitch + pitchAngle;
220
      
221
      //calc actual leg length - the new Z to pass on
222
      zz2 = cos(footWholeAnglePitch) * zz2a;
223
      
224
      //calc new Z to pass on
225
      xx1 = sin(footWholeAnglePitch) * zz2a;  
226
      
227
      if (leg == 1 || leg == 2) {
228
      xx1 = xx1*-1;
229
      }         
230

231
      // *** ROLL AXIS *** 
232
      
233
      //turn around roll angle for each side of the robot
234
      if (leg == 2 || leg == 3) {
235
        roll = 0-roll;
236
        yy3 = yy3*-1;
237
       
238
      }
239
      else if (leg == 1 || leg == 4) {
240
        roll = 0+roll;       
241
      }
242
      
243
      // convert roll angle to radians
244
      rollAngle = (PI/180) * roll;    //covert degrees from the stick to radians
245
      
246
      // calc the top triangle sides
247
      legDiffRoll = sin(rollAngle) * bodyWidth;
248
      bodyDiffRoll = cos(rollAngle) * bodyWidth;  
249
      
250
      // calc actual height from the ground for each side
251
      legDiffRoll = zz2 - legDiffRoll; 
252
      
253
      // calc foot displacement
254
      footDisplacementRoll = (((bodyDiffRoll - bodyWidth)*-1)+hipOffset)-yy3;
255
      
256
      //calc smaller displacement angle
257
      footDisplacementAngleRoll = atan(footDisplacementRoll/legDiffRoll); 
258
      
259
      //calc distance from the ground at the displacement angle (the hypotenuse of the final triangle)
260
      zz1a = legDiffRoll/cos(footDisplacementAngleRoll);
261
      
262
      // calc the whole angle for the leg
263
      footWholeAngleRoll = footDisplacementAngleRoll + rollAngle;
264
      
265
      //calc actual leg length - the new Z to pass on
266
      zz1 = cos(footWholeAngleRoll) * zz1a;
267
      
268
      //calc new Y to pass on
269
      yy1 = (sin(footWholeAngleRoll) * zz1a)-hipOffset;       // take away the offset so we can pivot around zero  
270
  
271
  
272
      // *** TRANSLATION AXIS ***
273
  
274
      // calculate the hip joint and new leg length based on how far the robot moves sideways
275
      // Y axis - side to side
276
      // first triangle
277
  
278
      if (leg == 1 || leg == 4) {   // reverse the calcs for each side of the robot
279
        hipOffset = hipOffset * -1; 
280
        yy1 = yy1*-1;     
281
      }
282
  
283
      yy1 = yy1 +  hipOffset;          // add on hip offset because there is default distance in Y
284
      hipAngle1a = atan(yy1/zz1);    
285
      hipAngle1Degrees = ((hipAngle1a * (180/PI)));   // convert to degrees
286
      hipHyp = zz1/cos(hipAngle1a);         // this is the hypotenuse of the first triangle
287
  
288
      // second triangle
289
      
290
      hipAngle1b = asin(hipOffset/hipHyp) ;     // calc 'the other angle' in the triangle
291
      hipAngle1 = (PI - (PI/2) - hipAngle1b) + hipAngle1a;     // calc total hip angle
292
      hipAngle1 = hipAngle1 - 1.5708;           // take away offset for rest position
293
      hipAngle1Degrees = ((hipAngle1 * (180/PI)));   // convert to degrees 
294
  
295
      // calc new leg length to give to the code  below
296
      z2 = hipOffset/tan(hipAngle1b);           // new leg length
297
  
298
      // ****************
299
      
300
      // X axis - front to back
301
      // calculate the shoulder joint offset and new leg length based on now far the foot moves forward/backwards
302
      shoulderAngle2 = atan(xx1/z2);     // calc how much extra to add to the shoulder joint
303
      shoulderAngle2Degrees = shoulderAngle2 * (180/PI);
304
      z3 = z2/cos(shoulderAngle2);     // calc new leg length to feed to the next bit of code below
305
  
306
      // ****************
307
  
308
      // Z axis - up and down
309
      // calculate leg length based on shin/thigh length and knee and shoulder angle
310
      z3 = constrain(z3,200,390);                 // constrain leg length to stop it turning inside out and breaking the trig
311
      shoulderAngle1a = sq(thighLength) + sq(z3) - sq(shinLength);
312
      shoulderAngle1b = 2 * thighLength * z3;
313
      shoulderAngle1c = shoulderAngle1a / shoulderAngle1b;
314
      shoulderAngle1 = acos(shoulderAngle1c);     // radians
315
      kneeAngle = PI - (shoulderAngle1 *2);       // radians
316
  
317
      //calc degrees from angles
318
      shoulderAngle1Degrees = shoulderAngle1 * (180/PI);    // degrees
319
      kneeAngleDegrees = kneeAngle * (180/PI);              // degrees 
320
  
321

322

323
      // write to joints
324
      
325
      float conversion;
326
      conversion = 0.02777777777777777777777777777778;    // factor for converting degrees to motor turns used by the ODrive
327
      
328
      if (leg == 1) {     // front right
329
          float shoulderAngle1Counts = (shoulderAngle1Degrees-45) * conversion; // convert to encoder counts
330
          float shoulderAngle2Counts = shoulderAngle2Degrees * conversion; // convert to encoder counts
331
          float shoulderAngleCounts = shoulderAngle1Counts + shoulderAngle2Counts;
332
          float kneeAngleCounts = (kneeAngleDegrees-90) * conversion; // convert to encoder counts 
333
          float hipAngleCounts = hipAngle1Degrees * conversion;    // convert to encoder counts       
334
          driveJoints (21, shoulderAngleCounts);    // front right shoulder
335
          driveJoints (20, kneeAngleCounts);    // front right knee  
336
          driveJoints (10, hipAngleCounts);     // front right hip
337
      }
338
      
339
      else if (leg == 2) {     // front left
340
          float shoulderAngle1Counts = (shoulderAngle1Degrees-45) * conversion; // convert to encoder counts
341
          float shoulderAngle2Counts = shoulderAngle2Degrees * conversion; // convert to encoder counts
342
          float shoulderAngleCounts = shoulderAngle1Counts + shoulderAngle2Counts;
343
          float kneeAngleCounts = (kneeAngleDegrees-90) * conversion; // convert to encoder counts 
344
          float hipAngleCounts = hipAngle1Degrees * conversion;    // convert to encoder counts       
345
          driveJoints (51, shoulderAngleCounts);    // front left shoulder
346
          driveJoints (50, kneeAngleCounts);    // front left knee
347
          driveJoints (40, hipAngleCounts);     // front left hip
348
      }
349
      
350
      else if (leg == 3) {     // back left
351
          float shoulderAngle1Counts = (shoulderAngle1Degrees-45) * conversion; // convert to encoder counts
352
          float shoulderAngle2Counts = shoulderAngle2Degrees * conversion; // convert to encoder counts
353
          float shoulderAngleCounts = shoulderAngle1Counts - shoulderAngle2Counts;
354
          float kneeAngleCounts = (kneeAngleDegrees-90) * conversion; // convert to encoder counts 
355
          float hipAngleCounts = hipAngle1Degrees * conversion;    // convert to encoder counts        
356
          driveJoints (61, shoulderAngleCounts);    // back left shoulder
357
          driveJoints (60, kneeAngleCounts);    // back left knee
358
          driveJoints (41, hipAngleCounts);     // back left hip
359
          
360
      }
361
      
362
      else if (leg == 4) {     // back right
363
          float shoulderAngle1Counts = (shoulderAngle1Degrees-45) * conversion; // convert to encoder counts
364
          float shoulderAngle2Counts = shoulderAngle2Degrees * conversion; // convert to encoder counts
365
          float shoulderAngleCounts = shoulderAngle1Counts - shoulderAngle2Counts;
366
          float kneeAngleCounts = (kneeAngleDegrees-90) * conversion; // convert to encoder counts 
367
          float hipAngleCounts = hipAngle1Degrees * conversion;    // convert to encoder counts        
368
          driveJoints (31, shoulderAngleCounts);    // back right shoulder
369
          driveJoints (30, kneeAngleCounts);    // back right knee
370
          driveJoints (11, hipAngleCounts);     // back right hip
371
      }
372

373

374

375

376

377
    
378
}
379