-
Notifications
You must be signed in to change notification settings - Fork 0
/
Motion.py
36 lines (29 loc) · 1.09 KB
/
Motion.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
from Timer import *
import time
import math
class LegMotion(object):
def __init__(self, startValues, endValues, time = 5.0):
self.startValues = startValues
self.endValues = endValues
self.time = time
self.motionTimer = Timer()
self.start()
def start(self):
self.motionTimer.start()
def currentValues(self, dt):
return LegMotion.extrapolateBatch(self.startValues, self.endValues, self.motionTimer.elapsedTime() + dt, self.time)
def isDone(self):
return self.motionTimer.elapsedTime() > self.time
@staticmethod
def extrapolate(startValue, endValue, currentTime, totalTime):
if currentTime > totalTime:
return endValue
return startValue + (endValue - startValue) * (currentTime / totalTime)
@staticmethod
def extrapolateBatch(startValues, endValues, currentTime, totalTime):
if len(startValues) != len(endValues):
print("Error: startValues and endValues must have the same length in LegMotion.extrapolateBatch()")
res = []
for i in range(len(startValues)):
res.append(LegMotion.extrapolate(startValues[i], endValues[i], currentTime, totalTime))
return res