-
Notifications
You must be signed in to change notification settings - Fork 16
/
GaitPlanner.py
186 lines (143 loc) · 6.78 KB
/
GaitPlanner.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
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
import numpy as np
from math import pi, cos, sin, fmod, floor
"""
Note:
Order of legs is: FR, FL, BR, BL
"""
class GaitPlanner:
"""
Takes desired velocity and current state and outputs foot placements, CoM trajectory, and gait phase.
"""
def update(self, state, contacts, t):
"""
Output CoM trajectory and foot placements
"""
raise NotImplementedError
return (None, None, None, None, None, None) # Foot placements, CoM ref position, body ref orientation (euler), active_feet, phase
class StandingPlanner(GaitPlanner):
"""
Stands still!
"""
def update(self, state, contacts, t):
freq = 1.0
phase = t * 2 * pi * freq
p_ref = np.array([ sin(phase)*0.00,
cos(phase)*0.00,
sin(phase)*0.00 + 0.32])
rpy_ref = np.array([ sin(phase)*15*pi/180.0,
cos(phase)*25*pi/180.0 + 0*pi/180,
sin(phase)*0*pi/180.0])
# Boolean representation of which feet the QP controller treats as in contact with the ground.
# A 1 in the ith slot means that the ith leg is in contact with the ground
active_feet = np.array([1,1,1,1])
return (None, None, p_ref, rpy_ref, active_feet, phase)
class StepPlanner(GaitPlanner):
"""
Plans two walking-trot steps forward. During the first half of the stride,
the front-left and back-right legs are planned to be in stance. During the
second half of the stride, the front-right and back-left legs are planned
to be in stance. After the full stride, all legs are planned for stance.
"""
def update(self, state, contacts, t, woof_config, gait_config):
"""
STEP_LENGTH: step length in meters
D: duration of the total two-step move in seconds
phase: 0->0.5 & step_phase 0->1: moving FR and BL forward
phase: 0.5 -> 1 & step_phase 0->1: moving FL and BR forward
Note that the foot placements vector is only used as reference positions for the swing controller
which means that the reference foot placements are meaningless for legs in contact
"""
# stride starts at phase = 0 and ends at phase = 1
phase = t/gait_config.D
foot_height = woof_config.FOOT_RADIUS
foot_locs = np.array([ woof_config.LEG_FB, -woof_config.LEG_LR, foot_height,
woof_config.LEG_FB, woof_config.LEG_LR, foot_height,
-woof_config.LEG_FB, -woof_config.LEG_LR, foot_height,
-woof_config.LEG_FB, woof_config.LEG_LR, foot_height])
p_foot_locs = foot_locs
active_feet = np.array([1,1,1,1])
step_phase = 0
if phase >= 0 and phase < 0.5:
# Move FR and BL forward
foot_locs = np.array([ woof_config.LEG_FB + gait_config.STEP_LENGTH, -woof_config.LEG_LR, foot_height,
woof_config.LEG_FB, woof_config.LEG_LR, foot_height,
-woof_config.LEG_FB, -woof_config.LEG_LR, foot_height,
-woof_config.LEG_FB + gait_config.STEP_LENGTH, woof_config.LEG_LR, foot_height])
p_foot_locs = np.array([ woof_config.LEG_FB, -woof_config.LEG_LR, foot_height,
woof_config.LEG_FB, woof_config.LEG_LR, foot_height,
-woof_config.LEG_FB, -woof_config.LEG_LR, foot_height,
-woof_config.LEG_FB, woof_config.LEG_LR, foot_height])
active_feet = np.array([0,1,1,0])
step_phase = phase * 2.0
elif phase >= 0.5 and phase < 1.0:
# Move FL and BR forward
foot_locs = np.array([ woof_config.LEG_FB + gait_config.STEP_LENGTH, -woof_config.LEG_LR, foot_height,
woof_config.LEG_FB + gait_config.STEP_LENGTH, woof_config.LEG_LR, foot_height,
-woof_config.LEG_FB + gait_config.STEP_LENGTH, -woof_config.LEG_LR, foot_height,
-woof_config.LEG_FB + gait_config.STEP_LENGTH, woof_config.LEG_LR, foot_height])
p_foot_locs = np.array([ woof_config.LEG_FB + gait_config.STEP_LENGTH, -woof_config.LEG_LR, foot_height,
woof_config.LEG_FB, woof_config.LEG_LR, foot_height,
-woof_config.LEG_FB, -woof_config.LEG_LR, foot_height,
-woof_config.LEG_FB + gait_config.STEP_LENGTH, woof_config.LEG_LR, foot_height])
active_feet = np.array([1,0,0,1])
step_phase = (phase - 0.5) * 2.0
elif phase >= 1.0:
# All feet are forward
foot_locs = np.array([ woof_config.LEG_FB + gait_config.STEP_LENGTH, -woof_config.LEG_LR, foot_height,
woof_config.LEG_FB + gait_config.STEP_LENGTH, woof_config.LEG_LR, foot_height,
-woof_config.LEG_FB + gait_config.STEP_LENGTH, -woof_config.LEG_LR, foot_height,
-woof_config.LEG_FB + gait_config.STEP_LENGTH, woof_config.LEG_LR, foot_height])
p_foot_locs = foot_locs
active_feet = np.array([1,1,1,1])
step_phase = 0.0
# Want body to be level during the step
rpy_ref = np.array([0,0,0])
# Want the body to move forward one step length
CoM_x = gait_config.STEP_LENGTH * np.clip(phase,0,1)
p_ref = np.array([CoM_x, 0, woof_config.LEG_L])
# print("foot placements:")
# print(foot_locs)
return (foot_locs, p_foot_locs, p_ref, rpy_ref, active_feet, phase, step_phase)
class TrotPlanner(GaitPlanner):
"""
Plans two walking-trot steps forward. During the first half of the stride,
the front-left and back-right legs are planned to be in stance. During the
second half of the stride, the front-right and back-left legs are planned
to be in stance. After the full stride, all legs are planned for stance.
"""
def update(self, state, contacts, t, woof_config, gait_config):
"""
STEP_LENGTH: step length in meters
D: duration of the total two-step move in seconds
phase: 0->0.5 & step_phase 0->1: moving FR and BL forward
phase: 0.5 -> 1 & step_phase 0->1: moving FL and BR forward
Note that the foot placements vector is only used as reference positions for the swing controller
which means that the reference foot placements are meaningless for legs in contact
"""
# total phase represents a fractional number of steps completed
total_phase = t/gait_config.D
gait_phase = fmod(total_phase,1.0)
step_number = floor(total_phase)
foot_height = woof_config.FOOT_RADIUS
START_LOCS = np.array([ woof_config.LEG_FB, -woof_config.LEG_LR, foot_height,
woof_config.LEG_FB, woof_config.LEG_LR, foot_height,
-woof_config.LEG_FB, -woof_config.LEG_LR, foot_height,
-woof_config.LEG_FB, woof_config.LEG_LR, foot_height])
foot_locs = foot_locs_start()
p_foot_locs = foot_locs
active_feet = np.array([1,1,1,1])
step_phase = 0
if gait_phase >= 0 and gait_phase < 0.5:
active_feet = np.array([0,1,1,0])
step_phase = phase * 2.0
elif phase >= 0.5 and phase < 1.0:
active_feet = np.array([1,0,0,1])
step_phase = (phase - 0.5) * 2.0
# Want body to be level during the step
rpy_ref = np.array([0,0,0])
# Want the body to move forward one step length
CoM_x = gait_config.STEP_LENGTH * total_phase
p_ref = np.array([CoM_x, 0, woof_config.LEG_L])
# print("foot placements:")
# print(foot_locs)
return (foot_locs, p_foot_locs, p_ref, rpy_ref, active_feet, phase, step_phase)