humoto
stance_finite_state_machine.h
Go to the documentation of this file.
1 /**
2  @file
3  @author Alexander Sherikov
4  @author Jan Michalczyk
5  @copyright 2014-2017 INRIA. Licensed under the Apache License, Version 2.0.
6  (see @ref LICENSE or http://www.apache.org/licenses/LICENSE-2.0)
7  @brief
8 */
9 
10 #pragma once
11 
12 namespace humoto
13 {
14  namespace walking
15  {
16 
17  /**
18  * @brief Possible stance types of the model
19  */
20  class StanceType
21  {
22  public:
23  enum Type
24  {
25  /// Undefined
26  UNDEFINED = 0,
27 
28  /// Double support
29  DS = 1,
30 
31  /// Left single support
32  LSS = 2,
33 
34  /// Right single support
35  RSS = 3,
36 
37  /// Transitional double support
38  TDS = 4
39  };
40  };
41 
42 
43  /**
44  * @brief Possible stance subtypes of the model
45  */
46  class StanceSubType
47  {
48  public:
49  enum Type
50  {
51  /// Undefined
52  UNDEFINED = 0,
53 
54  /// First stance
55  FIRST = 1,
56 
57  /// Intermediate stance
58  INTERMEDIATE = 2,
59 
60  /// Last stance
61  LAST = 3
62  };
63  };
64 
65 
66 
67  /**
68  * @brief Determines support foot for the given single support stance.
69  *
70  * @param[in] stance_type
71  *
72  * @return Left or right.
73  */
74  humoto::LeftOrRight::Type determineSupportFoot(const StanceType::Type & stance_type)
75  {
76  switch(stance_type)
77  {
78  case StanceType::LSS:
79  return(humoto::LeftOrRight::LEFT);
80  case StanceType::RSS:
81  return(humoto::LeftOrRight::RIGHT);
82  default:
83  HUMOTO_THROW_MSG("Given stance type does not have corresponding support foot.");
84  }
85  }
86 
87 
88 
89  /**
90  * @brief Class containing parameters of the stance finite state machine
91  */
92  class HUMOTO_LOCAL StanceFSMParameters : public humoto::config::ConfigurableBase
93  {
94  #define HUMOTO_CONFIG_SECTION_ID "StanceFSMParameters"
95  #define HUMOTO_CONFIG_CONSTRUCTOR StanceFSMParameters
96  #define HUMOTO_CONFIG_ENTRIES \
97  HUMOTO_CONFIG_SCALAR_(ss_duration_ms) \
98  HUMOTO_CONFIG_SCALAR_(tds_duration_ms) \
99  HUMOTO_CONFIG_SCALAR_(first_stance_duration_ms) \
100  HUMOTO_CONFIG_SCALAR_(last_stance_duration_ms) \
101  HUMOTO_CONFIG_SCALAR_(num_steps) \
102  HUMOTO_CONFIG_ENUM_(first_stance) \
103  HUMOTO_CONFIG_ENUM_(first_ss_type) \
104  HUMOTO_CONFIG_ENUM_(last_stance)
105  #include HUMOTO_CONFIG_DEFINE_ACCESSORS
106 
107 
108  public:
109  StanceType::Type first_stance_;
110  StanceType::Type first_ss_type_;
111  StanceType::Type last_stance_;
112 
113  std::size_t ss_duration_ms_;
114  std::size_t tds_duration_ms_;
115  std::size_t first_stance_duration_ms_;
116  std::size_t last_stance_duration_ms_;
117  std::size_t num_steps_;
118 
119 
120  /**
121  * @brief Default constructor
122  */
123  StanceFSMParameters()
124  {
125  setDefaults();
126  }
127 
128 
129 
130  /**
131  * @brief Returns duration of a stance
132  *
133  * @param[in] stance_type
134  *
135  * @return
136  */
137  std::size_t getDurationMs(const StanceType::Type stance_type) const
138  {
139  switch(stance_type)
140  {
141  case StanceType::DS:
142  return last_stance_duration_ms_;
143  case StanceType::TDS:
144  return tds_duration_ms_;
145  case StanceType::LSS:
146  return ss_duration_ms_;
147  case StanceType::RSS:
148  return ss_duration_ms_;
149  default:
150  HUMOTO_THROW_MSG("Unsupported stance type.");
151  break;
152  }
153  }
154 
155 
156  /**
157  * @brief Default parameters of the finite state machine
158  */
159  void setDefaults()
160  {
161  first_ss_type_ = StanceType::RSS;
162  first_stance_ = StanceType::DS;
163  last_stance_ = StanceType::DS;
164 
165  ss_duration_ms_ = 700;
166  tds_duration_ms_ = 100;
167  first_stance_duration_ms_ = 300;
168  last_stance_duration_ms_ = 2900;
169 
170  num_steps_ = 7;
171  }
172  };
173 
174 
175  /**
176  * @brief Stance
177  */
178  class HUMOTO_LOCAL Stance
179  {
180  public:
181  StanceType::Type type_;
182  StanceType::Type previous_nonds_stance_type_;
183  StanceType::Type previous_nontds_stance_type_;
184  StanceSubType::Type subtype_;
185 
186  std::size_t duration_ms_;
187  std::size_t total_duration_ms_;
188 
189 
190  public:
191  /**
192  * @brief Default constructor
193  */
194  Stance()
195  {
196  type_ = StanceType::UNDEFINED;
197  previous_nonds_stance_type_ = StanceType::UNDEFINED;
198  previous_nontds_stance_type_ = StanceType::UNDEFINED;
199  subtype_ = StanceSubType::UNDEFINED;
200  duration_ms_ = 0;
201  total_duration_ms_ = 0;
202  }
203 
204  /**
205  * @brief Log
206  *
207  * @param[in,out] logger logger
208  * @param[in] parent parent
209  * @param[in] name name
210  */
211  void log( humoto::Logger &logger HUMOTO_GLOBAL_LOGGER_IF_DEFINED,
212  const LogEntryName &parent = LogEntryName(),
213  const std::string &name = "stance") const
214  {
215  LogEntryName subname = parent; subname.add(name);
216 
217  logger.log(LogEntryName(subname).add("type") , type_ );
218  logger.log(LogEntryName(subname).add("previous_nonds_stance_type") , previous_nonds_stance_type_ );
219  logger.log(LogEntryName(subname).add("previous_nontds_stance_type"), previous_nontds_stance_type_);
220  logger.log(LogEntryName(subname).add("subtype") , subtype_ );
221  logger.log(LogEntryName(subname).add("duration_ms") , duration_ms_ );
222  logger.log(LogEntryName(subname).add("total_duration_ms") , total_duration_ms_ );
223  }
224  };
225 
226 
227  /**
228  * @brief Finite state machine for walking. [initialize_contact_walk_fsm.m]
229  *
230  * Supports online states sequence generation
231  */
232  class HUMOTO_LOCAL StanceFiniteStateMachine
233  {
234  private:
235  void finalize()
236  {
237  HUMOTO_ASSERT(sfsm_params_.num_steps_ != 1, "Unsupported number of steps.");
238 
239  ss_states_to_termination_ = sfsm_params_.num_steps_;
240  current_time_ms_ = 0;
241  is_tds_started_ = false;
242  current_stance_.previous_nontds_stance_type_ = StanceType::UNDEFINED;
243  current_stance_.subtype_ = StanceSubType::FIRST;
244  current_stance_.type_ = sfsm_params_.first_stance_;
245  current_stance_.duration_ms_ = sfsm_params_.first_stance_duration_ms_;
246 
247  switch (current_stance_.type_)
248  {
249  case StanceType::LSS:
250  case StanceType::RSS:
251  current_stance_.previous_nonds_stance_type_ = current_stance_.type_;
252  break;
253  case StanceType::DS:
254  switch(sfsm_params_.first_ss_type_)
255  {
256  case StanceType::RSS:
257  current_stance_.previous_nonds_stance_type_ = StanceType::LSS;
258  break;
259  case StanceType::LSS:
260  current_stance_.previous_nonds_stance_type_ = StanceType::RSS;
261  break;
262  case StanceType::UNDEFINED:
263  current_stance_.previous_nonds_stance_type_ = StanceType::UNDEFINED;
264  sfsm_params_.num_steps_ = 0;
265  break;
266  default:
267  HUMOTO_THROW_MSG("Misformed walking options.");
268  break;
269  }
270  break;
271  default:
272  HUMOTO_THROW_MSG("Misformed walking options.");
273  break;
274  }
275  }
276 
277 
278  public:
279  bool is_tds_started_;
280  std::size_t ss_states_to_termination_;
281  std::size_t current_time_ms_;
282  Stance current_stance_;
283  StanceFSMParameters sfsm_params_;
284 
285 
286  public:
287  void shiftTime(const std::size_t shift_time_ms);
288  void shiftStance();
289  Stance getNextStance() const;
290  std::vector<Stance> previewStances(const std::size_t preview_duration_ms) const;
291 
292  StanceFiniteStateMachine()
293  {
294  finalize();
295  }
296 
297  explicit StanceFiniteStateMachine(const StanceFSMParameters &sfsm_params);
298 
299  void setParameters(const StanceFSMParameters &sfsm_params)
300  {
301  sfsm_params_ = sfsm_params;
302  finalize();
303  }
304 
305  void log(humoto::Logger &, const LogEntryName &, const std::string &) const;
306  };
307 
308 
309  /**
310  * @brief Preview sequence of Stances of the FSM
311  *
312  * @param[in] preview_duration_ms
313  *
314  * @return
315  */
316  std::vector<Stance> StanceFiniteStateMachine::previewStances(const std::size_t preview_duration_ms) const
317  {
318  bool not_enough_states = false;
319 
320  std::vector<Stance> stance_types;
321  Stance stance_type;
322 
323  StanceFiniteStateMachine stance_fsm = StanceFiniteStateMachine(*this);
324 
325 
326  for (std::size_t duration_ms = preview_duration_ms; duration_ms > 0; )
327  {
328  stance_type.type_ = stance_fsm.current_stance_.type_;
329  stance_type.subtype_ = stance_fsm.current_stance_.subtype_;
330  stance_type.duration_ms_ = std::min(stance_fsm.current_stance_.duration_ms_ - stance_fsm.current_time_ms_, duration_ms);
331  stance_type.total_duration_ms_ = stance_fsm.current_stance_.duration_ms_;
332  stance_type.previous_nonds_stance_type_ = stance_fsm.current_stance_.previous_nonds_stance_type_;
333  stance_type.previous_nontds_stance_type_ = stance_fsm.current_stance_.previous_nontds_stance_type_;
334 
335  if(stance_fsm.current_time_ms_ + duration_ms <= stance_fsm.current_stance_.duration_ms_)
336  {
337  duration_ms = 0;
338  stance_fsm.current_time_ms_ = stance_fsm.current_time_ms_ + duration_ms;
339  }
340  else
341  {
342  duration_ms -= (stance_fsm.current_stance_.duration_ms_ - stance_fsm.current_time_ms_);
343 
344  if((stance_fsm.ss_states_to_termination_ == 0) && (stance_fsm.current_stance_.type_ == StanceType::DS))
345  {
346  not_enough_states = true;
347  break;
348  }
349  else
350  {
351  stance_fsm.shiftStance();
352  }
353  }
354 
355  stance_types.push_back(stance_type);
356  }
357 
358  //always add one state afted TDS
359  if((not_enough_states == false) && (stance_types.back().type_ == StanceType::TDS))
360  {
361  if(stance_fsm.current_stance_.type_ == StanceType::TDS)
362  {
363  stance_fsm.shiftStance();
364  }
365 
366  stance_type.type_ = stance_fsm.current_stance_.type_;
367  stance_type.subtype_ = stance_fsm.current_stance_.subtype_;
368  stance_type.duration_ms_ = 0;
369  stance_type.total_duration_ms_ = 0;
370  stance_type.previous_nonds_stance_type_ = stance_fsm.current_stance_.previous_nonds_stance_type_;
371  stance_type.previous_nontds_stance_type_ = stance_fsm.current_stance_.previous_nontds_stance_type_;
372 
373  stance_types.push_back(stance_type);
374  }
375 
376  return stance_types;
377  }
378 
379 
380  /**
381  * @brief Preview next walk state of the FSM
382  */
383  Stance StanceFiniteStateMachine::getNextStance() const
384  {
385  StanceFiniteStateMachine stance_fsm = StanceFiniteStateMachine(*this);
386  stance_fsm.shiftStance();
387  return stance_fsm.current_stance_;
388  }
389 
390 
391  /**
392  * @brief Constructor
393  *
394  * @param[in] sfsm_params
395  */
396  StanceFiniteStateMachine::StanceFiniteStateMachine(const StanceFSMParameters &sfsm_params)
397  {
398  sfsm_params_ = sfsm_params;
399  finalize();
400  }
401 
402 
403  /**
404  * @brief Shift time
405  *
406  * @param[in] shift_time_ms
407  */
408  void StanceFiniteStateMachine::shiftTime(const std::size_t shift_time_ms)
409  {
410  for (std::size_t time_ms = shift_time_ms; time_ms > 0; )
411  {
412  is_tds_started_ = false;
413 
414  if(current_time_ms_ + time_ms < current_stance_.duration_ms_)
415  {
416  current_time_ms_ = current_time_ms_ + time_ms;
417  time_ms = 0;
418  }
419  else
420  {
421  time_ms -= (current_stance_.duration_ms_ - current_time_ms_);
422  shiftStance();
423  }
424  }
425  }
426 
427 
428  /**
429  * @brief Shift state of the model
430  */
431  void StanceFiniteStateMachine::shiftStance()
432  {
433  is_tds_started_ = true;
434 
435  current_stance_.subtype_ = StanceSubType::INTERMEDIATE;
436 
437  switch(current_stance_.type_)
438  {
439  case StanceType::LSS:
440  current_stance_.previous_nonds_stance_type_ = current_stance_.type_;
441  current_stance_.previous_nontds_stance_type_ = current_stance_.type_;
442  current_stance_.type_ = StanceType::TDS;
443  current_stance_.duration_ms_ = sfsm_params_.getDurationMs(StanceType::TDS);
444  current_time_ms_ = 0;
445  break;
446 
447  case StanceType::RSS:
448  current_stance_.previous_nonds_stance_type_ = current_stance_.type_;
449  current_stance_.previous_nontds_stance_type_ = current_stance_.type_;
450  current_stance_.type_ = StanceType::TDS;
451  current_stance_.duration_ms_ = sfsm_params_.getDurationMs(StanceType::TDS);
452  current_time_ms_ = 0;
453  break;
454 
455  case StanceType::TDS:
456  is_tds_started_ = false;
457 
458  if(ss_states_to_termination_ == 0)
459  {
460  current_stance_.subtype_ = StanceSubType::LAST;
461  current_stance_.type_ = StanceType::DS;
462  current_stance_.duration_ms_ = sfsm_params_.getDurationMs(StanceType::DS);
463  current_time_ms_ = 0;
464  }
465  else
466  {
467  if(ss_states_to_termination_ > 0)
468  {
469  ss_states_to_termination_ = ss_states_to_termination_ - 1;
470  }
471 
472  if(current_stance_.previous_nonds_stance_type_ == StanceType::LSS)
473  {
474  current_stance_.type_ = StanceType::RSS;
475  current_stance_.duration_ms_ = sfsm_params_.getDurationMs(StanceType::RSS);
476  current_time_ms_ = 0;
477  }
478  else if(current_stance_.previous_nonds_stance_type_ == StanceType::RSS)
479  {
480  current_stance_.type_ = StanceType::LSS;
481  current_stance_.duration_ms_ = sfsm_params_.getDurationMs(StanceType::LSS);
482  current_time_ms_ = 0;
483  }
484  else
485  {
486  HUMOTO_THROW_MSG("FSM is in an incorrect state.");
487  }
488  }
489  break;
490 
491  case StanceType::DS:
492  current_stance_.previous_nontds_stance_type_ = current_stance_.type_;
493 
494  if(ss_states_to_termination_ == 0)
495  {
496  current_stance_.type_ = StanceType::DS;
497  current_stance_.duration_ms_ = sfsm_params_.getDurationMs(StanceType::DS);
498  current_time_ms_ = 0;
499  }
500  else
501  {
502  current_stance_.type_ = StanceType::TDS;
503  current_stance_.duration_ms_ = sfsm_params_.getDurationMs(StanceType::TDS);
504  current_time_ms_ = 0;
505  }
506  break;
507 
508  default:
509  HUMOTO_THROW_MSG("FSM is in an incorrect state.");
510  }
511  }
512 
513 
514  /**
515  * @brief Log
516  *
517  * @param[in,out] logger logger
518  * @param[in] parent parent
519  * @param[in] name name
520  */
521  void StanceFiniteStateMachine::log( humoto::Logger & logger HUMOTO_GLOBAL_LOGGER_IF_DEFINED,
522  const LogEntryName &parent = LogEntryName(),
523  const std::string &name = "stance_fsm") const
524  {
525  LogEntryName subname = parent; subname.add(name);
526 
527  current_stance_.log(logger, subname, "current_stance");
528  logger.log(LogEntryName(subname).add("ss_states_to_termination"), ss_states_to_termination_);
529  logger.log(LogEntryName(subname).add("current_time") , current_time_ms_ );
530  logger.log(LogEntryName(subname).add("is_tds_started") , is_tds_started_ );
531  }
532 
533  }//end namespace walking
534 }//end namespace humoto
535 
humoto::walking::determineSupportFoot
humoto::LeftOrRight::Type determineSupportFoot(const StanceType::Type &stance_type)
Determines support foot for the given single support stance.
Definition: stance_finite_state_machine.h:74
humoto::walking::StanceFiniteStateMachine::shiftTime
void shiftTime(const std::size_t shift_time_ms)
Shift time.
Definition: stance_finite_state_machine.h:408
humoto::walking::StanceType
Possible stance types of the model.
Definition: stance_finite_state_machine.h:20
HUMOTO_LOCAL
#define HUMOTO_LOCAL
Definition: export_import.h:26
humoto::walking::StanceSubType
Possible stance subtypes of the model.
Definition: stance_finite_state_machine.h:46
humoto::walking::StanceFiniteStateMachine::setParameters
void setParameters(const StanceFSMParameters &sfsm_params)
Definition: stance_finite_state_machine.h:299
HUMOTO_GLOBAL_LOGGER_IF_DEFINED
#define HUMOTO_GLOBAL_LOGGER_IF_DEFINED
Definition: logger.h:997
HUMOTO_ASSERT
#define HUMOTO_ASSERT(condition, message)
Definition: humoto_helpers.h:86
humoto::config::ConfigurableBase
Default configurable base is strict.
Definition: config.h:353
HUMOTO_THROW_MSG
#define HUMOTO_THROW_MSG(s)
HUMOTO_THROW_MSG throws an error message concatenated with the name of the function (if supported)...
Definition: humoto_helpers.h:77
humoto::LogEntryName
Represents log entry name.
Definition: logger.h:169
humoto::Logger
Threaded logger: any data sent to this logger is wrapped in a message and pushed to a queue...
Definition: logger.h:555
humoto::walking::StanceFSMParameters::setDefaults
void setDefaults()
Default parameters of the finite state machine.
Definition: stance_finite_state_machine.h:159
humoto::walking::Stance::log
void log(humoto::Logger &logger, const LogEntryName &parent=LogEntryName(), const std::string &name="stance") const
Log.
Definition: stance_finite_state_machine.h:211
humoto::walking::StanceFiniteStateMachine::previewStances
std::vector< Stance > previewStances(const std::size_t preview_duration_ms) const
Preview sequence of Stances of the FSM.
Definition: stance_finite_state_machine.h:316
humoto
The root namespace of HuMoTo.
Definition: config.h:12
humoto::walking::StanceFSMParameters::getDurationMs
std::size_t getDurationMs(const StanceType::Type stance_type) const
Returns duration of a stance.
Definition: stance_finite_state_machine.h:137
humoto::walking::StanceFiniteStateMachine::getNextStance
Stance getNextStance() const
Preview next walk state of the FSM.
Definition: stance_finite_state_machine.h:383
humoto::walking::StanceFiniteStateMachine
Finite state machine for walking. [initialize_contact_walk_fsm.m].
Definition: stance_finite_state_machine.h:232
humoto::LogEntryName::add
LogEntryName & add(const char *name)
extends entry name with a subname
Definition: logger.h:232
humoto::walking::StanceFiniteStateMachine::log
void log(humoto::Logger &, const LogEntryName &, const std::string &) const
Log.
Definition: stance_finite_state_machine.h:521
humoto::walking::StanceFSMParameters
Class containing parameters of the stance finite state machine.
Definition: stance_finite_state_machine.h:92
Generated on Fri Sep 29 2017 16:44:40 for humoto by   doxygen 1.8.13