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
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]])
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]])
