def connect_to_goal(self, mps, exp_rate, dist_to_end=float("inf")):
new_mps = Motion_plan_state(mps.x, mps.y)
d, theta = self.get_distance_angle(new_mps, self.goal)
new_mps.path = [new_mps]
if dist_to_end > d:
dist_to_end = d
n_expand = math.floor(dist_to_end / exp_rate)
for _ in range(n_expand):
new_mps.x += exp_rate * math.cos(theta)
new_mps.y += exp_rate * math.sin(theta)
new_mps.path.append(Motion_plan_state(new_mps.x, new_mps.y))
d, _ = self.get_distance_angle(new_mps, self.goal)
if d <= dist_to_end:
new_mps.path.append(self.goal)
new_mps.path[0] = mps
return new_mps
def steer(self,
mps,
dist_to_end,
diff_max,
freq,
min_dist,
velocity=1,
traj_time_stamp=False):
#dubins library
'''new_mps = Motion_plan_state(from_mps.x, from_mps.y, theta = from_mps.theta)
new_mps.path = []
q0 = (from_mps.x, from_mps.y, from_mps.theta)
q1 = (to_mps.x, to_mps.y, to_mps.theta)
turning_radius = 1.0
path = dubins.shortest_path(q0, q1, turning_radius)
configurations, _ = path.sample_many(exp_rate)
for configuration in configurations:
new_mps.path.append(Motion_plan_state(x = configuration[0], y = configuration[1], theta = configuration[2]))
new_mps.path.append(to_mps)
dubins_path = new_mps.path
new_mps = dubins_path[-2]
new_mps.path = dubins_path'''
new_mps = Motion_plan_state(mps.x,
mps.y,
theta=mps.theta,
plan_time_stamp=time.time() - self.t_start,
traj_time_stamp=mps.traj_time_stamp,
length=mps.length)
new_mps.path = [mps]
'''if dist_to_end > d:
dist_to_end = dist_to_end / 10
n_expand = math.floor(dist_to_end / exp_rate)'''
n_expand = random.uniform(0, freq)
n_expand = math.floor(n_expand / 1)
for _ in range(n_expand):
#setting random parameters
dist = random.uniform(0, dist_to_end) #setting random range
diff = random.uniform(-diff_max, diff_max) #setting random range
if abs(dist) > abs(diff):
s1 = dist + diff
s2 = dist - diff
radius = (s1 + s2) / (-s1 + s2)
phi = (s1 + s2) / (2 * radius)
ori_theta = new_mps.theta
new_mps.theta += phi
delta_x = radius * (math.sin(new_mps.theta) -
math.sin(ori_theta))
delta_y = radius * (-math.cos(new_mps.theta) +
math.cos(ori_theta))
new_mps.x += delta_x
new_mps.y += delta_y
velocity_temp = random.uniform(0, 2 * velocity)
movement = math.sqrt(delta_x**2 + delta_y**2)
new_mps.traj_time_stamp += movement / velocity_temp
new_mps.length += movement
if movement >= min_dist:
new_mps.path.append(
Motion_plan_state(
new_mps.x,
new_mps.y,
v=velocity_temp,
theta=new_mps.theta,
traj_time_stamp=new_mps.traj_time_stamp,
plan_time_stamp=time.time() - self.t_start,
length=new_mps.length))
#d, theta = self.get_distance_angle(new_mps, to_mps)
'''d, _ = self.get_distance_angle(new_mps, to_mps)
if d <= dist_to_end:
new_mps.path.append(to_mps)'''
#new_node.parent = from_node
new_mps.path[0] = mps
return new_mps