-
Notifications
You must be signed in to change notification settings - Fork 54
/
FSM_State_RecoveryStand.py
235 lines (188 loc) · 8.38 KB
/
FSM_State_RecoveryStand.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
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
214
215
216
217
218
219
220
221
222
223
224
225
226
227
228
229
230
231
232
233
234
235
from math import floor
import numpy as np
from MPC_Controller.FSM_states.ControlFSMData import ControlFSMData
from MPC_Controller.FSM_states.FSM_State import FSM_State, FSM_StateName
from MPC_Controller.Parameters import Parameters
from MPC_Controller.utils import DTYPE
StandUp = 0
FoldLegs = 1
RollOver = 2
class FSM_State_RecoveyrStand(FSM_State):
def __init__(self, _controlFSMData: ControlFSMData):
super().__init__(_controlFSMData, FSM_StateName.RECOVERY_STAND, "RECOVERY_STAND")
# Set the pre controls safety checks
self.checkSafeOrientation = False
# Post control safety checks
self.checkPDesFoot = False
self.checkForceFeedForward = False
self.iter = 0
self._state_iter = 0
self._motion_start_iter = 0
self._flag = FoldLegs
self.zero_vec3 = np.zeros((3,1), dtype=DTYPE)
# goal configuration
# Folding
# * test passed
self.fold_ramp_iter = int(45 / (Parameters.controller_dt*100))
self.fold_settle_iter = int(75 / (Parameters.controller_dt*100))
self.fold_jpos = np.array([0.0, 1.4, -2.7,
-0.0, 1.4, -2.7,
0.0, 1.4, -2.7,
-0.0, 1.4, -2.7],
dtype=DTYPE).reshape((4,3,1))
# Stand Up
# * test passed
self.standup_ramp_iter = int(30 / (Parameters.controller_dt*100))
self.standup_settle_iter = int(30 / (Parameters.controller_dt*100))
self.stand_jpos = np.array([0., 0.8, -1.6,
0., 0.8, -1.6,
0., 0.8, -1.6,
0., 0.8, -1.6],
dtype=DTYPE).reshape((4,3,1))
# Rolling
# * test passed
self.rollover_ramp_iter = int(13 / (Parameters.controller_dt*100))
self.rollover_settle_iter = int(15 / (Parameters.controller_dt*100))
self.rolling_jpos = np.array([1.3, 3.1, -2.77,
0.0, 1.6, -2.77,
1.3, 3.1, -2.77,
0.0, 1.6, -2.77],
dtype=DTYPE).reshape((4,3,1))
self.initial_jpos = np.zeros((4,3,1), dtype=DTYPE)
def onEnter(self):
# Default is to not transition
self.nextStateName = self.stateName
# Reset the transition data
self.transitionDone = False
# Reset iteration counter
self.iter = 0
self._state_iter = 0
# initial configuration, position
for leg in range(4):
self.initial_jpos[leg] = self._data._legController.datas[leg].q
body_height = self._data._stateEstimator.getResult().position[2]
self._flag = FoldLegs
if not self._UpsideDown():
if 0.2<body_height and body_height<0.45:
print("[Recovery Balance] body height is %.3f; Stand Up"%body_height)
self._flag = StandUp
else:
print("[Recovery Balance] body height is %.3f; Folding legs"%body_height)
else:
print("[Recovery Balance] UpsideDown (%d)"%self._UpsideDown())
self._motion_start_iter = 0
def run(self):
"""
* Calls the functions to be executed on each control loop iteration.
"""
if self._flag == StandUp:
self._StandUp(self._state_iter - self._motion_start_iter)
elif self._flag == FoldLegs:
self._FoldLegs(self._state_iter - self._motion_start_iter)
elif self._flag == RollOver:
self._RollOver(self._state_iter - self._motion_start_iter)
self._state_iter += 1
def onExit(self):
"""
* Cleans up the state information on exiting the state.
"""
# Nothing to clean up when exiting
pass
def checkTransition(self):
"""
* Manages which states can be transitioned into either by the user
* commands or state event triggers.
*
* @return the enumerated FSM state name to transition into
"""
self.nextStateName = self.stateName
self.iter += 1
# Switch FSM control mode
if Parameters.control_mode is FSM_StateName.RECOVERY_STAND:
pass
elif Parameters.control_mode is FSM_StateName.LOCOMOTION:
self.nextStateName = FSM_StateName.LOCOMOTION
elif Parameters.control_mode is FSM_StateName.PASSIVE:
self.nextStateName = FSM_StateName.PASSIVE
else:
print("[CONTROL FSM] Bad Request: Cannot transition from "
+ self.stateName.name
+ " to "
+ Parameters.control_mode.name)
return self.nextStateName
def transition(self):
"""
* Handles the actual transition for the robot between states.
* Returns true when the transition is completed.
*
* @return true if transition is complete
"""
# Finish Transition
if self.nextStateName==FSM_StateName.PASSIVE:
self.transitionDone = True
elif self.nextStateName==FSM_StateName.LOCOMOTION:
self.transitionDone = True
else:
print("[CONTROL FSM] Something went wrong in transition")
# Return the transition data to the FSM
return self.transitionDone
def _UpsideDown(self):
if self._data._stateEstimator.getResult().rBody[2,2]<0:
return True
return False
def _SetJPosInterPts(self,
curr_iter:int, max_iter:int, leg:int,
ini:np.ndarray, fin:np.ndarray):
a = 0.0
b = 1.0
# if we're done interpolating
if curr_iter <= max_iter:
b = float(curr_iter) / max_iter
a = 1.0 - b
# compute setpoints
inter_pos = a * ini + b * fin
# do control
self.jointPDControl(leg, inter_pos, self.zero_vec3)
def _StandUp(self, curr_iter:int):
body_height = self._data._quadruped._bodyHeight #self._data._stateEstimator.getResult().position[2]
something_wrong = False
if self._UpsideDown() or body_height<0.1:
something_wrong = True
if curr_iter > floor(self.standup_ramp_iter*0.7) and something_wrong:
# If body height is too low because of some reason
# even after the stand up motion is almost over
for leg in range(4):
self.initial_jpos[leg] = self._data._legController.datas[leg].q
self._flag = FoldLegs
self._motion_start_iter = self._state_iter + 1
print("[Recovery Balance - Warning] body height is still too low (%f) or UpsideDown (%d); Folding legs"
%(body_height, self._UpsideDown()))
else:
for leg in range(4):
self._SetJPosInterPts(curr_iter, self.standup_ramp_iter,
leg, self.initial_jpos[leg], self.stand_jpos[leg])
# setContactPhase = np.array([0.5,0.5,0.5,0.5], dtype=DTYPE).reshape((4,1))
# self._data._stateEstimator.setContactPhase(setContactPhase)
def _FoldLegs(self, curr_iter:int):
for leg in range(4):
self._SetJPosInterPts(curr_iter, self.rollover_ramp_iter, leg,
self.initial_jpos[leg], self.fold_jpos[leg])
if curr_iter >= self.fold_ramp_iter + self.fold_settle_iter:
if self._UpsideDown():
self._flag = RollOver
for leg in range(4):
self.initial_jpos[leg] = self.fold_jpos[leg]
else:
self._flag = StandUp
for leg in range(4):
self.initial_jpos[leg] = self.fold_jpos[leg]
self._motion_start_iter = self._state_iter + 1
def _RollOver(self, curr_iter:int):
for leg in range(4):
self._SetJPosInterPts(curr_iter, self.rollover_ramp_iter, leg,
self.initial_jpos[leg], self.rolling_jpos[leg])
if curr_iter > self.rollover_ramp_iter + self.rollover_settle_iter:
self._flag = FoldLegs
for leg in range(4):
self.initial_jpos[leg] = self.rolling_jpos[leg]
self._motion_start_iter = self._state_iter + 1