def latlon_diff(lat_ref, lon_ref, lat, lon): """Calculate difference in North and East from two GPS coordinates Parameters ---------- lat_ref : float Latitude of origin (decimal format) lon_ref : float Longitude of origin (decimal format) offset_N : float Offset in North direction (meters) offset_E : float Offset in East direction (meters) lat : lon : Returns ------- """ d_lon = lon - lon_ref d_lat = lat - lat_ref dist_N = deg2rad(d_lat) * EARTH_RADIUS_M dist_E = deg2rad(d_lon) * EARTH_RADIUS_M * cos(deg2rad(lat)) return (dist_N, dist_E)
def __init__(self): self.arm_length = 0.4 self.arm_config = "x" self.pose = [ 0.0, 0.0, 10.0, deg2rad(10.0), deg2rad(20.0), deg2rad(30.0) ]
def test_sandbox(self): p_I_C = np.array([1.0, 2.0, 3.0]) p_G_I = np.array([1.0, 0.0, 0.0]) rpy_IG = np.array([deg2rad(0.0), deg2rad(0.0), deg2rad(0.0)]) q_IG = euler2quat(rpy_IG) C_IG = C(q_IG) print(p_G_I + np.dot(C_IG, p_I_C))
def test_update(self): setpoints = np.array([deg2rad(0.0), deg2rad(0.0), deg2rad(10.0), 0.5]) dt = 0.001 for i in range(1000): actual = np.array([ self.quadrotor.states[0], self.quadrotor.states[1], self.quadrotor.states[2], self.quadrotor.states[8] ]) motor_inputs = self.attitude_controller.update( setpoints, actual, dt) self.quadrotor.update(motor_inputs, dt)
def test_observed_features(self): # Setup camera model K = camera_intrinsics(554.25, 554.25, 320.0, 320.0) camera_model = PinholeCameraModel(640, 640, K) # Setup random 3d features feature = np.array([[0.0], [0.0], [10.0]]) # Test rpy = np.array([deg2rad(0.0), deg2rad(-10.0), 0.0]) t = np.array([0.0, 0.0, 0.0]) observed = camera_model.observed_features(feature, rpy, t) # Assert self.assertTrue(len(observed) > 0)
def update(self, setpoints, actual, dt): """ Update attitude controller Args: setpoint actual dt Returns: outputs """ self.dt += dt if self.dt < 0.001: return self.outputs # update yaw error actual_yaw = rad2deg(actual[2]) setpoint_yaw = rad2deg(setpoints[2]) error_yaw = setpoint_yaw - actual_yaw if error_yaw > 180.0: error_yaw -= 360.0 elif error_yaw < -180.0: error_yaw += 360.0 error_yaw = deg2rad(error_yaw) # roll pitch yaw r = self.roll_controller.update(setpoints[0], actual[0], self.dt) p = self.pitch_controller.update(setpoints[1], actual[1], self.dt) y = self.yaw_controller.update(error_yaw, 0.0, self.dt) # thrust max_thrust = 5.0 t = max_thrust * setpoints[3] # convert relative to true thrust t = max_thrust if t > max_thrust else t # limit thrust t = 0.0 if t < 0 else t # limit thrust # map roll, pitch, yaw and thrust to motor outputs outputs = [0.0, 0.0, 0.0, 0.0] outputs[0] = -p - y + t outputs[1] = -r + y + t outputs[2] = p - y + t outputs[3] = r + y + t # limit outputs for i in range(4): if outputs[i] > max_thrust: outputs[i] = max_thrust elif outputs[i] < 0.0: outputs[i] = 0.0 # keep track of outputs self.outputs = outputs self.dt = 0.0 return outputs
def test_plot_elbow_manipulator(self): # Link angles link1_theta = 20.0 link2_theta = 10.0 # Create base frame rpy = [0.0, 0.0, 0.0] R_BG = euler2rot([deg2rad(i) for i in rpy], 321) t_G_B = np.array([2.0, 1.0, 0.0]) T_GB = np.array([[R_BG[0, 0], R_BG[0, 1], R_BG[0, 2], t_G_B[0]], [R_BG[1, 0], R_BG[1, 1], R_BG[1, 2], t_G_B[1]], [R_BG[2, 0], R_BG[2, 1], R_BG[2, 2], t_G_B[2]], [0.0, 0.0, 0.0, 1.0]]) # Create DH Transforms T_B1 = dh_transform(deg2rad(link1_theta), 0.0, 1.0, 0.0) T_12 = dh_transform(deg2rad(link2_theta), 0.0, 1.0, 0.0) # Create Transforms T_G1 = np.dot(T_GB, T_B1) T_G2 = np.dot(T_GB, np.dot(T_B1, T_12)) # Transform from origin to end-effector links = [] links.append(T_G1) links.append(T_G2) # Plot first link # debug = True debug = False if debug: plt.figure() plt.plot([T_GB[0, 3], links[0][0, 3]], [T_GB[1, 3], links[0][1, 3]]) plt.plot([links[0][0, 3], links[1][0, 3]], [links[0][1, 3], links[1][1, 3]]) obs = np.array([1.0, 1.0, 0.0, 1.0]) obs_end = np.dot(T_G2, obs) plt.plot(obs_end[0], obs_end[1], marker="x") plt.xlim([0.0, 6.0]) plt.ylim([0.0, 6.0]) plt.show()
def plot_3d_cube(ax, width, origin, orientation): """ Plot 3D cube Parameters ---------- ax : matplotlib.axes.Axes Plot axes width : float Cube width origin : np.array Origin orientation : np.array Orientation """ # Cube points points = np.array([[-1, -1, -1], [1, -1, -1], [1, 1, -1], [-1, 1, -1], [-1, -1, 1], [1, -1, 1], [1, 1, 1], [-1, 1, 1]]) points = points * (width / 2.0) # Rotate Cube R = euler2rot([deg2rad(i) for i in orientation], 123) points = np.array([dot(R, pt) for pt in points]) # Translate Cube points += origin # Plot mesh grid r = [-1, 1] X, Y = np.meshgrid(r, r) # List of sides' polygons of figure verts = [[points[0], points[1], points[2], points[3]], [points[4], points[5], points[6], points[7]], [points[0], points[1], points[5], points[4]], [points[2], points[3], points[7], points[6]], [points[1], points[2], points[6], points[5]], [points[4], points[7], points[3], points[0]], [points[2], points[3], points[7], points[6]]] # Plot sides ax.add_collection3d(Poly3DCollection(verts, facecolors="black", linewidths=1, edgecolors="red", alpha=0.25))
def test_calculate_residuals(self): # Camera states # -- Camera state 0 p_G_C0 = np.array([0.0, 0.0, 0.0]) rpy_C0G = np.array([deg2rad(0.0), deg2rad(0.0), deg2rad(0.0)]) q_C0G = euler2quat(rpy_C0G) C_C0G = C(q_C0G) # -- Camera state 1 p_G_C1 = np.array([1.0, 1.0, 0.0]) rpy_C1G = np.array([deg2rad(0.0), deg2rad(0.0), deg2rad(0.0)]) q_C1G = euler2quat(rpy_C1G) C_C1G = C(q_C1G) # Features landmark = np.array([0.0, 0.0, 10.0]) kp1 = self.cam_model.project(landmark, C_C0G, p_G_C0)[0:2] kp2 = self.cam_model.project(landmark, C_C1G, p_G_C1)[0:2] # Setup feature track track_id = 0 frame_id = 1 data1 = KeyPoint(kp1, 21) data2 = KeyPoint(kp2, 21) track = FeatureTrack(track_id, frame_id, data1, data2) # Setup track cam states self.msckf.augment_state() self.msckf.min_track_length = 2 self.msckf.cam_states[0].p_G = p_G_C0.reshape((3, 1)) self.msckf.cam_states[0].q_CG = q_C0G.reshape((4, 1)) self.msckf.cam_states[1].p_G = p_G_C1.reshape((3, 1)) self.msckf.cam_states[1].q_CG = q_C1G.reshape((4, 1)) # Test self.msckf.calculate_residuals([track])
def test_triangulate(self): # Camera states # -- Camera state 0 p_G_C0 = np.array([0.0, 0.0, 0.0]) rpy_C0G = np.array([deg2rad(0.0), deg2rad(0.0), deg2rad(0.0)]) q_C0G = euler2quat(rpy_C0G) C_C0G = C(q_C0G) # -- Camera state 1 p_G_C1 = np.array([1.0, 1.0, 0.0]) rpy_C1G = np.array([deg2rad(0.0), deg2rad(0.0), deg2rad(0.0)]) q_C1G = euler2quat(rpy_C1G) C_C1G = C(q_C1G) # Features landmark = np.array([0.0, 0.0, 10.0]) kp1 = self.cam_model.project(landmark, C_C0G, p_G_C0)[0:2] kp2 = self.cam_model.project(landmark, C_C1G, p_G_C1)[0:2] # Calculate rotation and translation of first and last camera states # -- Obtain rotation and translation from camera 0 to camera 1 C_C0C1 = dot(C_C0G, C_C1G.T) t_C0_C1C0 = dot(C_C0G, (p_G_C1 - p_G_C0)) # -- Convert from pixel coordinates to image coordinates pt1 = self.cam_model.pixel2image(kp1) pt2 = self.cam_model.pixel2image(kp2) # Triangulate p_C0_C1C0, r = self.estimator.triangulate(pt1, pt2, C_C0C1, t_C0_C1C0) # Assert self.assertTrue(np.allclose(p_C0_C1C0.ravel(), landmark))
def focal_length(image_width, image_height, fov): """Calculate focal length in the x and y axis from: - image width - image height - field of view Parameters ---------- image_width : int Image width image_height : int Image height fov : float Field of view Returns ------- (fx, fy) : (float, float) Focal length in x and y axis """ fx = (image_width / 2.0) / tan(deg2rad(fov) / 2.0) fy = (image_height / 2.0) / tan(deg2rad(fov) / 2.0) return (fx, fy)
def setUp(self): # Generate random features nb_features = 100 feature_bounds = { "x": { "min": -1.0, "max": 1.0 }, "y": { "min": -1.0, "max": 1.0 }, "z": { "min": 10.0, "max": 20.0 } } self.features = rand3dfeatures(nb_features, feature_bounds) # Pinhole Camera model image_width = 640 image_height = 480 fov = 60 fx, fy = focal_length(image_width, image_height, fov) cx, cy = (image_width / 2.0, image_height / 2.0) K = camera_intrinsics(fx, fy, cx, cy) self.cam = PinholeCameraModel(image_width, image_height, K) # Rotation and translation of camera 0 and camera 1 self.R_0 = np.eye(3) self.t_0 = np.zeros((3, 1)) self.R_1 = roty(deg2rad(10.0)) self.t_1 = np.array([1.0, 0.0, 0.0]).reshape((3, 1)) # Points as observed by camera 0 and camera 1 self.obs0 = self.project_points(self.features, self.cam, self.R_0, self.t_0) self.obs1 = self.project_points(self.features, self.cam, self.R_1, self.t_1)
def latlon_offset(lat_ref, lon_ref, offset_N, offset_E): """Calculate new latitude and longitude coordinates with an offset in North and East directions Parameters ---------- lat_ref : float Latitude of origin (decimal format) lon_ref : float Longitude of origin (decimal format) offset_N : float Offset in North direction (meters) offset_E : float Offset in East direction (meters) Returns ------- (lat, lon) """ lat_new = lat_ref + (offset_E / EARTH_RADIUS_M) lon_new = lon_ref + (offset_N / EARTH_RADIUS_M) / cos(deg2rad(lat_ref)) return (lat_new, lon_new)
def test_residualize_track(self): # Camera states # -- Camera state 0 p_G_C0 = np.array([0.0, 0.0, 0.0]) rpy_C0G = np.array([deg2rad(0.0), deg2rad(0.0), deg2rad(0.0)]) q_C0G = euler2quat(rpy_C0G) C_C0G = C(q_C0G) # -- Camera state 1 p_G_C1 = np.array([1.0, 1.0, 0.0]) rpy_C1G = np.array([deg2rad(0.0), deg2rad(0.0), deg2rad(0.0)]) q_C1G = euler2quat(rpy_C1G) C_C1G = C(q_C1G) # Features landmark = np.array([0.0, 0.0, 10.0]) kp1 = self.cam_model.project(landmark, C_C0G, p_G_C0)[0:2] kp2 = self.cam_model.project(landmark, C_C1G, p_G_C1)[0:2] # Setup feature track track_id = 0 frame_id = 1 data1 = KeyPoint(kp1, 21) data2 = KeyPoint(kp2, 21) track = FeatureTrack(track_id, frame_id, data1, data2) # Setup track cam states self.msckf.augment_state() self.msckf.min_track_length = 2 self.msckf.cam_states[0].p_G = p_G_C0.reshape((3, 1)) self.msckf.cam_states[0].q_CG = q_C0G.reshape((4, 1)) self.msckf.cam_states[1].p_G = p_G_C1.reshape((3, 1)) self.msckf.cam_states[1].q_CG = q_C1G.reshape((4, 1)) print(self.msckf.cam_states[0]) print(self.msckf.cam_states[1]) # Test # self.msckf.enable_ns_trick = False H_o_j, r_o_j, R_o_j = self.msckf.residualize_track(track) plt.matshow(H_o_j) plt.show()
def test_estimate(self): estimator = DatasetFeatureEstimator() # Pinhole Camera model image_width = 640 image_height = 480 fov = 60 fx, fy = focal_length(image_width, image_height, fov) cx, cy = (image_width / 2.0, image_height / 2.0) K = camera_intrinsics(fx, fy, cx, cy) cam_model = PinholeCameraModel(image_width, image_height, K) # Camera states track_cam_states = [] # -- Camera state 0 p_G_C0 = np.array([0.0, 0.0, 0.0]) rpy_C0G = np.array([deg2rad(0.0), deg2rad(0.0), deg2rad(0.0)]) q_C0G = euler2quat(rpy_C0G) C_C0G = C(q_C0G) track_cam_states.append(CameraState(0, q_C0G, p_G_C0)) # -- Camera state 1 p_G_C1 = np.array([1.0, 0.0, 0.0]) rpy_C1G = np.array([deg2rad(0.0), deg2rad(0.0), deg2rad(0.0)]) q_C1G = euler2quat(rpy_C1G) C_C1G = C(q_C1G) track_cam_states.append(CameraState(1, q_C1G, p_G_C1)) # Feature track p_G_f = np.array([[0.0], [0.0], [10.0]]) kp0 = KeyPoint(cam_model.project(p_G_f, C_C0G, p_G_C0)[0:2], 0) kp1 = KeyPoint(cam_model.project(p_G_f, C_C1G, p_G_C1)[0:2], 0) track = FeatureTrack(0, 1, kp0, kp1, ground_truth=p_G_f) estimate = estimator.estimate(cam_model, track, track_cam_states) self.assertTrue(np.allclose(p_G_f.ravel(), estimate.ravel(), atol=0.1))
def update(self, motor_inputs, dt): """Update quadrotor motion model Parameters ---------- motor_inputs : np.array Motor inputs dt : float Time difference """ # States ph = self.states[0] th = self.states[1] ps = self.states[2] p = self.states[3] q = self.states[4] r = self.states[5] x = self.states[6] # NOQA y = self.states[7] # NOQA z = self.states[8] # NOQA vx = self.states[9] vy = self.states[10] vz = self.states[11] # Convert motor inputs to angular p, q, r and total thrust A = np.array([[1.0, 1.0, 1.0, 1.0], [0.0, -self.arm_length, 0.0, self.arm_length], [-self.arm_length, 0.0, self.arm_length, 0.0], [-self.d, self.d, -self.d, self.d]]) tau = dot(A, motor_inputs) tauf = tau[0] taup = tau[1] tauq = tau[2] taur = tau[3] # Update self.states[0] = ph + (p + q * sin(ph) * tan(th) + r * cos(ph) * tan(th)) * dt # NOQA self.states[1] = th + (q * cos(ph) - r * sin(ph)) * dt # NOQA self.states[2] = ps + ((1 / cos(th)) * (q * sin(ph) + r * cos(ph))) * dt # NOQA self.states[3] = p + (-((self.Iz - self.Iy) / self.Ix) * q * r - (self.kr * p / self.Ix) + (1 / self.Ix) * taup) * dt # NOQA self.states[4] = q + (-((self.Ix - self.Iz) / self.Iy) * p * r - (self.kr * q / self.Iy) + (1 / self.Iy) * tauq) * dt # NOQA self.states[5] = r + (-((self.Iy - self.Ix) / self.Iz) * p * q - (self.kr * r / self.Iz) + (1 / self.Iz) * taur) * dt # NOQA self.states[6] = x + vx * dt self.states[7] = y + vy * dt self.states[8] = z + vz * dt ax = ((-self.kt * vx / self.m) + (1 / self.m) * (cos(ph) * sin(th) * cos(ps) + sin(ph) * sin(ps)) * tauf) # NOQA ay = ((-self.kt * vy / self.m) + (1 / self.m) * (cos(ph) * sin(th) * sin(ps) - sin(ph) * cos(ps)) * tauf) # NOQA az = (-(self.kt * vz / self.m) + (1 / self.m) * (cos(ph) * cos(th)) * tauf - self.g) # NOQA self.states[9] = vx + ax * dt self.states[10] = vy + ay * dt self.states[11] = vz + az * dt # Constrain yaw to be [-180, 180] self.states[2] = rad2deg(self.states[2]) self.states[2] = deg2rad(self.states[2])
data=np.dot(x.data(), self.data())) # Input is rotation matrix elif x.shape == (3, 3): R = x T_x = np.block([[R, np.zeros((3, 1))], [0.0, 0.0, 0.0, 1.0]]) return np.dot(T, T_x)[0:3, 0:3] # Input is vector (not in homogeneous coordinates) elif len(x) != 4 and len(x) == 3: x = x.ravel() x = np.array([[x[0]], [x[1]], [x[2]], [1.0]]) return np.dot(T, x)[0:3] # Input is vector (in homogeneous coordinates) elif x.shape == (4, 1): return np.dot(T, x) # Input is matrix elif x.shape == (4, 4): return np.dot(T, x) # Useful default transforms R_camera_global = np.dot(rotx(deg2rad(-90.0)), rotz(deg2rad(-90.0))) R_global_camera = np.dot(rotz(deg2rad(90.0)), rotx(deg2rad(90.0))) T_camera_global = Transform("camera", "global", R=R_camera_global) T_global_camera = Transform("global", "camera", data=np.linalg.inv(T_camera_global.data()))
def plot(self, ax): """ Plot quadrotor Parameters ---------- ax : """ # Quadrotor center center = np.array([0.0, 0.0, 0.0]) # Quadrotor axis front = np.array( [center[0] + (self.arm_length * 1.0), center[1], center[2]]) left = np.array( [center[0], center[1] + (self.arm_length * 1.0), center[2]]) up = np.array( [center[0], center[1], center[2] + (self.arm_length * 1.0)]) # Quadrotor arms ("+" configuration) arm_l = np.array([center[0], center[1] + self.arm_length, center[2]]) arm_r = np.array([center[0], center[1] - self.arm_length, center[2]]) arm_f = np.array([center[0] + self.arm_length, center[1], center[2]]) arm_b = np.array([center[0] - self.arm_length, center[1], center[2]]) # Quadrotor arms ("x" configuration) if self.arm_config == "x": R_z = rotz(deg2rad(45.0)) arm_l = np.dot(R_z, arm_l) arm_r = np.dot(R_z, arm_r) arm_f = np.dot(R_z, arm_f) arm_b = np.dot(R_z, arm_b) # Transform quadrotor R = euler2rot(self.pose[3:6], 123) t = np.array(self.pose[0:3]) arm_l = np.dot(R, arm_l) + t arm_r = np.dot(R, arm_r) + t arm_f = np.dot(R, arm_f) + t arm_b = np.dot(R, arm_b) + t front = np.dot(R, front) + t left = np.dot(R, left) + t up = np.dot(R, up) + t center = np.dot(R, center) + t # Plot quadrotor ax.scatter(*center) ax.plot([center[0], front[0]], [center[1], front[1]], [center[2], front[2]], color="r") ax.plot([center[0], left[0]], [center[1], left[1]], [center[2], left[2]], color="g") ax.plot([center[0], up[0]], [center[1], up[1]], [center[2], up[2]], color="b") for arm in [arm_l, arm_r, arm_f, arm_b]: ax.plot([center[0], arm[0]], [center[1], arm[1]], [center[2], arm[2]], color="black", marker="o")
def update(self, setpoints, actual, yaw, dt): """Update controller Parameters ---------- setpoints : np.array - 1x3 Position setpoints in world frame (x, y, z) actual : np.array - 1x3 Actual position in world frame (x, y, z) yaw : float Yaw setpoint dt : float Time difference Returns ------- outputs : np.array - 1x4 Position controller output where the elements represent roll, pitch, yaw and throttle """ # Check rate self.dt += dt if self.dt < 0.01: return self.outputs # Update RPY errors relative to quadrotor by incorporating yaw errors = [0.0, 0.0, 0.0] errors[0] = setpoints[0] - actual[0] errors[1] = setpoints[1] - actual[1] errors[2] = setpoints[2] - actual[2] euler = [0.0, 0.0, actual[3]] R = euler2rot(euler, 123) errors = dot(R, errors) # Roll, pitch, yaw and thrust r = -1.0 * self.y_controller.update(errors[1], 0.0, dt) p = self.x_controller.update(errors[0], 0.0, dt) y = yaw t = 0.5 + self.z_controller.update(errors[2], 0.0, dt) outputs = [r, p, y, t] # Limit roll, pitch for i in range(2): if outputs[i] > deg2rad(30.0): outputs[i] = deg2rad(30.0) elif outputs[i] < deg2rad(-30.0): outputs[i] = deg2rad(-30.0) # Limit thrust if outputs[3] > 1.0: outputs[3] = 1.0 elif outputs[3] < 0.0: outputs[3] = 0.0 # Yaw first if threshold reached if fabs(yaw - actual[3]) > deg2rad(2.0): outputs[0] = 0.0 outputs[1] = 0.0 # Keep track of outputs self.outputs = outputs self.dt = 0.0 return outputs
def update(self, setpoints, actual, dt): """Update Parameters ---------- setpoints : np.array - 1x3 Attitude setpoints in body frame (roll, pitch, yaw, z) actual : np.array - 1x3 Actual attitude in body frame (roll, pitch, yaw, z) yaw : float Yaw setpoint dt : float Time difference Returns ------- outputs : np.array - 1x4 Attitude controller output where the elements represent roll, pitch, yaw and throttle """ self.dt += dt if self.dt < 0.001: return self.outputs # update yaw error actual_yaw = rad2deg(actual[2]) setpoint_yaw = rad2deg(setpoints[2]) error_yaw = setpoint_yaw - actual_yaw if error_yaw > 180.0: error_yaw -= 360.0 elif error_yaw < -180.0: error_yaw += 360.0 error_yaw = deg2rad(error_yaw) # roll pitch yaw r = self.roll_controller.update(setpoints[0], actual[0], self.dt) p = self.pitch_controller.update(setpoints[1], actual[1], self.dt) y = self.yaw_controller.update(error_yaw, 0.0, self.dt) # thrust max_thrust = 5.0 t = max_thrust * setpoints[3] # convert relative to true thrust t = max_thrust if t > max_thrust else t # limit thrust t = 0.0 if t < 0 else t # limit thrust # map roll, pitch, yaw and thrust to motor outputs outputs = [0.0, 0.0, 0.0, 0.0] outputs[0] = -p - y + t outputs[1] = -r + y + t outputs[2] = p - y + t outputs[3] = r + y + t # limit outputs for i in range(4): if outputs[i] > max_thrust: outputs[i] = max_thrust elif outputs[i] < 0.0: outputs[i] = 0.0 # keep track of outputs self.outputs = outputs self.dt = 0.0 return outputs
def test_world_to_body(self): wp = np.array([1.0, 1.0, 3.0]) pos = np.array([0.0, 0.0, 4.0]) yaw = deg2rad(45.0) R = euler2rot(np.array([0.0, 0.0, yaw]), 321)
def test_calc_yaw_to_waypoint(self): wp = np.array([1.0, 1.0, 3.0]) pos = np.array([0.0, 0.0, 3.0]) heading = self.wp_controller.calc_yaw_to_waypoint(wp, pos) self.assertEqual(heading, deg2rad(45.0))
def update(self, pos, yaw, dt): """Update waypoint controller Parameters ---------- pos : numpy array robot position yaw : float Yaw setpoint dt : float Time difference Returns ------- carrot_pt : numpy array carrot point """ # Time pre-check self.dt += dt if self.dt < 0.01: return self.outputs # Get new waypoint wp = self.update_waypoint(pos) # Calculate yaw error # actual_yaw = yaw # setpoint_yaw = self.calc_yaw_to_waypoint(wp, pos) # error_yaw = setpoint_yaw - actual_yaw # if error_yaw > pi: # error_yaw -= 2 * pi # elif error_yaw < -pi: # error_yaw += 2 * pi # Calculate along and cross track errors # R_BG = euler2rot(np.array([0.0, 0.0, yaw]), 321).T # error_p_B = np.dot(R_BG, (wp - pos)) # error_at = error_p_B[1] # error_ct = error_p_B[0] # error_z = error_p_B[2] error_at = wp[0] - pos[0] error_ct = wp[1] - pos[1] error_z = wp[2] - pos[2] # Roll, pitch, yaw and thrust r = -1 * self.ct_ctrl.update(error_ct, 0.0, self.dt) p = self.at_ctrl.update(error_at, 0.0, self.dt) # r = 0.0 # p = 0.0 # y = self.yaw_ctrl.update(error_yaw, 0.0, self.dt) y = 0.0 t = 0.5 + self.z_ctrl.update(error_z, 0.0, self.dt) outputs = [r, p, y, t] # Limit roll, pitch for i in range(2): if outputs[i] > deg2rad(30.0): outputs[i] = deg2rad(30.0) elif outputs[i] < deg2rad(-30.0): outputs[i] = deg2rad(-30.0) # Limit thrust if outputs[3] > 1.0: outputs[3] = 1.0 elif outputs[3] < 0.0: outputs[3] = 0.0 # # Yaw first if threshold reached # if fabs(yaw - actual[3]) > deg2rad(2.0): # outputs[0] = 0.0 # outputs[1] = 0.0 # Keep track of outputs self.outputs = outputs self.dt = 0.0 return outputs
def test_plot(self): # Gimbal initial guess # # -- tau (x, y, z, roll, pitch, yaw) # tau_s = np.array([-0.045, -0.085, 0.08, 0.0, 0.0, 0.0]) # tau_d = np.array([0.01, 0.0, -0.03, pi / 2.0, 0.0, -pi / 2.0]) # # -- link (theta, alpha, a, d) # alpha = pi / 2.0 # offset = -pi / 2.0 # roll = 0.0 # pitch = 0.1 # link1 = np.array([offset + roll, alpha, 0.0, 0.045]) # link2 = np.array([pitch, 0.0, 0.0, 0.0]) # # Optimized gimbal params # offset = -pi / 2.0 # roll = 0.0 # pitch = 0.0 # # tau_s = np.array([-0.0419, -0.0908, 0.0843, 0.0189, -0.0317, -0.0368]) # tau_d = np.array([0.0032, 0.0007, -0.0302, 1.5800, 0.0123, -1.5695]) # link1 = np.array([offset + roll, 1.5731, -0.0012, 0.0404]) # link2 = np.array([pitch, 0.0, 0.0, 0.0]) # Real 2 initial guess # offset = -pi / 2.0 # roll = 0.0 # pitch = 0.0 # tau_s = np.array([0.0, -0.08, 0.1143, 0.0, 0.0, 0.0]) # tau_d = np.array([0.0, 0.0, 0.0, pi / 2.0, 0.0, -pi / 2.0]) # link1 = np.array([offset + roll, # theta # pi / 2.0, # alpha # 0.0, # a # 0.0]) # d # link2 = np.array([pitch, 0.0, 0.0, 0.0]) # Real 2 optimized params offset = -pi / 2.0 roll = 0.0 pitch = 0.0 tau_s = np.array([ 0.01519, -0.036509, 0.096354, deg2rad(2.15041521), deg2rad(1.41060977), deg2rad(18.52822371) ]) link1 = np.array([ offset + roll, # theta deg2rad(83.57483), # alpha -0.0050, # a 0.01683 ]) # d link2 = np.array([pitch, 0.0, 0.0, 0.0]) tau_d = np.array([ -0.00029, 0.00138, -0.03656, deg2rad(84.14281), deg2rad(0.28527), deg2rad(-91.17418) ]) # -- Gimbal model gimbal_model = GimbalModel(tau_s=tau_s, tau_d=tau_d, link1=link1, link2=link2) # T_ds = gimbal_model.T_ds() # p_s = np.array([1.0, 0.0, 0.0, 1.0]) # Plot gimbal model plot = PlotGimbal(gimbal=gimbal_model, show_static_frame=True, show_base_frame=True, show_end_frame=True, show_dynamic_frame=True) plot.set_attitude([0.0, 0.0]) plot.plot() # Plot debug = True # debug = False if debug: plt.show()
def test_deg2rad(self): r = deg2rad(360) self.assertTrue((r - 2 * math.pi) < 0.0001)