def ConvertToAbs():
resAcc = []
resW = []
n = len(t)
for i in range(0, n):
m = EulerRotationMatrix(0, angle[i, 1], angle[i, 2])
acc2 = pylab.dot(m, acc[i, :])
resAcc = Utils.AddArrayLine(resAcc, acc2)
w2 = pylab.dot(m, w[i, :])
resW = Utils.AddArrayLine(resW, w2)
return resAcc, resW
def OdomSpeed():
res = []
for i in range(0, len(t)):
vx = deltaNP.dx[i] / tcycle
vy = deltaNP.dy[i] / tcycle
res = Utils.AddArrayLine(res, numpy.array([vx, vy]))
return res
def IntegratedSpeed():
speed = pylab.array([0.0, 0.0, 0.0])
res = []
for i in range(0, pylab.shape(ac2)[0] - 1):
speed = speed + ac2[i, :] * dt[i]
res = Utils.AddArrayLine(res, speed)
return res
