doc
d-practical-exercises
src
foot_steps.py
Go to the documentation of this file.
1
class
FootSteps
:
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
8
for time.
9
The list of times stores the time intervals, i.e. each element of the list is
10
the start of a time interval. The first element of the list is 0.
11
The value of the functions left, right, flying_foot one this time interval is stored
12
at the same position is their respective list (i.e. value of left on interval
13
[time[i], time[i+1]] is stored in left[i].
14
15
The 4 lists are set up using function add_phase().
16
The values of functions left, right, flying_foot can be accessed through the
17
function get_phase_type(t), get_left_position(t), get_right_position(t). phase_type
18
are 'left' (meaning left foot is flying, right foot is fixed), 'right' (ie the
19
opposite) or 'none' (meaning no foot is flying, both are fixed on the ground).
20
21
Additionnally, functions get_left_next_position(t),
22
get_right_next_position(t) can be used to get the next position of the
23
flying foot (in that case, additional work is needed to compute the
24
position of flying foot at time t by interpolating get_left_position(t)
25
and get_left_next_position(t).
26
27
Functions get_phase_start(t), get_phase_duration(t) and get_phase_remaining(t)
28
can be used to get the starting time, the duration and the remaining time of the
29
current phase at time t.
30
"""
31
32
def
__init__
(self, right, left):
33
"""
34
The class is initiated from the initial positions of left and right feet.
35
"""
36
self.
right
= [right]
37
self.
left
= [left]
38
self.
time
= [0.0]
39
self.
flying_foot
= []
40
41
def
add_phase
(self, duration, foot, position=None):
42
"""
43
Add a phase lasting <duration> where the flyhing foot <foot> (either 'left' or
44
'right') moves to <position> (being a vector or a SE3 placement).
45
Alternatively, <foot> might be set to 'none' (i.e double support). In that case,
46
<position> is not specified (or is set to None, default).
47
"""
48
assert
foot ==
"left"
or
foot ==
"right"
or
foot ==
"none"
49
self.
time
.
append
(self.
time
[-1] + duration)
50
self.
right
.
append
(self.
right
[-1])
51
self.
left
.
append
(self.
left
[-1])
52
self.
flying_foot
.
append
(foot)
53
if
foot ==
"left"
:
54
self.
left
[-1] = position
55
elif
foot ==
"right"
:
56
self.
right
[-1] = position
57
58
def
get_index_from_time
(self, t):
59
"""
60
Return the index i of the interval containing t, i.e. t in time[i], time[i+1]
61
"""
62
if
t > self.
time
[-1]:
63
return
len(self.
time
) - 1
64
return
next(i
for
i, ti
in
enumerate(self.
time
)
if
ti > t) - 1
65
66
def
get_phase_type
(self, t):
67
i = self.
get_index_from_time
(t)
68
return
self.
flying_foot
[i]
69
70
def
is_double_from_left_to_right
(self, t):
71
"""
72
Suppose that phase at time <t> is a double support phase.
73
Return True if the previous phase is left and/or the next phase is right.
74
"""
75
assert
self.
get_phase_type
(t) ==
"none"
76
i = self.
get_index_from_time
(t)
77
return
(
78
self.
flying_foot
[i - 1] ==
"left"
79
if
i > 0
80
else
self.
flying_foot
[i + 1] ==
"right"
81
)
82
83
def
get_left_position
(self, t):
84
return
self.
left
[self.
get_index_from_time
(t)]
85
86
def
get_right_position
(self, t):
87
return
self.
right
[self.
get_index_from_time
(t)]
88
89
def
get_left_next_position
(self, t):
90
i = self.
get_index_from_time
(t)
91
i = i + 1
if
i + 1 < len(self.
time
)
else
i
92
return
self.
left
[i]
93
94
def
get_right_next_position
(self, t):
95
i = self.
get_index_from_time
(t)
96
i = i + 1
if
i + 1 < len(self.
time
)
else
i
97
return
self.
right
[i]
98
99
def
get_phase_start
(self, t):
100
return
self.
time
[self.
get_index_from_time
(t)]
101
102
def
get_phase_duration
(self, t):
103
i = self.
get_index_from_time
(t)
104
return
self.
time
[i + 1] - self.
time
[i]
105
106
def
get_phase_remaining
(self, t):
107
return
self.
time
[self.
get_index_from_time
(t) + 1] - t
foot_steps.FootSteps
Definition:
foot_steps.py:1
foot_steps.FootSteps.get_left_next_position
def get_left_next_position(self, t)
Definition:
foot_steps.py:89
foot_steps.FootSteps.flying_foot
flying_foot
Definition:
foot_steps.py:39
foot_steps.FootSteps.get_phase_start
def get_phase_start(self, t)
Definition:
foot_steps.py:99
foot_steps.FootSteps.time
time
Definition:
foot_steps.py:38
foot_steps.FootSteps.get_phase_type
def get_phase_type(self, t)
Definition:
foot_steps.py:66
foot_steps.FootSteps.get_index_from_time
def get_index_from_time(self, t)
Definition:
foot_steps.py:58
foot_steps.FootSteps.get_phase_duration
def get_phase_duration(self, t)
Definition:
foot_steps.py:102
foot_steps.FootSteps.get_phase_remaining
def get_phase_remaining(self, t)
Definition:
foot_steps.py:106
foot_steps.FootSteps.get_left_position
def get_left_position(self, t)
Definition:
foot_steps.py:83
foot_steps.FootSteps.left
left
Definition:
foot_steps.py:37
foot_steps.FootSteps.right
right
Definition:
foot_steps.py:36
boost::fusion::append
result_of::push_front< V const, T >::type append(T const &t, V const &v)
Append the element T at the front of boost fusion vector V.
Definition:
fusion.hpp:32
foot_steps.FootSteps.get_right_next_position
def get_right_next_position(self, t)
Definition:
foot_steps.py:94
foot_steps.FootSteps.add_phase
def add_phase(self, duration, foot, position=None)
Definition:
foot_steps.py:41
foot_steps.FootSteps.__init__
def __init__(self, right, left)
Definition:
foot_steps.py:32
foot_steps.FootSteps.is_double_from_left_to_right
def is_double_from_left_to_right(self, t)
Definition:
foot_steps.py:70
foot_steps.FootSteps.get_right_position
def get_right_position(self, t)
Definition:
foot_steps.py:86
pinocchio
Author(s):
autogenerated on Sun Dec 22 2024 03:41:09