import numpy as np
from sympy import *
from pprint import pprint
import csv
from kinematic_model import KinematicModel
if __name__ == "__main__":
l, m, s = 1, 1, 1
d30 = 30. * pi / 180.
A0 = Matrix([[0], [-2], [0]])
A1 = Matrix([[2*cos(d30)], [2*sin(d30)], [0]])
A2 = Matrix([[-2*cos(d30)], [2*sin(d30)], [0]])
km = KinematicModel(A0, A1, A2, pi/2., pi+d30, -d30, l, m, s)
with open('training.csv', 'w') as training:
writer = csv.writer(training)
writer.writerow([
'theta0', 'theta1', 'theta2',
'phi0', 'phi1', 'phi2',
'centroid_x', 'centroid_y', 'centroid_z',
'normal_x', 'normal_y', 'normal_z',
'yaw', 'pitch'
])
i = 0
while i < 100:
try:
theta = np.random.rand(3) - 0.5
phi = km.solve_phi(theta)
fs = 10
theta_0 = 0.2 * pi/2.0 * sin(2.*pi*rate*v[0]/fs)
theta_1 = 0.2 * pi/2.0 * sin(2.*pi*rate*v[1]/fs + pi / 3.)
theta_2 = 0.2 * pi/2.0 * sin(2.*pi*rate*v[2]/fs + 2. * pi / 3.)
return np.array([theta_0, theta_1, theta_2])
if __name__ == "__main__":
l, m, s = 1, 1, 1
d30 = 30. * np.pi / 180.
A0 = Matrix([[0], [-2], [0]])
A1 = Matrix([[2*cos(d30)], [2*sin(d30)], [0]])
A2 = Matrix([[-2*cos(d30)], [2*sin(d30)], [0]])
km = KinematicModel(A0, A1, A2, np.pi/2., np.pi+d30, -d30, l, m, s)
t_domain = np.linspace(0, 10, 30)
theta = np.vstack([t_domain, t_domain, t_domain]).T
theta = np.apply_along_axis(motion, 1, theta)
plt.subplot(211)
plt.plot(t_domain, theta[:, 0], label="theta0")
plt.plot(t_domain, theta[:, 1], label="theta1")
plt.plot(t_domain, theta[:, 2], label="theta2")
plt.legend()
platform = np.empty([0, 3])
info = np.array([]).reshape(0, 3)
