def get_random_pose(self, bias_pose: PoseR2S2 = None, bias=0.05):
if bias_pose is not None:
rand = random.uniform(0, 1)
if rand <= bias:
return bias_pose.copy()
x = random.uniform(0, self.width)
y = random.uniform(0, self.height)
theta = random.uniform(-np.pi, np.pi)
return PoseR2S2(x, y, theta)
def update_shape(self, pose: PoseR2S2):
# adjust the tractor vertices based on articulation angel
o = PoseR2S2(pose.x, pose.y, pose.theta)
p0 = o.compose_point(PointR2(0, -self.tractor_w / 2.))
p1 = o.compose_point(PointR2(self.tractor_l, -self.tractor_w / 2.))
p2 = o.compose_point(PointR2(self.tractor_l, self.tractor_w / 2.))
p3 = o.compose_point(PointR2(0, self.tractor_w / 2.))
self.shape = (p0, p1, p2, p3)
def execute_motion(self, pose: PoseR2S2, v: float, w: float,
dt: float) -> PoseR2S2:
theta1 = pose.theta
theta2 = wrap_to_npi_pi(theta1 - pose.phi)
# tan_alpha = w * self.tractor_l / v
L2 = self.trailer_l + self.link_l
x_new = pose.x + dt * v * cos(pose.phi) * cos(theta2)
y_new = pose.y + dt * v * cos(pose.phi) * sin(theta2)
theta_new = theta1 + dt * w
phi_new = theta_new - (theta2 + dt * v * sin(pose.phi) / L2)
return PoseR2S2(x_new, y_new, theta_new, phi_new)
def update_shape(self, pose: PoseR2S2):
# adjust the tractor vertices based on articulation angel
theta2 = (pose.theta - pose.phi)
o = PoseR2S2(pose.x, pose.y, theta2, 0.)
p0 = o.compose_point(PointR2(0, self.trailer_w / 2.))
p1 = o.compose_point(PointR2(0, -self.trailer_w / 2.))
p2 = o.compose_point(PointR2(self.trailer_l, -self.trailer_w / 2.))
p3 = o.compose_point(PointR2(self.trailer_l, self.trailer_w / 2.))
p4 = o.compose_point(PointR2(self.trailer_l, 0.))
p5 = o.compose_point(PointR2(self.trailer_l + self.link_l, 0.))
hitch_pose = PoseR2S2(p5.x, p5.y, pose.theta)
p6 = hitch_pose.compose_point(PointR2(0, -self.tractor_w / 2.))
p9 = hitch_pose.compose_point(PointR2(0, self.tractor_w / 2.))
p8 = hitch_pose.compose_point(
PointR2(self.tractor_l, self.tractor_w / 2.))
p7 = hitch_pose.compose_point(
PointR2(self.tractor_l, -self.tractor_w / 2.))
self.shape = (p0, p1, p2, p3, p4, p5, p6, p7, p8, p9)
self.hitch_pose = hitch_pose
self.last_phi = pose.phi
def _sim_move_forward(self, pose: PoseR2S2, v: float, w: float,
dt: float) -> (PoseR2S2, float):
final_pose = PoseR2S2()
final_pose.x = pose.x + v * dt * cos(pose.theta)
final_pose.y = pose.y + v * dt * sin(pose.theta)
final_pose.theta = pose.theta + w * dt
#final_pose.phi = pose.phi - dt * ((v / (self.trailer_l / 2)) * sin(pose.phi) - (
# (self.tractor_l / 2 + self.link_l) * w / (self.trailer_l / 2)) * cos(pose.phi) - w)
final_pose.phi = pose.phi - dt * (
(v / (self.trailer_l)) * sin(pose.phi) -
((self.tractor_l + self.link_l) * w /
(self.trailer_l)) * cos(pose.phi) - w)
return final_pose
def test_plot(av: ArticulatedVehicle, x: float, y: float, theta: float,
phi: float):
import matplotlib.pyplot as plt
from prrt.primitive import PoseR2S2
from math import radians as rad
pose = PoseR2S2(x, y, theta, phi)
grid_size = av.trailer_l * 4
fig, ax = plt.subplots()
ax.set_xlim([x - grid_size, x + grid_size])
ax.set_ylim([y - grid_size, y + grid_size])
av.plot(ax, pose, None)
ax.title.set_text('Pose = {0} '.format(pose))
plt.show()
def __init__(self, config: dict):
# check the provided config file for details on the variables initialized
self.v_max = config['v_max'] # type: float
self.w_max = rad(config['w_max']) # type: float
self.phi_max = rad(config['phi_max']) # type: float
self.tractor_w = config['tractor_w'] # type: float
self.tractor_l = config['tractor_l'] # type: float
self.link_l = config['link_l'] # type: float
self.trailer_w = config['trailer_w'] # type: float
self.trailer_l = config['trailer_l'] # type: float
self.last_phi = 0. # keeps track of changes to phi in order to update vehicle shape
# vertices numbered as shown below
#
# 8----------------7 2-------1
# | | | |
# Trl_W | 5-------4 o | Trk_W
# | | L_L | |
# 9----------------6 3-------0
# Trl_L Trk_L
o = PoseR2S2(0., 0., 0., 0.)
p0 = o.compose_point(PointR2(self.tractor_l / 2.,
-self.tractor_w / 2.))
p1 = o.compose_point(PointR2(self.tractor_l / 2., self.tractor_w / 2.))
p2 = o.compose_point(PointR2(-self.tractor_l / 2.,
self.tractor_w / 2.))
p3 = o.compose_point(
PointR2(-self.tractor_l / 2., -self.tractor_w / 2.))
p4 = o.compose_point(PointR2(-self.tractor_l / 2., 0.))
p5 = o.compose_point(PointR2(-self.tractor_l / 2. - self.link_l, 0.))
hitch_pose = PoseR2S2(p5.x, p5.y, self.last_phi)
p6 = hitch_pose.compose_point(PointR2(0, -self.trailer_w / 2.))
p7 = hitch_pose.compose_point(PointR2(0, self.trailer_w / 2.))
p8 = hitch_pose.compose_point(
PointR2(-self.trailer_l, self.trailer_w / 2.))
p9 = hitch_pose.compose_point(
PointR2(-self.trailer_l, -self.trailer_w / 2.))
self.shape = (p0, p1, p2, p3, p4, p5, p6, p7, p8, p9)
self.last_hitch_pose = hitch_pose
def build_cpoints(self):
from math import exp
r = self.vehicle.tractor_l # see ref 1
# generate trajectories for each steering angle
for alpha in np.arange(-self.alpha_max,
self.alpha_max + self.alpha_resolution,
self.alpha_resolution):
n = 0
v = self.K * self.vehicle.v_max
pose = PoseR2S2(0., 0., 0., self.init_phi)
dist = 0. # tp_space distance
last_pose = pose.copy()
w = v / self.vehicle.tractor_l * tan(alpha) # rotational velocity
rotation = 0. # same as pose.theta but defined over the range [0: 2PI)
self.idx_to_alpha.append(alpha)
cpoints_at_alpha = [CPoint(pose, 0., v, w,
alpha)] # type: List[CPoint]
while abs(
rotation
) < 1.95 * PI and dist < self.d_max and n < self.n_max and abs(
pose.phi) <= self.vehicle.phi_max:
# calculate control parameters
delta = helper.wrap_to_npi_pi(alpha - pose.theta)
v = self.K * self.vehicle.v_max * exp(-(delta / 1.)**2)
w = self.K * self.vehicle.w_max * (-0.5 +
(1 /
(1 + exp(-delta / 1.))))
pose = self.vehicle.execute_motion(pose, v, w, self.dt)
rotation += w * self.dt
v_tp_space = sqrt(v * v + (w * r) * (w * r))
dist += v_tp_space * self.dt
delta_pose = pose - last_pose
dist1 = delta_pose.norm
dist2 = abs(delta_pose.theta) * self.k_theta
dist_max = max(dist1, dist2)
if dist_max > self.min_dist_between_cpoints:
cpoints_at_alpha.append(
CPoint(pose.copy(), dist, v, w, alpha, n))
last_pose.copy_from(pose)
n += 1
self.cpoints.append(cpoints_at_alpha)
print('Completed building cpoints for {0}'.format(self.name))
def build_cpoints(self):
"""
Builds a list of cpoint trajectories for each alpha value (steering angle)
References:
1- Blanco, Jose-Luis, Javier González, and Juan-Antonio Fernández-Madrigal. "Extending obstacle avoidance methods through multiple parameter-space transformations." Autonomous Robots 24.1 (2008): 29-48.
2- Lamiraux, Florent, Sepanta Sekhavat, and J-P. Laumond. "Motion planning and control for Hilare pulling a trailer." IEEE Transactions on Robotics and Automation 15.4 (1999): 640-652.
"""
r = self.vehicle.tractor_l # see ref 1
# generate trajectories for each steering angle
for alpha in np.arange(-self.alpha_max,
self.alpha_max + self.alpha_resolution,
self.alpha_resolution):
n = 0
v = self.K * self.vehicle.v_max
pose = PoseR2S2(0., 0., 0., self.init_phi)
dist = 0. # tp_space distance
last_pose = pose.copy()
w = v / self.vehicle.tractor_l * tan(alpha) # rotational velocity
rotation = 0. # same as pose.theta but defined over the range [0: 2PI)
self.idx_to_alpha.append(alpha)
cpoints_at_alpha = [CPoint(pose, 0., v, w,
alpha)] # type: List[CPoint]
while abs(
rotation
) < 1.95 * PI and dist < self.d_max and n < self.n_max and abs(
pose.phi) <= self.vehicle.phi_max:
pose = self.vehicle.execute_motion(pose, v, w, self.dt)
rotation += w * self.dt
v_tp_space = sqrt(v * v + (w * r) * (w * r))
dist += v_tp_space * self.dt
delta_pose = pose - last_pose
dist1 = delta_pose.norm
dist2 = abs(delta_pose.theta) * self.k_theta
dist_max = max(dist1, dist2)
if dist_max > self.min_dist_between_cpoints:
cpoints_at_alpha.append(
CPoint(pose.copy(), dist, v, w, alpha, n))
last_pose.copy_from(pose)
n += 1
self.cpoints.append(cpoints_at_alpha)
print('Completed building cpoints for {0}'.format(self.name))
def __init__(self, config: dict):
self.v_max = config['v_max'] # type: float
self.w_max = rad(config['w_max']) # type: float
self.phi_max = 0.
self.tractor_w = config['tractor_w'] # type: float
self.tractor_l = config['tractor_l'] # type: float
# vertices numbered as shown below
# Origin is the center of the tractor back axle
#
# 3-------2
# | |
# o | Trk_W
# | |
# 0-------1
# Trk_L
o = PoseR2S2(0., 0., 0., 0.)
p0 = o.compose_point(PointR2(0, -self.tractor_w / 2.))
p1 = o.compose_point(PointR2(self.tractor_l, -self.tractor_w / 2.))
p2 = o.compose_point(PointR2(self.tractor_l, self.tractor_w / 2.))
p3 = o.compose_point(PointR2(0, self.tractor_w / 2.))
self.shape = (p0, p1, p2, p3)
def _sim_move_reverse(self, pose: PoseR2S2, v: float, w: float, dt: float):
# Transform to simulated car pulling the trailer backwards
#x_rev = pose.x - (self.trailer_l / 2.) * cos(pose.theta) - \
# (self.tractor_l / 2 + self.link_l) * cos(pose.theta + pose.phi)
#y_rev = pose.y - (self.trailer_l / 2) * sin(pose.theta) - \
# (self.tractor_l / 2 + self.link_l) * sin(pose.theta + pose.phi)
x_rev = pose.x - (self.trailer_l) * cos(pose.theta) - \
(self.tractor_l + self.link_l) * cos(pose.theta + pose.phi)
y_rev = pose.y - (self.trailer_l) * sin(pose.theta) - \
(self.tractor_l + self.link_l) * sin(pose.theta + pose.phi)
theta_rev = pose.theta + pose.phi + PI
phi_rev = -pose.phi
# Move forward one step
x_rev += v * dt * cos(theta_rev)
y_rev += v * dt * sin(theta_rev)
theta_rev += dt * w
#phi_rev += dt * ((v / (self.trailer_l / 2)) * sin(phi_rev) - (
# (self.tractor_l / 2 + self.link_l) * w / (self.trailer_l / 2)) * cos(phi_rev) - w)
phi_rev += dt * ((v / (self.trailer_l)) * sin(phi_rev) -
((self.tractor_l + self.link_l) * w /
(self.trailer_l)) * cos(phi_rev) - w)
# Transform back
new_pose = PoseR2S2()
new_pose.phi = -phi_rev
new_pose.theta = theta_rev - new_pose.phi - PI
#new_pose.x = x_rev + (self.trailer_l / 2.) * cos(new_pose.theta) + \
# (self.tractor_l / 2 + self.link_l) * cos(new_pose.theta + new_pose.phi)
#new_pose.y = y_rev + (self.trailer_l / 2) * sin(new_pose.theta) + \
# (self.tractor_l / 2 + self.link_l) * sin(new_pose.theta + new_pose.phi)
new_pose.x = x_rev + (self.trailer_l) * cos(new_pose.theta) + \
(self.tractor_l + self.link_l) * cos(new_pose.theta + new_pose.phi)
new_pose.y = y_rev + (self.trailer_l) * sin(new_pose.theta) + \
(self.tractor_l + self.link_l) * sin(new_pose.theta + new_pose.phi)
return new_pose
def solve(self):
self.setup() # load aptgs and world map
init_pose = PoseR2S2.from_dict(self.config['init_pose'])
goal_pose = PoseR2S2.from_dict(self.config['goal_pose'])
self.tree = Tree(init_pose)
goal_dist_tolerance = self.config['goal_dist_tolerance']
goal_ang_tolerance = self.config['goal_ang_tolerance']
debug_tree_state = self.config['debug_tree_state']
debug_tree_state_file = self.config['debug_tree_state_file']
bias = self.config['rrt_bias']
obs_R = self.config['obs_R']
D_max = self.config['D_max']
solution_found = False
max_count = self.config['max_count']
counter = 0
min_goal_dist_yet = float('inf')
start_time = time.time()
while not solution_found and len(self.tree.nodes) < max_count:
counter += 1
rand_pose = self.world.get_random_pose(goal_pose, bias)
candidate_new_nodes = sorteddict.SortedDict()
rand_node = Node(ptg=None, pose=rand_pose)
for aptg in self.aptgs:
ptg, ptg_nearest_node, ptg_d_min = self.tree.get_aptg_nearest_node(
rand_node, aptg)
if ptg_nearest_node is None:
print('APTG {0} can\'t find nearest pose to {1}'.format(
aptg.name, rand_node))
continue
ptg_nearest_pose = ptg_nearest_node.pose
rand_pose_rel = rand_pose - ptg_nearest_pose
d_max = min(D_max, ptg.distance_ref)
is_exact, k_rand, d_rand = ptg.inverse_WS2TP(rand_pose_rel)
d_rand *= ptg.distance_ref
max_dist_for_obstacles = obs_R * ptg.distance_ref
obstacles_rel = self.world.transform_point_cloud(
ptg_nearest_pose, max_dist_for_obstacles)
obstacles_TP = self.transform_toTP_obstacles(
ptg, obstacles_rel, k_rand, max_dist_for_obstacles)
d_free = obstacles_TP[k_rand]
d_new = min(d_max, d_rand)
if debug_tree_state > 0 and counter % debug_tree_state == 0:
self.tree.plot_nodes(
self.world, ptg_nearest_pose,
'{0}{1:04d}.png'.format(debug_tree_state_file,
counter))
# Skip if the current ptg and alpha (k_ran) can't reach this point
if ptg.cpoints[k_rand][-1].d < d_new:
#print('Node leads to invalid trajectory. Node Skipped!')
continue
if d_free >= d_new:
# get cpoint at d_new
cpoint = ptg.get_cpoint_at_d(d_new, k_rand)
new_pose_rel = cpoint.pose.copy()
new_pose = ptg_nearest_pose + new_pose_rel # type: PoseR2S2
accept_this_node = True
goal_dist = new_pose.distance_2d(goal_pose)
goal_ang = abs(
helper.angle_distance(new_pose.theta, goal_pose.theta))
is_acceptable_goal = goal_dist < goal_dist_tolerance and goal_ang < goal_ang_tolerance
new_nearest_node = None # type: Node
if not is_acceptable_goal:
new_node = Node(ptg, new_pose)
new_nearest_ptg, new_nearest_node, new_nearest_dist = self.tree.get_aptg_nearest_node(
new_node, aptg)
if new_nearest_node is not None:
new_nearest_ang = abs(
helper.angle_distance(
new_pose.theta,
new_nearest_node.pose.theta))
accept_this_node = new_nearest_dist >= 0.1 or new_nearest_ang >= 0.35
# ToDo: make 0.1 and 0.35 configurable parameters
if not accept_this_node:
continue
new_edge = Edge(ptg, k_rand, d_new, ptg_nearest_node,
new_pose)
candidate_new_nodes.update({d_new: new_edge})
#print('Candidate node found')
else: # path is not free
#print('Obstacle ahead!')
# do nothing for now
pass
if len(candidate_new_nodes) > 0:
best_edge = candidate_new_nodes.peekitem(-1)[1] # type : Edge
new_state_node = Node(best_edge.ptg, best_edge.end_pose,
best_edge.parent)
self.tree.insert_node_and_edge(best_edge.parent,
new_state_node, best_edge)
#print('new node added to tree from ptg {0}'.format(best_edge.ptg.name))
goal_dist = best_edge.end_pose.distance_2d(goal_pose)
print("New note : ", new_state_node.pose)
print("Goal distance of the current node : ", goal_dist)
goal_ang = abs(
helper.angle_distance(best_edge.end_pose.theta,
goal_pose.theta))
is_acceptable_goal = goal_dist < goal_dist_tolerance and goal_ang < goal_ang_tolerance
min_goal_dist_yet = min(goal_dist, min_goal_dist_yet)
print("Best Goal distance : ", min_goal_dist_yet)
if is_acceptable_goal:
print('goal reached!')
break
# To do: continue running to refine solution
print("Counter = ", counter, " Number of nodes :",
len(self.tree.nodes))
self.solving_time = time.time() - start_time
print('Done in {0:.2f} seconds'.format(time.time() - start_time))
#self.solving_time = time.time() - start_time
print('Minimum distance to goal reached is {0}'.format(
min_goal_dist_yet))
if not is_acceptable_goal:
print('Solution not found within iteration limit')
self.planner_success = False
self.total_number_of_iterations = counter
self.total_number_of_nodes = len(self.tree.nodes)
self.best_path_length = 0.0 #Todo : add calculation!
self.best_distance_to_target = min_goal_dist_yet
# dump results
if self.config['csv_out_file'] != '':
self.solution_to_csv(self.config['csv_out_file'])
if self.config['plot_tree_file'] != '':
self.tree.plot_nodes(self.world, goal_pose,
self.config['goal_dist_tolerance'],
self.config['plot_tree_file'])
#if self.config['plot_solution'] != '':
# self.trace_solution(self.aptgs[0].vehicle, goal_pose, self.config['plot_solution'])
return
# set parameters to get results
self.planner_success = True
self.total_number_of_iterations = counter
self.total_number_of_nodes = len(self.tree.nodes)
self.best_path_length = 0.0 # Todo : add calculation!
self.best_distance_to_target = min_goal_dist_yet
# dump results
if self.config['csv_out_file'] != '':
self.solution_to_csv(self.config['csv_out_file'])
if self.config['plot_tree_file'] != '':
self.tree.plot_nodes(self.world, goal_pose,
self.config['goal_dist_tolerance'],
self.config['plot_tree_file'])
if self.config['plot_solution'] != '':
self.trace_solution(self.aptgs[0].vehicle, goal_pose,
self.config['plot_solution'])
def get_distance_metric(self, from_pose: PoseR2S2,
to_pose: PoseR2S2) -> float:
delta_pose = to_pose.diff(from_pose)
return sqrt(delta_pose.x**2 + delta_pose.y**2 + delta_pose.theta**2)
def execute_motion(self, pose: PoseR2S2, v: float, w: float,
dt: float) -> PoseR2S2:
x_new = pose.x + dt * v * cos(pose.theta)
y_new = pose.y + dt * v * sin(pose.theta)
theta_new = wrap_to_npi_pi(pose.theta + dt * w)
return PoseR2S2(x_new, y_new, theta_new)