img = mycv.AddCircle(img,
num=2,
radius=3,
std=1,
rand_range=rand_range)
# init RRT
q_start = Node(x=40, y=60)
q_target = Node(x=60, y=40)
rrt = RRT()
rrt.init(img,
q_start,
q_target,
expand_dis=5,
dis_goal=8,
goal_sample_rate=0.1,
max_iter=2000)
(find_target, steps) = rrt.Plan()
# print result
rrt.show_RRT_result()
print 'steps =', steps, ', nodes =', len(rrt.q_list)
if rrt.PATH_FIND == 0:
print 'path not found'
else:
print 'path found !'
mycv.savefig(IMAGE_PATH + "/RRTstar.png", border_size=10)
plt.show()
def main_PurePursuit(robot, traj, T):
dt = 0.1 # control period
traj.compute_discrete_trajecotry(t_start=0, t_end=T, dt=dt)
v_mean=traj.total_length/T
idea_dist=0.1
goal_dist=0.2
# start
t_curr = robot.query_time()
path=Path() # record path
path.append(robot.x, robot.y, robot.yaw)
# for step in range(1, int(T/dt) + 100 ):
for step in range(1, 1000 ):
# set the goal as the point after 2*dt seconds
t_goal = min(t_curr + idea_dist/v_mean, T)
# get current pose and goal pose
(x_curr, y_curr, yaw_curr) = (robot.x, robot.y, robot.yaw)
(x_next, y_next, yaw_next) = traj.query_pose(t=t_goal)
# angle error -- reduce it
angle_target = np.arctan2(y_next-y_curr, x_next-x_curr)
yaw_error = angle_pi_to_pi(
angle_target-yaw_curr) # to [-pi, pi]
# distance error to the goal point -- reduce it
dist_error = calc_dist(x_curr, y_curr, x_next, y_next)
# control (v, w)
if step == 1:
pid_v = PIDcontroller(P=2) # If dis=0.1m, we want v=0.1m/s
pid_w = PIDcontroller(P=4, D=0)
v = pid_v.feedback(dist_error-idea_dist)
w = pid_w.feedback(yaw_error)
# output
robot.move(v, w, dt)
path.append(robot.x, robot.y, robot.yaw)
# print current state
t_curr = robot.query_time()
print("step=%d, t=%.3f, x=%.2f, y=%.2f, v=%.2f, w=%.2f, yaw=%.2f, theta=%.2f; d_dist=%.2f, d_yaw=%.2f"
% (step, t_curr, robot.x, robot.y, v, w,
robot.yaw/np.pi*180, robot.theta/np.pi*180,
dist_error, yaw_error))
# check if reach target
reach_target = False
if calc_dist(x_curr, y_curr, traj.x_list[-1], traj.y_list[-1])<goal_dist:
reach_target=True
# plot
if (ANIMATION_ON and (step % ANIMATION_GAP == 0)) or reach_target:
ar=[float(x) for x in [x_curr, y_curr, 0.2*math.cos(robot.yaw), 0.2*math.sin(robot.yaw)]]
plt.arrow(ar[0],ar[1],ar[2],ar[3],
head_width=0.1, head_length=0.1, linewidth=5, fc='k', ec='k')
plot_traj(traj)
plt.plot(path.xl, path.yl, 'r-,')
plt.plot(x_next, y_next, 'xr',ms=12) # next
plt.plot(x_curr, y_curr, 'ok',ms=12) # current
plt.pause(0.0001)
if reach_target:
mycv.savefig(IMAGE_PATH+"/PurePursuit.png")
plt.show()
if reach_target:
print "\nReach the target !!!"
path.plot_velocity(dt, x_axis_time=robot.query_time())
mycv.savefig(IMAGE_PATH+"/PurePursuit_vel.png")
plt.show()
break
return # end of PurePursuit
def main_PurePursuit(robot, traj, T):
dt = 0.1 # control period
traj.compute_discrete_trajecotry(t_start=0, t_end=T, dt=dt)
v_mean = traj.total_length / T
goal_dist = 0.2
# start
t_curr = robot.query_time()
idx_near = 0 # the index of the nearest point
path = Path() # record path
path.append(robot.x, robot.y, robot.yaw)
for step in range(1, 2000):
# get current pose and goal pose
(x_curr, y_curr, yaw_curr) = (robot.x, robot.y, robot.yaw)
# distance to the nearest point -- reduce it
search_range = 20
idx_l = max(0, idx_near - search_range) # left search index
idx_r = min(traj.N, idx_near + search_range) # right search index
dist_near_list=(x_curr-traj.x_list[idx_l:idx_r])**2+\
(y_curr-traj.y_list[idx_l:idx_r])**2
idx_near = idx_l + np.argmin(dist_near_list)
dist_near = np.sqrt(dist_near_list[idx_near - idx_l])
(x_near, y_near, yaw_near) = traj.query_pose(idx=idx_near)
# print("x_near=%.2f, y_near=%.2f, yaw_near=%.2f" % (x_near, y_near, yaw_near))
# Angle error
# ang1: where is the nearest point relatvie to current pose
ang1 = np.arctan2(y_near - y_curr, x_near - x_curr)
ang1_error = angle_pi_to_pi(ang1 - yaw_curr)
if ang1_error < 0: # point on the left
dist_near = -dist_near # w negative
# ang2: what is the desired orientatino at that nearest point
ang2 = yaw_near
ang2_error = angle_pi_to_pi(ang2 - yaw_curr)
# Apply PID control
Pw1 = 20 # make robot closer to the nearest point
Pw2 = 8 # make robot same orientation as the nearest point
if step == 1:
pid_w = PIDcontroller(P=[Pw1, Pw2], D=[0, 0], I=[0, 0],
dim=2) # dim=2: 2 params
w = pid_w.feedback(err=[dist_near, ang2_error])
v = v_mean
# output
robot.move(v, w, dt)
path.append(robot.x, robot.y, robot.yaw)
# print current state
t_curr = robot.query_time()
print(
"step=%d, t=%.3f, x=%.2f, y=%.2f, v=%.2f, w=%.2f, yaw=%.2f, theta=%.2f; dist_near=%.2f, ang2_error=%.2f"
% (step, t_curr, robot.x, robot.y, v, w, robot.yaw / np.pi * 180,
robot.theta / np.pi * 180, dist_near, ang2_error))
# check if reach target
reach_target = False
if calc_dist(x_curr, y_curr, traj.x_list[-1],
traj.y_list[-1]) < goal_dist:
reach_target = True
# plot
if (ANIMATION_ON and (step % ANIMATION_GAP == 0)) or reach_target:
ar = [
float(x) for x in [
x_curr, y_curr, 0.2 * math.cos(robot.yaw), 0.2 *
math.sin(robot.yaw)
]
]
plt.arrow(ar[0],
ar[1],
ar[2],
ar[3],
head_width=0.08,
head_length=0.08,
linewidth=4,
fc='k',
ec='k')
plot_traj(traj)
plt.plot(path.xl, path.yl, 'r-,')
plt.plot(x_near, y_near, 'xr', ms=12, linewidth=2) # next
plt.plot(x_curr, y_curr, 'ok', ms=5, linewidth=2) # current
plt.pause(0.0001)
if reach_target:
mycv.savefig(IMAGE_PATH + "/FollowLine.png")
plt.show()
if reach_target:
print "\nReach the target !!!"
path.plot_velocity(dt, x_axis_time=robot.query_time())
mycv.savefig(IMAGE_PATH + "/FollowLine_vel.png")
plt.show()
break
return # end of PurePursuit
def main():
dt = 0.1 # control period
# set path points
if 0: # this is shorter, for testing
xl0 = [10, 6, 5, 2]
yl0 = [5, -3, 1, 1]
xl0 = np.array(xl0) / 5.0
yl0 = np.array(yl0) / 5.0
T = 15.0 # total time (s)
else: # this is longer
xl0 = [0, 1, 5, 9, 11, 10, 6, 5, 2]
yl0 = [0, 3, 5, -1, -4, 5, -3, 1, 1]
xl0 = np.array(xl0) / 5.0
yl0 = np.array(yl0) / 5.0
T = 30.0 # total time (s)
# spline a function for the path
spline = Spline(xl0, yl0, T)
traj = Trajectory(spline.fxt, spline.fyt,
param=None) # assign spline to trajectory
traj.compute_discrete_trajecotry(0, T, dt)
# set robot
robot = RobotModel_SingleTrack()
print("\nMax velocity of the robot: v=%.2f, w=%.2f" %\
(robot.vmax, robot.get_max_angular_velocity_at_v(robot.vmax)))
robot.v_mean = traj.total_length / T
# set optimization parameters
opti_dt = 0.5 # run optimization for every opti_dt seconds
opti_length = 1.0 # how long (in seconds) to optimize future traj in each iteration
MAX_OPT_POINTS_PER_LOOP = int(
opti_length /
dt) # max number of points to optimize in a low level loop
# in side my optimization function.
# Originally I added this functionality in order to speed up the optimization
# by planning segment by segment.
# However, it's functionality is the same as setting "opti_dt" and "opti_length" here.
# So currently, just set its value as "opti_length/dt".
opt = PlanByOptimization()
### some data history to record
path = Path() # true path
path_opti_thought = Path(
) # what the optimizer thought about the robot's pose
path.append(robot.x, robot.y, robot.yaw)
path_opti_thought.append(robot.x, robot.y, robot.yaw)
vl_input = []
wl_input = []
### start robot !!!
idx_control_commands = 0
ite = -1
opti_dt_cnt = 0
while robot.query_time() < T:
t_curr = robot.query_time()
ite += 1
if ite == 0 or (opti_dt_cnt >= opti_dt / dt) or len(
vl_input_k) == 0: # run opti for every opti_dt seconds
opti_dt_cnt = 0
if flag_path_CubicSpline:
traj.compute_discrete_trajecotry(t_start=t_curr,
t_end=t_curr + opti_length,
dt=dt)
else:
traj.compute_discrete_trajecotry_with_constant_v(
t_start=t_curr,
t_end=t_curr + opti_length,
dt=dt,
v=robot.v_mean,
prec_ratio=0.1)
curr_state = np.array([[
robot.v,
robot.get_angular_velocity(), robot.x, robot.y, robot.yaw
]]).T
(vl_input_k, wl_input_k,
xl_res_k, yl_res_k, yawl_res_k, states_res) =\
opt.optimize(curr_state, traj, method_ID=0,
max_opt_pts=MAX_OPT_POINTS_PER_LOOP, print_detials=False,
robot=robot)
vl_input_k = list(vl_input_k)
wl_input_k = list(wl_input_k)
path_opti_thought.append_list(xl_res_k, yl_res_k, yawl_res_k)
if IF_ANIMATION:
plt.clf()
plt.plot(xl_res_k, yl_res_k, 'gx')
# plt.plot(traj.x_list, traj.y_list,'bo-')
# plt.plot(path_opti_thought.xl, path_opti_thought.yl, 'gx-') # What optimizer thought
plt.plot(path.xl, path.yl, 'ro-') # real traj
plt.plot(traj.x_list, traj.y_list, 'b.-',
linewidth=1) # desired traj
plt.legend(["optimizer thought", "real,", "desired traj"])
plt.pause(0.0001)
plt.show()
opti_dt_cnt += 1
# read in the control input
v = vl_input_k.pop(0)
w = wl_input_k.pop(0)
idx_control_commands += 1
# move robot
robot.move(v, w, dt)
print("ite=%d, x=%.2f, y=%.2f, yaw=%.2f; v=%.2f, w=%.2f"\
%(ite, robot.x, robot.y, robot.yaw, v, w))
path.append(robot.x, robot.y, robot.yaw)
vl_input.append(v)
wl_input.append(w)
### plot trajectory
if flag_path_CubicSpline:
traj.compute_discrete_trajecotry(t_start=0, t_end=T, dt=dt)
else:
traj.compute_discrete_trajecotry_with_constant_v(t_start=0,
t_end=T,
dt=dt,
v=robot.v_mean,
prec_ratio=0.1)
fig = plt.figure(figsize=(10, 7)) # set figure
plt.plot(path_opti_thought.xl, path_opti_thought.yl,
'gx') # What optimizer thought
plt.plot(path.xl, path.yl, 'ro-') # real traj
plt.plot(traj.x_list, traj.y_list, 'b.-', linewidth=1) # desired traj
plt.title("trajectory")
plt.legend(["What optimizer thought", "real traj", "desired traj"])
plt.xlabel("x (m)")
plt.ylabel("y (m) ")
mycv.savefig(IMAGE_PATH + "/NonlinearOptimization.png")
### plot velocity
(_, _, wl_real, vl_real) = path.compute_derivative(dt=dt)
t = np.arange(traj.N) * 1.0 / traj.N * T + traj.dt
fig = plt.figure(figsize=(12, 6))
plt.subplot(121)
plt.plot(t, vl_real, 'r-.')
plt.plot(t, vl_input, 'g-.')
plt.legend(["v(real)", "v(input)"])
plt.xlabel("t (s)")
plt.ylabel("v (m/s)")
plt.subplot(122)
plt.plot(t, wl_real, 'r-.')
plt.plot(t, wl_input, 'g-.')
plt.legend(["w(real)", "w(input)"])
plt.xlabel("t (s)")
plt.ylabel("w (rad/s)")
mycv.savefig(IMAGE_PATH + "/NonlinearOptimization_vel.png")
plt.show()
rand_ranges = [[0, 30, 0, 100], [70, 100, 0, 100], [0, 100, 0, 30],
[0, 100, 70, 100]]
for rand_range in rand_ranges:
img = mycv.AddCircle(img,
num=2,
radius=3,
std=1,
rand_range=rand_range)
# Astar
q_start = Node(x=40, y=60)
q_target = Node(x=60, y=40)
astar = Astar()
(find_target, path) = astar.Plan(img, q_start, q_target)
# print result
if find_target:
print "Find path"
else:
print "Path not found"
mycv.imshow(astar.img_show)
if find_target:
xs = [node.x for node in astar.path]
ys = [node.y for node in astar.path]
plt.plot(xs, ys, 'ro', ms=2)
mycv.savefig(IMAGE_PATH + "/AStar.png")
plt.show()