Cheetah Software  1.0
GaitScheduler.cpp
Go to the documentation of this file.
1 /*========================= Gait Scheduler ============================*/
6 #include "Controllers/GaitScheduler.h"
7 
8 /*=========================== Gait Data ===============================*/
12 template <typename T>
13 void GaitData<T>::zero() {
14  // Stop any gait transitions
15  _nextGait = _currentGait;
16 
17  // General Gait descriptors
18  periodTimeNominal = 0.0; // overall period time to scale
19  initialPhase = 0.0; // initial phase to offset
20  switchingPhaseNominal = 0.0; // nominal phase to switch contacts
21 
22  // Enable flag for each foot
23  gaitEnabled = Eigen::Vector4i::Zero(); // enable gaint controlled legs
24 
25  // Time based descriptors
26  periodTime = Vec4<T>::Zero(); // overall gait period time
27  timeStance = Vec4<T>::Zero(); // total stance time
28  timeSwing = Vec4<T>::Zero(); // total swing time
29  timeStanceRemaining = Vec4<T>::Zero(); // stance time remaining
30  timeSwingRemaining = Vec4<T>::Zero(); // swing time remaining
31 
32  // Phase based descriptors
33  switchingPhase = Vec4<T>::Zero(); // phase to switch to swing
34  phaseVariable = Vec4<T>::Zero(); // overall gait phase for each foot
35  phaseOffset = Vec4<T>::Zero(); // nominal gait phase offsets
36  phaseScale = Vec4<T>::Zero(); // phase scale relative to variable
37  phaseStance = Vec4<T>::Zero(); // stance subphase
38  phaseSwing = Vec4<T>::Zero(); // swing subphase
39 
40  // Scheduled contact states
41  contactStateScheduled = Eigen::Vector4i::Zero(); // contact state of the foot
42  contactStatePrev =
43  Eigen::Vector4i::Zero(); // previous contact state of the foot
44  touchdownScheduled = Eigen::Vector4i::Zero(); // scheduled touchdown flag
45  liftoffScheduled = Eigen::Vector4i::Zero(); // scheduled liftoff flag
46 
47  // Position of the feet in the world frame at events
48  posFootTouchdownWorld =
49  Mat34<T>::Zero(); // foot position when scheduled to lift off
50  posFootLiftoffWorld =
51  Mat34<T>::Zero(); // foot position when scheduled to touchdown
52 }
53 
54 template struct GaitData<double>;
55 template struct GaitData<float>;
56 
57 /*========================= Gait Scheduler ============================*/
58 
62 template <typename T>
63 GaitScheduler<T>::GaitScheduler() {
64  initialize();
65 }
66 
70 template <typename T>
71 void GaitScheduler<T>::initialize() {
72  std::cout << "[GAIT] Initialize Gait Scheduler" << std::endl;
73 
74  // Start the gait in a trot since we use this the most
75  gaitData._currentGait = GaitType::STAND;
76 
77  // Zero all gait data
78  gaitData.zero();
79 
80  // Create the gait from the nominal initial
81  createGait();
82 }
83 
88 template <typename T>
89 void GaitScheduler<T>::step() {
90  // Create a new gait structure if a new gait has been requested
91  if (gaitData._currentGait != gaitData._nextGait) {
92  std::cout << "[GAIT] Transitioning gait from " << gaitData.gaitName
93  << " to ";
94  createGait();
95  std::cout << gaitData.gaitName << "\n" << std::endl;
96  gaitData._currentGait = gaitData._nextGait;
97  }
98 
99  // Iterate over the feet
100  for (int foot = 0; foot < 4; foot++) {
101  if (gaitData.gaitEnabled(foot) == 1) {
102  // Monotonic time based phase incrementation
103  if (gaitData._currentGait == GaitType::STAND) {
104  // Don't increment the phase when in stand mode
105  dphase = 0.0;
106  } else {
107  dphase = gaitData.phaseScale(foot) * (dt / gaitData.periodTimeNominal);
108  }
109 
110  // Find each foot's current phase
111  gaitData.phaseVariable(foot) =
112  fmod((gaitData.phaseVariable(foot) + dphase), 1);
113 
114  // Check the current contact state
115  if (gaitData.phaseVariable(foot) <= gaitData.switchingPhase(foot)) {
116  // Foot is scheduled to be in contact
117  gaitData.contactStateScheduled(foot) = 1;
118 
119  // Stance subphase calculation
120  gaitData.phaseStance(foot) =
121  gaitData.phaseVariable(foot) / gaitData.switchingPhase(foot);
122 
123  // Swing phase has not started since foot is in stance
124  gaitData.phaseSwing(foot) = 0.0;
125 
126  // Calculate the remaining time in stance
127  gaitData.timeStanceRemaining(foot) =
128  gaitData.periodTime(foot) *
129  (gaitData.switchingPhase(foot) - gaitData.phaseVariable(foot));
130 
131  // Foot is in stance, no swing time remaining
132  gaitData.timeSwingRemaining(foot) = 0.0;
133 
134  // First contact signifies scheduled touchdown
135  if (gaitData.contactStatePrev(foot) == 0) {
136  // Set the touchdown flag to 1
137  gaitData.touchdownScheduled(foot) = 1;
138 
139  // Remember the location of the feet at touchdown
140  // posFootTouchdownWorld = ;
141 
142  } else {
143  // Set the touchdown flag to 0
144  gaitData.touchdownScheduled(foot) = 0;
145  }
146 
147  } else {
148  // Foot is not scheduled to be in contact
149  gaitData.contactStateScheduled(foot) = 0;
150 
151  // Stance phase has completed since foot is in swing
152  gaitData.phaseStance(foot) = 1.0;
153 
154  // Swing subphase calculation
155  gaitData.phaseSwing(foot) =
156  (gaitData.phaseVariable(foot) - gaitData.switchingPhase(foot)) /
157  (1.0 - gaitData.switchingPhase(foot));
158 
159  // Foot is in swing, no stance time remaining
160  gaitData.timeStanceRemaining(foot) = 0.0;
161 
162  // Calculate the remaining time in swing
163  gaitData.timeSwingRemaining(foot) =
164  gaitData.periodTime(foot) * (1 - gaitData.phaseVariable(foot));
165 
166  // First contact signifies scheduled touchdown
167  if (gaitData.contactStatePrev(foot) == 1) {
168  // Set the liftoff flag to 1
169  gaitData.liftoffScheduled(foot) = 1;
170 
171  // Remember the location of the feet at touchdown
172  // posFootLiftoffWorld = ;
173 
174  } else {
175  // Set the liftoff flag to 0
176  gaitData.liftoffScheduled(foot) = 0;
177  }
178  }
179 
180  } else {
181  // Leg is not enabled
182  gaitData.phaseVariable(foot) = 0.0;
183 
184  // Foot is not scheduled to be in contact
185  gaitData.contactStateScheduled(foot) = 0;
186  }
187 
188  // Set the previous contact state for the next timestep
189  gaitData.contactStatePrev(foot) = gaitData.contactStateScheduled(foot);
190  }
191 }
192 
211 template <typename T>
212 void GaitScheduler<T>::createGait() {
213  // Case structure gets the appropriate parameters
214  switch (gaitData._nextGait) {
215  case GaitType::STAND:
216  gaitData.gaitName = "STAND";
217  gaitData.gaitEnabled << 1, 1, 1, 1;
218  gaitData.periodTimeNominal = 10.0;
219  gaitData.initialPhase = 0.0;
220  gaitData.switchingPhaseNominal = 1.0;
221  gaitData.phaseOffset << 0.5, 0.5, 0.5, 0.5;
222  gaitData.phaseScale << 1.0, 1.0, 1.0, 1.0;
223  break;
224 
225  case GaitType::STAND_CYCLE:
226  gaitData.gaitName = "STAND_CYCLE";
227  gaitData.gaitEnabled << 1, 1, 1, 1;
228  gaitData.periodTimeNominal = 1.0;
229  gaitData.initialPhase = 0.0;
230  gaitData.switchingPhaseNominal = 1.0;
231  gaitData.phaseOffset << 0.5, 0.5, 0.5, 0.5;
232  gaitData.phaseScale << 1.0, 1.0, 1.0, 1.0;
233  break;
234 
235  case GaitType::STATIC_WALK:
236  gaitData.gaitName = "STATIC_WALK";
237  gaitData.gaitEnabled << 1, 1, 1, 1;
238  gaitData.periodTimeNominal = 1.25;
239  gaitData.initialPhase = 0.0;
240  gaitData.switchingPhaseNominal = 0.8;
241  gaitData.phaseOffset << 0.25, 0.0, 0.75, 0.5;
242  gaitData.phaseScale << 1.0, 1.0, 1.0, 1.0;
243  break;
244 
245  case GaitType::AMBLE:
246  gaitData.gaitName = "AMBLE";
247  gaitData.gaitEnabled << 1, 1, 1, 1;
248  gaitData.periodTimeNominal = 1.0;
249  gaitData.initialPhase = 0.0;
250  gaitData.switchingPhaseNominal = 0.8;
251  gaitData.phaseOffset << 0.0, 0.5, 0.25, 0.75;
252  gaitData.phaseScale << 1.0, 1.0, 1.0, 1.0;
253  break;
254 
255  case GaitType::TROT_WALK:
256  gaitData.gaitName = "TROT_WALK";
257  gaitData.gaitEnabled << 1, 1, 1, 1;
258  gaitData.periodTimeNominal = 0.5;
259  gaitData.initialPhase = 0.0;
260  gaitData.switchingPhaseNominal = 0.6;
261  gaitData.phaseOffset << 0.0, 0.5, 0.5, 0.0;
262  gaitData.phaseScale << 1.0, 1.0, 1.0, 1.0;
263  break;
264 
265  case GaitType::TROT:
266  gaitData.gaitName = "TROT";
267  gaitData.gaitEnabled << 1, 1, 1, 1;
268  gaitData.periodTimeNominal = 0.5;
269  gaitData.initialPhase = 0.0;
270  gaitData.switchingPhaseNominal = 0.5;
271  gaitData.phaseOffset << 0.0, 0.5, 0.5, 0.0;
272  gaitData.phaseScale << 1.0, 1.0, 1.0, 1.0;
273  break;
274 
275  case GaitType::TROT_RUN:
276  gaitData.gaitName = "TROT_RUN";
277  gaitData.gaitEnabled << 1, 1, 1, 1;
278  gaitData.periodTimeNominal = 0.5;
279  gaitData.initialPhase = 0.0;
280  gaitData.switchingPhaseNominal = 0.4;
281  gaitData.phaseOffset << 0.0, 0.5, 0.5, 0.0;
282  gaitData.phaseScale << 1.0, 1.0, 1.0, 1.0;
283  break;
284 
285  case GaitType::PACE:
286  gaitData.gaitName = "PACE";
287  gaitData.gaitEnabled << 1, 1, 1, 1;
288  gaitData.periodTimeNominal = 0.5;
289  gaitData.initialPhase = 0.0;
290  gaitData.switchingPhaseNominal = 0.5;
291  gaitData.phaseOffset << 0.0, 0.5, 0.0, 0.5;
292  gaitData.phaseScale << 1.0, 1.0, 1.0, 1.0;
293  break;
294 
295  case GaitType::BOUND:
296  gaitData.gaitName = "BOUND";
297  gaitData.gaitEnabled << 1, 1, 1, 1;
298  gaitData.periodTimeNominal = 0.5;
299  gaitData.initialPhase = 0.0;
300  gaitData.switchingPhaseNominal = 0.5;
301  gaitData.phaseOffset << 0.0, 0.0, 0.5, 0.5;
302  gaitData.phaseScale << 1.0, 1.0, 1.0, 1.0;
303  break;
304 
305  case GaitType::ROTARY_GALLOP:
306  gaitData.gaitName = "ROTARY_GALLOP";
307  gaitData.gaitEnabled << 1, 1, 1, 1;
308  gaitData.periodTimeNominal = 0.4;
309  gaitData.initialPhase = 0.0;
310  gaitData.switchingPhaseNominal = 0.2;
311  gaitData.phaseOffset << 0.0, 0.8571, 0.3571, 0.5;
312  gaitData.phaseScale << 1.0, 1.0, 1.0, 1.0;
313  break;
314 
315  case GaitType::TRAVERSE_GALLOP:
316  // TODO: find the right sequence, should be easy
317  gaitData.gaitName = "TRAVERSE_GALLOP";
318  gaitData.gaitEnabled << 1, 1, 1, 1;
319  gaitData.periodTimeNominal = 0.5;
320  gaitData.initialPhase = 0.0;
321  gaitData.switchingPhaseNominal = 0.2;
322  gaitData.phaseOffset << 0.0, 0.8571, 0.3571, 0.5;
323  gaitData.phaseScale << 1.0, 1.0, 1.0, 1.0;
324  break;
325 
326  case GaitType::PRONK:
327  gaitData.gaitName = "PRONK";
328  gaitData.gaitEnabled << 1, 1, 1, 1;
329  gaitData.periodTimeNominal = 0.5;
330  gaitData.initialPhase = 0.0;
331  gaitData.switchingPhaseNominal = 0.5;
332  gaitData.phaseOffset << 0.0, 0.0, 0.0, 0.0;
333  gaitData.phaseScale << 1.0, 1.0, 1.0, 1.0;
334  break;
335 
336  case GaitType::THREE_FOOT:
337  gaitData.gaitName = "THREE_FOOT";
338  gaitData.gaitEnabled << 0, 1, 1, 1;
339  gaitData.periodTimeNominal = 0.5;
340  gaitData.initialPhase = 0.0;
341  gaitData.switchingPhaseNominal = 0.5;
342  gaitData.phaseOffset << 0.0, 0.666, 0.0, 0.333;
343  gaitData.phaseScale << 0.0, 1.0, 1.0, 1.0;
344  break;
345 
346  case GaitType::CUSTOM:
347  gaitData.gaitName = "CUSTOM";
348  // TODO: get custom gait parameters from operator GUI
349  break;
350 
351  case GaitType::TRANSITION_TO_STAND:
352  gaitData.gaitName = "TRANSITION_TO_STAND";
353  gaitData.gaitEnabled << 1, 1, 1, 1;
354  T oldGaitPeriodTimeNominal = gaitData.periodTimeNominal;
355  gaitData.periodTimeNominal = 3 * gaitData.periodTimeNominal;
356  gaitData.initialPhase = 0.0;
357  gaitData.switchingPhaseNominal =
358  (gaitData.periodTimeNominal +
359  oldGaitPeriodTimeNominal * (gaitData.switchingPhaseNominal - 1)) /
360  gaitData.periodTimeNominal;
361  gaitData.phaseOffset << (gaitData.periodTimeNominal +
362  oldGaitPeriodTimeNominal *
363  (gaitData.phaseVariable(0) - 1)) /
364  gaitData.periodTimeNominal,
365  (gaitData.periodTimeNominal +
366  oldGaitPeriodTimeNominal * (gaitData.phaseVariable(1) - 1)) /
367  gaitData.periodTimeNominal,
368  (gaitData.periodTimeNominal +
369  oldGaitPeriodTimeNominal * (gaitData.phaseVariable(2) - 1)) /
370  gaitData.periodTimeNominal,
371  (gaitData.periodTimeNominal +
372  oldGaitPeriodTimeNominal * (gaitData.phaseVariable(3) - 1)) /
373  gaitData.periodTimeNominal;
374  gaitData.phaseScale << 1.0, 1.0, 1.0, 1.0;
375 
376  break;
377  }
378 
379  // Set the gait parameters for each foot
380  for (int foot = 0; foot < 4; foot++) {
381  if (gaitData.gaitEnabled(foot) == 1) {
382  // The scaled period time for each foot
383  gaitData.periodTime(foot) =
384  gaitData.periodTimeNominal / gaitData.phaseScale(foot);
385 
386  // Phase at which to switch the foot from stance to swing
387  gaitData.switchingPhase(foot) = gaitData.switchingPhaseNominal;
388 
389  // Initialize the phase variables according to offset
390  gaitData.phaseVariable(foot) =
391  gaitData.initialPhase + gaitData.phaseOffset(foot);
392 
393  // Find the total stance time over the gait cycle
394  gaitData.timeStance(foot) =
395  gaitData.periodTime(foot) * gaitData.switchingPhase(foot);
396 
397  // Find the total swing time over the gait cycle
398  gaitData.timeSwing(foot) =
399  gaitData.periodTime(foot) * (1.0 - gaitData.switchingPhase(foot));
400 
401  } else {
402  // The scaled period time for each foot
403  gaitData.periodTime(foot) = 0.0;
404 
405  // Phase at which to switch the foot from stance to swing
406  gaitData.switchingPhase(foot) = 0.0;
407 
408  // Initialize the phase variables according to offset
409  gaitData.phaseVariable(foot) = 0.0;
410 
411  // Foot is never in stance
412  gaitData.timeStance(foot) = 0.0;
413 
414  // Foot is always in "swing"
415  gaitData.timeSwing(foot) = 1.0 / gaitData.periodTime(foot);
416  }
417  }
418 }
419 
423 template <typename T>
424 void GaitScheduler<T>::printGaitInfo() {
425  // Increment printing iteration
426  printIter++;
427 
428  // Print at requested frequency
429  if (printIter == printNum) {
430  std::cout << "[GAIT SCHEDULER] Printing Gait Info...\n";
431  std::cout << "Gait Type: " << gaitData.gaitName << "\n";
432  std::cout << "---------------------------------------------------------\n";
433  std::cout << "Enabled: " << gaitData.gaitEnabled(0) << " | "
434  << gaitData.gaitEnabled(1) << " | " << gaitData.gaitEnabled(2)
435  << " | " << gaitData.gaitEnabled(3) << "\n";
436  std::cout << "Period Time: " << gaitData.periodTime(0) << "s | "
437  << gaitData.periodTime(1) << "s | " << gaitData.periodTime(2)
438  << "s | " << gaitData.periodTime(3) << "s\n";
439  std::cout << "---------------------------------------------------------\n";
440  std::cout << "Contact State: " << gaitData.contactStateScheduled(0) << " | "
441  << gaitData.contactStateScheduled(1) << " | "
442  << gaitData.contactStateScheduled(2) << " | "
443  << gaitData.contactStateScheduled(3) << "\n";
444  std::cout << "Phase Variable: " << gaitData.phaseVariable(0) << " | "
445  << gaitData.phaseVariable(1) << " | " << gaitData.phaseVariable(2)
446  << " | " << gaitData.phaseVariable(3) << "\n";
447  std::cout << "Stance Time Remaining: " << gaitData.timeStanceRemaining(0)
448  << "s | " << gaitData.timeStanceRemaining(1) << "s | "
449  << gaitData.timeStanceRemaining(2) << "s | "
450  << gaitData.timeStanceRemaining(3) << "s\n";
451  std::cout << "Swing Time Remaining: " << gaitData.timeSwingRemaining(0)
452  << "s | " << gaitData.timeSwingRemaining(1) << "s | "
453  << gaitData.timeSwingRemaining(2) << "s | "
454  << gaitData.timeSwingRemaining(3) << "s\n";
455  std::cout << std::endl;
456 
457  // Reset iteration counter
458  printIter = 0;
459  }
460 }
461 
462 template class GaitScheduler<double>;
463 template class GaitScheduler<float>;
GaitType::PACE
GaitType::AMBLE
GaitType::TRANSITION_TO_STAND
GaitType::STAND
GaitType::BOUND
GaitType::STAND_CYCLE
GaitType::THREE_FOOT
Mat34
Eigen::Matrix< T, 3, 4 > Mat34
Definition: cppTypes.h:86
GaitType::TROT_RUN
GaitType::CUSTOM
GaitData::zero
void zero()
Definition: GaitScheduler.cpp:13
Vec4
typename Eigen::Matrix< T, 4, 1 > Vec4
Definition: cppTypes.h:30
GaitType::TRAVERSE_GALLOP
GaitType::TROT_WALK
GaitType::STATIC_WALK
GaitType::TROT
GaitType::PRONK
GaitType::ROTARY_GALLOP
GaitScheduler.h