-
Notifications
You must be signed in to change notification settings - Fork 0
/
continuous_trajectory.py
105 lines (85 loc) · 2.58 KB
/
continuous_trajectory.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
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
import math
from math import *
import numpy as np
from numpy import *
import matplotlib.pyplot as plt
from mpl_toolkits.mplot3d import Axes3D
from mpl_toolkits.mplot3d.art3d import Poly3DCollection
import time
fig = plt.figure()
ax = Axes3D(fig)
#######################################################################################
t = 0
time_res = 0.05
t_temp = TIME = 0
steps = 0
(X_l, Y_l, Z_l) = (0, 0, 0)
foot_left_i = [ 3, 0, 0]
def step_mark(xl,yl,color):
global foot_size
foot_size = 2
lx = [xl-1.5 ,xl+1.5, xl+1.5, xl-1.5]
ly = [yl-2 ,yl-2, yl+2, yl+2]
lz = [0,0,0,0]
verts = [list(zip(lx,ly,lz))]
ax.add_collection3d(Poly3DCollection(verts,alpha = 0.3,color = color))
def Trajectory(X,Y,H,T):
global t_temp,track_l,trail,steps,X_traverse,Y_traverse,Z_traverse
a = (8*H)/(T**3)
b = -(16*H)/(T**2)
c = (8*H)/(T)
d = 0
if t_temp <= T:
Z_traverse = a*(t_temp**3) + b*(t_temp**2) + c*t_temp
X_traverse = X*(((t_temp/T) - (1/(2*pi))*cos((2*pi*t_temp)/T - pi/2 )))
Y_traverse = Y*(((t_temp/T) - (1/(2*pi))*cos((2*pi*t_temp)/T - pi/2 )))
# ax.scatter(X_traverse,Y_traverse,Z_traverse,c = 'red',marker = 'o')
t_temp+=time_res
# track_l.append([[X_traverse],[Y_traverse],[Z_traverse]])
# print(track_l)
#trail(track_l,'red')
else:
t_temp = 0
steps += 1
return steps, [X_traverse, Y_traverse, Z_traverse]
def step(pos0,pos,leg):
global X_l, Y_l, Z_l, X_r, Y_r, Z_r
if steps >= 0:
for i in range(0,1):
#Trajectory(5,5,2,1)
traject = Trajectory(pos[0]-pos0[0] ,pos[1]-pos0[1], pos[2], pos[3])[1]
#print(traject)
if leg == 'L':
X_l = traject[0]
Y_l = traject[1]
Z_l = traject[2]
return [X_l + pos0[0], Y_l + pos0[1], Z_l]
while(t < 2.3):
ax.plot([ 30] ,[-30] ,[ -1] )
ax.plot([-30] ,[ 30] ,[ 20] )
pos0 = 0 ,0 ,2 ,0.5
pos1 = 0 ,10 ,2 ,0.5
pos2 = 10,10 ,2 ,0.5
pos3 = 10,0 ,2 ,0.5
print(sign(0))
if steps == 0:
ff = step(pos0, pos1, 'L')
if steps == 1:
ff = step(pos1, pos2, 'L')
if steps == 2:
ff = step(pos2, pos3, 'L')
if steps == 3:
ff = step(pos3, pos0, 'L')
print(ff)
foot_left = ff
print("steps : ", steps)
ax.scatter(foot_left[0] , foot_left[1], foot_left[2], c = 'red',marker = 'o')
step_mark(0,0,'green')
step_mark(0,10,'green')
step_mark(10,0,'green')
step_mark(10,10,'green')
ax.grid()
plt.pause(0.000000000000000001)
ax.cla()
t += time_res
TIME += 0.175