Skip to content

Commit

Permalink
Azi_Drive: Speed up 25-50x, fix many bugs
Browse files Browse the repository at this point in the history 
Closes #9
Closes #10
Addresses #42

Note: This retains Fossen's nautical convention for boat coordinate
axes. You can put a negative sign in Y if it floats your boat.
sorry-not-sorry

Conflicts:
	azi_drive/nodes/azi_drive_node.py
	azi_drive/src/azimuth_drive/azimuth_drive.py
  • Loading branch information
@jpanikulam @fnivek
jpanikulam authored and fnivek committed Jul 10, 2015
1 parent 3eeb263 commit 62da0b7
Show file tree
Hide file tree
Showing 3 changed files with 48 additions and 172 deletions.
52 changes: 11 additions & 41 deletions azi_drive/nodes/azi_drive_node.py
View file
Original file line number Diff line number Diff line change
Expand Up @@ -27,7 +27,7 @@ class Controller(object):
def __init__(self, rate=20):
'''I can't swim on account of my ass-mar'''
self.rate = rate
self.servo_max_rotation = 0.3
self.servo_max_rotation = 0.1
self.controller_max_rotation = self.servo_max_rotation / self.rate
print rospy.get_param('~simulate')
if rospy.get_param('~simulate', False) is False:
Expand Down Expand Up @@ -149,18 +149,18 @@ def main_loop(self):
self.set_servo_angles(angles)
print self.pwm_forces
self.set_forces(self.pwm_forces)
elif self.float == True:
elif self.float == True:
angles = np.array([0, 0])
self.set_servo_angles(angles)
self.set_forces((0.0, 0.0))
# Otherwise normal operation
else:
iteration_num += 1

cur_time = time()

rospy.logdebug("Targeting Fx: {} Fy: {} Torque: {}".format(self.des_fx, self.des_fy, self.des_torque))
if (cur_time - self.last_msg_time) > self.control_timeout:
rospy.logerr("AZI_DRIVE: No control input in over {} seconds! Turning off motors".format(self.control_timeout))
rospy.logwarn("AZI_DRIVE: No control input in over {} seconds! Turning off motors".format(self.control_timeout))
self.stop()

thrust_solution = Azi_Drive.map_thruster(
Expand All @@ -175,28 +175,24 @@ def main_loop(self):
# print 'Took {} seconds'.format(toc)

d_theta, d_force, success = thrust_solution
if any(np.fabs(self.cur_angles + d_theta) > np.pi / 2):
rospy.logwarn('|cur_angles + d_theata| > pi/2, setting angles to 0')
if any(np.fabs(self.cur_angles + d_theta) > np.pi/2):
self.cur_angles = np.array([0.0, 0.0])
continue

self.cur_angles += d_theta
self.cur_forces += d_force

if iteration_num > 8:
if iteration_num > 4:
iteration_num = 0

self.set_servo_angles(self.cur_angles)
if success:
self.set_forces(self.cur_forces)
else:
rospy.logwarn("AZI_DRIVE: Failed to attain valid solution setting forces to 0")
rospy.logwarn("Offending wrench:\n" + str(self.last_wrench))
self.set_forces((0.0, 0.0))

rate.sleep()
rospy.logdebug("Achieving net: {}".format(np.round(Azi_Drive.net_force(self.cur_angles, self.cur_forces)), 2))

rate.sleep()

def control_callback(self, msg):

Expand All @@ -219,39 +215,13 @@ def reset_all(self):

def _wrench_cb(self, msg):
'''Recieve a force, torque command for the mapper to achieve
Compute the minimum and maximum wrenches by multiplying the minimum and maximum thrusts
by the thrust matrix at the current thruster orientations. This gives a strong estimate
for what is a feasible wrench in the neighborhood of linearity
The 0.9 is just a fudge factor for extra certainty
'''
self.last_wrench = msg

'''
force = msg.wrench.force
torque = msg.wrench.torque

# Compute the minimum and maximum wrenches the boat can produce
# By linearity, everything in between should be reasonably achievable
max_fx, max_fy, _ = Azi_Drive.net_force(self.cur_angles, [80, 80])
min_fx, min_fy, _ = Azi_Drive.net_force(self.cur_angles, [-70, -70])

_, _, max_torque = Azi_Drive.net_force(self.cur_angles, [-70, 80])
_, _, min_torque = Azi_Drive.net_force(self.cur_angles, [80, -70])

# max_f* could be positive, and min_f* could be negative; clip requires ordered inputs
fx_range = (min_fx, max_fx)
fy_range = (min_fy, max_fy)
tz_range = (min_torque, max_torque)
#print '----Ranges-----'
#print 'fx\t', fx_range
#print 'fy\t', fy_range
#print 'z\t', tz_range

self.des_fx = np.clip(force.x, min(fx_range), max(fx_range)) * 0.9
# I put a negative sign here to work with Forrest's pd_controller
self.des_fy = -np.clip(force.y, min(fy_range), max(fy_range)) * 0.9
self.des_torque = np.clip(torque.z, min(tz_range), max(tz_range)) * 0.9
self.des_fx = force.x
self.des_fy = force.y
self.des_torque = torque.z

self.last_msg_time = time()

Expand Down
5 changes: 4 additions & 1 deletion azi_drive/package.xml
View file
Original file line number Diff line number Diff line change
Expand Up @@ -8,17 +8,20 @@
<maintainer email="[email protected]">Jacob Panikulam</maintainer>
<license>MIT</license>
<buildtool_depend>catkin</buildtool_depend>

<build_depend>rospy</build_depend>
<run_depend>rospy</run_depend>
<build_depend>rostest</build_depend>
<test_depend>unittest</test_depend>

<build_depend>message_generation</build_depend>
<build_depend>message_runtime</build_depend>
<build_depend>std_msgs</build_depend>


<run_depend>std_msgs</run_depend>
<run_depend>message_runtime</run_depend>
<run_depend>message_generation</run_depend>
<build_depend>std_msgs</build_depend>

<export>
</export>
Expand Down
163 changes: 33 additions & 130 deletions azi_drive/src/azimuth_drive/azimuth_drive.py
View file
Original file line number Diff line number Diff line change
Expand Up @@ -4,6 +4,7 @@
import numpy as np
import time
from tools import Tools
from azi_drive import azi_cvxbind

'''
Ideas for optimization:
Expand Down Expand Up @@ -185,6 +186,9 @@ def map_thruster(self, fx_des, fy_des, m_des, alpha_0, u_0, test_plot=False):
Surface Vehicle Utilizing Azimuth Thrusters",
See: http://bit.ly/1ayOdlg
[3] Mattingley, Jacob "CVXGEN"
See: http://cvxgen.com/docs/license.html
Author: Jacob Panikulam
'''
Expand All @@ -195,137 +199,37 @@ def map_thruster(self, fx_des, fy_des, m_des, alpha_0, u_0, test_plot=False):

# Desired
tau = np.array([fx_des, fy_des, -1 * m_des])
if np.allclose([0.0, 0.0, 0.0], tau, atol=5.0):
return np.array([0.0, 0.0]), -u_0, True

# Linearizations
d_singularity = Tools.jacobian(self.singularity_avoidance, pt=alpha_0, order=3, dx=0.01)
# Singularity unused
# d_singularity = Tools.jacobian(self.singularity_avoidance, pt=alpha_0, order=3, dx=0.01)
dBu_dalpha = self.thrust_jacobian(alpha_0, u_0)
d_power = self.power_cost_scale

B = self.thrust_matrix(alpha_0)

def get_s((delta_angle, delta_u)):
'''Equality constraint
'S' is the force/torque error
s + B(alpha_0) * delta_u + jac(B*u)*delta_alpha
= -tau - B(alpha_0) * u_0
-->
s = (-B(alpha_0) * delta_u) - jac(B*u)*delta_alpha - tau - B*alpha_0 *u_0
'''
s = -np.dot(B, delta_u) - np.dot(dBu_dalpha, delta_angle) + tau - np.dot(B, u_0)
return s

def objective((delta_angle_1, delta_angle_2, delta_u_1, delta_u_2)):
'''Objective function for minimization'''

delta_u = np.array([delta_u_1, delta_u_2])

delta_angle = np.array([delta_angle_1, delta_angle_2])
s = get_s((delta_angle, delta_u))

# Sub-costs
# P = np.diag([1.0, 1.0])
# power = Tools.quadratic(delta_angle + alpha_0, P)
power = 0

G = thrust_error_weights = np.diag([20.0, 20.0, 40.0])
thrust_error = (s * G * s.T).A1
Q = np.diag([10., 10., 30.]).astype(np.double)
Ohm = np.diag([5., 5.]).astype(np.double)

soln = azi_cvxbind.ksolve(
u_0.flatten(),
alpha_0.flatten(),
Ohm.flatten(),
Q.flatten(),
B.transpose().flatten(), # Transpose because cvxgen is column-major indexed
dBu_dalpha.transpose().A1,
self.u_max,
self.u_min,
self.alpha_min,
self.alpha_max,
self.delta_alpha_max,
tau.flatten(),
)

Q = angle_change_weight = np.diag([4.0, 4.0])
angle_change = np.dot(
np.dot(delta_angle, angle_change_weight),
np.transpose(delta_angle)
)
du_1, du_2, da_1, da_2 = soln
delta_u = np.array([du_1, du_2]).astype(np.double)
delta_alpha = np.array([da_1, da_2]).astype(np.double)

A = angle_weight = np.diag([2.0, 2.0])
angle_cost = Tools.quadratic((delta_angle + alpha_0), A)

singularity = np.dot(d_singularity, delta_angle).A1

# Note, the error will increase when adding power and singularity costs
# cost = power + thrust_error + angle_change + singularity + angle_cost
cost = thrust_error
return cost

# Formatted as such because I thought I would do something
# funky by varying the bounds
u_max = self.u_max
u_min = self.u_min
alpha_max = self.alpha_max
alpha_min = self.alpha_min
delta_alpha_min = self.delta_alpha_min
delta_alpha_max = self.delta_alpha_max
thruster_min = self.thruster_min

# Plot the cost function (visually observe convexity)
if test_plot:
def objective_r(*args):
return objective(args)

x_range = np.linspace(-np.pi, np.pi, 100)
y_range = np.linspace(-np.pi, np.pi, 100)

X, Y = np.meshgrid(x_range, y_range)
f = np.vectorize(objective_r)
Z = f(X, Y, np.zeros(len(X)), np.zeros(len(Y)))

fig = plt.figure(figsize=(14, 6))
ax = fig.add_subplot(1, 2, 1, projection='3d')
p = ax.plot_surface(X, Y, Z, rstride=4, cstride=4, linewidth=0)
plt.show()

minimization = optimize.minimize(
fun=objective,
x0=(0.1, 0.1, 0.0, 0.0),
method='SLSQP',
constraints=[
# Inequality requires nonnegativity, i.e. fun(o) >= 0
{'type': 'ineq', 'fun':
lambda (delta_angle_1, delta_angle_2, delta_u_1, delta_u_2): -(delta_u_1 + u_0[0]) + u_max},
{'type': 'ineq', 'fun':
lambda (delta_angle_1, delta_angle_2, delta_u_1, delta_u_2): delta_u_1 + u_0[0] - u_min},
{'type': 'ineq', 'fun':
lambda (delta_angle_1, delta_angle_2, delta_u_1, delta_u_2): -(delta_angle_1 + alpha_0[0]) + alpha_max},
{'type': 'ineq', 'fun':
lambda (delta_angle_1, delta_angle_2, delta_u_1, delta_u_2): delta_angle_1 + alpha_0[0] - alpha_min},
{'type': 'ineq', 'fun':
lambda (delta_angle_1, delta_angle_2, delta_u_1, delta_u_2): -(delta_angle_1) + delta_alpha_max},
{'type': 'ineq', 'fun':
lambda (delta_angle_1, delta_angle_2, delta_u_1, delta_u_2): delta_angle_1 - delta_alpha_min},

{'type': 'ineq', 'fun':
lambda (delta_angle_1, delta_angle_2, delta_u_1, delta_u_2): -(delta_u_2 + u_0[1]) + u_max},
{'type': 'ineq', 'fun':
lambda (delta_angle_1, delta_angle_2, delta_u_1, delta_u_2): delta_u_2 + u_0[1] - u_min},

# Account for minimum thruster output
# {'type': 'ineq', 'fun':
# lambda (delta_angle_1, delta_angle_2, delta_u_1, delta_u_2): abs(delta_u_1 + u_0[0]) - thruster_min},
# {'type': 'ineq', 'fun':
# lambda (delta_angle_1, delta_angle_2, delta_u_1, delta_u_2): abs(delta_u_2 + u_0[1]) - thruster_min},

{'type': 'ineq', 'fun':
lambda (delta_angle_1, delta_angle_2, delta_u_1, delta_u_2): -(delta_angle_2 + alpha_0[1]) + alpha_max},
{'type': 'ineq', 'fun':
lambda (delta_angle_1, delta_angle_2, delta_u_1, delta_u_2): delta_angle_2 + alpha_0[1] - alpha_min},
{'type': 'ineq', 'fun':
lambda (delta_angle_1, delta_angle_2, delta_u_1, delta_u_2): -(delta_angle_2) + delta_alpha_max},
{'type': 'ineq', 'fun':
lambda (delta_angle_1, delta_angle_2, delta_u_1, delta_u_2): delta_angle_2 - delta_alpha_min},
]
)
delta_alpha_1, delta_alpha_2, delta_u_1, delta_u_2 = minimization.x
delta_alpha, delta_u = np.array([delta_alpha_1, delta_alpha_2]), np.array([delta_u_1, delta_u_2])

if not minimization.success:
success = False
if minimization.message == 'Inequality constraints incompatible':
pass # WE FUCT UP!
else:
success = True
success = True # Minimization always succeeds, QP's, baby!
return delta_alpha, delta_u, success


Expand All @@ -335,17 +239,16 @@ def objective_r(*args):
u_0 = np.array([0.0, 0.0])
alpha_0 = np.array([0.0, 0.0])

for k in range(10):


for k in range(500):
delta_alpha, delta_u, s = Azi_Drive.map_thruster(
100, 10, 5,
50, 30, 10,
alpha_0=alpha_0,
u_0=u_0,
)
toc = time.time() - tic
print 'took {} seconds'.format(toc)
# print 'took {} seconds'.format(toc)
u_0 += delta_u
alpha_0 += delta_alpha
print u_0
print Azi_Drive.net_force(alpha_0, u_0)
# print u_0
print Azi_Drive.net_force(alpha_0, u_0)
print 'took {} seconds'.format(toc)

0 comments on commit 62da0b7

Please sign in to comment.