def __init__(self):
self.thrusters = thrusters
# constraint functions always have to be positive for allowed thrusts
self.constraints = []
# bounds is an array of min, max pairs for each thruster
self.bounds = []
for i, t in enumerate(thrusters):
self.constraints.append(lambda x, m=t.max_thrust, i=i: m-x[i])
self.constraints.append(lambda x, mn=t.max_neg_thrust, i=i: x[i]-mn)
self.bounds.append((t.max_neg_thrust, t.max_thrust))
# calculate some "starting step" guesses for the optimization algorithm
# by using 1/4 of the average max thrust
# good start guesses are important and make the algorithm run faster
self.rhobeg = 0.25 * sum([t.max_thrust - t.max_neg_thrust \
for t in self.thrusters]) / (len(self.thrusters) * 2)
# initial guess for optimizing function, currently static; 0 on all
self.initial_guess = np.array((0,) * len(self.thrusters))
# this is multiplied by the errors so we prioritize certain DOFs
self.error_scale = np.ones(6)
self.quat_pid = PIDLoop()
# other options
self.DEBUG = False
def __init__(self):
self.thrusters = thrusters
# this is multiplied by the errors so we prioritize certain DOFs
self.error_scale = np.ones(6)
self.quat_pid = PIDLoop()
# other options
self.DEBUG = False
def __init__(self):
self.thrusters = thrusters
# constraint functions always have to be positive for allowed thrusts
self.constraints = []
# bounds is an array of min, max pairs for each thruster
self.bounds = []
for i, t in enumerate(thrusters):
self.constraints.append(lambda x, m=t.max_thrust, i=i: m-x[i])
self.constraints.append(lambda x, mn=t.max_neg_thrust, i=i: x[i]-mn)
self.bounds.append((t.max_neg_thrust, t.max_thrust))
# calculate some "starting step" guesses for the optimization algorithm
# by using 1/4 of the average max thrust
# good start guesses are important and make the algorithm run faster
self.rhobeg = 0.25 * sum([t.max_thrust - t.max_neg_thrust \
for t in self.thrusters]) / (len(self.thrusters) * 2)
# initial guess for optimizing function, currently static; 0 on all
self.initial_guess = np.array((0,) * len(self.thrusters))
# this is multiplied by the errors so we prioritize certain DOFs
self.error_scale = np.ones(6)
self.quat_pid = PIDLoop()
# other options
self.DEBUG = False
def loop(kalman_watcher, quit_event):
# PID stepping class
pid = PIDLoop(speed=args['speed'])
pid.clean()
# Optimizer class
opt = Optimizer()
opt.DEBUG = bool(args['verbose'])
controller_enabled = True
tm = ThrusterManager()
kalman_watcher.watch(kalman)
# The main loop
while not quit_event.is_set():
if not args['rate']:
kalman_watcher.wait()
start_time = time.time()
g = kalman.get()
dt_qs = timed(tm.update, g)[1]
# Get passive forces and passive torques on the sub in model frame
passives, dt_pass = timed(vehicle.passive_forces, g, tm)
set_shm_wrench(control_passive_forces, passives)
# Handle controller enable / disable feature
if not settings_control.enabled.get() or \
switches.soft_kill.get() or switches.hard_kill.get():
# controller is disabled
if controller_enabled:
set_shm_wrench(control_internal_wrench, [0] * 6)
zero_motors()
controller_enabled = False
if args['rate']:
time.sleep(0.1)
continue
if not controller_enabled: # We have been re-enabled
controller_enabled = True
pid.clean()
# Execute one PID step for all PID controllers.
dt_pid = timed(pid.step)[1]
# Set motors from PID desires.
dt_opt = timed(opt.set_motors, tm, passives)[1]
total_time = time.time() - start_time
if args['verbose']:
times = [("Qs", dt_qs), ("Passives", dt_pass),
("PID", dt_pid), ("Optimizer", dt_opt), ("Total", total_time)]
for name, dt in times:
log("%s step ran in %0.2f HZ (%f ms)" % (name, (1/dt), 1000*dt), copy_to_stdout=True)
if args['rate']:
if total_time > step_time:
log(str(datetime.now()) + \
"## WARN: UNABLE TO MEET RATE. RUNNING AT %f HZ" % (1 / total_time), copy_to_stdout=True)
else:
if args['verbose']:
log("Rate capped at %f HZ" % args['rate'], copy_to_stdout=True)
log("-----", copy_to_stdout=True) #visual break
time.sleep(step_time - total_time)
pid.clean()
zero_motors()
class Optimizer:
""" Class to set motors using optimization techniques (see set_motors) """
def __init__(self):
self.thrusters = thrusters
# constraint functions always have to be positive for allowed thrusts
self.constraints = []
# bounds is an array of min, max pairs for each thruster
self.bounds = []
for i, t in enumerate(thrusters):
self.constraints.append(lambda x, m=t.max_thrust, i=i: m-x[i])
self.constraints.append(lambda x, mn=t.max_neg_thrust, i=i: x[i]-mn)
self.bounds.append((t.max_neg_thrust, t.max_thrust))
# calculate some "starting step" guesses for the optimization algorithm
# by using 1/4 of the average max thrust
# good start guesses are important and make the algorithm run faster
self.rhobeg = 0.25 * sum([t.max_thrust - t.max_neg_thrust \
for t in self.thrusters]) / (len(self.thrusters) * 2)
# initial guess for optimizing function, currently static; 0 on all
self.initial_guess = np.array((0,) * len(self.thrusters))
# this is multiplied by the errors so we prioritize certain DOFs
self.error_scale = np.ones(6)
self.quat_pid = PIDLoop()
# other options
self.DEBUG = False
def update_error_scale(self):
"""
Reads shared memory for the weight of each DOF
"""
#self.error_scale[DOFSet.yaw_i] = priorities.heading.get()
#self.error_scale[DOFSet.pitch_i] = priorities.pitch.get()
#self.error_scale[DOFSet.roll_i] = priorities.roll.get()
#self.error_scale[DOFSet.forward_i] = priorities.forward.get()
#self.error_scale[DOFSet.sway_i] = priorities.sway.get()
#self.error_scale[DOFSet.depth_i] = priorities.depth.get()
# TODO figure out how to do priorities for individual components
# of orientation
self.error_scale[3] = priorities.torque.get()
self.error_scale[4] = priorities.torque.get()
self.error_scale[5] = priorities.torque.get()
self.error_scale_sq = self.error_scale * self.error_scale
def get_error(self, thrusts):
return self.thrusts_output_mat_s.dot(thrusts) - self.desired_output_s
def objective(self, thrusts):
"""
Takes in a thrust for each motor and calculates our "error"
We want to minimize this.
"""
error = self.get_error(thrusts)
error *= self.error_scale
return error.dot(error)
def derivative(self, thrusts):
"""
The derivative of the objective function with respect to
the thrusts. Greatly speeds up optimization.
"""
return 2 * self.thrusts_output_mat_s.T.dot(self.get_error(thrusts) * \
self.error_scale_sq)
def optimize(self, A, b):
"""
Attempts to find the motor values that best achieve the desired
thruster response. First tries the exact solution, then resorts to
a black box optimization routine if the solution will saturate
thrusters.
"""
# After this step, the error should always be zero.
x = A.dot(b)
good = True
for bound, v in zip(self.bounds, x):
if bound[0] > v or bound[1] < v:
good = False
break
# If the zero error point is outside the thruster bounds,
# initiate black box optimization!
if not good:
# Update priorities - possibly remove after tuning is finalized
self.update_error_scale()
x = fmin_slsqp(self.objective, self.initial_guess,
bounds=self.bounds, fprime=self.derivative,
disp=int(self.DEBUG))
return x
def set_motors(self, tm, ft_passive_world, I):
"""
Sets motors using the PID control loop outputs by a sketchy
multivariable optimization technique (aka. control voodoo v3.0)
Takes in:
a ThrusterManager
a wrench of passive forces and passive torques in WORLD SPACE
the moment of inertia tensor
return: Time taken to assign motor values
"""
t1 = time.time()
###########
# TORQUES #
###########
if quat_pid_enabled.get():
# Get the quaternion desired angular accel in world coordinates
desired_angular_accel = self.quat_pid.quat_pid()
else:
# If we are using individual PID loops, we need to do a bit of work
# to get them all correctly into world space
desired_angular_accel = tm.hpr_to_world(
heading_out.get(), pitch_out.get(), roll_out.get()
)
# Convert desired angular accel into desired torque using inertia tensor
desired_torque = I.dot(desired_angular_accel)
# Subtract torques already on the vehicle from the desires
# Our thrusters need only provide the torques not already provided by
# passive torques such as those due to buoyancy and drag
# This greatly reduces the complexities the PID loops have to handle
desired_torque -= ft_passive_world[3:]
# Convert to sub space
desired_torque = tm.orientation.conjugate() * desired_torque
##########
# FORCES #
##########
# Get the desired linear forces from the 3 linear PID loops
# These are returned in "sub" space that is adjusted for heading
# but not pitch and roll
desired_force = np.array((velx_out.get(), vely_out.get(), \
depth_out.get()))
# Convert to world space
desired_force = tm.heading_quat * desired_force
# subtract passive forces from desired forces in world space
# Our thrusters need only provide the forces not already provided by
# passive forces such as those due to drag
desired_force -= ft_passive_world[:3]
# Convert to sub space
desired_force = tm.orientation.conjugate() * desired_force
# limit final desired forces in sub space
# This prevents an aggressive PID loop from destabilizing the sub
desired_force = tm.limit_thrusts(desired_force)
###############################################################
# Assimilate desired forces and torques into a vector of length 6
self.desired_output_s = np.hstack((desired_force, desired_torque))
# Output final desires to shared memory.
set_shm_wrench(control_internal_outs, self.desired_output_s)
################
# OPTIMIZATION #
################
# Get the matrix that calculates outputs from thrusts (6 x 8)
# This is needed for the optimization routine, which uses get_error.
self.thrusts_output_mat_s = tm.thrusts_to_sub
x = self.optimize(tm.sub_to_thrusts, self.desired_output_s)
actual_output_s = self.thrusts_output_mat_s.dot(x)
set_shm_wrench(control_internal_wrench, actual_output_s)
# Output errors to shared memory.
error = actual_output_s - self.desired_output_s
set_shm_wrench(control_internal_opt_errors, error)
# Finally, we convert thrusts to PWM and output values to shared memory
out = [0] * len(self.thrusters)
for i, t in enumerate(self.thrusters):
# this only happens when desires are wild and slsqp fails
# at that point we don't really care about accuracy and just
# truncate for safety.
if x[i] < t.max_neg_thrust:
x[i] = t.max_neg_thrust
elif x[i] > t.max_thrust:
x[i] = t.max_thrust
out[i] = t.thrust_to_pwm(x[i])
if t.reversed_polarity:
out[i] = -out[i]
set_all_motors_from_seq(out)
# Return time required to compute this optimization
return time.time() - t1
def loop(kalman_watcher, quit_event):
# PID stepping class
pid = PIDLoop(speed=args['speed'])
pid.clean()
# Optimizer class
opt = Optimizer()
opt.DEBUG = bool(args['verbose'])
controller_enabled = True
tm = ThrusterManager()
kalman_watcher.watch(shm.kalman)
# The main loop
while not quit_event.is_set():
if not args['rate']:
kalman_watcher.wait()
start_time = time.time()
g = shm.kalman.get()
dt_qs = timed(tm.update, g)[1]
# Get passive forces and passive torques on the sub in model frame
passives, dt_pass = timed(vehicle.passive_forces, g, tm)
set_shm_wrench(shm.control_passive_forces, passives)
# Handle controller enable / disable feature
if not shm.settings_control.enabled.get() or \
shm.switches.soft_kill.get() or shm.switches.hard_kill.get():
# controller is disabled
if controller_enabled:
set_shm_wrench(shm.control_internal_wrench, [0] * 6)
zero_motors()
controller_enabled = False
if args['rate']:
time.sleep(0.1)
continue
if not controller_enabled: # We have been re-enabled
controller_enabled = True
pid.clean()
# Execute one PID step for all PID controllers.
dt_pid = timed(pid.step, tm)[1]
# Set motors from PID desires.
dt_opt = timed(opt.set_motors, tm, passives)[1]
total_time = time.time() - start_time
if args['verbose']:
times = [("Qs", dt_qs), ("Passives", dt_pass), ("PID", dt_pid),
("Optimizer", dt_opt), ("Total", total_time)]
for name, dt in times:
log("%s step ran in %0.2f HZ (%f ms)" %
(name, (1 / dt), 1000 * dt),
copy_to_stdout=True)
if args['rate']:
if total_time > step_time:
log(str(datetime.now()) + \
"## WARN: UNABLE TO MEET RATE. RUNNING AT %f HZ" % (1 / total_time), copy_to_stdout=True)
else:
if args['verbose']:
log("Rate capped at %f HZ" % args['rate'],
copy_to_stdout=True)
log("-----", copy_to_stdout=True) #visual break
time.sleep(step_time - total_time)
pid.clean()
zero_motors()
class Optimizer:
""" Class to set motors using optimization techniques (see set_motors) """
def __init__(self):
self.thrusters = thrusters
# constraint functions always have to be positive for allowed thrusts
self.constraints = []
# bounds is an array of min, max pairs for each thruster
self.bounds = []
for i, t in enumerate(thrusters):
self.constraints.append(lambda x, m=t.max_thrust, i=i: m-x[i])
self.constraints.append(lambda x, mn=t.max_neg_thrust, i=i: x[i]-mn)
self.bounds.append((t.max_neg_thrust, t.max_thrust))
# calculate some "starting step" guesses for the optimization algorithm
# by using 1/4 of the average max thrust
# good start guesses are important and make the algorithm run faster
self.rhobeg = 0.25 * sum([t.max_thrust - t.max_neg_thrust \
for t in self.thrusters]) / (len(self.thrusters) * 2)
# initial guess for optimizing function, currently static; 0 on all
self.initial_guess = np.array((0,) * len(self.thrusters))
# this is multiplied by the errors so we prioritize certain DOFs
self.error_scale = np.ones(6)
self.quat_pid = PIDLoop()
# other options
self.DEBUG = False
def update_error_scale(self):
"""
Reads shared memory for the weight of each DOF
"""
#self.error_scale[DOFSet.yaw_i] = priorities.heading.get()
#self.error_scale[DOFSet.pitch_i] = priorities.pitch.get()
#self.error_scale[DOFSet.roll_i] = priorities.roll.get()
#self.error_scale[DOFSet.forward_i] = priorities.forward.get()
#self.error_scale[DOFSet.sway_i] = priorities.sway.get()
#self.error_scale[DOFSet.depth_i] = priorities.depth.get()
# TODO figure out how to do priorities for individual components
# of orientation
self.error_scale[3] = priorities.torque.get()
self.error_scale[4] = priorities.torque.get()
self.error_scale[5] = priorities.torque.get()
self.error_scale_sq = self.error_scale * self.error_scale
def get_error(self, thrusts):
return self.thrusts_output_mat_s.dot(thrusts) - self.desired_output_s
def objective(self, thrusts):
"""
Takes in a thrust for each motor and calculates our "error"
We want to minimize this.
"""
error = self.get_error(thrusts)
error *= self.error_scale
return error.dot(error)
def derivative(self, thrusts):
"""
The derivative of the objective function with respect to
the thrusts. Greatly speeds up optimization.
"""
return 2 * self.thrusts_output_mat_s.T.dot(self.get_error(thrusts) * \
self.error_scale_sq)
def optimize(self, A, b):
"""
Attempts to find the motor values that best achieve the desired
thruster response. First tries the exact solution, then resorts to
a black box optimization routine if the solution will saturate
thrusters.
"""
# After this step, the error should always be zero.
x = A.dot(b)
good = True
for bound, v in zip(self.bounds, x):
if bound[0] > v or bound[1] < v:
good = False
break
# If the zero error point is outside the thruster bounds,
# initiate black box optimization!
if not good:
# Update priorities - possibly remove after tuning is finalized
self.update_error_scale()
x = fmin_slsqp(self.objective, self.initial_guess,
bounds=self.bounds, fprime=self.derivative,
disp=int(self.DEBUG))
return x
def set_motors(self, tm, ft_passive_s):
"""
Sets motors using the PID control loop outputs by a sketchy
multivariable optimization technique (aka. control voodoo v3.0)
Takes in:
a ThrusterManager
a wrench of passive forces and passive torques in SUB SPACE
the moment of inertia tensor
"""
##########
# FORCES #
##########
# Get the desired linear forces from the 3 linear PID loops
# These are returned in "sub" space that is adjusted for heading
# but not pitch and roll
desired_accel = np.array((velx_out.get(), vely_out.get(),
depth_out.get()))
# Convert to sub space
desired_force = tm.spitz_to_sub_quat * (vehicle.mass * desired_accel)
###########
# TORQUES #
###########
if quat_pid_enabled.get():
# Get the quaternion desired angular accel in world coordinates
desired_ang_accel = self.quat_pid.quat_pid()
else:
# If we are using individual PID loops, we need to do a bit of work
# to get them all correctly into world space
desired_ang_accel = tm.hpr_to_world(
heading_out.get(), pitch_out.get(), roll_out.get()
)
# Convert to sub space
desired_ang_accel_s = tm.orientation.conjugate() * desired_ang_accel
# Convert desired angular accel into desired torque using inertia tensor
desired_torque = vehicle.I.dot(desired_ang_accel_s)
###############################################################
# Assimilate desired forces and torques into a vector of length 6
self.desired_output_s = np.hstack((desired_force, desired_torque))
# Subtract forces and torques already on the vehicle from the desires.
# Our thrusters need only provide the forces and torques not already
# provided by passives such as those due to buoyancy and drag.
# This greatly reduces the complexities the PID loops have to handle.
self.desired_output_s -= ft_passive_s
# We should make sure that any DISABLED degrees of freedom are actually
# disabled; PID loops will output 0 but passive forces may not!
disables = [ (velx_active, tm.spitz_to_sub_quat * np.array((1, 0, 0)), False),
(vely_active, tm.spitz_to_sub_quat * np.array((0, 1, 0)), False),
(depth_active, tm.orientation.conjugate() * np.array((0, 0, 1)), False),
(heading_active, tm.orientation.conjugate() * np.array((0, 0, 1)), True),
(pitch_active, tm.spitz_to_sub_quat * np.array((0, 1, 0)), True),
(roll_active, np.array((1, 0, 0)), True) ]
for enable, direction, is_torque in disables:
if not enable.get():
if is_torque:
direction = np.hstack((np.array((0, 0, 0)), direction))
else:
direction = np.hstack((direction, np.array((0, 0, 0))))
self.desired_output_s -= self.desired_output_s.dot(direction) * direction
# limit final desired forces in sub space
# This prevents an aggressive PID loop from destabilizing the sub
# TODO This is not implemented.
# Output final desires to shared memory.
set_shm_wrench(control_internal_outs, self.desired_output_s)
################
# OPTIMIZATION #
################
# Get the matrix that calculates outputs from thrusts (6 x 8)
# This is needed for the optimization routine, which uses get_error.
self.thrusts_output_mat_s = tm.thrusts_to_sub
x = self.optimize(tm.sub_to_thrusts, self.desired_output_s)
actual_output_s = self.thrusts_output_mat_s.dot(x)
set_shm_wrench(control_internal_wrench, actual_output_s)
# Output errors to shared memory.
error = actual_output_s - self.desired_output_s
set_shm_wrench(control_internal_opt_errors, error)
# Finally, we convert thrusts to PWM and output values to shared memory
out = [0] * len(self.thrusters)
for i, t in enumerate(self.thrusters):
# this only happens when desires are wild and slsqp fails
# at that point we don't really care about accuracy and just
# truncate for safety.
if x[i] < t.max_neg_thrust:
x[i] = t.max_neg_thrust
elif x[i] > t.max_thrust:
x[i] = t.max_thrust
out[i] = t.thrust_to_pwm(x[i])
if t.reversed_polarity:
out[i] = -out[i]
set_all_motors_from_seq(out)
def loop(kalman_watcher, quit_event):
# PID stepping class
pid = PIDLoop(speed=args['speed'])
pid.clean()
# Optimizer class
opt = Optimizer()
opt.DEBUG = bool(args['verbose'])
controller_enabled = True
tm = ThrusterManager()
kalman_watcher.watch(kalman)
# The main loop
while not quit_event.is_set():
if not args['rate']:
kalman_watcher.wait()
start_time = time.time()
g = kalman.get()
tm.update(g)
# Get passive forces and passive torques on the sub in world space
passives = vehicle.passive_forces(g, tm)
set_shm_wrench(control_passive_forces, passives)
# Handle controller enable / disable feature
if not settings_control.enabled.get() or \
switches.soft_kill.get() or switches.hard_kill.get():
# controller is disabled
if controller_enabled:
zero_motors()
controller_enabled = False
if args['rate']:
time.sleep(0.1)
continue
if not controller_enabled: # We have been re-enabled
controller_enabled = True
pid.clean()
dt_pid = pid.step() # execute one PID step for all PID controllers
# Get the correct inertia tensor of the sub in world coordinates
I = vehicle.get_inertia_tensor(tm.orientation)
# Controller step
dt_opt = opt.set_motors(tm, passives, I)
total_time = time.time() - start_time
if args['verbose']:
sys.stdout.write("PID step ran in %0.2f HZ\n" % (1 / dt_pid))
sys.stdout.write("Optimizer ran in %0.2f HZ\n" % (1 / dt_opt))
sys.stdout.write("Total rate: %0.2f HZ\n" % (1 / total_time))
sys.stdout.flush()
if args['rate']:
total_time = dt_pid + dt_opt
if total_time > step_time:
print(str(datetime.now()) + \
"## WARN: UNABLE TO MEET RATE. RUNNING AT %f HZ" % (1 / total_time))
else:
if args['verbose']:
print("Rate capped at %f HZ" % args['rate'])
print("-----") #visual break
time.sleep(step_time - total_time)
pid.clean()
zero_motors()