class HoloFly(object):
"""Docstring for HoloFly. """
def __init__(self):
"""TODO: to be defined1. """
# Init an instance of holodeck environment.
self.env = Holodeck.make("UrbanCity")
# Init vimfly to get keyboard commands.
self.vfly = VimFly()
# Init our state vector:
# x = [x, y, z, roll, pitch, yaw, u, v, w, p, q, r]
self.states = np.zeros((12,))
self.commands = np.zeros((4,))
# Make an opencv window.
cv2.namedWindow('Holodeck Image')
# Init previous img
self.prev_frame = None
# Optical flow stuff.
# Make grid of points to compute OF at.
grid = np.indices((512,512))
pixels_to_skip = 8;
row_start = 128
row_end = 384
# Canyon following
# Make points for left section.
left = grid[:, row_start:row_end:pixels_to_skip, pixels_to_skip:128:pixels_to_skip]
left_px = left[1, :, :].reshape((-1,))
left_py = left[0, :, :].reshape((-1,))
left_pixels = np.vstack((left_px, left_py))
self.left_pixels = left_pixels.T.reshape(-1,1,2)
# Make points for right section.
right = grid[:, row_start:row_end:pixels_to_skip, 384+pixels_to_skip:512 :pixels_to_skip]
right_px = right[1, :, :].reshape((-1,))
right_py = right[0, :, :].reshape((-1,))
right_pixels = np.vstack((right_px, right_py))
self.right_pixels = right_pixels.T.reshape(-1,1,2)
# Things to control off
self.lr_diff = 0
# Obstacle avoidance
# Make points for mleft section.
mleft = grid[:, row_start:row_end:pixels_to_skip, 64 + pixels_to_skip:256:pixels_to_skip]
mleft_px = mleft[1, :, :].reshape((-1,))
mleft_py = mleft[0, :, :].reshape((-1,))
mleft_pixels = np.vstack((mleft_px, mleft_py))
self.mleft_pixels = mleft_pixels.T.reshape(-1,1,2)
# Make points for mright section.
mright = grid[:, row_start:row_end:pixels_to_skip, 256+pixels_to_skip:448:pixels_to_skip]
mright_px = mright[1, :, :].reshape((-1,))
mright_py = mright[0, :, :].reshape((-1,))
mright_pixels = np.vstack((mright_px, mright_py))
self.mright_pixels = mright_pixels.T.reshape(-1,1,2)
# Things to control off
self.mlr_diff = 0
# Height
# Make points for mleft section.
bot = grid[:, 384:448:pixels_to_skip, 192:321:pixels_to_skip]
bot_px = bot[1, :, :].reshape((-1,))
bot_py = bot[0, :, :].reshape((-1,))
bot_pixels = np.vstack((bot_px, bot_py))
self.bot_pixels = bot_pixels.T.reshape(-1,1,2)
# Things to control off
self.bot = 0
self.alt_command = 0
# Time to Collision
# Make points for mleft section.
collision_points = grid[:, 200, 200-24:256+24:pixels_to_skip]
# print ("col points: ", collision_points)
col_px = collision_points[1, :].reshape((-1,))
col_py = collision_points[0, :].reshape((-1,))
col_pixels = np.vstack((col_px, col_py))
self.col_pixels = col_pixels.T.reshape(-1,2)
# Things to control off
self.ttc = 9999
# PID controllers.
self.vx_pid = PID(p=1.0, d=0.1, max_=np.pi*30./180., min_=-np.pi*30./180.)
self.vy_pid = PID(p=1.0, d=0.1, max_=np.pi*30./180., min_=-np.pi*30./180.)
self.PID_yaw = PID(p=1.0, i=0.1)
self.yaw_control = 0.0
self.yaw_command = 0.0
# Flag for automode
self.autonomous = False
# Run til we press escape.
self.run()
def run(self):
while(1):
# Get commands from vimfly
command, auto, quit, reset = self.vfly.manual_control()
# Switch autonomous flag
if auto:
self.autonomous = not self.autonomous
print ("Autonomous mode now: ", self.autonomous)
# Set my yaw command
self.yaw_command = self.states[5]
self.alt_command = 0.
# Take a step in the holodeck.
if not self.autonomous:
state, reward, terminal, _ = self.env.step(command)
else:
auto_command = self.auto_control(command)
state, reward, terminal, _ = self.env.step(auto_command)
# Process camera and other sensors.
self.process_sensors(state)
pixels = state[Sensors.PRIMARY_PLAYER_CAMERA]
if self.prev_frame is None:
self.prev_frame = cv2.cvtColor(pixels, cv2.COLOR_BGR2GRAY)
else:
self.image_filters(pixels)
self.prev_frame = cv2.cvtColor(pixels, cv2.COLOR_BGR2GRAY)
# Reset environment if we pressed r
if reset:
self.env.reset()
# Quit if we press ESC
ch = cv2.waitKey(1)
if ch == 27 or quit:
break
def auto_control(self, command):
# print ("\nCommand in", command)
auto_command = command.copy()
# Vx
if self.ttc < 0.25:
vx_desired = 0.0
print ("\n\n\n\nSTOP\n\n\n\n")
else:
vx_desired = 10.0
auto_command[1] = -self.vx_pid.computePID(vx_desired, self.states[6], 1./30)
# auto_command[1] = -self.vx_pid.computePID(command[1].copy()*10.0, self.states[6], 1./30)
# Use OF to compute vy desired
k = 2.0
vy_desired = k*self.lr_diff
# print ("vy desired: ", vy_desired)
auto_command[0] = -self.vy_pid.computePID(vy_desired, self.states[7], 1./30)
# auto_command[0] = -self.vy_pid.computePID(command[0].copy()*10.0, self.states[7], 1./30)
# print ("vx desired", command[1].copy()*5.0)
# print ("vx true", self.states[6])
# print ("vy desired", command[0].copy()*5.0)
# print ("vy true", self.states[7])
# auto_command[2] = self.PID_yaw.computePID(0., self.states[5], 1./30)
# print ("auto_command", auto_command)
# # Use middle pixels to control yaw
# if abs(self.mlr_diff) > 0.2:
# auto_command[2] = 10.0*self.mlr_diff
# # print ("control yaw")
# else:
# auto_command[2] = 0.0
# # print ("no")
# Yaw control
auto_command[2] = self.PID_yaw.computePID(self.yaw_command, self.states[5], 1./30)
# Altitude control
# k_alt = 1.0
# hdot = k_alt*(5. - self.bot_diff)
# self.alt_command += (1./30) * hdot
# auto_command[3] = command[3] + self.alt_command
# print ("altitude command: ", auto_command[3])
return auto_command
def image_filters(self, pixels):
"""Display camera image in a window. """
# Get grayscale of new img.
gray_img = cv2.cvtColor(pixels, cv2.COLOR_BGR2GRAY)
# # Calulate the optical flow at our grid of points.
# grid_points = (256, 256)
# points, status, err = cv2.calcOpticalFlowPyrLK(self.prev_frame, gray_img, grid_points, None)
# print ("points", points)
pixels = self.compute_lr_diff(pixels, gray_img)
pixels = self.compute_mlr_diff(pixels, gray_img)
pixels = self.compute_bot_diff(pixels, gray_img)
pixels = self.compute_ttc(pixels, gray_img)
# Print command on screen
# if self.lr_diff > 0:
# print ("Move Right!")
# else:
# print ("Move Left!")
# if self.mlr_diff > 0:
# print ("Yaw Right!")
# else:
# print ("Yaw Left!")
# Draw circles at points we are compute OF
for x, y in self.mleft_pixels.reshape(-1,2):
cv2.circle(pixels, (x, y), 2, (255, 0, 0), -1)
for x, y in self.mright_pixels.reshape(-1,2):
cv2.circle(pixels, (x, y), 2, (255, 0, 0), -1)
for x, y in self.bot_pixels.reshape(-1,2):
cv2.circle(pixels, (x, y), 2, (0, 0, 255), -1)
# Draw rectangles
cv2.rectangle(pixels, (512, 128), (384, 384), (0, 255, 0), 1)
cv2.rectangle(pixels, (0, 128), (128, 384), (0, 255, 0), 1)
# Display image in its own window.
cv2.imshow('Holodeck Image', pixels)
def compute_lr_diff(self, pixels, gray_img):
# params for ShiTomasi corner detection
feature_params = dict( maxCorners = 100,
qualityLevel = 0.3,
minDistance = 7,
blockSize = 7 )
# Parameters for lucas kanade optical flow
lk_params = dict( winSize = (15,15),
maxLevel = 2,
criteria = (cv2.TERM_CRITERIA_EPS | cv2.TERM_CRITERIA_COUNT, 10, 0.03))
# Create some random colors
color = np.random.randint(0,255,(100,3))
# Compute left OF.
p1_left, st, err = cv2.calcOpticalFlowPyrLK(self.prev_frame, gray_img, self.left_pixels.astype(np.float32), None, **lk_params)
diff_left = p1_left - self.left_pixels
left_px_mean = np.sum(diff_left[:,0,0])/len(diff_left[:,0,0])
left_py_mean = np.sum(diff_left[:,0,1])/len(diff_left[:,0,0])
# print ("\nLeft means: \n", left_px_mean, "\n", left_py_mean)
# Compute right OF.
p1_right, st, err = cv2.calcOpticalFlowPyrLK(self.prev_frame, gray_img, self.right_pixels.astype(np.float32), None, **lk_params)
diff_right = p1_right - self.right_pixels
right_px_mean = np.sum(diff_right[:,0,0])/len(diff_right[:,0,0])
right_py_mean = np.sum(diff_right[:,0,1])/len(diff_right[:,0,0])
# print ("\nright means: \n", right_px_mean, "\n", right_py_mean)
# Compute difference of OF
right_mean_mag = np.sqrt(right_px_mean**2 + right_py_mean**2)
left_mean_mag = np.sqrt(left_px_mean**2 + left_py_mean**2)
self.lr_diff = (left_mean_mag - right_mean_mag)/(left_mean_mag + right_mean_mag)
# self.lr_diff = (left_px_mean - right_px_mean)/(left_px_mean + right_px_mean)
# print ("LR diff: ", self.lr_diff)
# Draw OF vector
scale = 10.
cv2.arrowedLine(pixels, (64, 256), (int(64 + scale*left_px_mean), int(256 + scale*left_py_mean)), (0, 255, 0), 2)
cv2.arrowedLine(pixels, (512-64, 256), (int(512 - 64 + scale*right_px_mean), int(256 + scale*right_py_mean)), (0, 255, 0), 2)
return pixels
def compute_mlr_diff(self, pixels, gray_img):
# params for ShiTomasi corner detection
feature_params = dict( maxCorners = 100,
qualityLevel = 0.3,
minDistance = 7,
blockSize = 7 )
# Parameters for lucas kanade optical flow
lk_params = dict( winSize = (15,15),
maxLevel = 2,
criteria = (cv2.TERM_CRITERIA_EPS | cv2.TERM_CRITERIA_COUNT, 10, 0.03))
# Compute left OF.
p1_left, st, err = cv2.calcOpticalFlowPyrLK(self.prev_frame, gray_img, self.mleft_pixels.astype(np.float32), None, **lk_params)
diff_left = p1_left - self.mleft_pixels
left_px_mean = np.sum(diff_left[:,0,0])/len(diff_left[:,0,0])
left_py_mean = np.sum(diff_left[:,0,1])/len(diff_left[:,0,0])
# print ("\nLeft means: \n", left_px_mean, "\n", left_py_mean)
# Compute right OF.
p1_right, st, err = cv2.calcOpticalFlowPyrLK(self.prev_frame, gray_img, self.mright_pixels.astype(np.float32), None, **lk_params)
diff_right = p1_right - self.mright_pixels
right_px_mean = np.sum(diff_right[:,0,0])/len(diff_right[:,0,0])
right_py_mean = np.sum(diff_right[:,0,1])/len(diff_right[:,0,0])
# print ("\nright means: \n", right_px_mean, "\n", right_py_mean)
# Compute difference of OF
right_mean_mag = np.sqrt(right_px_mean**2 + right_py_mean**2)
left_mean_mag = np.sqrt(left_px_mean**2 + left_py_mean**2)
# self.lr_diff = (left_mean_mag - right_mean_mag)/(left_mean_mag + right_mean_mag)
self.mlr_diff = (abs(left_px_mean) - abs(right_px_mean))/(abs(left_px_mean) + abs(right_px_mean))
# print ("LR diff: ", self.lr_diff)
# Draw OF vector
scale = 10.
cv2.arrowedLine(pixels, (192, 256), (int(192 + scale*left_px_mean), int(256 + scale*left_py_mean)), (255, 0, 0), 2)
cv2.arrowedLine(pixels, (256+64, 256), (int(256 + 64 + scale*right_px_mean), int(256 + scale*right_py_mean)), (255, 0, 0), 2)
return pixels
def compute_bot_diff(self, pixels, gray_img):
# params for ShiTomasi corner detection
feature_params = dict( maxCorners = 100,
qualityLevel = 0.3,
minDistance = 7,
blockSize = 7 )
# Parameters for lucas kanade optical flow
lk_params = dict( winSize = (15,15),
maxLevel = 2,
criteria = (cv2.TERM_CRITERIA_EPS | cv2.TERM_CRITERIA_COUNT, 10, 0.03))
# Compute left OF.
p1_bot, st, err = cv2.calcOpticalFlowPyrLK(self.prev_frame, gray_img, self.bot_pixels.astype(np.float32), None, **lk_params)
diff_bot = p1_bot - self.bot_pixels
bot_px_mean = np.sum(diff_bot[:,0,0])/len(diff_bot[:,0,0])
bot_py_mean = np.sum(diff_bot[:,0,1])/len(diff_bot[:,0,0])
# print ("\nLeft means: \n", left_px_mean, "\n", left_py_mean)
# # Compute difference of OF
bot_mean_mag = np.sqrt(bot_px_mean**2 + bot_py_mean**2)
# left_mean_mag = np.sqrt(left_px_mean**2 + left_py_mean**2)
# # self.lr_diff = (left_mean_mag - right_mean_mag)/(left_mean_mag + right_mean_mag)
# self.mlr_diff = (abs(left_px_mean) - abs(right_px_mean))/(abs(left_px_mean) + abs(right_px_mean))
self.bot_diff = bot_mean_mag
# print ("Bot y: ", bot_mean_mag)
#
# # Draw OF vector
scale = 10.
cv2.arrowedLine(pixels, (256, 384+64), (int(256 + scale*bot_px_mean), int(384 + 64 + scale*bot_py_mean)), (0, 0, 255), 2)
# cv2.arrowedLine(pixels, (256+64, 256), (int(256 + 64 + scale*right_px_mean), int(256 + scale*right_py_mean)), (255, 0, 0), 2)
return pixels
def compute_ttc(self, pixels, gray_img):
# params for ShiTomasi corner detection
feature_params = dict( maxCorners = 100,
qualityLevel = 0.3,
minDistance = 7,
blockSize = 7 )
# Parameters for lucas kanade optical flow
lk_params = dict( winSize = (15,15),
maxLevel = 2,
criteria = (cv2.TERM_CRITERIA_EPS | cv2.TERM_CRITERIA_COUNT, 10, 0.03))
# Compute left OF.
p1_col, st, err = cv2.calcOpticalFlowPyrLK(self.prev_frame, gray_img, self.col_pixels.astype(np.float32), None, **lk_params)
diff_col = p1_col - self.col_pixels
# print ("ttc: ", diff_col)
# print ("ttc pix: ", self.col_pixels)
sx = (self.col_pixels[:,0] - 255)
sxdot = diff_col[:,0]*30.
ttc = abs(np.divide(sx, sxdot))
# print ("sx: ",sx)
# print ("sx dot: ", sxdot)
print ("ttc: ", ttc.mean())
self.ttc = ttc.mean()
# col_px_mean = np.sum(diff_col[:,0,0])/len(diff_col[:,0,0])
# # bot_py_mean = np.sum(diff_bot[:,0,1])/len(diff_bot[:,0,0])
# # print ("\nLeft means: \n", left_px_mean, "\n", left_py_mean)
#
#
# # # Compute difference of OF
# bot_mean_mag = np.sqrt(bot_px_mean**2 + bot_py_mean**2)
# # left_mean_mag = np.sqrt(left_px_mean**2 + left_py_mean**2)
# # # self.lr_diff = (left_mean_mag - right_mean_mag)/(left_mean_mag + right_mean_mag)
# # self.mlr_diff = (abs(left_px_mean) - abs(right_px_mean))/(abs(left_px_mean) + abs(right_px_mean))
# self.bot_diff = bot_mean_mag
# print ("Bot y: ", bot_mean_mag)
# #
# # # Draw OF vector
# scale = 10.
# cv2.arrowedLine(pixels, (256, 384+64), (int(256 + scale*bot_px_mean), int(384 + 64 + scale*bot_py_mean)), (0, 0, 255), 2)
# cv2.arrowedLine(pixels, (256+64, 256), (int(256 + 64 + scale*right_px_mean), int(256 + scale*right_py_mean)), (255, 0, 0), 2)
return pixels
def process_sensors(self, state):
"""Put sensor data into state vector. """
# Orientation Sensor
orientation = state[Sensors.ORIENTATION_SENSOR]
# Make orientation 4x4 homogenous matrix.
homo_orientation = np.diag([1., 1., 1., 1.])
homo_orientation[0:3, 0:3] = orientation
# Convert homo_orientation to roll, pitch, yaw as we know.
rpy = tf.euler_from_matrix(homo_orientation, axes='rxyz')
# Position Sensor
position = state[Sensors.LOCATION_SENSOR]
# Velocity Sensor
vel = state[Sensors.VELOCITY_SENSOR]
# Rotate velocities to get vx, vy
psi_rot = tf.euler_matrix(0, 0, rpy[2])
rotated_vel = (psi_rot[0:3,0:3].dot(vel))/100.
# IMU Sensor
imu = state[Sensors.IMU_SENSOR]
# Save off states in array.
self.states[0:3] = np.resize(position, (3,))
self.states[3:6] = np.resize(rpy, (3,))
self.states[6:9] = np.resize(rotated_vel, (3,))
self.states[9:12] = np.resize(imu[3:6], (3,))
# Lets Fly :)
dt = 0.01
t = 0.0
# Altitude Hold
throttle_eq = quad.mass * 9.81 / quad.max_F
alt_controller_ = PID(p=0.6, i=0.00, d=-0.4)
alt_c = 10.0
for i in range(10000):
t += dt
phi_c = 0.0
theta_c = 0.0
psirate_c = 0.0
throttle_c = alt_controller_.computePID(
alt_c, -quad.x[2], dt) + throttle_eq # eq force = -28.01736
quad.force_and_moments(phi_c, theta_c, psirate_c, throttle_c, dt)
if (i % 10 == 0):
plotter.plot(quad.x)
print "--------------------"
print "iteration #", i
print "pos:", quad.x[0], quad.x[1], quad.x[2], quad.x[5]
print "rot:", quad.x[6], quad.x[7], quad.x[8]
print "time: ", t
print "reward: ", quad.reward
else:
time.sleep(0.01)
if quad.reward > 1000:
alt_c = 0.
if quad.reward > 2000:
alt_c = 20.
class Quadcopter:
def __init__(self, waypoints, x_init=None):
# Waypoints
self.waypoints = waypoints
self.current_wp = waypoints[0, :]
self.wps_complete = 0
self.reward = 0.
self.wp_accept = 0.25 # acceptable distance to pass off waypoint
# Dynamics params
self.mass = 2.856
self.linear_mu = 0.2
self.angular_mu = 0.3
self.ground_effect = [-55.3516, 181.8265, -203.9874, 85.3735, -7.6619]
self.max_l = 6.5080
self.max_m = 5.087
self.max_n = 0.25
self.max_F = 59.844
self.Jx = 0.07
self.Jy = 0.08
self.Jz = 0.12
self.J = np.diag([self.Jx, self.Jy, self.Jz])
# Filter Outputs
self.tau_up_l = 0.1904
self.tau_up_m = 0.1904
self.tau_up_n = 0.1644
self.tau_up_F = 0.1644
self.tau_down_l = 0.1904
self.tau_down_m = 0.1904
self.tau_down_n = 0.2164
self.tau_down_F = 0.2164
# Wind
self.wind_n = 0.0
self.wind_e = 0.0
self.wind_d = 0.0
# Definite intial conditions
if x_init == None:
self.x = np.zeros((12, 1))
# Init controllers
self.roll_controller_ = PID(p=0.1, i=0.0, d=0.0)
self.pitch_controller_ = PID(p=0.1, i=0.0, d=0.0)
self.yaw_controller_ = PID(p=0.1, i=0.0, d=0.0)
# Forces
self.desired_forces_ = Forces()
self.actual_forces_ = Forces()
self.applied_forces_ = Forces()
def force_and_moments(self, phi_c, theta_c, psi_rate_c, throttle, dt):
# unpack state
phi = self.x[6]
theta = self.x[7]
# psi = self.x[8]
p = self.x[9]
q = self.x[10]
r = self.x[11]
# Compute desired forces
self.desired_forces_.l = self.roll_controller_.computePID(
phi_c, phi, dt, p)
self.desired_forces_.m = self.roll_controller_.computePID(
theta_c, theta, dt, q)
self.desired_forces_.n = self.roll_controller_.computePID(
psi_rate_c, r, dt)
self.desired_forces_.Fz = throttle * self.max_F
# Calc acutal output with low-pass filters
taul = self.tau_up_l if (self.desired_forces_.l >
self.applied_forces_.l) else self.tau_down_l
taum = self.tau_up_m if (self.desired_forces_.m >
self.applied_forces_.m) else self.tau_down_m
taun = self.tau_up_n if (self.desired_forces_.n >
self.applied_forces_.n) else self.tau_down_n
tauF = self.tau_up_F if (self.desired_forces_.Fz >
self.applied_forces_.Fz) else self.tau_down_F
# Calc alpha for filter
alphal = dt / (taul + dt)
alpham = dt / (taum + dt)
alphan = dt / (taun + dt)
alphaF = dt / (tauF + dt)
# Apply discrete first-order filter
self.applied_forces_.l = self.sat(
(1 - alphal) * self.applied_forces_.l +
alphal * self.desired_forces_.l, self.max_l, -1.0 * self.max_l)
self.applied_forces_.m = self.sat(
(1 - alpham) * self.applied_forces_.m +
alpham * self.desired_forces_.m, self.max_m, -1.0 * self.max_m)
self.applied_forces_.n = self.sat(
(1 - alphan) * self.applied_forces_.n +
alphan * self.desired_forces_.n, self.max_n, -1.0 * self.max_n)
self.applied_forces_.Fz = self.sat(
(1 - alphaF) * self.applied_forces_.Fz +
alphaF * self.desired_forces_.Fz, self.max_F, 0.0)
# print "applied", self.applied_forces_.n
# TODO add ground effect
ground_effect = 0.0
# TODO add Wind effect
ur = 0.0
vr = 0.0
wr = 0.0
# Apply other forces (i.e. wind)
self.actual_forces_.Fx = -1.0 * self.linear_mu * ur
self.actual_forces_.Fy = -1.0 * self.linear_mu * vr
self.actual_forces_.Fz = -1.0 * self.linear_mu * wr - self.applied_forces_.Fz - ground_effect
self.actual_forces_.l = -1.0 * self.angular_mu * p + self.applied_forces_.l
self.actual_forces_.m = -1.0 * self.angular_mu * q + self.applied_forces_.m
self.actual_forces_.n = -1.0 * self.angular_mu * r + self.applied_forces_.n
# print "actual", self.actual_forces_.n
k1 = self.derivatives(self.x)
k2 = self.derivatives(self.x + (dt / 2.) * k1)
k3 = self.derivatives(self.x + (dt / 2.) * k2)
k4 = self.derivatives(self.x + (dt) * k3)
self.x += (dt / 6.) * (k1 + 2 * k2 + 2 * k3 + k4)
# Compute Reward
# Compute distance to current waypoint
dist = sqrt((self.current_wp[0] - self.x[0])**2 +
(self.current_wp[1] - self.x[1])**2 +
(self.current_wp[2] - self.x[2])**2)
if (dist < self.wp_accept):
if self.wps_complete == (len(self.waypoints[:, 0]) - 1):
self.reward = 1e10
else:
self.wps_complete += 1
self.current_wp = self.waypoints[self.wps_complete, :]
self.reward = self.wps_complete * 1000.
else:
self.reward = self.wps_complete * 1000. + (100. - dist) * 10.
def derivatives(self, state):
# Unpack state
pn = state[0]
pe = state[1]
pd = state[2]
u = state[3]
v = state[4]
w = state[5]
phi = state[6]
theta = state[7]
psi = state[8]
p = state[9]
q = state[10]
r = state[11]
# pos = np.array([[pn], [pe], [pd]])
pos = np.array([pn, pe, pd])
vel = np.array([u, v, w])
# att = np.array([phi, theta, psi])
ang_vel = np.array([p, q, r])
force = np.array([
self.actual_forces_.Fx, self.actual_forces_.Fy,
self.actual_forces_.Fz
])
torque = np.array([
self.actual_forces_.l / self.Jx, self.actual_forces_.m / self.Jy,
self.actual_forces_.n / self.Jz
])
force = np.reshape(force, (3, 1))
# position dot
# Calc trigs
cp = cos(phi)
sp = sin(phi)
ct = cos(theta)
st = sin(theta)
tt = tan(theta)
cpsi = cos(psi)
spsi = sin(psi)
# calc rotation matrix
# R_body_veh = np.array([[ct*cpsi, ct*spsi, -st],
# [sp*st*cpsi-cp*spsi, sp*st*spsi+cp*cpsi, sp*ct],
# [cp*st*cpsi+sp*spsi, cp*st*spsi-sp*cpsi, cp*ct]])
grav = 9.81
fg_body = np.array([
-self.mass * grav * st, self.mass * grav * ct * sp,
self.mass * grav * ct * cp
])
fg_body = np.reshape(fg_body, (3, 1))
rot_posdot = np.array([[
ct * cpsi, sp * st * cpsi - cp * spsi, cp * st * cpsi + sp * spsi
], [ct * spsi, sp * st * spsi + cp * cpsi, cp * st * spsi - sp * cpsi],
[st, -sp * ct, -cp * ct]])
pos_dot = np.matmul(rot_posdot, vel)
vel_dot = np.array([
r * v - q * w, p * w - r * u, q * u - p * v
]) + (1. / self.mass) * force + (1. / self.mass) * fg_body
vel_dot = np.reshape(vel_dot, (3, 1))
rot_attdot = np.array([[1., sp * tt, cp * tt], [0., cp, -sp],
[0., (sp / ct), (cp / ct)]])
att_dot = np.matmul(rot_attdot, ang_vel)
ang_veldot = np.array([((self.Jy - self.Jz) / self.Jx) * q * r,
((self.Jz - self.Jx) / self.Jy) * p * r,
((self.Jx - self.Jy) / self.Jz) * p * q
]) + torque
# xdot
xdot = np.zeros((12, 1))
xdot[0:3] = pos_dot
xdot[2] = -xdot[2] # convert from hdot to pddot
xdot[3:6] = vel_dot
xdot[6:9] = att_dot
xdot[9:] = ang_veldot
return xdot
def sat(self, x, _max, _min):
if (x > _max):
# print "SAT MAX"
return _max
elif (x < _min):
# print "SAT MIN"
return _min
else:
return x
class Controller(object):
def __init__(self):
# load ROS params
# PID gains
u_P = rospy.get_param('~u_P', 0.2)
u_I = rospy.get_param('~u_I', 0.0)
u_D = rospy.get_param('~u_D', 0.01)
v_P = rospy.get_param('~v_P', 0.2)
v_I = rospy.get_param('~v_I', 0.0)
v_D = rospy.get_param('~v_D', 0.01)
w_P = rospy.get_param('~w_P', 3.0)
w_I = rospy.get_param('~w_I', 0.05)
w_D = rospy.get_param('~w_D', 0.5)
x_P = rospy.get_param('~x_P', 0.5)
x_I = rospy.get_param('~x_I', 0.01)
x_D = rospy.get_param('~x_D', 0.1)
y_P = rospy.get_param('~y_P', 0.5)
y_I = rospy.get_param('~y_I', 0.01)
y_D = rospy.get_param('~y_D', 0.1)
z_P = rospy.get_param('~z_P', 1.0)
z_I = rospy.get_param('~z_I', 0.1)
z_D = rospy.get_param('~z_D', 0.4)
psi_P = rospy.get_param('~psi_P', 0.5)
psi_I = rospy.get_param('~psi_I', 0.0)
psi_D = rospy.get_param('~psi_D', 0.0)
tau = rospy.get_param('~tau', 0.04)
# quadcopter params
self.max_thrust = rospy.get_param('dynamics/max_F', 60.0)
self.mass = rospy.get_param('dynamics/mass', 3.0)
self.thrust_eq = (9.80665 * self.mass) / self.max_thrust
self.drag_constant = rospy.get_param('dynamics/linear_mu', 0.1)
# initialize state variables
self.pn = 0.0
self.pe = 0.0
self.pd = 0.0
self.phi = 0.0
self.theta = 0.0
self.psi = 0.0
self.u = 0.0
self.v = 0.0
self.w = 0.0
self.p = 0.0
self.q = 0.0
self.r = 0.0
self.ax = 0.0
self.ay = 0.0
self.az = 0.0
self.throttle = 0.0
# initialize command variables
self.xc_pn = 0.0
self.xc_pe = 0.0
self.xc_pd = 0.0
self.xc_phi = 0.0
self.xc_theta = 0.0
self.xc_psi = 0.0
self.xc_u = 0.0
self.xc_v = 0.0
self.xc_pd = 0.0
self.xc_r = 0.0
self.xc_ax = 0.0
self.xc_ay = 0.0
self.xc_az = 0.0
self.xc_r = 0.0
self.xc_throttle = 0.0
# initialize saturation values
self.max_roll = rospy.get_param('~max_roll', 0.15)
self.max_pitch = rospy.get_param('~max_pitch', 0.15)
self.max_yaw_rate = rospy.get_param('~max_yaw_rate', np.radians(45.0))
self.max_throttle = rospy.get_param('~max_throttle', 1.0)
self.max_u = rospy.get_param('~max_u', 1.0)
self.max_v = rospy.get_param('~max_v', 1.0)
self.max_w = rospy.get_param('~max_x', 1.0)
# initialize PID controllers
self.PID_u = PID(u_P, u_I, u_D, None, None, tau)
self.PID_v = PID(v_P, v_I, v_D, None, None, tau)
self.PID_w = PID(w_P, w_I, w_D, None, None, tau)
self.PID_x = PID(x_P, x_I, x_D, None, None, tau)
self.PID_y = PID(y_P, y_I, y_D, None, None, tau)
self.PID_z = PID(z_P, z_I, z_D, None, None, tau)
self.PID_psi = PID(psi_P, psi_I, psi_D, None, None, tau)
# initialize other class variables
self.prev_time = 0.0
self.is_flying = False
self.control_mode = 4 # MODE_XPOS_YPOS_YAW_ALTITUDE
self.ibvs_active = False
self.command = Command()
# dynamic reconfigure
self.server = Server(ControllerConfig, self.reconfigure_callback)
# initialize subscribers
self.state_sub = rospy.Subscriber('estimate', Odometry,
self.state_callback)
self.is_flying_sub = rospy.Subscriber('is_flying', Bool,
self.is_flying_callback)
self.cmd_sub = rospy.Subscriber('high_level_command', Command,
self.cmd_callback)
self.ibvs_active_sub = rospy.Subscriber('ibvs_active', Bool,
self.ibvs_active_callback)
# initialize publishers
self.command_pub = rospy.Publisher('command', Command, queue_size=10)
# initialize timer
self.update_rate = 200.0
self.update_timer = rospy.Timer(rospy.Duration(1.0 / self.update_rate),
self.send_command)
def send_command(self, event):
# t = time.time() # for seeing how fast this runs
if self.prev_time == 0:
self.prev_time = rospy.get_time()
return
# get dt
now = rospy.get_time()
dt = now - self.prev_time
self.prev_time = now
if dt <= 0:
return
if self.is_flying:
self.compute_control(dt)
self.command_pub.publish(self.command)
else:
reset_integrators()
self.prev_time = rospy.get_time()
# elapsed = time.time() - t
# hz_approx = 1.0/elapsed
# print(hz_approx)
def state_callback(self, msg):
# this should already be coming in NED
self.pn = msg.pose.pose.position.x
self.pe = msg.pose.pose.position.y
self.pd = msg.pose.pose.position.z
self.u = msg.twist.twist.linear.x
self.v = msg.twist.twist.linear.y
self.w = msg.twist.twist.linear.z
# convert quaternion to RPY
quaternion = (msg.pose.pose.orientation.x, msg.pose.pose.orientation.y,
msg.pose.pose.orientation.z, msg.pose.pose.orientation.w)
euler = tf.transformations.euler_from_quaternion(quaternion)
self.phi = euler[0]
self.theta = euler[1]
self.psi = euler[2]
self.p = msg.twist.twist.angular.x
self.q = msg.twist.twist.angular.y
self.r = msg.twist.twist.angular.z
def is_flying_callback(self, msg):
self.is_flying = msg.data
def ibvs_active_callback(self, msg):
self.ibvs_active = msg.data
def cmd_callback(self, msg):
mode = msg.mode
if mode == Command.MODE_XPOS_YPOS_YAW_ALTITUDE:
self.xc_pn = msg.x
self.xc_pe = msg.y
self.xc_pd = msg.F
self.xc_psi = msg.z
self.control_mode = mode
elif mode == Command.MODE_XVEL_YVEL_YAWRATE_ALTITUDE:
self.xc_u = msg.x
self.xc_v = msg.y
self.xc_pd = msg.F
self.xc_r = msg.z
self.control_mode = mode
elif mode == Command.MODE_XACC_YACC_YAWRATE_AZ:
self.xc_ax = msg.x
self.xc_ay = msg.y
self.xc_az = msg.F
self.xc_r = msg.z
self.control_mode = mode
elif mode == Command.MODE_ROLL_PITCH_YAWRATE_THROTTLE:
self.xc_phi = msg.x
self.xc_theta = msg.y
self.xc_throttle = msg.F
self.xc_r = msg.z
self.control_mode = mode
else:
print('roscopter/controller: Unhandled command message of type {}'.
format(mode))
def reconfigure_callback(self, config, level):
tau = config.tau
P = config.u_P
I = config.u_I
D = config.u_D
self.PID_u.setGains(P, I, D, tau)
P = config.v_P
I = config.v_I
D = config.v_D
self.PID_v.setGains(P, I, D, tau)
P = config.w_P
I = config.w_I
D = config.w_D
self.PID_w.setGains(P, I, D, tau)
P = config.x_P
I = config.x_I
D = config.x_D
self.PID_x.setGains(P, I, D, tau)
P = config.y_P
I = config.y_I
D = config.y_D
self.PID_y.setGains(P, I, D, tau)
P = config.z_P
I = config.z_I
D = config.z_D
self.PID_z.setGains(P, I, D, tau)
P = config.psi_P
I = config.psi_I
D = config.psi_D
self.PID_psi.setGains(P, I, D, tau)
self.max_roll = config.max_roll
self.max_pitch = config.max_pitch
self.max_yaw_rate = config.max_yaw_rate
self.max_throttle = config.max_throttle
self.max_u = config.max_u
self.max_v = config.max_v
self.max_w = config.max_w
print('roscopter/controller: new gains')
self.reset_integrators()
return config
def compute_control(self, dt):
if dt <= 0.0000001: # messes up derivative calculation in PID controllers
return
mode_flag = self.control_mode
if mode_flag == Command.MODE_XPOS_YPOS_YAW_ALTITUDE:
# figure out desired velocities (in inertial frame)
# by running the position controllers
pndot_c = self.PID_x.computePID(self.xc_pn, self.pn, dt)
pedot_c = self.PID_y.computePID(self.xc_pe, self.pe, dt)
# calculate desired yaw rate
# first, determine the shortest direction to the commanded psi
if abs(self.xc_psi + 2.0 * np.pi - self.psi) < abs(self.xc_psi -
self.psi):
self.xc_psi += 2.0 * np.pi
elif abs(self.xc_psi - 2.0 * np.pi - self.psi) < abs(self.xc_psi -
self.psi):
self.xc_psi -= 2.0 * np.pi
self.xc_r = self.saturate(
self.PID_psi.computePID(self.xc_psi, self.psi, dt),
self.max_yaw_rate, -self.max_yaw_rate)
# rotate into body frame
# TODO: include pitch and role in this mapping
self.xc_u = self.saturate(
pndot_c * np.cos(self.psi) + pedot_c * np.sin(self.psi),
self.max_u, -self.max_u)
self.xc_v = self.saturate(
-pndot_c * np.sin(self.psi) + pedot_c * np.cos(self.psi),
self.max_v, -self.max_v)
mode_flag = Command.MODE_XVEL_YVEL_YAWRATE_ALTITUDE
if mode_flag == Command.MODE_XVEL_YVEL_YAWRATE_ALTITUDE:
max_ax = np.sin(np.arccos(self.thrust_eq))
max_ay = np.sin(np.arccos(self.thrust_eq))
self.xc_ax = self.saturate(
self.PID_u.computePID(self.xc_u, self.u, dt) +
self.drag_constant * self.u / (9.80665 * self.mass), max_ax,
-max_ax)
self.xc_ay = self.saturate(
self.PID_v.computePID(self.xc_v, self.v, dt) +
self.drag_constant * self.v / (9.80665 * self.mass), max_ay,
-max_ay)
# nested loop for altitude
pddot = -np.sin(self.theta) * self.u + np.sin(self.phi) * np.cos(
self.theta) * self.v + np.cos(self.phi) * np.cos(
self.theta) * self.w
# check to see if IBVS is active
if self.ibvs_active:
pddot_c = self.saturate(
self.xc_pd, self.max_w, -self.max_w
) # this term should be coming in as w and here we are assuming w is close enough to pddot
else:
pddot_c = self.saturate(
self.PID_w.computePID(self.xc_pd, self.pd, dt, pddot),
self.max_w, -self.max_w)
self.xc_az = self.PID_z.computePID(pddot_c, pddot, dt)
# print statement if you want
mode_flag = Command.MODE_XACC_YACC_YAWRATE_AZ
if mode_flag == Command.MODE_XACC_YACC_YAWRATE_AZ:
# Model inversion (m[ax;ay;az] = m[0;0;g] + R'[0;0;-T]
# This model tends to pop the MAV up in the air when a large change
# in control is commanded as the MAV rotates to it's commanded attitude while also ramping up throttle.
# It works quite well, but it is a little oversimplified.
total_acc_c = np.sqrt(
(1.0 - self.xc_az) * (1.0 - self.xc_az) +
self.xc_ax * self.xc_ax + self.xc_ay * self.xc_ay) #(in g's)
# print('roscopter/controller: total_acc = {}'.format(total_acc_c))
if total_acc_c > 0.001:
self.xc_phi = np.arcsin(self.xc_ay / total_acc_c)
self.xc_theta = -1.0 * np.arcsin(self.xc_ax / total_acc_c)
else:
self.xc_phi = 0.0
self.xc_theta = 0.0
# calculate actual throttle (saturate az to be falling at 1 g)
max_az = 1.0 / self.thrust_eq
self.xc_az = self.saturate(self.xc_az, 1.0, -max_az)
total_acc_c = np.sqrt(
(1.0 - self.xc_az) * (1.0 - self.xc_az) +
self.xc_ax * self.xc_ax + self.xc_ay * self.xc_ay) #(in g's)
self.xc_throttle = total_acc_c * self.thrust_eq # calculate the total thrust in normalized units
# print '\nxc_az:', self.xc_az, '\nmax_az:', max_az, '\ntotal_acc_c:', total_acc_c, '\nthrottle:', self.xc_throttle
mode_flag = Command.MODE_ROLL_PITCH_YAWRATE_THROTTLE
if mode_flag == Command.MODE_ROLL_PITCH_YAWRATE_THROTTLE:
# pack up and send the command
self.command.mode = Command.MODE_ROLL_PITCH_YAWRATE_THROTTLE
self.command.F = self.saturate(self.xc_throttle, self.max_throttle,
0.0)
self.command.x = self.saturate(self.xc_phi, self.max_roll,
-self.max_roll)
self.command.y = self.saturate(self.xc_theta, self.max_pitch,
-self.max_pitch)
self.command.z = self.saturate(self.xc_r, self.max_yaw_rate,
-self.max_yaw_rate)
def reset_integrators(self):
self.PID_u.clearIntegrator()
self.PID_v.clearIntegrator()
self.PID_w.clearIntegrator()
self.PID_x.clearIntegrator()
self.PID_y.clearIntegrator()
self.PID_z.clearIntegrator()
self.PID_psi.clearIntegrator()
def saturate(self, x, maximum, minimum):
if (x > maximum):
rVal = maximum
elif (x < minimum):
rVal = minimum
else:
rVal = x
return rVal
