def get_control(self, odom): R = np.array([[np.cos(self.th), -np.sin(self.th)],[np.sin(self.th), np.cos(self.th)]]) p_b = - np.matmul(R.T, self.init_org) + np.matmul(R.T, odom[:2]) p_b_tp1 = [p_b[0] + self.x_interval, self.b * np.sin(self.a * (p_b[0] + self.x_interval))] p_g_tp1 = self.init_org + np.matmul(R, p_b_tp1) th_b_tp1 = np.arctan(self.b * self.a * np.cos( p_b_tp1[0] * self.a )) th_g_tp1 = util.wrap_around(th_b_tp1 + self.th) ang_vel = util.wrap_around(th_g_tp1 - odom[2]) / self.sampling_period lin_vel = np.sqrt(np.sum((p_g_tp1 - odom[:2])**2)) / self.sampling_period return np.clip(np.array([lin_vel, ang_vel]), self.limit[0], self.limit[1])
def update(self, z_t, x_t): """ Parameters -------- z_t : observation - radial and angular distances from the agent. x_t : agent state (x, y, orientation) in the global frame. """ # Kalman Filter Update r_pred, alpha_pred = util.relative_distance_polar( self.state[:2], x_t[:2], x_t[2]) diff_pred = np.array(self.state[:2]) - np.array(x_t[:2]) if self.dim == 2: Hmat = np.array([[diff_pred[0], diff_pred[1]], [-diff_pred[1] / r_pred, diff_pred[0] / r_pred] ]) / r_pred elif self.dim == 4: Hmat = np.array([ [diff_pred[0], diff_pred[1], 0.0, 0.0], [-diff_pred[1] / r_pred, diff_pred[0] / r_pred, 0.0, 0.0] ]) / r_pred else: raise ValueError('target dimension for KF must be either 2 or 4') innov = z_t - np.array([r_pred, alpha_pred]) innov[1] = util.wrap_around(innov[1]) R = np.matmul(np.matmul(Hmat, self.cov), Hmat.T) \ + self.obs_noise_func((r_pred, alpha_pred)) K = np.matmul(np.matmul(self.cov, Hmat.T), LA.inv(R)) C = np.eye(self.dim) - np.matmul(K, Hmat) self.cov = np.matmul(C, self.cov) self.state = np.clip(self.state + np.matmul(K, innov), self.limit[0], self.limit[1])
def residual_z_(z, zp): """ z : observation, [r, alpha] zp : predicted observation """ r_z = z - zp r_z[1] = util.wrap_around(r_z[1]) return r_z
def get_control(self, odom): th = self.d_th/180.0*np.pi r = np.sqrt((odom[0] - self.maporigin[0])**2 + (odom[1] - self.maporigin[1])**2) alpha = np.arctan2(odom[1] - self.maporigin[0], odom[0] - self.maporigin[1]) x = r*np.cos(alpha+th) + self.maporigin[0] + np.random.random() - 0.5 y = r*np.sin(alpha+th) + self.maporigin[1] + np.random.random() - 0.5 v = np.sqrt((x - odom[0])**2 + (y - odom[1])**2)/self.sampling_period w = util.wrap_around(np.arctan2(y - odom[1], x - odom[0]) - odom[2])/self.sampling_period return np.clip(np.array([v,w]), self.limit[0], self.limit[1])
def residual_x_(x, xp): """ x : state, [x, y, theta] xp : predicted state """ if dim == 3 or dim == 5: r_x = x - xp r_x[2] = util.wrap_around(r_x[2]) return r_x else: return None
def SE2Dynamics(x, dt, u): """ update dynamics function with a control input -- linear, angular velocities """ assert (len(x) == 3) tw = dt * u[1] # Update the agent state if abs(tw) < 0.001: diff = np.array([ dt * u[0] * np.cos(x[2] + tw / 2), dt * u[0] * np.sin(x[2] + tw / 2), tw ]) else: diff = np.array([ u[0] / u[1] * (np.sin(x[2] + tw) - np.sin(x[2])), u[0] / u[1] * (np.cos(x[2]) - np.cos(x[2] + tw)), tw ]) new_x = x + diff new_x[2] = util.wrap_around(new_x[2]) return new_x
def gen_rand_pose(self, frame_xy, frame_theta, min_lin_dist, max_lin_dist, min_ang_dist, max_ang_dist, additional_frame=None): """Genertes random position and yaw. Parameters -------- frame_xy, frame_theta : xy and theta coordinate of the frame you want to compute a distance from. min_lin_dist : the minimum linear distance from o_xy to a sample point. max_lin_dist : the maximum linear distance from o_xy to a sample point. min_ang_dist : the minimum angular distance (counter clockwise direction) from c_theta to a sample point. max_ang_dist : the maximum angular distance (counter clockwise direction) from c_theta to a sample point. """ if max_ang_dist < min_ang_dist: max_ang_dist += 2*np.pi rand_ang = util.wrap_around(np.random.rand() * \ (max_ang_dist - min_ang_dist) + min_ang_dist) rand_r = np.random.rand() * (max_lin_dist - min_lin_dist) + min_lin_dist rand_xy = np.array([rand_r*np.cos(rand_ang), rand_r*np.sin(rand_ang)]) rand_xy_global = util.transform_2d_inv(rand_xy, frame_theta, np.array(frame_xy)) if additional_frame: rand_xy_global = util.transform_2d_inv(rand_xy_global, additional_frame[2], np.array(additional_frame[:2])) is_valid = not(self.MAP.is_collision(rand_xy_global)) return is_valid, [rand_xy_global[0], rand_xy_global[1], rand_ang + frame_theta]
def collision(self, odom): self.init_org = odom[:2] self.th = util.wrap_around(odom[2] + np.random.random()*np.pi/2 - np.pi/4)