def generate(self): # Setup nb_images = 100 R_CG = euler2rot([-pi / 2.0, 0.0, -pi / 2.0], 123) # Generate static camera data self.intrinsics.K_new = self.intrinsics.K() static_cam_data = ECData(None, self.intrinsics, self.chessboard) x = self.calc_static_camera_view() X = dot(R_CG, self.chessboard.grid_points3d.T).T for i in range(nb_images): static_cam_data.corners2d_ud.append(x) static_cam_data.corners3d_ud.append(X) static_cam_data.corners2d_ud = np.array(static_cam_data.corners2d_ud) static_cam_data.corners3d_ud = np.array(static_cam_data.corners3d_ud) # Generate gimbal data roll_vals, pitch_vals = self.calc_roll_pitch_combo(nb_images) gimbal_cam_data = ECData(None, self.intrinsics, self.chessboard) joint_data = [] for roll in roll_vals: for pitch in pitch_vals: self.gimbal.set_attitude([roll, pitch]) x, X = self.calc_gimbal_camera_view() gimbal_cam_data.corners2d_ud.append(x) gimbal_cam_data.corners3d_ud.append(X) joint_data.append([roll, pitch]) gimbal_cam_data.corners2d_ud = np.array(gimbal_cam_data.corners2d_ud) gimbal_cam_data.corners3d_ud = np.array(gimbal_cam_data.corners3d_ud) joint_data = np.array(joint_data) return [static_cam_data, gimbal_cam_data], joint_data
def T_de(self, tau_d): """ Form transform matrix from end-effector to dynamic camera Parameters ---------- tau_s : np.array Parameterization of the transform matrix where the first 3 elements in the vector is the translation from end effector to dynamic camera frame. Second 3 elements is rpy, which is the roll pitch yaw from from end effector to dynamic camera frame. Returns ------- T_de : np.array Transform matrix from end effector to dynamic camera """ # Setup t = tau_d[0:3] R = euler2rot(tau_d[3:6], 321) # Create transform T_de = np.array([[R[0, 0], R[0, 1], R[0, 2], t[0]], [R[1, 0], R[1, 1], R[1, 2], t[1]], [R[2, 0], R[2, 1], R[2, 2], t[2]], [0.0, 0.0, 0.0, 1.0]]) return T_de
def calc_gimbal_camera_view(self): # Create transform from global to static camera frame t_g_sg = np.array([0.0, 0.0, 0.0]) R_sg = euler2rot([-pi / 2.0, 0.0, -pi / 2.0], 321) T_gs = np.array([[R_sg[0, 0], R_sg[0, 1], R_sg[0, 2], t_g_sg[0]], [R_sg[1, 0], R_sg[1, 1], R_sg[1, 2], t_g_sg[1]], [R_sg[2, 0], R_sg[2, 1], R_sg[2, 2], t_g_sg[2]], [0.0, 0.0, 0.0, 1.0]]) # Calculate transform from global to dynamic camera frame links = self.gimbal.calc_transforms() T_sd = links[-1] T_gd = dot(T_gs, T_sd) # Project chessboard grid points in global to dynamic camera frame # -- Convert 3D points to homogeneous coordinates X = self.chessboard.grid_points3d.T X = np.block([[X], [np.ones(X.shape[1])]]) # -- Project to dynamica camera image frame X = dot(np.linalg.inv(T_gd), X)[0:3, :] x = dot(self.gimbal_camera.K, X) # -- Normalize points x[0, :] = x[0, :] / x[2, :] x[1, :] = x[1, :] / x[2, :] x = x[0:2, :].T return x, X.T
def update(self, v_B, w_B, dt): """Update motion model Parameters ---------- v_B : np.array - 3x1 Velocity in body frame w_B : np.array - 3x1 Angular velocity in body frame """ # Update robot state # -- Position p_kp1_G = self.p_G + self.v_G * dt # -- Velocity R_BG = euler2rot(self.rpy_G, 321) v_kp1_G = np.dot(R_BG, v_B) # -- Acceleration a_kp1_G = v_kp1_G - self.v_G a_kp1_G[2] = 9.81 # -- Orientation rpy_kp1_G = self.rpy_G + w_B * dt # -- Finish up self.p_G = p_kp1_G self.v_G = v_kp1_G self.a_G = a_kp1_G self.rpy_G = rpy_kp1_G # Update IMU measurements # -- Acceleration self.a_B = np.dot(R_BG.T, self.a_G) # -- Velocity self.v_B = v_B # -- Angular velocity self.w_B = w_B
def T_bs(self, tau_s): """ Form transform matrix from static camera to base mechanism Parameters ---------- tau_s : np.array Parameterization of the transform matrix where the first 3 elements in the vector is the translation from static camera to base frame. Second 3 elements is rpy, which is the roll pitch yaw from static camera to base frame. Returns ------- T_bs : np.array Transform matrix from static camera to base_frame """ # Setup t = tau_s[0:3] rpy = tau_s[3:6] R = euler2rot(rpy, 321) # Create base frame T_bs = np.array([[R[0, 0], R[0, 1], R[0, 2], t[0]], [R[1, 0], R[1, 1], R[1, 2], t[1]], [R[2, 0], R[2, 1], R[2, 2], t[2]], [0.0, 0.0, 0.0, 1.0]]) return T_bs
def calc_static_camera_view(self): # Transforming chessboard grid points in global to camera frame R = np.eye(3) t = np.zeros(3) R_CG = euler2rot([-pi / 2.0, 0.0, -pi / 2.0], 123) X = dot(R_CG, self.chessboard.grid_points3d.T) x = self.static_camera.project(X, R, t).T[:, 0:2] return x
def generate(self): # Setup nb_images = 4 R_CG = euler2rot([-pi / 2.0, 0.0, -pi / 2.0], 123) # Generate static camera data self.intrinsics.K_new = self.intrinsics.K() static_cam_data = PreprocessData("IMAGES", images_dir=None, intrinsics=self.intrinsics, chessboard=self.chessboard) x = self.calc_static_camera_view() X = dot(R_CG, self.chessboard.grid_points3d.T).T for i in range(nb_images): static_cam_data.corners2d_ud.append(x) static_cam_data.corners3d_ud.append(X) static_cam_data.corners2d_ud = np.array(static_cam_data.corners2d_ud) static_cam_data.corners3d_ud = np.array(static_cam_data.corners3d_ud) # Generate gimbal data roll_vals, pitch_vals = self.calc_roll_pitch_combo(nb_images) gimbal_cam_data = PreprocessData("IMAGES", images_dir=None, intrinsics=self.intrinsics, chessboard=self.chessboard) joint_data = [] for roll in roll_vals: for pitch in pitch_vals: self.gimbal.set_attitude([roll, pitch]) x, X = self.calc_gimbal_camera_view() gimbal_cam_data.corners2d_ud.append(x) gimbal_cam_data.corners3d_ud.append(X) joint_data.append([roll, pitch]) gimbal_cam_data.corners2d_ud = np.array(gimbal_cam_data.corners2d_ud) gimbal_cam_data.corners3d_ud = np.array(gimbal_cam_data.corners3d_ud) joint_data = np.array(joint_data) # Setup measurement sets Z = [] for i in range(nb_images): # Corners 3d observed in both the static and dynamic cam P_s = static_cam_data.corners3d_ud[i] P_d = gimbal_cam_data.corners3d_ud[i] # Corners 2d observed in both the static and dynamic cam Q_s = static_cam_data.corners2d_ud[i] Q_d = gimbal_cam_data.corners2d_ud[i] Z_i = [P_s, P_d, Q_s, Q_d] Z.append(Z_i) K_s = static_cam_data.intrinsics.K_new K_d = gimbal_cam_data.intrinsics.K_new # Distortion - assume no distortion D_s = np.zeros((4, )) D_d = np.zeros((4, )) return Z, K_s, K_d, D_s, D_d, joint_data
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 observed_features(self, features, rpy, t): """Return features are observed by camera Parameters ---------- features : np.array Features rpy : np.array or list - size 3 Roll, pitch and yaw t : np.array - size 3 Translation vector Returns ------- observed : list of [[np.array - 1x2, int], ...] List of observed 3D features as a tuple (pixel coordinates, feature idx) """ observed = [] # rotation matrix R = euler2rot(rpy, 123) # projection matrix P = projection_matrix(self.K, R, np.dot(-R, t)) # check which features are observable from camera feature_idx = 0 for f in features.T: # project 3D world point to 2D image plane point = np.array([f[0], f[1], f[2], 1.0]) img_pt = np.dot(P, point) # check to see if feature is valid and infront of camera if img_pt[2] < 1.0: continue # skip this feature! It is not infront of camera # normalize pixels img_pt[0] = img_pt[0] / img_pt[2] img_pt[1] = img_pt[1] / img_pt[2] img_pt[2] = img_pt[2] / img_pt[2] # check to see if feature observed is within image plane x_ok = (img_pt[0] < self.image_width) and (img_pt[0] > 0.0) y_ok = (img_pt[1] < self.image_height) and (img_pt[1] > 0.0) if x_ok and y_ok: observed.append((img_pt[0:2], feature_idx)) # Update feature_idx += 1 return observed
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 __init__(self, **kwargs): self.nb_rows = kwargs.get("nb_rows", 10) self.nb_cols = kwargs.get("nb_cols", 10) self.square_size = kwargs.get("square_size", 0.1) center_x = ((self.nb_cols - 1) * self.square_size) / 2.0 center_y = ((self.nb_rows - 1) * self.square_size) / 2.0 self.center = np.array([center_x, center_y]) self.R_BG = kwargs.get("R_BG", euler2rot([-pi / 2, 0.0, -pi / 2], 321)) self.t_G = kwargs.get("t_G", np.zeros(3)) # Grid_points as a list of (x, y) points # starting from top left corner to bottom right self.grid_points = [] for i in range(self.nb_rows): for j in range(self.nb_rows): self.grid_points.append([i, j]) self.grid_points = self.square_size * np.array(self.grid_points)
def plot(self, ax=None): """ Plot gimbal Parameters ---------- ax : matplotlib.axes.Axes Plot axes """ if ax is None: fig = plt.figure() ax = fig.gca(projection='3d') ax.set_xlabel("x") ax.set_ylabel("y") ax.set_zlabel("z") # Create transform from global origin to static camera t_g_sg = np.array([0.0, 0.0, 0.0]) R_sg = euler2rot([-pi / 2.0, 0.0, -pi / 2.0], 123) T_sg = np.array([[R_sg[0, 0], R_sg[0, 1], R_sg[0, 2], t_g_sg[0]], [R_sg[1, 0], R_sg[1, 1], R_sg[1, 2], t_g_sg[1]], [R_sg[2, 0], R_sg[2, 1], R_sg[2, 2], t_g_sg[2]], [0.0, 0.0, 0.0, 1.0]]) # Calculate gimbal transforms T_bs, T_eb, T_de = self.gimbal.calc_transforms() # Plot static camera frame length = 0.01 if self.show_static_frame: T_gs = np.linalg.inv(T_sg) self.plot_coord_frame(ax, T_gs, "S", length=length) # Plot base mechanism frame if self.show_base_frame: T_bg = dot(T_bs, T_sg) T_gb = np.linalg.inv(T_bg) self.plot_coord_frame(ax, T_gb, "B", length=length) # Plot end effector frame if self.show_end_frame: T_eg = dot(T_eb, dot(T_bs, T_sg)) T_ge = np.linalg.inv(T_eg) self.plot_coord_frame(ax, T_ge, "E", length=length) # Plot dynamic camera frame if self.show_dynamic_frame: T_dg = dot(T_de, dot(T_eb, dot(T_bs, T_sg))) T_gd = np.linalg.inv(T_dg) self.plot_coord_frame(ax, T_gd, "D", length=length) # Plot links # self.link0 = ax.plot([0, T_gs[0, 3]], # [0, T_gs[1, 3]], # [0, T_gs[2, 3]], # '--', color="black") # self.link1 = ax.plot([T_gs[0, 3], T_gb[0, 3]], # [T_gs[1, 3], T_gb[1, 3]], # [T_gs[2, 3], T_gb[2, 3]], # '--', color="black") # self.link2 = ax.plot([T_gb[0, 3], T_ge[0, 3]], # [T_gb[1, 3], T_ge[1, 3]], # [T_gb[2, 3], T_ge[2, 3]], # '--', color="black") # self.link3 = ax.plot([T_ge[0, 3], T_gd[0, 3]], # [T_ge[1, 3], T_gd[1, 3]], # [T_ge[2, 3], T_gd[2, 3]], # '--', color="black") # Plot settings # if ax is None: # axis_equal_3dplot(ax) axis_equal_3dplot(ax) return ax
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 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 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