forked from Nate711/PupperPythonSim
-
Notifications
You must be signed in to change notification settings - Fork 0
Expand file tree
/
Copy pathGaits.py
More file actions
66 lines (56 loc) · 1.79 KB
/
Copy pathGaits.py
File metadata and controls
66 lines (56 loc) · 1.79 KB
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
def phase_index(ticks, gaitparams):
"""Calculates which part of the gait cycle the robot should be in given the time in ticks.
Parameters
----------
ticks : int
Number of timesteps since the program started
gaitparams : GaitParams
GaitParams object
Returns
-------
Int
The index of the gait phase that the robot should be in.
"""
phase_time = ticks % gaitparams.phase_length
phase_sum = 0
for i in range(gaitparams.num_phases):
phase_sum += gaitparams.phase_times[i]
if phase_time < phase_sum:
return i
assert False
def subphase_time(ticks, gaitparams):
"""Calculates the number of ticks (timesteps) since the start of the current phase.
Parameters
----------
ticks : Int
Number of timesteps since the program started
gaitparams : GaitParams
GaitParams object
Returns
-------
Int
Number of ticks since the start of the current phase.
"""
phase_time = ticks % gaitparams.phase_length
phase_sum = 0
subphase_t = 0
for i in range(gaitparams.num_phases):
phase_sum += gaitparams.phase_times[i]
if phase_time < phase_sum:
subphase_t = phase_time - phase_sum + gaitparams.phase_times[i]
return subphase_t
assert False
def contacts(ticks, gaitparams):
"""Calculates which feet should be in contact at the given number of ticks
Parameters
----------
ticks : Int
Number of timesteps since the program started.
gaitparams : GaitParams
GaitParams object
Returns
-------
numpy array (4,)
Numpy vector with 0 indicating flight and 1 indicating stance.
"""
return gaitparams.contact_phases[:, phase_index(ticks, gaitparams)]