def __init__(self, x, y, robot_state, robot_param, vel = 1.0, length_path_end=5.0) : # Class Variables self._mutex = Lock() # Arguments self.x = x self.y = y self.wheelbase = robot_param.wheelbase self.offset = robot_param.length_offset self.min_length_goal_reached = length_path_end self.robot_state = robot_state # Calculate related path element/stuff/foo self.dx, self.dy = self._calc_dot() self.ddx, self.ddy = self._calc_ddot() self.yaw = self._calc_yaw(self.dx, self.dy, vel) self.curve = self._calc_curve(self.dx, self.ddx, self.dy, self.ddy) self.steer = self._calc_steer(self.curve, vel) self.x_offset, self.y_offset = self._calc_offset_path(self.x, self.y, self.yaw) self.arclength, self.ds, self.ds_mean = self._calc_arclength(self.dx, self.dy) self.steerRate = self._calc_steerRate(self.steer, self.ds, vel) # Set Goal self.lateral_error = np.full(shape=(len(self.arclength),), fill_value=np.NAN) self.goal = xyYaw_to_matrix(self.x_offset[-1], self.y_offset[-1], self.yaw[-1]) self.goalInv = inv(self.goal) self.index_nn = -1 self.index_nn_lh = -1 self.meters_around_last_nnidx = 0.3 * vel # Multiplication with the desired/mean velocity for normalization self.drivenPath = DrivenPath( ) self.referencePath = None self.predictionPath__dlqt_mpc = None
def _get_errorMatrix(self, desired, l10nOffset, in_robotframe=False) : xo, yo, yaw = l10nOffset[0], l10nOffset[1], l10nOffset[2] actual = xyYaw_to_matrix(xo, yo, yaw) if in_robotframe == False: diff_tf = inv(desired) @ actual else: # in Robot-Frame diff_tf = inv(actual) @ desired pass return diff_tf
def _l10n_discontinous(self, yawRate): xyYaw_now = self.get_xyYaw() tfMatrix_now = mytf.xyYaw_to_matrix(xyYaw_now) if self.xyYaw_update is None: self.xyYaw_update = xyYaw_now if mytf.getLength2D(xyYaw_now, self.xyYaw_update) > self.l10n_update_distance: self.xyYaw_update = xyYaw_now dx = np.random.normal(0.0, self.var_xy) dy = np.random.normal(0.0, self.var_xy) dYaw = np.random.normal(0.0, self.var_yaw) dYaw = mytf.getYawRightRange(dYaw) self.dtfMatrix = mytf.xyYaw_to_matrix(dx, dy, dYaw) map_2_l10n = tfMatrix_now @ self.dtfMatrix lx, ly, lYaw = map_2_l10n[0, 3], map_2_l10n[1, 3], mytf.matrix_to_yaw( map_2_l10n) self._set_l10n_pose(lx, ly, lYaw)
def _get_indexNearestPosition_byCircle(self, l10nOffset, lookahead_distance): idx_around_last_nn = int( self.meters_around_last_nnidx / np.fabs(self.ds_mean)+1 ) # Define Start-Idx for the Loop start_index = self.index_nn_lh - idx_around_last_nn if start_index < 0 : start_index = 0 # Define End-Idx for the Loop if self.index_nn_lh < 0: # self.index_lookahead is not inited end_index = len(self.x) else: end_index = self.index_nn_lh + idx_around_last_nn if end_index > len(self.x) : end_index = len(self.x) # GET DESIRED-POSE dist_to_lookahead = float("inf") path_desired_idx = self.index_nn_lh actual_tfMatrix = xyYaw_to_matrix(l10nOffset[0], l10nOffset[1], l10nOffset[2]) for i in range(start_index, end_index) : iPath_tfMat = xyYaw_to_matrix(self.x_offset[i], self.y_offset[i], self.yaw[i]) dist_offset2path = getLength2DSign(actual_tfMatrix, iPath_tfMat) if dist_offset2path < 0.0 : continue else : eDist = dist_offset2path - lookahead_distance if eDist <= 0.0 : continue elif eDist < dist_to_lookahead : dist_to_lookahead = eDist path_desired_idx = i if np.fabs(dist_to_lookahead) < np.fabs(self.ds_mean): break desired_pose_matrix = xyYaw_to_matrix(self.x_offset[path_desired_idx], self.y_offset[path_desired_idx], self.yaw[path_desired_idx]) return path_desired_idx, dist_offset2path, desired_pose_matrix
def _get_indexNearestPosition(self, robot_state, look_ahead=None): robot_state_lh = [robot_state[0], robot_state[1], robot_state[2] ] # print("robot_state_lh:", robot_state_lh) # sleep(11.1) if look_ahead is not None: # print("use look ahead") # print("before lh:", robot_state[0] ) robot_state_lh[0] = robot_state_lh[0] + look_ahead * cos(robot_state[2]) robot_state_lh[1] = robot_state_lh[1] + look_ahead * sin(robot_state[2]) # print("after lh:", robot_state[0] ) last_nn_idx = self.index_nn_lh else : last_nn_idx = self.index_nn #-1 # Define Start-Index and End-Index for the Loop start_index = last_nn_idx - int(self.meters_around_last_nnidx / np.fabs(self.ds_mean)+1 ) if start_index < 0 : start_index = 0 if last_nn_idx < 0: # at init, search in whole path end_index = len(self.x) else: end_index = last_nn_idx + int( self.meters_around_last_nnidx / np.fabs(self.ds_mean)+1 ) if end_index > len(self.x) : end_index = len(self.x) # Get index for neatest euclidean distance dL = float('Inf') index = 0 xo, yo, yaw = robot_state_lh[0], robot_state_lh[1], robot_state_lh[2] # print("start_index: ", start_index) # print("end_index: ", end_index) # print("self.meters_around_last_nnidx: ", self.meters_around_last_nnidx) # print("int( self.meters_around_last_nnidx / np.fabs(self.ds_mean) = ", int( self.meters_around_last_nnidx / np.fabs(self.ds_mean) +1) ) for i in range(start_index, end_index) : dL_ = np.hypot(self.x_offset[i] - xo, self.y_offset[i] - yo) if dL_ < dL : index = i dL = dL_ # xyYaw = [self.x[index], self.y[index], self.yaw[index] ] pose_matrix = xyYaw_to_matrix(self.x_offset[index], self.y_offset[index], self.yaw[index]) return index, dL, pose_matrix
def __init__(self, robot_parameter, x=0.0, y=0.0, yaw=0.0, steerAngle=0.0, velocity=0.0, var_xy=0.0, var_yaw=0.0, l10n_update_distance=0.0): self._mutex = Lock() self.look_ahead = robot_parameter.look_ahead self.length_offset = robot_parameter.length_offset self.var_xy = var_xy self.var_yaw = var_yaw self.l10n_update_distance = l10n_update_distance self.xyYaw_update = None self.dtfMatrix = mytf.xyYaw_to_matrix(x=0.0, y=0.0, yaw=0.0) # POSE self.x = x self.y = y self.xo = x + self.length_offset * cos(yaw) self.yo = y + self.length_offset * sin(yaw) self.yaw = yaw self.yawRate = 0.0 # Localization self.lx = x self.ly = y self.lYaw = yaw self.lxo = self.xo self.lyo = self.yo # STEERING self.steerAngle = steerAngle self.steerAngleDes = 0.0 self.steerAngleRate = 0.0 self.steerAngleRateDes = 0.0 self.deltaVel = 0.0 self.deltaVelDes = 0.0 # DRIVE self.velocity = velocity self.accel = 0.0
def _is_behindGoal(self, robot_state, velocity): xo, yo, yaw = robot_state[0], robot_state[1], robot_state[2] actual = xyYaw_to_matrix(xo, yo, yaw) goal2actual = self.goalInv @ actual length_to_goal = np.hypot( goal2actual[0,3], goal2actual[1,3] ) if length_to_goal > self.min_length_goal_reached : return False else : dx = goal2actual[0, 3] if velocity > 0.0 and dx > 0.0: return True elif velocity < 0.0 and dx < 0.0: return True else : False
def _get_errorMatrix(self, desired, robot_state) : xo, yo, yaw = robot_state[0], robot_state[1], robot_state[2] actual = xyYaw_to_matrix(xo, yo, yaw) diff_tf = inv(desired) @ actual return diff_tf
################## if __name__ == "__main__": print("MAIN for testing the class") rp = Robot_Parameter() rs = Robot_State(rp) # rs.x = 3.0 # print(rsd.get_asVector() ) rsd_disc.set_pose(x=4.9, y=2.6, yaw=1.2) print("BLA:", rsd_disc.get_asVector()) # Test Varaiance max_ = 0.0 for a in range(1): rand_x = np.random.normal(0.0, rsd_disc.var_xy) rand_y = np.random.normal(0.0, rsd_disc.var_xy) print("dist", np.hypot(rand_x, rand_y), "rand_x:", rand_x, "rand_y:", rand_y) dist = np.hypot(rand_x, rand_y) if dist > max_: max_ = dist print("max_", max_) print(mytf.xyYaw_to_matrix(x=1.0, y=2.0, yaw=1.4))