def test_pid_wrapper(): """ """ from pid import PID history_length = 5 setpoint = 100.0 Kp = 1.5 Ki = 0.0 Kd = 0.0 pid = PID(history_length, setpoint, Kp, Ki, Kd) process_value = 90.0 delta_time = 1.0 control = pid.control(process_value, delta_time) print("control = {0}".format(control))
class Camera(): def __init__(self, S, dir_queue, cam_data, getCoords_event, navigate_event, END_event): self.font = cv2.FONT_HERSHEY_SIMPLEX self.cam_data = cam_data # for calibration self.db = 0 self.chbEdgeLength = CHB_SIDE self.start = True self.tstart = 0 self.calib = False # termination criteria self.criteria = (cv2.TERM_CRITERIA_EPS + cv2.TERM_CRITERIA_MAX_ITER, 30, 0.001) # prepare object points, like (0,0,0), (1,0,0), (2,0,0) ....,(8,5,0) self.objp = np.zeros((6 * 9, 3), np.float32) self.objp[:, :2] = np.mgrid[0:9, 0:6].T.reshape(-1, 2) * self.chbEdgeLength # arrays to store object points and image points from all the images self.objpoints = [] # 3d point in real world space self.imgpoints = [] # 2d points in image plane. # for loading camera matrices self.not_loaded = True # drone navigation self.speed = S self.amplify = 10 self.dir_queue = dir_queue self.getCoords_event = getCoords_event self.navigate_event = navigate_event self.END_event = END_event self.resetCoords = False self.t_lost = 0. self.t_inPos = 0. self.last_marker_pos = 1. self.beenThere = [] self.TargetID = 1 self.findNew = False self.MarkerTarget = TargetDefine() # controller self.yaw_pid = PID(0.1, 0.00001, 0.001) self.v_pid = PID(0.5, 0.00001, 0.0001) self.vz_pid = PID(0.8, 0.00001, 0.0001) self.TargetPos = np.array([[0., 0., 1., 0.]]) # for aruco markers self.markerEdge = MARKER_SIDE # ArUco marker edge length in meters self.seenMarkers = Markers(MARKER_SIDE, self.getCoords_event) self.getFirst = True # Calibrate camera def calibrator(self, frame): if self.calib == False: gray = cv2.cvtColor(frame, cv2.COLOR_BGR2GRAY) # find the chessboard corners ret, corners = cv2.findChessboardCorners(gray, (9, 6), None) if self.db < 20: # if found, add object points, image points (after refining them) if ret == True and self.start: self.start = False self.tstart = time.time() self.objpoints.append(self.objp) corners2 = cv2.cornerSubPix(gray, corners, (11, 11), (-1, -1), self.criteria) self.imgpoints.append(corners2) # draw and display the corners frame = cv2.drawChessboardCorners(frame, (9, 6), corners2, ret) self.db = self.db + 1 elif ret == True and time.time() - self.tstart > 0.5: self.tstart = time.time() self.objpoints.append(self.objp) corners2 = cv2.cornerSubPix(gray, corners, (11, 11), (-1, -1), self.criteria) self.imgpoints.append(corners2) # draw and display the corners frame = cv2.drawChessboardCorners(frame, (9, 6), corners2, ret) self.db = self.db + 1 else: if ret == True: corners2 = cv2.cornerSubPix(gray, corners, (11, 11), (-1, -1), self.criteria) frame = cv2.drawChessboardCorners(frame, (9, 6), corners2, ret) else: cv2.putText(frame, "Please show chessboard.", (0, 64), self.font, 1, (0, 0, 255), 2, cv2.LINE_AA) else: if self.calib == False: # save the camera matrices first self.calib = True ret, self.mtx, self.dist, self.rvecs, self.tvecs = cv2.calibrateCamera( self.objpoints, self.imgpoints, gray.shape[::-1], None, None) h, w = frame.shape[:2] self.newcameramtx, self.roi = cv2.getOptimalNewCameraMatrix( self.mtx, self.dist, (w, h), 1, (w, h)) np.savez("camcalib", ret=ret, mtx=self.mtx, dist=self.dist, rvecs=self.rvecs, tvecs=self.tvecs) # undistort frame = cv2.undistort(frame, self.mtx, self.dist, None, self.newcameramtx) # crop the image x, y, w, h = self.roi frame = frame[y:y + h, x:x + w] cv2.putText(frame, "Camera calibrated.", (0, 64), self.font, 1, (0, 255, 0), 2, cv2.LINE_AA) return frame # Write battery percentage on frame def writeBattery(self, frame, bat): w = frame.shape[1] h = frame.shape[0] if bat < 25: cv2.putText(frame, "Battery: " + str(bat), (w - 170, h - 10), self.font, 0.8, (0, 0, 255), 2, cv2.LINE_AA) elif bat < 50: cv2.putText(frame, "Battery: " + str(bat), (w - 170, h - 10), self.font, 0.8, (0, 255, 255), 2, cv2.LINE_AA) else: cv2.putText(frame, "Battery: " + str(bat), (w - 170, h - 10), self.font, 0.8, (0, 255, 0), 2, cv2.LINE_AA) return frame # Detect ArUco markers def aruco(self, frame, CoordGet, CoordReset, angles_tof): # Get the calibrated camera matrices if self.not_loaded: with np.load(self.cam_data) as X: self.mtx = X['mtx'] self.dist = X['dist'] self.not_loaded = False h, w = frame.shape[:2] newcameramtx, roi = cv2.getOptimalNewCameraMatrix( self.mtx, self.dist, (w, h), 1, (w, h)) # Undistort frame = cv2.undistort(frame, self.mtx, self.dist, None, newcameramtx) # Crop image x, y, w, h = roi frame = frame[y:y + h, x:x + w] #origFrame=np.copy(frame) gray = cv2.cvtColor(frame, cv2.COLOR_BGR2GRAY) aruco_dict = cv2.aruco.Dictionary_get(cv2.aruco.DICT_7X7_100) parameters = cv2.aruco.DetectorParameters_create() # detecting markers: get corners and IDs corners, ids, _ = cv2.aruco.detectMarkers(gray, aruco_dict, parameters=parameters) # list for all currently seen IDs id_list = [] if CoordReset or self.resetCoords: print("Coordinates reset") self.resetCoords = False self.seenMarkers.nullCoords() if self.getCoords_event.is_set(): CoordGet = True if np.all(ids != None): ### IDs found # pose estimation with marker edge length rvecs, tvecs, _ = cv2.aruco.estimatePoseSingleMarkers( corners, self.markerEdge, self.mtx, self.dist) for i in range(0, ids.size): cv2.aruco.drawAxis(frame, self.mtx, self.dist, rvecs[i], tvecs[i], 0.1) # Draw axis id_list.append(ids[i][0]) # draw square around the markers cv2.aruco.drawDetectedMarkers(frame, corners) # only if navigation is turned on if self.navigate_event.is_set(): if not self.findNew: # draw yellow line to target frame = self.drawCenter(frame, id_list, corners, w, h) # marker navigation self.navigateToMarker(id_list, tvecs, rvecs) else: # if no new markers can be seen, rotate if self.TargetPos[0][3] <= 0 and self.TargetPos[0][3] != -5: self.dir_queue.put([0, 0, 0, self.speed * 2]) else: self.dir_queue.put([0, 0, 0, -self.speed * 2]) # if new marker found, set as target for ID in id_list: if ID not in self.beenThere: self.TargetID = ID self.TargetPos = self.MarkerTarget.changeTarget(ID) self.findNew = False break if not self.getFirst: # Reject markers, which have corners on the edges of the frame id_list, tvecs, rvecs = self.filterCorners( w, h, corners, id_list, tvecs, rvecs) if len(id_list) > 0: # from drone state queue angles = np.array( [[angles_tof[0], angles_tof[1], angles_tof[2]]]) tof = angles_tof[3] # add new marker self.seenMarkers.appendMarker(id_list, tvecs, rvecs, angles, tof) # calculate positions, if marker is already allowed self.seenMarkers.getMov(id_list, tvecs, rvecs, angles) # after the end, it should close, back to first if not self.getCoords_event.is_set(): self.getFirst = True else: ### No IDs found cv2.putText(frame, "No Ids", (0, 64), self.font, 1, (0, 0, 255), 2, cv2.LINE_AA) if timer() - self.t_lost > 2: if self.last_marker_pos >= 0: self.dir_queue.put([0, 0, 0, self.speed * 2]) else: self.dir_queue.put([0, 0, 0, -self.speed * 2]) return frame # Look for origin marker and set directions def navigateToMarker(self, seen_id_list, tvecs, rvecs): if self.TargetID not in seen_id_list: # if lost, rotate in direction of last seen marker's x position if timer() - self.t_lost > 1: if self.last_marker_pos >= 0: self.dir_queue.put([0, 0, 0, self.speed * 2]) else: self.dir_queue.put([0, 0, 0, -self.speed * 2]) else: # select needed vectors ind = seen_id_list.index(self.TargetID) tvec = tvecs[ind] rvec = rvecs[ind] # only selected vectors from now self.last_marker_pos = tvec[0][0] rvec = tf.rotationVectorToEulerAngles(rvec) * 180 / math.pi # flip the yaw angle if marker is upside down if abs(rvec[0][2]) > 90: rvec[0][1] = -rvec[0][1] # print(tvec) # print(rvec) directions = [0., 0., 0., 0.] A = self.amplify * self.speed # amplifier # yaw speed control err_yaw = rvec[0][1] - self.TargetPos[0][3] directions[3] = self.speed / 2 * self.yaw_pid.control(err_yaw) # vx, vy, vz speed control err_x = self.TargetPos[0][0] - tvec[0][0] directions[0] = -A * self.v_pid.control(err_x) err_y = self.TargetPos[0][2] - tvec[0][2] directions[1] = -A * self.v_pid.control(err_y) err_z = self.TargetPos[0][1] - tvec[0][1] directions[2] = A * self.vz_pid.control(err_z) # aligned to setpoint if abs(err_x) < ERROR and abs(err_y) < ERROR and abs( err_z) < ERROR and abs(err_yaw) < 5: if timer() - self.t_inPos > 1: if self.TargetID == 1: # origin marker self.getFirst = False print("Positioned to origin, begin navigation") self.seenMarkers.nullCoords() self.getCoords_event.set() if self.TargetID == 50: # end marker print("End of flight") self.getCoords_event.clear() self.resetNavigators() self.navigate_event.clear() self.END_event.set() # target change if not self.getFirst and not self.END_event.is_set(): self.beenThere.append(self.TargetID) self.changeObjective(seen_id_list, tvecs) if not self.findNew: self.TargetPos = self.MarkerTarget.changeTarget( self.TargetID) # reset PIDs self.yaw_pid.reset() self.v_pid.reset() self.vz_pid.reset() print("Target changed to " + str(self.TargetID)) else: print("Searching for new marker...") else: self.t_inPos = timer() # if not in position, null timer self.t_lost = timer() # null lost timer self.dir_queue.put( directions) # put directions intu navigation queue # Reset all variables needed for navigation def resetNavigators(self): self.beenThere = [] self.TargetID = 1 self.yaw_pid.reset() self.v_pid.reset() self.vz_pid.reset() self.t_lost = 0. self.t_inPos = 0. self.last_marker_pos = 1. self.TargetPos = np.array([[0., 0., 0.8, 0.]]) # Change target of navigation def changeObjective(self, seen_id_list, tvecs): length = len(seen_id_list) dist_minimum = 10000 min_ind = self.TargetID for i in range(length): if np.linalg.norm(tvecs[i]) < dist_minimum and seen_id_list[ i] not in self.beenThere: dist_minimum = np.linalg.norm(tvecs[i]) min_ind = seen_id_list[i] if self.TargetID == min_ind: self.findNew = True else: self.TargetID = min_ind # Draw line to targeted marker def drawCenter(self, frame, seen_id_list, corners, w, h): if self.TargetID not in seen_id_list: pass else: ind = seen_id_list.index(self.TargetID) cx = int((corners[ind][0][0][0] + corners[ind][0][1][0] + corners[ind][0][2][0] + corners[ind][0][3][0]) / 4) cy = int((corners[ind][0][0][1] + corners[ind][0][1][1] + corners[ind][0][2][1] + corners[ind][0][3][1]) / 4) cv2.line(frame, (int(w / 2), int(h / 2)), (cx, cy), (0, 255, 255), 3) return frame # Filter out marker corners on the frame's edges def filterCorners(self, w, h, corners, id_list, tvecs, rvecs): length = len(id_list) rejectIDs = [] mask = np.ones(length) for i in range(length): for j in range(4): xc = corners[i][0][j][0] yc = corners[i][0][j][1] if (xc < w * EDGE) or (xc > w - w * EDGE) or ( yc < h * EDGE) or (yc > h - h * EDGE): rejectIDs.append(id_list[i]) mask[i] = 0 #print("ID "+str(id_list[i])+" rejected") break for i in range(len(rejectIDs)): id_list.remove(rejectIDs[i]) okay = np.where(mask > 0) tvecs = np.r_[tvecs[okay]] rvecs = np.r_[rvecs[okay]] return id_list, tvecs, rvecs
class Drone(VRepObject): SAFETY_RADIUS = 0.3 # meters. Value to add to the real radius of the UAV def __init__(self, client: VRepClient): self._body = client.get_object("Quadricopter_base") self._model = client.get_object("Quadricopter") super().__init__(client.id, self._body.handle, "") self._target = client.get_object("Quadricopter_target") self.sensor = client.get_object("fast3DLaserScanner_sensor") self._rotation_pid = PID(0.2, 0.05, 0.2, 1, max_int=3) self._altitude_pid = PID(0.2, 0.02, 0.2, 1) self._pid = PID(4.5, 0.01, 0.1, 3, 0.15, max_int=10) self.total_distance = 0 self.sensor_offset = self._body.get_position(self.sensor) self.radius = self._get_radius() + self.SAFETY_RADIUS # Base and height of the visibility cone (actually a pyramid) B = 2 * self.sensor.max_depth * sin(radians(self.sensor.angle)) H = 2 * self.sensor.max_depth * cos(radians(self.sensor.angle)) # Constant for pixel-to-meters conversion self.K = self.sensor.res if abs(B - H) > 1e-3: self.K *= H / B def _get_radius(self) -> float: """Return the effective radius of the drone The radius is half of the distance between the extrema of the model's bounding box. """ bbox = self._model.get_bbox() bbox_span = np.linalg.norm(bbox[1]-bbox[0]) return bbox_span / 2 def altitude_adjust(self, goal: VRepObject) -> None: good = err = 0.5 # meters while abs(err) >= good: goal_pos = goal.get_position(self._body) err = goal_pos[2] # z-coordinate correction = self._altitude_pid.control(err) if __debug__: print("Adjusting altitude...", correction) self._target.set_position(self._target.get_position() + np.array([0, 0, correction])) self.total_distance += np.linalg.norm(correction) sleep(1) else: if __debug__: print("...Adjusted. Goal at {} m".format(err)) self._altitude_pid.reset() sleep(2) # Wait for the drone to stabilize def can_reach(self, goal: VRepObject): dist, azimuth, elevation = goal.get_spherical(self._body, self.sensor_offset) delta = goal.get_position(self._target) h_dist = np.linalg.norm(delta[0:2]) res, d = self.sensor.get_depth_buffer() X, Y = pinhole_projection(azimuth, elevation) ball_r = self.radius_to_pixels(dist) mask = cv2.circle(np.zeros_like(d), (X, Y), ball_r, 1, -1) try: min_depth = np.min(d[mask == 1]) * self.sensor.max_depth except ValueError: # Mask has no white pixel. Center view on goal and retry self.lock(goal) return self.can_reach(goal) reachable = h_dist < 1 or dist - min_depth < -self.radius or \ min_depth == self.sensor.max_depth return reachable, d, min_depth, mask def radius_to_pixels(self, dist: float) -> int: """Converts a drone radius in pixels, at the given distance. This function returns the size in pixels of a segment of length RADIUS, placed at distance `dist` and orthogonal to the principal axis of the camera. """ return max(int(self.K * self.radius / dist), 1) def reset_controllers(self): self._pid.reset() self._altitude_pid.reset() self._rotation_pid.reset() def rotate_towards(self, goal: VRepObject): """Rotate the drone until it points towards the goal. Actually, the function rotates the `target` object which is then followed by the `drone` (inside V-REP). """ good = azimuth = 2 # Degrees while abs(azimuth) >= good: euler = self._target.get_orientation() __, azimuth, __ = goal.get_spherical(self._body, self.sensor_offset) correction_angle = self._rotation_pid.control(azimuth) if __debug__: print("Adjusting orientation...", correction_angle) euler[2] += radians(correction_angle) # euler[2] = Yaw self._target.set_orientation(euler) sleep(1) else: if __debug__: print("...Adjusted. Goal at {}°".format(azimuth)) self._rotation_pid.reset() self.stabilize() # Wait for the drone to stabilize on new angle def lock(self, goal: VRepObject): __, azimuth, elevation = goal.get_spherical(self._body, self.sensor_offset) X, Y = pinhole_projection(azimuth, elevation) if abs(elevation) > self.sensor.angle or not 0 <= Y < 256: self.altitude_adjust(goal) self.rotate_towards(goal) def stabilize(self): eps = 0.001 if __debug__: print("UAV stabilization in progress...") while True: lin_v, ang_v = self._body.get_velocity() if all(i < eps for i in lin_v) and all(i < eps for i in ang_v): if __debug__: sleep(0.5) print("...done.") return else: sleep(0.05) def step_towards(self, goal: VRepObject): """Move the drone towards the goal. """ target_pos = self._target.get_position() correction = self._pid.control(-self._target.get_position(goal)) self.total_distance += np.linalg.norm(correction) self._target.set_position(target_pos + correction) def escape(self, goal): # TODO implement wall-following algorithm self.rotate(60) __, d = self.sensor.get_depth_buffer() left_space = len(d[d == 1]) self.rotate(-120) __, d = self.sensor.get_depth_buffer() right_space = len(d[d == 1]) go_left = left_space >= right_space def rotate(self, angle: float): """Perform an arbitrary yaw rotation. Args: angle (float): Yaw angle, in degrees. Positive = rotates left """ self._rotation_pid.reset() while abs(angle) > 2: euler = self._target.get_orientation() correction = self._rotation_pid.control(angle) angle -= correction euler[2] += radians(correction) # euler[2] = Yaw self._target.set_orientation(euler) sleep(1) self.stabilize()
class PIDDemoBase(object): """ """ def __init__(self): """ """ self._units = None # simulation time self._delta_time = None # [s] self._max_time = None # [s] self._time = None # [s] # controller time self._control_delta = None # [s] self._control_interval = None # [steps] # forcing self._forcing_mean = None self._forcing_standard_deviation = None self._forcing = None # state self._control = None self._state_control = None self._state_no_control = None # controller self._set_point = None self._control_bias = None self._pid = None def __str__(self): """ """ message = '' message += "control_delta = {0} s".format(self._control_delta) return message def _initialize_units(self, units): """Assign user supplied units to various variables. """ default = '?' if not units: units = [] needed_units = ['time', 'process', 'control', 'forcing'] for var in needed_units: if var not in units: units[var] = default self._units = units def _initialize_simulation_time(self, delta_time, max_time): """Initialize the time stepping for the process simeanlation """ self._delta_time = delta_time self._max_time = max_time self._time = np.arange(0.0, self._max_time, self._delta_time) def _initialize_controller_time(self, control_delta): """Initialize the timing of the controller, e.g. how often process samples are taken and send to the controller for computation of the control variable. """ self._control_delta = control_delta self._control_interval = int(self._control_delta / self._delta_time) print("Control delta = {0} [s]".format(self._control_delta)) print("Control interval = {0} [time steps]".format( self._control_interval)) def _initialize_constant_forcing(self, forcing_mean): """Use a constant forcing. """ self._forcing_mean = forcing_mean self._forcing = self._forcing_mean * np.ones(len(self._time)) def _initialize_normal_forcing(self, forcing_mean, forcing_standard_deviation): """Use a random forcing from a normal distribution with the specified meand and standard deviation. """ self._forcing_mean = forcing_mean self._forcing_standard_deviation = forcing_standard_deviation np.random.seed(770405) self._forcing = np.random.normal( self._forcing_mean, self._forcing_standard_deviation, len(self._time)) def _initialize_pid(self, history_length, set_point, Kp, Ki, Kd, control_bias): """ """ print("Initializing PID controller:") print(" history length = {0}".format(history_length)) print(" set_point = {0}".format(set_point)) print(" Kp = {0}".format(Kp)) print(" Ki = {0}".format(Ki)) print(" Kd = {0}".format(Kd)) self._pid = PID(history_length, set_point, Kp, Ki, Kd) self._control_bias = control_bias def simulate_no_control(self): """ """ self._state_no_control = np.zeros(len(self._time)) self._state_no_control[0] = self._initial_condition for t in range(1, len(self._time)): previous_state = self._state_no_control[t-1] self._state_no_control[t] = self.process( self._forcing[t], self._delta_time, previous_state, self._control_bias) def simulate_with_control(self): """ """ self._state_control = np.zeros(len(self._time)) self._control = np.zeros(len(self._time)) self._state_control[0] = self._initial_condition self._control[0] = self._control_bias control = self._control[0] for t in range(1, len(self._time)): previous_state = self._state_control[t-1] process_value = self.process(self._forcing[t], self._delta_time, previous_state, control) self._state_control[t] = process_value if t % self._control_interval == 0: control_delta = self._pid.control(process_value, self._delta_time) control = self._control_bias - control_delta # NOTE(bja, 2016-11) for plotting. Want control at all # time points, not just when it is being changed! self._control[t] = control def plot(self): """ """ nrows = 3 ncols = 1 plt.figure(1) plt.subplot(nrows, ncols, 1) plt.plot(self._time, self._state_control, label='controled') plt.plot(self._time, self._state_no_control, label='no control') set_point = self._set_point * np.ones(len(self._time)) plt.plot(self._time, set_point, label='set point') plt.legend(loc='best', ncol=2) plt.ylabel("Process variable [{0}]".format(self._units['process'])) plt.subplot(nrows, ncols, 2) plt.plot(self._time, self._control, label='control variable') control_bias = self._control_bias * np.ones(len(self._time)) plt.plot(self._time, control_bias, label='control_bias') plt.legend(loc='best', ncol=2) plt.ylabel("control variable [{0}]".format(self._units['control'])) plt.subplot(nrows, ncols, 3) plt.plot(self._time, self._forcing, label='forcing') forcing_mean = self._forcing_mean * np.ones(len(self._time)) plt.plot(self._time, forcing_mean, label='forcing mean') plt.legend(loc='best', ncol=2) plt.ylabel("Forcing [{0}]".format(self._units['forcing'])) plt.xlabel("time [{0}]".format(self._units['time'])) plt.show() def summary(self): """summar of the final system state """ print("System summary:") print(" Without control:") value = self._state_no_control[-1] print(" Final process value = {0:1.6e} [{1}]".format( value, self._units['process'])) print(" pv - sp = {0:1.6e} [{1}]".format( value - self._set_point, self._units['process'])) print(" With control:") value = self._state_no_control[-1] print(" Final process value = {0:1.6e} [{1}]".format( value, self._units['process'])) print(" pv - sp = {0:1.6e} [{1}]".format( value - self._set_point, self._units['process'])) value = self._control[-1] print(" Final control value = {0:1.6e} [{1}]".format( value, self._units['control'])) print(" control - bias = {0:1.6e} [{1}]".format( value - self._control_bias, self._units['control'])) def run(self): """ """ self.simulate_no_control() self.simulate_with_control() self.summary() self.plot() @abc.abstractmethod def process(self, forcing, delta_time, previous_state, control_bias): """ """ return None @abc.abstractmethod def _calculate_control_bias(self, steady_state_forcing, set_point): """ """ return None @abc.abstractmethod def _calculate_set_point(self): """ """ return None @abc.abstractmethod def _calculate_dCVdPV(self): """Calculate the steady state dCV/dPV as an approximation for Kp. """ return None @abc.abstractmethod def _calculate_time_scales(self, q_in, h_sp, a_out, h): """Estimate the time scales of the problem: """ return None