Cheetah Software  1.0
Public Member Functions | Public Attributes | Private Attributes | List of all members
GaitScheduler< T > Class Template Reference

#include <GaitScheduler.h>

+ Collaboration diagram for GaitScheduler< T >:

Public Member Functions

 GaitScheduler ()
 
 ~GaitScheduler ()
 
void initialize ()
 
void step ()
 
void createGait ()
 
void printGaitInfo ()
 

Public Attributes

GaitData< T > gaitData
 

Private Attributes

T dt = 0.001
 
T dphase
 
int printNum = 5
 
int printIter = 0
 

Detailed Description

template<typename T>
class GaitScheduler< T >

Definition at line 87 of file GaitScheduler.h.

Constructor & Destructor Documentation

template<typename T >
GaitScheduler< T >::GaitScheduler ( )

Constructor to automatically setup a basic gait

Definition at line 63 of file GaitScheduler.cpp.

63  {
64  initialize();
65 }
template<typename T >
GaitScheduler< T >::~GaitScheduler ( )
inline

Definition at line 91 of file GaitScheduler.h.

91 {};

Member Function Documentation

template<typename T >
void GaitScheduler< T >::createGait ( )

Creates the gait structure from the important defining parameters of each gait

To create a standard gait you should only need to define the following:

gaitData.periodTimeNominal gaitData.switchingPhaseNominal gaitData.phaseOffset

The rest can be set to:

gaitData.gaitEnabled << 1, 1, 1, 1; gaitData.initialPhase = 0.0; gaitData.phaseScale << 1.0, 1.0, 1.0, 1.0;

These add flexibility to be used for very irregular gaits and transitions.

Definition at line 212 of file GaitScheduler.cpp.

References AMBLE, BOUND, CUSTOM, PACE, PRONK, ROTARY_GALLOP, STAND, STAND_CYCLE, STATIC_WALK, THREE_FOOT, TRANSITION_TO_STAND, TRAVERSE_GALLOP, TROT, TROT_RUN, and TROT_WALK.

212  {
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 }
GaitType::PACE
GaitType::AMBLE
GaitType::TRANSITION_TO_STAND
GaitType::STAND
GaitType::BOUND
GaitType::STAND_CYCLE
GaitType::THREE_FOOT
GaitScheduler::gaitData
GaitData< T > gaitData
Definition: GaitScheduler.h:106
GaitType::TROT_RUN
GaitType::CUSTOM
GaitType::TRAVERSE_GALLOP
GaitType::TROT_WALK
GaitType::STATIC_WALK
GaitType::TROT
GaitType::PRONK
GaitType::ROTARY_GALLOP
template<typename T >
void GaitScheduler< T >::initialize ( )

Initialize the gait data

Definition at line 71 of file GaitScheduler.cpp.

References STAND.

71  {
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 }
GaitType::STAND
GaitScheduler::gaitData
GaitData< T > gaitData
Definition: GaitScheduler.h:106
template<typename T >
void GaitScheduler< T >::printGaitInfo ( )

Prints relevant information about the gait and current gait state

Definition at line 424 of file GaitScheduler.cpp.

424  {
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 }
GaitScheduler::gaitData
GaitData< T > gaitData
Definition: GaitScheduler.h:106
template<typename T >
void GaitScheduler< T >::step ( )

Executes the Gait Schedule step to calculate values for the defining gait parameters.

Definition at line 89 of file GaitScheduler.cpp.

References STAND.

89  {
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 }
GaitType::STAND
GaitScheduler::gaitData
GaitData< T > gaitData
Definition: GaitScheduler.h:106

Member Data Documentation

template<typename T >
T GaitScheduler< T >::dphase
private

Definition at line 116 of file GaitScheduler.h.

template<typename T >
T GaitScheduler< T >::dt = 0.001
private

Definition at line 113 of file GaitScheduler.h.

template<typename T >
GaitData<T> GaitScheduler< T >::gaitData

Definition at line 106 of file GaitScheduler.h.

template<typename T >
int GaitScheduler< T >::printIter = 0
private

Definition at line 122 of file GaitScheduler.h.

template<typename T >
int GaitScheduler< T >::printNum = 5
private

Definition at line 119 of file GaitScheduler.h.

The documentation for this class was generated from the following files: