示例#1
0
def polyline(Nbpoints, *args):
    n_arg = len(args)
    N = floor(Nbpoints / (n_arg - 1))
    t = np.linspace(0, 1, N)
    X1 = args[0].x * (1 - t) + args[1].x * t
    Y1 = args[0].y * (1 - t) + args[1].y * t
    points = [Point(X1[i], Y1[i]) for i in range(N)]
    for i in range(1, n_arg - 1):
        print(i)
        X1 = args[i].x * (1 - t) + args[i + 1].x * t
        Y1 = args[i].y * (1 - t) + args[i + 1].y * t
        points += [Point(X1[i], Y1[i]) for i in range(N)]
    return Path(points, 0.15, 0.9, p.SPEED_MAX)
示例#2
0
 def add_precise_path(self, distance, forward=True):
     self.forward = forward
     self.move = Trajectory.PRECISE_MOVE
     self.distance = distance
     self.is_stopped = False
     self.target = Point(self.robot.x + distance * cos(self.robot.theta),
                         self.robot.y + distance * sin(self.robot.theta))
示例#3
0
    def decelerate_to_low_speed(self, loop=False):
        look_ahead_distance = p.L0 + p.k * abs(self.robot.speed)

        # Test : look_ahead_distance égale à la distance de freinage à vitesse max
        # t_stop = current_robot.speed / p.MAX_ACCEL
        # dist_foresee = (current_robot.speed*t_stop-0.5*p.MAX_ACCEL*t_stop**2)

        # look_ahead_distance = max(self.look_min,dist_foresee)
        # print("L = {}, speed = {}".format(look_ahead_distance,current_robot.speed))

        p_robot = Point(self.robot.x, self.robot.y)
        theta = self.robot.theta

        if not loop:
            closest_point_index, p_goal_index, p_goal = self.path.find_goal_point(
                p_robot, look_ahead_distance)
        else:
            closest_point_index, p_goal_index, p_goal = self.path.find_goal_point_loop(
                p_robot, look_ahead_distance)

        x_body = -sin(theta) * (p_goal.x - p_robot.x) + cos(theta) * (
            p_goal.y - p_robot.y)

        # print(p_goal.x, p_goal.y)

        # dist_to_end = dist(p_robot, self.path.points[-1])

        # dist_to_end = dist(p_robot, self.path.points[-1])
        # dist_to_end = self.path.dists[-1]-self.path.dists[closest_point_index]

        # print("closest_index : ", closest_point_index)
        # print("goal_point_index : ", p_goal_index)
        # print("Goal : ({}, {})".format(p_goal.x, p_goal.y))

        # if p_goal == self.path.points[-1] :
        # speed_cons = max(0, self.previous_cons- p.MAX_ACCEL*p.NAVIGATOR_TIME_PERIOD)
        # else:
        # speed_cons = min(p.SPEED_MAX, self.previous_cons + p.MAX_ACCEL*p.NAVIGATOR_TIME_PERIOD)
        speed_cons = max(
            p.SPEED_MAX_DECER,
            abs(self.robot.speed) - p.MAX_ACCEL * p.NAVIGATOR_TIME_PERIOD)
        omega_cons = ((2 * abs(speed_cons)) /
                      (look_ahead_distance**2)) * x_body

        #print("Closest : {}, total : {}".format(closest_point, self.path.length))

        if speed_cons <= p.SPEED_MAX_DECER or dist(p_robot,
                                                   self.path.points[-1]) < 200:
            self.move_set = MoveSet.PATH_FINAL
            print("-------------------FINAL----------------------")

        if abs(omega_cons) > 4:
            self.move_set = Trajectory.ABORTING
            omega_cons = 0
            speed_cons = 0

        #print("speed : {}, L : {}, x : {}, omega : {}".format(speed_cons, look_ahead_distance, x_body, omega_cons))
        return omega_cons, speed_cons

        return omega_cons, speed_cons
示例#4
0
def line(Nbpoints, A, B):
    t = np.linspace(0, 1, Nbpoints)
    X1 = A.x * (1 - t) + B.x * t
    Y1 = A.y * (1 - t) + B.y * t
    path = Path([Point(X1[i], Y1[i]) for i in range(Nbpoints)], 0.15, 0.9,
                p.SPEED_MAX)
    return path
示例#5
0
 def add_forwarding(self, distance, forward=True):
     print("On forwarde")
     self.forward = forward
     self.move = Trajectory.FORWARDING
     self.move_set = MoveSet.FORWARDING_BEGIN
     self.distance = distance
     self.is_stopped = False
     self.target = Point(self.robot.x + distance * cos(self.robot.theta),
                         self.robot.y + distance * sin(self.robot.theta))
示例#6
0
def parametric_curve(Nbpoints, x_equation, y_equation):
    """
    Param t between 0 and 1.
    :param Nbpoints: int, nb of points of the path
    :param x_equation: a function f(t) for t in [0,1]
    :param y_equation: a function f(t) for t in [0,1]
    :return: path, class Path
    """
    t = np.linspace(0, 1, Nbpoints)
    X = [x_equation(t[i]) for i in range(Nbpoints)]
    Y = [y_equation(t[i]) for i in range(Nbpoints)]
    path = Path([Point(X[i], Y[i]) for i in range(Nbpoints)])
    return X, Y
示例#7
0
def circle(Nbpoints, C, r):
    """
    :param Nbpoints: int, nb of points of the path
    :param C:  cennter of the circle
    :param A: departure point of the path, element of the circle. ||C-A|| = R
    :return: path, class Path
    """
    x_c, y_c = C.x, C.y
    t = np.linspace(0, 1, Nbpoints)
    X = r * np.cos(2 * pi * t) + x_c
    Y = r * np.sin(2 * pi * t) + y_c
    path = Path([Point(X[i], Y[i]) for i in range(Nbpoints)], 0.15, 0.9,
                p.SPEED_MAX)
    return path
示例#8
0
def lemniscate(Nbpoints, a, b):
    t = np.linspace(0, 1, Nbpoints)
    X = a * np.sin(2 * np.pi * (t - 0.25)) + a
    Y = b * np.cos(2 * np.pi * (t - 0.25)) * np.sin(2 * np.pi * (t - 0.25))
    path = Path([Point(X[i], Y[i]) for i in range(Nbpoints)])
    return path