def getTrajectories(wArray):
c = np.linspace(0, 1, 10) # 30
h = 0.01
zValue, time = computePhase()
psiNorm = featureCalc(zValue, c, h)
trajectory = np.zeros((2, 100))
# wArray = np.zeros((y.shape[1], 50)) # 30
wArray = wArray.reshape((2, 10))
g = np.zeros((2, 1))
a = 1
b = 1
dt = time[1] - time[0]
for i in range(0, wArray.shape[0]):
# plt.plot(w)
# fTargetPredicted = psiNorm.dot(w)
# plt.plot(fTargetPredicted, 'g')
trajectory[i, :] = getTrajectory(psiNorm, wArray[i, :], g[i], a, b, 0,
dt)
# Above the function works the same way as the previous stage's main function.
length1 = 1
length2 = 1
# Here the program sets the leg length.
y = np.zeros((2, 100)) # This set the size of the co-ordinate array
y[0, :] = length1 * np.cos(trajectory[0, :]) + length2 * np.cos(
trajectory[0, :] + trajectory[1, :])
y[1, :] = length1 * np.sin(trajectory[0, :]) + length2 * np.sin(
trajectory[0, :] + trajectory[1, :])
# Above the x and y co-ordinates are calculated.
return trajectory, y, dt
psiVector = np.exp(-0.5 * ((zValue - c[i])**2) / h)
psiMatrix[:, i] = psiVector
# Above the program calculates each individual psi vector value.
for i in range(len(zValue)):
psiMatrix[i, :] = psiMatrix[i, :] / sum(psiMatrix[i, :])
for i in range(len(c)):
psiMatrix[:, i] = psiMatrix[:, i] * zValue
# Above the program normalises the psi matrix.
return psiMatrix # The function returns the normalised psi matrix array.
if __name__ == "__main__":
c = np.linspace(0, 1, 10) # The initialisation of the features.
h = 0.01 # The variance value.
zValue = computePhase(
) # Getting the phase values from the previous stage.
psiNorm = featureCalc(
zValue, c,
h) # Calls the feature calculation function to get the psi matrix.
plt.plot(psiNorm)
plt.ylabel('psi value')
plt.xlabel('t value')
plt.show()
# Above charts the psi matrix.
y[0] = y0
for i in range(0,psiNorm.shape[0]-1):
acceleration = a * (b * (g - y[i]) - velocity[i]) + f[i]
velocity[i + 1] = velocity[i] + (acceleration * dt)
y[i + 1] = y[i] + (velocity[i + 1] * dt)
return y
if __name__ == "__main__":
c = np.linspace(0, 1, 30)
h = 0.01
zValue, time = computePhase()
psiNorm = featureCalc(zValue, c, h)
y = np.sin(2 * np.pi * time) * np.exp(-time * 5)
g = y[-1]
a = 1
b = 1
dt = time[1] - time[0]
plt.figure()
plt.plot(time, y) #
# \/ \/ \/ create for loop \/ \/ \/
target = targetCalc(zValue, psiNorm, a, b, g, y, dt)
from robot_vars import *
import numpy as np
from hasimpy import *
import matplotlib.pyplot as plt
import matplotlib.animation as animation
from phase_calculation import computePhase
from feature_calculation import featureCalc, c, h
from run_compass_gait import *
def linAlg(psiNorm, a, F):
aVal = (((np.transpose(psiNorm))*psiNorm)+(a*np.eye(psiNorm.shape(1)))) # add power of minus one
bVal = (np.transpose(psiNorm)) * F
linAlg = np.linalg.solve(aVal,bVal)
return linAlg
zValue = computePhase()
psiNorm = featureCalc(zValue, c, h)
w = linAlg(psiNorm, alpha, F)
plt.plot(w)
plt.ylabel('I value')
plt.xlabel('t value')
plt.show()