-
Notifications
You must be signed in to change notification settings - Fork 2
/
gait.py
96 lines (66 loc) · 3.01 KB
/
gait.py
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
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
import math
import sys
import copy
SWING = 0
STANCE = 1
# A swing leg that collides with the ground.
EARLY_CONTACT = 2
# A stance leg that loses contact.
LOSE_CONTACT = 3
_STANCE_DURATION_SECONDS = [0.3] * 4
_DUTY_FACTOR = [0.6] * 4
_INIT_PHASE_FULL_CYCLE = [0.9, 0, 0, 0.9]
_INIT_LEG_STATE = [
SWING,
STANCE,
STANCE,
SWING,
]
class GaitGenerator:
def __init__(self, robot):
self._robot = robot
self._stance_duration = copy.deepcopy(_STANCE_DURATION_SECONDS)
self._duty_cycle = copy.deepcopy(_DUTY_FACTOR)
self._initial_leg_phase = copy.deepcopy(_INIT_PHASE_FULL_CYCLE)
self._initial_state_ratio_in_cycle = [0.0, 0.0, 0.0, 0.0]
self._init_leg_state = copy.deepcopy(_INIT_LEG_STATE)
self._next_leg_state = copy.deepcopy(_INIT_LEG_STATE)
self._desired_leg_state = copy.deepcopy(_INIT_LEG_STATE)
self._leg_state = copy.deepcopy(_INIT_LEG_STATE)
self._normalized_phase = [0.0]*4
self._contact_detection_phase_threshold = 0.1
for leg_id in range(4):
if self._init_leg_state[leg_id] == STANCE:
self._initial_state_ratio_in_cycle[leg_id] = self._duty_cycle[leg_id]
self._next_leg_state[leg_id] = SWING
elif self._init_leg_state[leg_id] == SWING:
self._initial_state_ratio_in_cycle[leg_id] = 1.0 - self._duty_cycle[leg_id]
self._next_leg_state[leg_id] = STANCE
def run(self, current_time):
leg_contact_state = self._robot.getFootContactState()
for leg_id in range(4):
full_cycle_period = (self._stance_duration[leg_id] /
self._duty_cycle[leg_id])
augmented_time = current_time + self._initial_leg_phase[
leg_id] * full_cycle_period
phase_in_full_cycle = math.fmod(augmented_time,
full_cycle_period) / full_cycle_period
ratio = self._initial_state_ratio_in_cycle[leg_id]
if phase_in_full_cycle < ratio:
self._desired_leg_state[leg_id] = self._init_leg_state[leg_id]
self._normalized_phase[leg_id] = phase_in_full_cycle / ratio
else:
# A phase switch happens for this leg.
self._desired_leg_state[leg_id] = self._next_leg_state[leg_id]
self._normalized_phase[leg_id] = (phase_in_full_cycle -
ratio) / (1 - ratio)
self._leg_state[leg_id] = self._desired_leg_state[leg_id]
if (self._normalized_phase[leg_id] <
self._contact_detection_phase_threshold):
continue
if (self._leg_state[leg_id] == SWING
and leg_contact_state[leg_id]):
self._leg_state[leg_id] = EARLY_CONTACT
if (self._leg_state[leg_id] == STANCE
and not leg_contact_state[leg_id]):
self._leg_state[leg_id] = LOSE_CONTACT