Exemple #1
0
def move_to_point(x_start, y_start, theta_start, x_goal, y_goal):
    #These values work fine, but try tuning them once your simulation is working
    #to see if you can improve how quickly your robot reaches the goal.
    Kh = 10
    Kv = 5

    dt = 0.01

    vehicle = SimpleVehicle(x_start, y_start, theta_start, 1)

    x = x_start
    y = y_start
    theta = theta_start

    while True:
        #############YOUR CODE GOES HERE#############

        x_goal = vehicle.goal[0]
        y_goal = vehicle.goal[1]

        x_diff = x_goal - x
        y_diff = y_goal - y
        distance = sqrt(x_diff**2 + y_diff**2)
        theta_goal = atan2(y_diff, x_diff)
        theta = theta + Kh * ang_diff(theta_goal, theta) * dt

        v = Kv * distance
        vx = v * cos(theta)
        vy = v * sin(theta)

        x = x + vx * (dt)
        y = y + vy * (dt)

        vehicle.update_pose(x, y, theta)
        vehicle.plot(xlims=[-10, 10], ylims=[-10, 10])
def pure_pursuit(x_start, y_start, theta_start):
        Kv = 3
	Kh = 6
	Ki = 0.00001
	d = 1
	dt = 0.1
	A = 5
        dx = .1
        
	vehicle = SimpleVehicle(x_start, y_start, theta_start, 1.0)
	
	x = x_start
	y = y_start
	theta = theta_start

	x_goal = -10
	y_goal = 3*sin(x_goal)

	x_diff = x_goal-x
	y_diff = y_goal-y
	
	error = sqrt(x_diff**2 + y_diff**2)-d
	totalError = 0
	T = 0
	Kv = 3
	Kh = 6
	Ki = 0.00001
	d = 1
	dt = 0.1
	A = 5

        x_goal = -10
        y_goal = A * sin(x_goal)

        x_diff = x_goal-x
        y_diff = y_goal-y

	while True:
		#############YOUR CODE GOES HERE#############
                x_goal = x_goal  + dx  
                y_goal = A * sin(x_goal)
                V = Kv * error + Ki *totalError * T
                x_diff = x_goal-x
                y_diff = y_goal-y
                thetagoal = atan2( y_diff, x_diff)
	        distance = sqrt(x_diff**2 + y_diff**2)	
                vx = V *  cos (theta)
                vy = V * sin(theta)
                x = x + vx * dt
                y = y + vy * dt
                theta = theta + Kh * ang_diff(thetagoal, theta) * dt
                vehicle.update_pose(x,y,theta) 
                vehicle.plot(xlims=[-10,10],ylims=[-10,10])


                error = sqrt(x_diff**2 + y_diff**2) - d
                totalError = totalError + error
                T = T + dt
Exemple #3
0
def move_to_point(x_start, y_start, theta_start, x_goal, y_goal):
    #These values work fine, but try tuning them once your simulation is working
    #to see if you can improve how quickly your robot reaches the goal.
    Kh = 3.5
    Kv = 2

    dt = 0.1

    vehicle = SimpleVehicle(x_start, y_start, theta_start, 1)

    x = x_start
    y = y_start
    theta = theta_start

    x_diff = x_goal - x
    y_diff = y_goal - y
    distance = sqrt(x_diff**2 + y_diff**2)

    while True:
        #############YOUR CODE GOES HERE#############

        x_goal = vehicle.goal[0]
        y_goal = vehicle.goal[1]
        # theta_diff = ang_diff(theta_start, atan2(y_diff, x_diff))
        x_diff = x_goal - x
        y_diff = y_goal - y
        theta_goal = atan2(y_diff, x_diff)
        distance = sqrt(x_diff**2 + y_diff**2)
        v = Kv * distance
        # v_theta = Kh*theta_diff
        v_x = v * cos(theta)
        v_y = v * sin(theta)
        x = x + v_x * dt
        y = y + v_y * dt
        theta = theta + Kh * ang_diff(theta_goal, theta) * dt
        vehicle.update_pose(x, y, theta)
        vehicle.plot(xlims=[-10, 10], ylims=[-10, 10])
def simple_trajectory():
	T=5
	dt=.1
	t=0
	        
	x = 0.0
	y = 0.0
	theta = 0.0
        
	x_obstacle = 10.0
	y_obstacle = 0.0

	x_waypoint = 10.0
	y_waypoint = 5.0

	x_goal = 20.0
	y_goal = 0.0

	vehicle = SimpleVehicle(x, y, theta, 1)

	traj = TrajectoryGenerator([0,0,0], [x_waypoint,y_waypoint,0], T, start_vel=[0,0,0],des_vel=[1,0,0])
	traj.solve()
	x_c = traj.x_c
	y_c = traj.y_c
	while x < x_obstacle:
                t= t + dt

                old_x = x
                old_y = y
                
                y= y_c[0,0] * t**5 + y_c[1,0] * t**4 + y_c[2,0] * t**3 + y_c[3,0]* t**2 + y_c[4,0]*t + y_c[5,0]
                x= x_c[0,0] * t**5 + x_c[1,0] * t**4 + x_c[2,0] * t**3 + x_c[3,0]* t**2 + x_c[4,0]*t + x_c[5,0]

                
                x_diff= x - old_x
                y_diff= y - old_y
                theta = atan2(y_diff,x_diff)


                vehicle.update_pose(x,y,theta) 
                vehicle.plot(goal=[x_goal,y_goal],xlims=[0,20],ylims=[-10,10], obstacles = [[x_obstacle,y_obstacle,150]])

      	traj = TrajectoryGenerator([x_waypoint,y_waypoint,0], [x_goal,y_goal,0], T, start_vel=[1,0,0],des_vel=[0,0,0])
	traj.solve()
	x_c = traj.x_c
	y_c = traj.y_c
        t=0.1
       	while x >=  x_obstacle:
                t= t + dt

                old_x = x
                old_y = y
                
                y= y_c[0,0] * t**5 + y_c[1,0] * t**4 + y_c[2,0] * t**3 + y_c[3,0]* t**2 + y_c[4,0]*t + y_c[5,0]
                x= x_c[0,0] * t**5 + x_c[1,0] * t**4 + x_c[2,0] * t**3 + x_c[3,0]* t**2 + x_c[4,0]*t + x_c[5,0]

                
                x_diff= x - old_x
                y_diff= y - old_y
                theta = atan2(y_diff,x_diff)


                vehicle.update_pose(x,y,theta) 
                vehicle.plot(goal=[x_goal,y_goal],xlims=[0,20],ylims=[-10,10], obstacles = [[x_obstacle,y_obstacle,150]])
Exemple #5
0
def avoid_obstacle():
    Kv = 3
    Kh = 6
    Ki = 0.00001
    d = 2
    dt = 0.1

    x = 0.0
    y = 0.0
    theta = 0.0

    x_obstacle = 10.0
    y_obstacle = 0.0

    x_waypoint = 10.0
    y_waypoint = 5.0

    x_goal = 20.0
    y_goal = 0.0

    m1 = (y_waypoint - y) / (x_waypoint - x)
    b1 = y_waypoint - m1 * x_waypoint

    m2 = (y_goal - y_waypoint) / (x_goal - x_waypoint)
    b2 = y_goal - m2 * x_goal

    x_pursuit = x
    y_pursuit = y

    vehicle = SimpleVehicle(x, y, theta, 1)

    x_diff = x_pursuit - x
    y_diff = y_pursuit - y

    error = sqrt(x_diff**2 + y_diff**2) - d
    totalError = 0
    T = 0

    while sqrt((x - x_goal)**2 + (y - y_goal**2)**2) > 1:
        #############YOUR CODE GOES HERE#############

        while x <= 10:
            x_pursuit = x_pursuit + 0.1
            y_pursuit = m1 * x_pursuit + b1
            v = Kv * error + Ki * totalError * T
            x_diff = x_pursuit - x
            y_diff = y_pursuit - y
            theta_goal = atan2(y_diff, x_diff)
            distance = sqrt(x_diff**2 + y_diff**2)
            v = Kv * distance
            v_x = v * cos(theta)
            v_y = v * sin(theta)
            x = x + v_x * dt
            y = y + v_y * dt
            theta = theta + Kh * ang_diff(theta_goal, theta) * dt
            vehicle.update_pose(x, y, theta)
            theta = theta + Kh * ang_diff(theta_goal, theta) * dt
            vehicle.update_pose(x, y, theta)
            vehicle.plot(goal=[x_goal, y_goal],
                         xlims=[0, 20],
                         ylims=[-10, 10],
                         obstacles=[[x_obstacle, y_obstacle, 150]])
            error = sqrt(x_diff**2 + y_diff**2) - d
            T = T + dt

        while x > 10:
            x_pursuit = x_pursuit + 0.1
            y_pursuit = m2 * x_pursuit + b2
            v = Kv * error + Ki * totalError * T
            x_diff = x_pursuit - x
            y_diff = y_pursuit - y
            theta_goal = atan2(y_diff, x_diff)
            distance = sqrt(x_diff**2 + y_diff**2)
            v = Kv * distance
            v_x = v * cos(theta)
            v_y = v * sin(theta)
            x = x + v_x * dt
            y = y + v_y * dt
            theta = theta + Kh * ang_diff(theta_goal, theta) * dt
            vehicle.update_pose(x, y, theta)
            theta = theta + Kh * ang_diff(theta_goal, theta) * dt
            vehicle.update_pose(x, y, theta)
            vehicle.plot(goal=[x_goal, y_goal],
                         xlims=[0, 20],
                         ylims=[-10, 10],
                         obstacles=[[x_obstacle, y_obstacle, 150]])
            error = sqrt(x_diff**2 + y_diff**2) - d
            T = T + dt
def trajectory_generation():
    t = 0
    dt = 0.1
    T = 5
    x = 0.0
    y = 0.0
    theta = 0.0

    vehicle = SimpleVehicle(x, y, theta, 1)

    x_obstacle = 10.0
    y_obstacle = 0.0

    x_waypoint = 10.0
    y_waypoint = 5.0

    x_goal = 20.0
    y_goal = 0.0

    traj = TrajectoryGenerator([0, 0, 0], [x_waypoint, y_waypoint, 0],
                               T,
                               start_vel=[0, 0, 0],
                               des_vel=[1, 0, 0])
    traj.solve()
    x_c = traj.x_c
    y_c = traj.y_c
    while x < x_obstacle:
        #############YOUR CODE GOES HERE#############
        old_x = x
        old_y = y

        x = x_c[0, 0] * t**5 + x_c[1, 0] * t**4 + x_c[2, 0] * t**3 + x_c[
            3, 0] * t**2 + x_c[4, 0] * t + x_c[5, 0]
        y = y_c[0, 0] * t**5 + y_c[1, 0] * t**4 + y_c[2, 0] * t**3 + y_c[
            3, 0] * t**2 + y_c[4, 0] * t + y_c[5, 0]
        x_diff = x - old_x
        y_diff = y - old_y

        theta = atan2(y_diff, x_diff)

        vehicle.update_pose(x, y, theta)
        vehicle.plot(goal=[x_goal, y_goal],
                     xlims=[0, 20],
                     ylims=[-10, 10],
                     obstacles=[[x_obstacle, y_obstacle, 150]])
        t = t + dt

    t = dt
    traj = TrajectoryGenerator([x_waypoint, y_waypoint, 0],
                               [x_goal, y_goal, 0],
                               T,
                               start_vel=[1, 0, 0],
                               des_vel=[0, 0, 0])
    traj.solve()
    x_c = traj.x_c
    y_c = traj.y_c

    while x >= x_obstacle:
        #############YOUR CODE GOES HERE#############

        old_x = x
        old_y = y

        x = x_c[0, 0] * t**5 + x_c[1, 0] * t**4 + x_c[2, 0] * t**3 + x_c[
            3, 0] * t**2 + x_c[4, 0] * t + x_c[5, 0]
        y = y_c[0, 0] * t**5 + y_c[1, 0] * t**4 + y_c[2, 0] * t**3 + y_c[
            3, 0] * t**2 + y_c[4, 0] * t + y_c[5, 0]
        x_diff1 = x - old_x
        y_diff1 = y - old_y

        theta = atan2(y_diff1, x_diff1)

        vehicle.update_pose(x, y, theta)
        vehicle.plot(goal=[x_goal, y_goal],
                     xlims=[0, 20],
                     ylims=[-10, 10],
                     obstacles=[[x_obstacle, y_obstacle, 150]])
        t = t + dt
def simple_trajectory():
	Kv = 3
	Kh = 6
	Ki = 0.00001
	d = 2
	dt = 0.1
        dx = .1
        
	x = 0.0
	y = 0.0
	theta = 0.0
        t= 0
        
	x_obstacle = 10.0
	y_obstacle = 0.0

	x_waypoint = 10.0
	y_waypoint = 5.0

	x_goal = 20.0
	y_goal = 0.0

	m1 = (y_waypoint-y)/(x_waypoint-x)
	b1 = y_waypoint-m1*x_waypoint

	m2 = (y_goal-y_waypoint)/(x_goal-x_waypoint)
	b2 = y_goal-m2*x_goal

	x_pursuit = x
	y_pursuit = y

	vehicle = SimpleVehicle(x, y, theta, 1)

	x_diff = x_pursuit-x
	y_diff = y_pursuit-y
	
	error = sqrt(x_diff**2 + y_diff**2)-d
	totalError = 0
	T = 10000

        A_x = np.array([[0,0,1], [(T**2)/4,T/2,1], [T**2,T,1]])
        b_x = np.array([[0],[x_waypoint],[x_goal]])
        x_coeff = np.linalg.solve(A_x,b_x)
        
        A_y = np.array([[0,0,1],[(T**2)/4,T/2,1],[T**2,T,1]])
        b_y = np.array([[0],[y_waypoint],[y_goal]])
        y_coeff = np.linalg.solve(A_y,b_y)

        
        
	while sqrt((x-x_goal)**2+(y-y_goal**2)**2) > 1:
		#############YOUR CODE GOES HERE#############
                t= t + dt

                old_x = x
                old_y = y
                
                y= y_coeff[0.0] * t**2 + y_coeff[1,0] * t + x_coeff[2,0]
                x = x_coeff[0,0] * t**2 + x_coeff[1,0] * t + x_coeff[2,0]

                
                x_diff= x - old_x
                y_diff= y - old_y
                theta = atan2(y_diff,x_diff)


                vehicle.update_pose(x,y,theta) 
                vehicle.plot(goal=[x_goal,y_goal],xlims=[0,20],ylims=[-10,10], obstacles = [[x_obstacle,y_obstacle,150]])