def __init__(self):
self.parent = 0
self.position = Vector2(0,0)
self.rotation = Vector2(0,0)
self.rotateArc = 0
self.forward = 1
self.f = 0
self.h = 0
self.hash = ""
def superGOGO(self,transformInfomation,obstacle_list):
self.position = Vector2(transformInfomation[0], transformInfomation[1])
self.rotation = Vector2(transformInfomation[2], transformInfomation[3])
shoes_position = Vector2(transformInfomation[4], transformInfomation[5])
shoes_rotation = Vector2(transformInfomation[6], transformInfomation[7])
goal_position = Vector2(transformInfomation[8], transformInfomation[9])
goal_rotation = Vector2(transformInfomation[10], transformInfomation[11])
goal1_pos = 0
goal1_rot = 0
goal2_pos = 0
goal2_rot = 0
if True : #not self.GETSTEP[0] :
goal1_pos = shoes_position + Vector2(-1*(SHOES_WIDTH /2 + ZUMO_LENGTH /2),0)
goal1_rot = shoes_rotation
r1 = Astar.FindRoute(self.position, self.rotation, goal1_pos, goal1_rot, obstacle_list)
#if self.position == goal1_pos and self.rotation == goal1_rot :
#self.GETSTEP[0] == True
if True : #self.GETSTEP[0] and not self.GETSTEP[1] :
goal2_pos = goal_position + Vector2(0, -1*(SHOES_LENGTH + ZUMO_LENGTH))
goal2_rot = goal_rotation
r2 = Astar.FindRoute(goal1_pos, goal1_rot, goal2_pos, goal2_rot, obstacle_list)
#if self.position == goal1_pos and self.rotation == goal1_rot :
# self.GETSTEP[1] == True
r1.extend(r2)
return r1
def bezier(t, points):
"""Calculate coordinate of a point in the bezier curve"""
n = len(points) - 1
x = y = 0
for i, pos in enumerate(points):
bern = bernstein(t, i, n)
x += pos.x * bern
y += pos.y * bern
return Vector2(x, y)
def gogoBezier(self,transformInfomation,):
self.position = Vector2(transformInfomation[0], transformInfomation[1])
self.rotation = Vector2(transformInfomation[2], transformInfomation[3])
shoes_position = Vector2(transformInfomation[4], transformInfomation[5])
shoes_rotation = Vector2(transformInfomation[6], transformInfomation[7])
target_position = Vector2(transformInfomation[8], transformInfomation[9])
target_rotation = Vector2(transformInfomation[10], transformInfomation[11])
goal1_pos = shoes_position + Vector2(-1 * (SHOES_WIDTH / 2 + ZUMO_LENGTH / 2), 0)
goal1_rot = shoes_rotation
goal2_pos = target_position + Vector2(0, -1 * (SHOES_LENGTH + ZUMO_LENGTH))
goal2_rot = target_rotation
r1 = Bezier.findRoute(self.position, self.rotation, goal1_pos, goal1_rot)
r2 = Bezier.findRoute(goal1_pos, goal1_rot, goal2_pos, goal2_rot)
r1.extend(r2)
return r1
def gogoSimple(self,transformInfomation):
#it always go forward first once and rotate once secondly
self.position = Vector2(transformInfomation[0], transformInfomation[1])
self.rotation = Vector2(transformInfomation[2], transformInfomation[3])
shoes_position = Vector2(transformInfomation[4], transformInfomation[5])
shoes_rotation = Vector2(transformInfomation[6], transformInfomation[7])
target_position = Vector2(transformInfomation[8], transformInfomation[9])
target_rotation = Vector2(transformInfomation[10], transformInfomation[11])
goal1_pos = shoes_position + Vector2(-1*(SHOES_WIDTH /2 + ZUMO_LENGTH /2),0)
goal1_rot = shoes_rotation
goal2_pos = target_position + Vector2(0, -1*(SHOES_LENGTH + ZUMO_LENGTH))
goal2_rot = target_rotation
self.route.append(self.position)
inter1 = Vector2(self.position.x, goal1_pos.y)
self.route.append(inter1)
distance = (inter1 - self.position).magnitude()
left = SPEED * 5
right = SPEED * 5
time = makeFour(int(distance / SPEED *1000))
self.action.append(["+100","+100",time])
self.action.append(rotateAction(-PI/2))
inter2 = Vector2 (goal2_pos.x - 0.5 * ZUMO_LENGTH, goal1_pos.y)
self.route.append(inter2)
distance = (inter2 - inter1 ).magnitude()
time = makeFour(int(distance / SPEED * 1000))
self.action.append(["+100","+100",time])
inter3 = inter2 + Vector2(-0.3 * ZUMO_LENGTH,0)
self.route.append(inter3)
distance = (inter3 - inter2).magnitude()
self.action.append(["-100","-100", makeFour(int (distance / SPEED * 1000 ))])
self.action.append(rotateAction(-PI/2))
inter4 = inter3 + Vector2(0, -1*(0.5 * SHOES_LENGTH + 0.8 * SHOES_WIDTH))
self.route.append(inter4)
distance = (inter4 - inter3).magnitude()
self.action.append(["+100","+100",makeFour( int (distance / SPEED * 1000 ))])
self.action.append(rotateAction(PI/2))
inter5 = Vector2(goal2_pos.x, inter4.y)
self.route.append(inter5)
distance = (inter5 - inter4).magnitude()
self.action.append(["+100","+100",makeFour(int (distance / SPEED * 1000 ))])
self.action.append(rotateAction(PI/2))
self.route.append(goal2_pos)
distance = (goal2_pos - inter5).magnitude()
self.action.append(["+100","+100",makeFour( int (distance / SPEED * 1000 ))])
def __init__(self):
self.position = Vector2(0,0)
self.rotation = Vector2(0,0)
self.route = list() #[target points ,target rotation]
self.action = list()
self.GETSTEP = [False, False, False] #arrive shoes ,
from Route import ZumoExtension
from Route.Vector2 import *
import matplotlib.pyplot as plt
zumo = ZumoExtension.ZumoExtension()
#zumo.gogoSimple([0,0, 0,1, 5,4, 0,1, 10,10, 0,1])
#zumo.printRoute()
#print(zumo.actionb
oo = [[Vector2(3, 5),
Vector2(4, 5),
Vector2(4, 8),
Vector2(3, 8)],
[
Vector2(1.5, 1.5),
Vector2(2.5, 1.5),
Vector2(2.5, 2.5),
Vector2(1.5, 2.5)
]]
ooo = [[Vector2(3, 2), Vector2(3, 3), Vector2(4, 1)]]
#res = zumo.superGOGO([0,0, 0,1, 60,40 ,1,0 , 80 ,100,1,0],[])
res = zumo.gogoBezier([0, 0, 0, 1, 60, 40, 1, 0, 80, 100, 0, 1])
x = []
y = []
#zumo.printRoute(res)
timer = 0
#zumo.gogoSimple([0,0, 0,1, 60,40 ,1,0 , 80 ,100,1,0])