def rotational_speed(self, speed_constraints, dt): diff = center_radians(self._rotation_goal - self.current_pose.theta) if diff < 0: acceleration = -ROTATION_ACCELERATION_MAX else: acceleration = ROTATION_ACCELERATION_MAX t_rotation_stop = abs(self.current_speed.vtheta / acceleration) mul = 1 if abs(diff) <= 3 * ADMITTED_ANGLE_ERROR: mul = 0.2 omega = self.current_speed.vtheta + dt * acceleration * mul planned_stop_angle = center_radians(self.current_pose.theta + 2 * ( self.current_speed.vtheta * t_rotation_stop - 1 / 2 * acceleration * t_rotation_stop ** 2)) self.robot.ivy.highlight_robot_angle(0, self._rotation_goal) self.robot.ivy.highlight_robot_angle(1, planned_stop_angle) planned_angular_error = center_radians(self._rotation_goal - planned_stop_angle) if abs(planned_angular_error) <= ADMITTED_ANGLE_ERROR or diff * planned_angular_error < 0: speed_diff = acceleration * dt if abs(speed_diff) > abs(self.current_speed.vtheta): omega = 0 else: omega = self.current_speed.vtheta - speed_diff elif abs(planned_angular_error) <= 3 * ADMITTED_ANGLE_ERROR and abs(self.current_speed.vtheta) > 0.3: omega = self.current_speed.vtheta omega = min(speed_constraints.max_vtheta, max(speed_constraints.min_vtheta, omega)) # print(omega) return Speed(0, 0, omega)
def compute_speed(self, delta_time, speed_constraints): if self.state == self.eState.IDLE: return Speed(0, 0, 0) elif self.state == self.eState.STRAIGHT: if abs(self._relative_straight_goal) <= ADMITTED_POSITION_ERROR: self.state = self.eState.IDLE return Speed(0, 0, 0) else: dist_remaining = abs(self._relative_straight_goal) - self._start_position.lin_distance_to_point(self.current_pose) if self._relative_straight_goal <0: dist_remaining = -dist_remaining if abs(dist_remaining) <= ADMITTED_POSITION_ERROR: self.state = self.eState.IDLE return Speed(0, 0, 0) else: return self.linear_speed(speed_constraints, dist_remaining, delta_time) elif self.state == self.eState.ROTATE: if abs(center_radians(self.theta - self._rotation_goal)) <= ADMITTED_ANGLE_ERROR: self.state = self.eState.IDLE return Speed(0, 0, 0) else: return self.rotational_speed(speed_constraints, delta_time)
def new_rotate_goal(self, relative_goal): if abs(relative_goal) <= ADMITTED_ANGLE_ERROR: return self._rotation_goal = center_radians(self.current_pose.theta + relative_goal) self.state = self.eState.ROTATE
def __init__(self, behavior): super().__init__(behavior) self.has_turned = False self.robot.locomotion.turn( center_radians(math.pi / 2 - self.robot.locomotion.theta))