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