def getLimpParametersStatic(pose_x, pose_y, current_vel, prev_traj_self, prev_param_self): # NEW prev_traj = np.copy(prev_traj_self) prev_param = np.copy(prev_param_self) s_tot = np.linspace(0, prev_param[0], trajectory_generator.NUM_STEPS) s_tot -= s_tot[0] _, _, _, idx = pure_pursuit_utils.nearest_point_on_trajectory_py2( np.array([pose_x, pose_y]), prev_traj[:, :2]) idx = min(idx, prev_traj.shape[0] - 3) s_idx = s_tot[idx:] s_idx -= s_idx[0] temp_traj = np.empty((trajectory_generator.NUM_STEPS, 5)) temp_traj[:, :4] = trajectory_generator_utils.multiInterp2( np.linspace(0, s_idx[-1], trajectory_generator.NUM_STEPS), s_idx, prev_traj[idx:, :4]) temp_traj[:, 4] = np.concatenate([ np.geomspace(current_vel, 0.4, BRAKE_STEPS), 0.4 * np.ones( (temp_traj.shape[0] - BRAKE_STEPS, )) ]) prev_traj = temp_traj extra_limp_s = s_tot[idx] prev_param[0] = s_idx[-1] # END NEW # OLD #self.prev_traj[:,4] = np.concatenate([np.geomspace(self.prev_traj[0,4],0.4,BRAKE_STEPS), 0.4*np.ones((self.prev_traj.shape[0]-BRAKE_STEPS,))]) # END OLD pp_traj = np.empty((prev_traj.shape[0], 4)) pp_traj[:, 0:2] = prev_traj[:, 0:2] pp_traj[:, 2] = prev_traj[:, 4] pp_traj[:, 3] = prev_traj[:, 2] return pp_traj, prev_traj, prev_param, extra_limp_s
def _get_current_speed(self, position, theta): wpts = self.waypoints[:, 0:2] position = position[0:2] nearest_point, nearest_dist, t, i = pure_pursuit_utils.nearest_point_on_trajectory_py2( position, wpts) speed = self.waypoints[i, 2] return speed, nearest_dist
def getLimpParameters(self, pose_x, pose_y, current_vel): s_tot = np.linspace(0, self.prev_param[0], trajectory_generator.NUM_STEPS) s_tot -= s_tot[0] _, _, _, idx = pure_pursuit_utils.nearest_point_on_trajectory_py2( np.array([pose_x, pose_y]), self.prev_traj[:, :2]) idx = min(idx, self.prev_traj.shape[0] - 3) s_idx = s_tot[idx:] s_idx -= s_idx[0] temp_traj = np.empty((trajectory_generator.NUM_STEPS, 5)) temp_traj[:, :4] = trajectory_generator_utils.multiInterp2( np.linspace(0, s_idx[-1], trajectory_generator.NUM_STEPS), s_idx, self.prev_traj[idx:, :4]) temp_traj[:, 4] = np.concatenate([ np.geomspace(current_vel, 0.4, BRAKE_STEPS), 0.4 * np.ones( (temp_traj.shape[0] - BRAKE_STEPS, )) ]) self.prev_traj = temp_traj self.limp_s += s_tot[idx] self.prev_param[0] = s_idx[-1] self.is_limping = True prev_traj_plot = self.prev_traj pp_traj = np.empty((self.prev_traj.shape[0], 4)) pp_traj[:, 0:2] = self.prev_traj[:, 0:2] pp_traj[:, 2] = self.prev_traj[:, 4] pp_traj[:, 3] = self.prev_traj[:, 2] return pp_traj, prev_traj_plot
def _get_current_waypoint(self, waypoints, lookahead_distance, position, theta): wpts = waypoints[:, 0:2] nearest_point, nearest_dist, t, i = pure_pursuit_utils.nearest_point_on_trajectory_py2( position, wpts) if nearest_dist < lookahead_distance: lookahead_point, i2, t2 = pure_pursuit_utils.first_point_on_trajectory_intersecting_circle( position, lookahead_distance, wpts, i + t, wrap=True) if i2 == None: return None current_waypoint = np.empty(waypoints[i2, :].shape) current_waypoint[0:2] = waypoints[i2, 0:2] current_waypoint[3] = waypoints[i2, 3] current_waypoint[2] = waypoints[i2, 2] return current_waypoint elif nearest_dist < self.max_reacquire: return waypoints[i, :] else: return None
def updateBeliefHelper(picked_idx_unique,glob_prev_s,glob_prev_opp_pose,waypoints): end_idx_start=np.searchsorted(glob_prev_s,glob_prev_s[0]+2,side='right') hi=range(end_idx_start,len(glob_prev_s)) if len(hi)==0: return None flow_samples=np.empty((len(hi),6)) for end_idx in range(end_idx_start,len(glob_prev_s)): knots=np.linspace(0,end_idx,4).astype(np.int32) knots=knots[1:] vels=np.empty((3,)) for i in range(knots.shape[0]): vels[i]=glob_prev_opp_pose[knots[i],3] _,min_dist,min_frac_t,min_i=pure_pursuit_utils.nearest_point_on_trajectory_py2(glob_prev_opp_pose[knots[-1],0:2],waypoints[:,0:2]) nearest_waypoint=waypoints[min_i] end_theta=glob_prev_opp_pose[knots[-1],2]-nearest_waypoint[3] end_s=glob_prev_s[knots[-1]]-glob_prev_s[0] vec_to_pt=glob_prev_opp_pose[knots[-1],0:2]-nearest_waypoint[0:2] wpt_pt=np.array([np.cos(end_theta),np.sin(end_theta)]) if cross(wpt_pt,vec_to_pt)<0: end_t=-min_dist else: end_t=min_dist flow_samples[end_idx-end_idx_start]=np.concatenate((np.array([end_s,end_t,end_theta]),vels)) return flow_samples