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