forked from unghee/Exoboot_Code
-
Notifications
You must be signed in to change notification settings - Fork 0
/
GroundContact_new.py
126 lines (92 loc) · 5.37 KB
/
GroundContact_new.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
import time
import numpy as np
from config import HS_THRESHOLD, TO_THRESHOLD
from utils import MovingAverageFilterPlus
class GroundContact:
def __init__(self):
self.inStance = False
self.TO_time = time.time()
self.HS_time = time.time()
self.time_in_stride = 0
# Track stride/stance periods
self.stance_period_filter = MovingAverageFilterPlus(cold_start=True, size=10) # cold_start=True start with empty buffer
self.stride_period_filter = MovingAverageFilterPlus(cold_start=True, size=10)
def update(self, force):
# Swing to stance transisition
if force > HS_THRESHOLD and not self.inStance:
new_HS_time = time.time()
self.time_in_stride = 0
self.inStance = True
self.HS_time = new_HS_time
stride_period = self.TO_time - self.HS_time
prev_stride_average = self.stride_period_filter.trimmed_average() # Historical estimate
# Add all periods if filter is cold (empty)
if not self.stride_period_filter.iswarm():
self.stride_period_filter.update(stride_period)
# Don't add period greater than +/-50% of previous stride period:
elif abs(stride_period - prev_stride_average) / prev_stride_average > 1.0:
self.stride_period_filter.update(stride_period)
self.HS_time = new_HS_time
# In stance
elif force > HS_THRESHOLD and self.inStance:
self.time_in_stride = time.time() - self.HS_time
# Stance to swing transition
elif force < TO_THRESHOLD and self.inStance:
self.TO_time = time.time()
self.time_in_stride = -1.0
self.inStance = False
stance_period = self.TO_time - self.HS_time
prev_stance_average = self.stance_period_filter.trimmed_average() # Historical estimate
# Add all periods if filter is cold (empty)
if not self.stride_period_filter.iswarm():
self.stride_period_filter.update(stance_period)
# Don't add period greater than +/-50% of previous stride period:
elif abs(stance_period - prev_stance_average) / prev_stance_average > 1.0:
self.stance_period_filter.update(stance_period)
# In swing
else:
self.time_in_stride = -1.0
return self.stance_period_filter.trimmed_average(), self.inStance, self.time_in_stride, self.stride_period_filter.trimmed_average()
# class GroundContact:
# def __init__(self):
# self.contact = False
# self.TO_time = time.time()
# self.HS_time = time.time()
# self.time_in_current_stance = 0
# self.stride_period_prev = 1.0 # Reasonable initial value
# self.stride_period_filter = MovingAverageFilterPlus(cold_start=True, size=10)
# self.stance_period_filter = MovingAverageFilterPlus(cold_start=True, size=10)
# def update(self, force):
# newContact = self.contact
# if self.contact: # if no state change, i.e. we are in contact
# # compute current time in stance
# self.time_in_current_stance = time.time() - self.HS_time
# if force < to_threshold: #there is no contact if the force is less than 20 N
# newContact = False
# else:
# if force >= hs_threshold: #there is a heel strike if force is greater than 50 N
# newContact = True
# # if newContact has changed to true, means heel-strike, otherwise toe-off
# if newContact != self.contact: # Detects a state change
# if newContact == True: # in this case we have a heel strike
# stride_period_bertec = time.time() - self.HS_time
# stride_average_prev = self.stride_period_filter.trimmed_average()
# # Add all periods if filter is cold (empty)
# if not self.stride_period_filter.iswarm():
# self.stride_period_filter.update(stride_period_bertec)
# # Don't add period greater than +/-50% of previous stride period:
# elif abs(stride_period_bertec - stride_average_prev) / stride_average_prev > 0.5:
# self.stride_period_filter.update(stride_period_bertec)
# self.HS_time = time.time()
# else: # in this case we have a toe off, so compute stance time
# self.TO_time = time.time()
# stance_period = self.TO_time - self.HS_time
# stance_average_prev = self.stance_period_filter.trimmed_average()
# # Add all periods if filter is cold (empty)
# if not self.stance_period_filter.iswarm():
# self.stride_period_filter.update(stance_period)
# # Don't add period greater than +/-50% of previous stride period:
# elif abs(stance_period - stance_average_prev) / stance_average_prev > 0.5:
# self.stance_period_filter.update(stance_period)
# self.contact = newContact # reset gait state to current gait state
# return self.stance_period_filter.trimmed_average(), newContact, self.time_in_current_stance, self.stride_period_filter.trimmed_average()