def create_line(p0, p1, width): line = Line(p0, p1) line.setWidth(width) color = color_rgb(200, 200, 200) line.setOutline(color) line.setArrow("last") return line
class Goal: def __init__(self, vel, pos, win): self.vel_x, self.vel_y = vel[0], vel[1] self.vel = np.array([self.vel_x, self.vel_y]) self.pos_x = pos[0] self.pos_y = pos[1] self.win = win def set_graphicals(self): # draw player self.body = Circle(Point(scale(self.pos_x), scale(self.pos_y)), 7) self.body.setFill('red') # Note: downwards in Y is the positive direction for this graphics lib self.arrow = Line( Point(scale(self.pos_x), scale(self.pos_y)), Point(scale(self.pos_x + self.vel_x), scale(self.pos_y + self.vel_y))) self.arrow.setFill('black') self.arrow.setArrow('last') self.body.draw(self.win) self.arrow.draw(self.win)
def drawAxis(win, new_view): L1 = Line(Point(0, 0), Point(new_view.xVmax, 0)) L2 = Line(Point(0, 0), Point(0, new_view.yVmax)) L3 = Line(Point(0, 0), Point(new_view.xVmin, new_view.yVmin)) L1.setFill('blue') L2.setFill('blue') L3.setFill('blue') L1.setArrow("last") L2.setArrow("last") L3.setArrow("last") L1.draw(win) L2.draw(win) L3.draw(win) return win
class DynamicPoint: def __init__(self, vel_start, pos_start, dt, vel_max, acc_max, win): # dynamics related self.vel_start = np.array(vel_start) self.pos_x = pos_start[0] self.pos_y = pos_start[1] self.dt = dt self.vel_max = vel_max self.acc_max = acc_max self.dist_max = vel_max * dt # path planning related self.finished = False self.path = None self.sling_path = None self.sling_path_calculated = None self.sling_vel = None self.sling_acc = None self.node_count = 0 self.node_count_sling = 0 self.at_node = True self.total_time = 0 # graphics related self.body = None self.body_radius = 10 self.vel_arrow = None self.acc_arrow = None self.win = win self.sling_path_drawables = [] self.get_trace = False # Change this to see the car in action or just to see the trace if self.get_trace is True: self.body_radius /= 10 # for actions self.current_action = None self.current_acc = np.array([0, 0], dtype=float) self.current_vel = np.array(vel_start, dtype=float) self.new_action = True #Flag if new action is received self.arc_length = 0 self.precision = 0.4 self.position_error = 0 # for circle/arc parameters self.theta = 0 self.T = 0 self.n = 0 self.beta = 0 self.circle = None def set_velocity(self, goal): self.total_time += self.dt # check if we have reached the next node self.position_error = math.sqrt( (self.pos_x - self.sling_path[-1][0].x)**2 + (self.pos_y - self.sling_path[-1][0].y)**2) #print(self.position_error, self.n) if self.position_error < self.precision or self.n < 0: # we have reached the next one, but is it # the goal node? if (len(self.sling_path) == 1): self.finished = True return else: self.current_action = self.sling_path.pop() self.pos_x = self.current_action[0].x self.pos_y = self.current_action[0].y vel_dummy = self.sling_vel.pop() #TODO Setting velocity to dirct to the next point if self.current_action[1] == 0: direc = 0 if self.sling_path[-1][0].x - self.pos_x < 0: direc = math.pi + math.atan( (self.sling_path[-1][0].y - self.pos_y) / (self.sling_path[-1][0].x - self.pos_x)) else: direc = math.atan( (self.sling_path[-1][0].y - self.pos_y) / (self.sling_path[-1][0].x - self.pos_x)) vel_mag = math.sqrt( np.dot(self.current_vel, self.current_vel)) self.current_vel[0] = vel_mag * math.cos(direc) self.current_vel[1] = vel_mag * math.sin(direc) self.current_acc = self.sling_acc.pop() self.new_action = True # are we going into a circle? if (self.current_action[1] != 0): if self.new_action: #self.set_circle_params() #self.circle = DubinCircle.fromArc(self.current_action[0], self.sling_path[-1][0], self.current_action[1]) #print("Status: ",self.current_action[0], self.current_action[1], self.current_vel) self.circle = DubinCircle.fromVel(self.current_action[0], self.current_action[1], self.current_vel) self.T = self.circle.arclength(self.current_action[0], self.sling_path[-1][0]) / \ math.sqrt(np.dot(self.current_vel, self.current_vel)) self.theta = self.circle.arcangle(self.current_action[0], self.sling_path[-1][0]) #if self.theta<0: # self.theta = 2*math.pi + self.theta self.n = self.T / self.dt self.beta = self.theta / self.n self.new_action = False self.move_circular() else: # we are moving straight if self.new_action: angle = atan(self.current_vel[1] / self.current_vel[0]) acc_mag = sqrt(np.dot(self.current_acc, self.current_acc)) vel_mag1 = sqrt(np.dot(self.current_vel, self.current_vel)) vel_mag2 = sqrt(np.dot(self.sling_vel[-1], self.sling_vel[-1])) #print("vel1", vel_mag1, "vel2", vel_mag2) acc_sign = 1 if (vel_mag2 < vel_mag1): acc_sign = -1 self.current_acc[0] = acc_sign * acc_mag * cos(angle) self.current_acc[1] = acc_sign * acc_mag * sin(angle) #self.current_acc = find_acc(self.current_vel, self.sling_vel[-1], # self.current_action[0].dist_to(self.sling_path[-1][0])) self.new_action = False self.move() #print("Velocity:", math.sqrt(np.dot(self.current_vel,self.current_vel)), # "Accel:", self.current_acc) #self.total_time += self.dt def move_circular(self): # TODO: sign fix rotation_mat = np.array([[cos(self.beta), sin(self.beta)], [-sin(self.beta), cos(self.beta)]]) position_vect = np.array( [self.pos_x - self.circle.c.x, self.pos_y - self.circle.c.y]) position_vect = position_vect @ rotation_mat self.pos_x = position_vect[0] + self.circle.c.x self.pos_y = position_vect[1] + self.circle.c.y #self.pos_x = (self.pos_x - self.circle.c.x) * cos(self.beta) - \ # (self.pos_y - self.circle.c.y) * sin(self.beta) + self.circle.c.x #self.pos_y = (self.pos_x - self.circle.c.x) * sin(self.beta) + \ # (self.pos_y - self.circle.c.y) * cos(self.beta) + self.circle.c.y #print("beta: ",self.beta,"radius: ",(self.pos_x-self.circle.c.x)**2 + (self.pos_y-self.circle.c.y)**2) self.current_vel = self.current_vel @ rotation_mat angle = math.atan2(-self.current_vel[0], self.current_vel[1]) self.current_acc[0] = self.acc_max * cos(angle) self.current_acc[1] = self.acc_max * sin(angle) self.n -= 1 #self.set_graphicals() def move(self): #print(self.current_vel) self.current_vel[ 0] = self.current_vel[0] + self.current_acc[0] * self.dt self.current_vel[ 1] = self.current_vel[1] + self.current_acc[1] * self.dt self.pos_x += self.dt * self.current_vel[0] self.pos_y += self.dt * self.current_vel[1] #self.set_graphicals() def add_path(self, path): # A path is a list of nodes self.path = path self.node_count = len(path) def add_sling_path(self, goal, obstacles=None): # A path is a list of nodes vel_series = get_velocity_series(self.path, self.vel_start, goal.vel, self.vel_max) acc_series = get_acceleration_series(vel_series, self.acc_max) self.sling_path, self.sling_vel, self.sling_acc = create_sling_path( self.path, vel_series, acc_series, obstacles=None) #TODO: set it back to obstacles if not self.sling_path: return False self.sling_path_calculated = self.sling_path #print("Path Generated : ", self.sling_path_calculated) self.node_count_sling = len(self.sling_path) self.sling_path = [el for el in reversed(self.sling_path)] self.sling_vel = [el for el in reversed(self.sling_vel)] self.sling_acc = [el for el in reversed(self.sling_acc)] ''' if self.sling_path_calculated is not None: self.sling_path_drawables = [Circle(action[0].get_scaled_point(), 3) for action in self.sling_path_calculated] for el in self.sling_path_drawables: el.draw(self.win) for el in self.sling_path: cir = Circle(el[0].get_scaled_point(), 3) cir.draw(self.win) ''' #for action in self.sling_path_calculated: # self.body = Circle(action[0].get_scaled_point(), self.body_radius) # self.body.setFill('yellow') # self.body.draw(self.win) def set_graphicals(self): draw_x = scale(self.pos_x) draw_y = scale(self.pos_y) if self.circle is not None and self.get_trace is False: dubinc = Circle(self.circle.c.get_scaled_point(), scale_vectors(self.circle.r)) dubinc.setOutline('Green') dubinc.draw(self.win) if self.body is not None and self.get_trace is False: self.body.undraw() self.body = Circle(Point(draw_x, draw_y), self.body_radius) self.body.setFill('yellow') self.body.draw(self.win) if self.vel_arrow: self.vel_arrow.undraw() self.vel_arrow = Line( Point(draw_x, draw_y), Point(scale(self.pos_x + self.current_vel[0]), scale(self.pos_y + self.current_vel[1]))) self.vel_arrow.setFill('black') self.vel_arrow.setArrow("last") self.vel_arrow.draw(self.win) if self.acc_arrow: self.acc_arrow.undraw() self.acc_arrow = Line( Point(draw_x, draw_y), Point(scale(self.pos_x + self.current_acc[0] * 5), scale(self.pos_y + self.current_acc[1] * 5))) self.acc_arrow.setFill('blue') self.acc_arrow.setArrow('last') self.acc_arrow.draw(self.win)
class DynamicPointTest: def __init__(self, vel_start, pos_start, dt, vel_max, acc_max, win): # dynamics related self.vel_start = np.array(vel_start) self.pos_x = pos_start[0] self.pos_y = pos_start[1] self.dt = 0.05 self.vel_max = vel_max self.acc_max = acc_max self.dist_max = vel_max * dt # path planning related self.finished = False self.path = None self.sling_path = None self.sling_path_calculated = None self.sling_vel = None self.sling_acc = None self.node_count = 0 self.node_count_sling = 0 self.at_node = True self.total_time = 0 # graphics related self.body = None self.body_radius = 10 self.vel_arrow = None self.acc_arrow = None self.win = win # for actions self.current_action = None self.current_acc = None self.current_vel = None self.new_action = True #Flag if new action is received self.arc_length = 0 # for circle/arc parameters self.theta = 0 self.T = 0 self.n = 0 self.beta = 0 self.circle = None def set_velocity(self, goal): # check if we have reached the next node if self.pos_x == self.sling_path[-1][ 0].x and self.pos_y == self.sling_path[-1][0].y or self.n == 0: # we have reached the next one, but is it # the goal node? if (len(self.sling_path) == 1): self.finished = True return else: self.current_action = self.sling_path.pop() self.current_vel = self.sling_vel.pop() self.current_acc = self.sling_acc.pop() self.new_action = True # are we going into a circle? if (self.current_action[1] != 0): if self.new_action: self.set_circle_params() self.T = self.arc_length / math.sqrt(self.current_vel[0]**2 + self.current_vel[1]**2) self.n = math.floor(self.T / self.dt) self.beta = self.theta / self.n self.new_action = False self.move_circular() else: # we are moving straight self.move() self.total_time += self.dt def move_circular(self): angle = atan(-self.current_vel[0] / self.current_vel[1]) self.current_acc[0] = self.current_acc * cos(angle) self.current_acc[1] = self.current_acc * sin(angle) rotation_mat = np.array([[cos(self.beta), sin(self.beta)], [-sin(self.beta), cos(self.beta)]]) self.current_vel = self.current_vel * rotation_mat self.pos_x = (self.pos_x - self.circle.c.x) * cos(self.beta) - ( self.pos_y - self.circle.c.y) * sin(self.beta) + self.circle.c.x self.pos_y = (self.pos_x - self.circle.c.x) * sin(self.beta) + ( self.pos_y - self.circle.c.y) * cos(self.beta) + self.circle.c.y self.n -= 1 self.set_graphicals() def move(self): angle = atan(self.current_vel[1] / self.current_vel[0]) self.current_acc[0] = self.current_acc * cos(angle) self.current_acc[1] = self.current_acc * sin(angle) self.current_vel[ 0] = self.current_vel[0] + self.current_acc[0] * self.dt self.current_vel[ 1] = self.current_vel[1] + self.current_acc[1] * self.dt self.pos_x += self.dt * self.current_vel[0] self.pos_y += self.dt * self.current_vel[1] self.set_graphicals() def add_path(self, path): # A path is a list of nodes self.path = path self.node_count = len(path) def add_sling_path(self, goal): # A path is a list of nodes vel_series = get_velocity_series(self.path, self.vel_start, goal.vel, self.vel_max) acc_series = get_acceleration_series(self.path, self.acc_max) self.sling_path, self.sling_vel, self.sling_acc = create_sling_path( self.path, vel_series, acc_series) self.sling_path_calculated = self.sling_path print("Path Generated : ", self.sling_path_calculated) self.node_count_sling = len(self.sling_path) self.sling_path = [el for el in reversed(self.sling_path)] self.sling_vel = [el for el in reversed(self.sling_vel)] self.sling_acc = [el for el in reversed(self.sling_acc)] def set_circle_params(self): p1, p2 = self.current_action[0], self.sling_path[-1][0] vel_1 = self.current_vel dir_1 = self.current_action[1] slope = -vel_1[0] / vel_1[1] radius = abs(1 / dir_1) k = radius / math.sqrt(1 + slope**2) centre = Node(p1.x + k, p1.y + k * slope) circle = DubinCircle(centre, radius, dir_1) theta = circle.arcangle(p1, p2) self.arc_length = circle.arclength(p1, p2) self.theta = theta self.circle = circle def set_graphicals(self): draw_x = scale(self.pos_x) draw_y = scale(self.pos_y) # Draw the new path if self.sling_path_calculated is not None: for action in self.sling_path_calculated: cir = Circle(action[0].get_scaled_point(), self.body_radius) cir.setFill('yellow') cir.draw(self.win) if self.circle is not None: dubinc = Circle(self.circle.c.get_scaled_point(), scale_vectors(self.circle.r)) dubinc.setOutline('Green') dubinc.draw(self.win) if self.body: self.body.undraw() self.body = Circle(Point(draw_x, draw_y), self.body_radius) self.body.setFill('yellow') self.body.draw(self.win) if self.vel_arrow: self.vel_arrow.undraw() self.vel_arrow = Line( Point(draw_x, draw_y), Point(scale(self.pos_x + self.current_vel[0] * 5), scale(self.pos_y + self.current_vel[1] * 5))) self.vel_arrow.setFill('black') self.vel_arrow.setArrow("last") self.vel_arrow.draw(self.win) if self.acc_arrow: self.acc_arrow.undraw() self.acc_arrow = Line( Point(draw_x, draw_y), Point(scale(self.pos_x + self.current_acc[0] * 5), scale(self.pos_y + self.current_acc[1] * 5))) self.acc_arrow.setFill('blue') self.acc_arrow.setArrow('last') self.acc_arrow.draw(self.win) '''