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
vehicle = SimpleVehicle(x_start, y_start, theta_start, 1.0)
x = x_start
y = y_start
theta = theta_start
x_goal = -10
y_goal = A * 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
while True:
#############YOUR CODE GOES HERE#############
x_goal = x_goal + 0.1
y_goal = A * sin(x_goal)
v = Kv * error + Ki * totalError * T
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_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])
theta = theta + Kh * ang_diff(theta_goal, theta) * dt
vehicle.update_pose(x, y, theta)
vehicle.plot(goal=[x_goal, y_goal], xlims=[-10, 10], ylims=[-10, 10])
error = sqrt(x_diff**2 + y_diff**2) - d
totalError = totalError + error
T = T + dt
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 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