def test_normalize_angle(self): self.assertTrue( math.isclose(utils.normalize_angle(radians(360 + 1)), radians(1))) self.assertTrue( math.isclose(utils.normalize_angle(radians(-360 - 50)), radians(-50))) self.assertTrue( math.isclose(utils.normalize_angle(radians(-360 - 190)), radians(170))) self.assertTrue( math.isclose(utils.normalize_angle(radians(-360 * 5 - 190)), radians(170)))
def get_ideal(self, current, t): """ Parameters: ---------- current: np.array(x, y, theta) current pose (is not used in this agent) t: float elapsed time Returns: ---------- np.array(x, y, theta) ideal pose of t """ d0 = 0.0 d1 = d0 + 1.0 / INPUT_V d2 = d1 + np.pi / 2.0 / INPUT_OMEGA d3 = d2 + 2.0 / INPUT_V d4 = d3 + np.pi / 2.0 / INPUT_OMEGA d5 = d4 + 2.0 / INPUT_V d6 = d5 + np.pi / 2.0 / INPUT_OMEGA d7 = d6 + 2.0 / INPUT_V d8 = d7 + np.pi / 2.0 / INPUT_OMEGA d9 = d8 + 1.0 / INPUT_V delta = t % d9 if d0 <= delta < d1: ideal = np.array((1.0, 0 + INPUT_V * delta, np.pi / 2.0)) elif d1 <= delta < d2: ideal = np.array( (1.0, 1.0, np.pi / 2.0 + INPUT_OMEGA * (delta - d1))) elif d2 <= delta < d3: ideal = np.array((1.0 - INPUT_V * (delta - d2), 1.0, np.pi)) elif d3 <= delta < d4: ideal = np.array((-1.0, 1.0, np.pi + INPUT_OMEGA * (delta - d3))) elif d4 <= delta < d5: ideal = np.array( (-1.0, 1.0 - INPUT_V * (delta - d4), np.pi * 3.0 / 2.0)) elif d5 <= delta < d6: ideal = np.array( (-1.0, -1.0, np.pi * 3.0 / 2.0 + INPUT_OMEGA * (delta - d5))) elif d6 <= delta < d7: ideal = np.array((-1.0 + INPUT_V * (delta - d6), -1.0, 0.0)) elif d7 <= delta < d8: ideal = np.array((1.0, -1.0, INPUT_OMEGA * (delta - d7))) elif d8 <= delta < d9: ideal = np.array((1.0, -1.0 + INPUT_V * (delta - d8), np.pi / 2.0)) else: raise NotImplementedError ideal[2] = utils.normalize_angle(ideal[2]) return ideal
def _eval_theta(cls, next, destination): """ Parameters: ---------- next: np.array(x, y, theta) candidate pose calculated from the Robot's motion model destination: np.array(x, y, theta) destination pose Returns: ---------- float the difference angle between the destination's theta and candidate pose's theta (smaller is better) """ return np.abs(utils.normalize_angle(next[2] - destination[2]))
def _eval_heading(cls, next, destination): """ Parameters: ---------- next: np.array(x, y, theta) candidate pose calculated from the Robot's motion model destination: np.array(x, y, theta) destination pose Returns: ---------- float the difference angle between the direction to destination and candidate pose's theta (smaller is better) """ angle = np.arctan2(destination[1] - next[1], destination[0] - next[0]) return np.abs(utils.normalize_angle(angle - next[2]))
def get_ideal(self, current, t): """ Parameters: ---------- current: np.array(x, y, theta) current pose (is not used in this agent) t: float elapsed time Returns: ---------- np.array(x, y, theta) ideal pose of t """ ideal = np.array((0, 0, np.pi / 2.0)) + np.array((np.cos( INPUT_OMEGA * t), np.sin(INPUT_OMEGA * t), INPUT_OMEGA * t)) ideal[2] = utils.normalize_angle(ideal[2]) return ideal
def get_ideal(self, current, t): """ Parameters: ---------- current: np.array(x, y, theta) current pose t: float elapsed time (is not used in this agent) Returns: ---------- np.array(x, y, theta) ideal pose of t """ if np.linalg.norm(self.target[:2] - current[:2]) < DISTANCE_THRESHOLD and \ np.abs(utils.normalize_angle(self.target[2] - current[2])) < ANGLE_THRESHOLD: self.target = next(self.itr) return self.target
def move(cls, current, input, delta): """ Parameters: ---------- current: np.array(x, y, theta) current pose input: np.array(v, omega) input vector delta: float time delta Returns: ---------- np.array(x, y, theta) predict pose of next tick """ theta = current[2] omega = input[1] next = current + Robot.T(theta, omega, delta).dot(input.T) next[2] = utils.normalize_angle(next[2]) return next