pinocchio  3.1.0 A fast and flexible implementation of Rigid Body Dynamics algorithms and their analytical derivatives
foot_steps.py
1 class FootSteps(object):
2  '''
3  The class stores three functions of time: left, right and flying_foot.
4  Each function is piecewise constant. For each function, the user can ask
5  what is the value of this function at time t.
6
7  The storage is composed of three lists for left, right and flying_foot, and a list for time.
8  The list of times stores the time intervals, i.e. each element of the list is
9  the start of a time interval. The first element of the list is 0.
10  The value of the functions left, right, flying_foot one this time interval is stored at
11  the same position is their respective list (i.e. value of left on interval
12  [time[i], time[i+1]] is stored in left[i].
13
14  The 4 lists are set up using function add_phase().
15  The values of functions left, right, flying_foot can be accessed through the function
16  get_phase_type(t), get_left_position(t), get_right_position(t).
17  phase_type are 'left' (meaning left foot is flying, right foot is fixed), 'right' (ie the opposite)
18  or 'none' (meaning no foot is flying, both are fixed on the ground).
19
21  get_right_next_position(t) can be used to get the next position of the
22  flying foot (in that case, additional work is needed to compute the
23  position of flying foot at time t by interpolating get_left_position(t)
24  and get_left_next_position(t).
25
26  Functions get_phase_start(t), get_phase_duration(t) and get_phase_remaining(t)
27  can be used to get the starting time, the duration and the remaining time of the
28  current phase at time t.
29  '''
30
31  def __init__(self, right, left):
32  '''
33  The class is initiated from the initial positions of left and right feet.
34  '''
35  self.rightright = [right]
36  self.leftleft = [left]
37  self.timetime = [0.]
38  self.flying_footflying_foot = []
39
40  def add_phase(self, duration, foot, position=None):
41  '''
42  Add a phase lasting <duration> where the flyhing foot <foot> (either 'left' or 'right')
43  moves to <position> (being a vector or a SE3 placement).
44  Alternatively, <foot> might be set to 'none' (i.e double support). In that case, <position>
45  is not specified (or is set to None, default).
46  '''
47  assert(foot == 'left' or foot == 'right' or foot == 'none')
48  self.timetime.append(self.timetime[-1] + duration)
49  self.rightright.append(self.rightright[-1])
50  self.leftleft.append(self.leftleft[-1])
51  self.flying_footflying_foot.append(foot)
52  if foot == 'left':
53  self.leftleft[-1] = position
54  elif foot == 'right':
55  self.rightright[-1] = position
56
57  def get_index_from_time(self, t):
58  '''Return the index i of the interval containing t, i.e. t in time[i], time[i+1] '''
59  if t > self.timetime[-1]:
60  return len(self.timetime) - 1
61  return next(i for i, ti in enumerate(self.timetime) if ti > t) - 1
62
63  def get_phase_type(self, t):
64  i = self.get_index_from_timeget_index_from_time(t)
65  return self.flying_footflying_foot[i]
66
68  '''
69  Suppose that phase at time <t> is a double support phase.
70  Return True if the previous phase is left and/or the next phase is right.
71  '''
72  assert(self.get_phase_typeget_phase_type(t) == 'none')
73  i = self.get_index_from_timeget_index_from_time(t)
74  return self.flying_footflying_foot[i - 1] == 'left' if i > 0 else self.flying_footflying_foot[i + 1] == 'right'
75
76  def get_left_position(self, t):
77  return self.leftleft[self.get_index_from_timeget_index_from_time(t)]
78
79  def get_right_position(self, t):
80  return self.rightright[self.get_index_from_timeget_index_from_time(t)]
81
82  def get_left_next_position(self, t):
83  i = self.get_index_from_timeget_index_from_time(t)
84  i = i + 1 if i + 1 < len(self.timetime) else i
85  return self.leftleft[i]
86
87  def get_right_next_position(self, t):
88  i = self.get_index_from_timeget_index_from_time(t)
89  i = i + 1 if i + 1 < len(self.timetime) else i
90  return self.rightright[i]
91
92  def get_phase_start(self, t):
93  return self.timetime[self.get_index_from_timeget_index_from_time(t)]
94
95  def get_phase_duration(self, t):
96  i = self.get_index_from_timeget_index_from_time(t)
97  return self.timetime[i + 1] - self.timetime[i]
98
99  def get_phase_remaining(self, t):
100  return self.timetime[self.get_index_from_timeget_index_from_time(t) + 1] - t
def __init__(self, right, left)
Definition: foot_steps.py:31