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