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
'''
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 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.last_msg_time = time()
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
'''
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 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.last_msg_time = time()
def test_thrust_jacobian(self):
'''Test thrust_jacobian
i.e. test that
jac(thrust_matrix(alpha) * u) * delta_alpha
+ thrust_matrix(alpha_0) * u_0
[jacobian evaluated at alpha = alpha_0, u = u_0]
is close to thrust_matrix(delta_alpha + alpha_0)
In English: That the implemented jacobian suffices to do a higher dimensional
first-order Taylor approximation of the nonlinear behavior of the thrust_matrix
'''
max_error = 0.2
tests = [
{
'u_0': (10, 10),
'alpha_0': (0.01, 0.01),
'delta_angle': (0.1, 0.1),
},
{
'u_0': (10, 10),
'alpha_0': (0.1, 0.1),
'delta_angle': (0.05, 0.05),
},
{
'u_0': (25, 10),
'alpha_0': (0.5, 0.3),
'delta_angle': (0.08, 0.05),
},
]
for test in tests:
u_0 = np.array(test['u_0'])
alpha_0 = np.array(test['alpha_0'])
delta_angle = np.array(test['delta_angle'])
jacobian = Azi_Drive.thrust_jacobian(alpha_0, u_0)
initial_thrust_matrix = Azi_Drive.thrust_matrix(alpha_0)
linear_estimate = np.dot(initial_thrust_matrix, u_0) + np.dot(jacobian, delta_angle)
true_thrust_matrix = Azi_Drive.thrust_matrix(alpha_0 + delta_angle)
nonlinear_estimate = np.dot(true_thrust_matrix, u_0)
error = np.linalg.norm(nonlinear_estimate - linear_estimate)
self.assertLess(
error,
max_error,
msg='error of {} exceeds maximum of {}\n Estimates: nonlinear {}, linear {}'.format(
error,
max_error,
nonlinear_estimate,
linear_estimate,
)
)
def main_loop(self):
rate = rospy.Rate(self.rate)
iteration_num = 0
while not rospy.is_shutdown():
# If killed zero everything
if self.killed == 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.logwarn(
"AZI_DRIVE: No control input in over {} seconds! Turning off motors"
.format(self.control_timeout))
self.stop()
thrust_solution = Azi_Drive.map_thruster(
fx_des=self.des_fx,
fy_des=-1 * self.des_fy,
m_des=-1 * self.des_torque,
alpha_0=self.cur_angles,
u_0=self.cur_forces,
)
# toc = time() - cur_time
# print 'Took {} seconds'.format(toc)
d_theta, d_force, success = thrust_solution
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 > 1:
iteration_num = 0
self._reset_desired_state()
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"
)
self.set_forces((0.0, 0.0))
rospy.logdebug("Achieving net: {}".format(
np.round(
Azi_Drive.net_force(self.cur_angles,
self.cur_forces)), 2))
rate.sleep()
def main_loop(self):
rate = rospy.Rate(self.rate)
iteration_num = 0
while not rospy.is_shutdown():
# If killed zero everything
if self.killed == 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.logwarn("AZI_DRIVE: No control input in over {} seconds! Turning off motors".format(self.control_timeout))
self.stop()
thrust_solution = Azi_Drive.map_thruster(
fx_des=self.des_fx,
fy_des=-1 * self.des_fy,
m_des= -1 * self.des_torque,
alpha_0= self.cur_angles,
u_0=self.cur_forces,
)
# toc = time() - cur_time
# print 'Took {} seconds'.format(toc)
d_theta, d_force, success = thrust_solution
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 > 1:
iteration_num = 0
self._reset_desired_state()
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")
self.set_forces((0.0, 0.0))
rospy.logdebug("Achieving net: {}".format(np.round(Azi_Drive.net_force(self.cur_angles, self.cur_forces)), 2))
rate.sleep()
def draw(self, display):
'''Draw the whole arm'''
# Update positions given current angles
for _id, position in self.thruster_positions.items():
self.ray(display,
start=position,
angle=saneify_angle(self.thruster_angles[_id]) -
(np.pi / 2),
length=self.thruster_forces[_id],
scale=0.1)
net = Azi_Drive.net_force(
alpha=[
saneify_angle(self.thruster_angles[3]),
saneify_angle(self.thruster_angles[2])
],
u=[self.thruster_forces[3], self.thruster_forces[2]],
)
# Swapped x and y
self.vector(display,
start=(0, 0),
direction=(net[0], net[1]),
scale=0.1)
Text_Box.draw(
display,
pos=(475, 200),
color=(60, 200, 30),
text="Fx: {}\nFy: {}\nTorque: {}\n".format(
round(net[0], 4),
round(net[1], 4),
round(net[2], 4),
),
)
def test_thrust_matrix(self):
'''Test the thrust matrix
i.e.: Prove that the thrust matrix generated by azi_drive accurately represents the
dynamics of the boat
TODO
This needs more real test cases, but I don't really want to do the math
'''
tests = [
{
'angles': (0.0, 0.0),
'u': (10, 10),
'answer': [20, 0, 0],
},
{
'angles': (0.0, 0.0),
'u': (-10, -10),
'answer': [-20, 0, 0],
},
{
'angles': (np.pi / 2, -np.pi / 2),
'u': (10, 10),
'answer': [0.0, 0.0, 0.0],
},
{
'angles': (np.pi, np.pi),
'u': (10, 10),
'answer': [-20.0, 0.0, 0.0],
},
{
'angles': (0.0, 0.0),
'u': (10, -10),
'answer': [0.0, 0.0, 6.0],
},
{
'angles': (0.0, 0.0),
'u': (10, -10),
'answer': [0.0, 0.0, 6.0],
},
# {
# 'angles': (np.pi / 2, 0.0),
# 'u': (10, 0.0),
# 'answer': [0.0, 0.0, -6.0],
# }
]
for test in tests:
B = Azi_Drive.thrust_matrix(test['angles'])
u = np.array(test['u'])
net = np.dot(B, u)
self.assertTrue(
np.allclose(net, test['answer']),
msg='Expecting net of {}, got {}, for inputs of u={}, alpha={}'
.format(
test['answer'],
net,
test['u'],
test['angles'],
))
def test_thrust_matrix(self):
'''Test the thrust matrix
i.e.: Prove that the thrust matrix generated by azi_drive accurately represents the
dynamics of the boat
TODO
This needs more real test cases, but I don't really want to do the math
'''
tests = [
{
'angles': (0.0, 0.0),
'u': (10, 10),
'answer': [20, 0, 0],
},
{
'angles': (0.0, 0.0),
'u': (-10, -10),
'answer': [-20, 0, 0],
},
{
'angles': (np.pi / 2, -np.pi / 2),
'u': (10, 10),
'answer': [0.0, 0.0, 0.0],
},
{
'angles': (np.pi, np.pi),
'u': (10, 10),
'answer': [-20.0, 0.0, 0.0],
},
{
'angles': (0.0, 0.0),
'u': (10, -10),
'answer': [0.0, 0.0, 6.0],
},
{
'angles': (0.0, 0.0),
'u': (10, -10),
'answer': [0.0, 0.0, 6.0],
},
# {
# 'angles': (np.pi / 2, 0.0),
# 'u': (10, 0.0),
# 'answer': [0.0, 0.0, -6.0],
# }
]
for test in tests:
B = Azi_Drive.thrust_matrix(test['angles'])
u = np.array(test['u'])
net = np.dot(B, u)
self.assertTrue(np.allclose(net, test['answer']),
msg='Expecting net of {}, got {}, for inputs of u={}, alpha={}'.format(
test['answer'],
net,
test['u'],
test['angles'],
)
)
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.controller_max_rotation = self.servo_max_rotation / self.rate
rospy.init_node('azi_drive', log_level=rospy.DEBUG)
# rospy.init_node('azi_drive', log_level=rospy.WARN)
rospy.logwarn("Setting maximum rotation speed to {} rad/s".format(self.controller_max_rotation))
Azi_Drive.set_delta_alpha_max(self.controller_max_rotation)
# These should not be queued! Old commands are garbage.
# Unfortunately, we have to queue these, because the subscriber cannot process two sequential
# thrust messages as quickly as they are sent
self.thrust_pub = rospy.Publisher('thruster_config', thrusterNewtons, queue_size=4)
self.servo_pub = rospy.Publisher('dynamixel/dynamixel_full_config', DynamixelFullConfig, queue_size=4)
rospy.Subscriber('wrench', WrenchStamped, self._wrench_cb, queue_size=1)
# Thrust topic id's for each thruster
self.left_id = 3
self.right_id = 2
# Time between messages before azi_drive shuts off
# self.control_timeout = 2 # secs
self.control_timeout = np.inf
self.default_angles = np.array([0.0, 0.0], dtype=np.float32)
self.default_forces = np.array([0.0, 0.0], dtype=np.float32)
# Left, Right
self.cur_angles = self.default_angles[:]
self.cur_forces = self.default_forces[:]
self.prev_angles = None
self.set_servo_angles(self.cur_angles)
self.set_forces(self.cur_forces)
# Initializations
self.last_msg_time = time()
self.des_fx, self.des_fy, self.des_torque = 0.0, 0.0, 0.0
# Prepare a shutdown service
rospy.loginfo("----------Waiting for control_manager service-------------")
rospy.wait_for_service('azi_drive/stop')
self.stop_boat_proxy = rospy.ServiceProxy('azi_drive/stop', AziStop)
def run_test(fx, fy, moment):
u_nought = np.array([0.0, 0.0])
alpha_nought = np.array([0.1, 0.1])
tau = np.array([fx, fy, moment])
# Accumulate 20 iterations
for k in range(20):
tic = time()
thrust_solution = Azi_Drive.map_thruster(
fx_des=fx,
fy_des=fy,
m_des=moment,
alpha_0=alpha_nought,
u_0=u_nought
)
toc = time() - tic
d_theta, d_force, success = thrust_solution
alpha_nought += d_theta
u_nought += d_force
if self.test_time:
self.assertLess(toc, max_time, msg="Executon took more then {} sec".format(toc))
net = Azi_Drive.net_force(alpha_nought, u_nought)
difference = np.linalg.norm(net - tau)
success = difference < 0.1
self.assertLess(
difference,
max_error,
msg="Failed on request {}, with error {}, producing {}".format(
(fx, fy, moment),
difference,
net
)
)
return success
def run_test(fx, fy, moment):
u_nought = np.array([0.0, 0.0])
alpha_nought = np.array([0.1, 0.1])
tau = np.array([fx, fy, moment])
# Accumulate 20 iterations
for k in range(20):
tic = time()
thrust_solution = Azi_Drive.map_thruster(fx_des=fx,
fy_des=fy,
m_des=moment,
alpha_0=alpha_nought,
u_0=u_nought)
toc = time() - tic
d_theta, d_force, success = thrust_solution
alpha_nought += d_theta
u_nought += d_force
if self.test_time:
self.assertLess(
toc,
max_time,
msg="Executon took more then {} sec".format(toc))
net = Azi_Drive.net_force(alpha_nought, u_nought)
difference = np.linalg.norm(net - tau)
success = difference < 0.1
self.assertLess(
difference,
max_error,
msg="Failed on request {}, with error {}, producing {}".format(
(fx, fy, moment), difference, net))
return success
# Ba = Azi_Drive.thrust_matrix(a_0)
# jBa_u = Azi_Drive.thrust_jacobian(a_0, u_0)
a_min = -3 * np.pi / 2
a_max = 3 * np.pi / 2
u_min = -100.0
u_max = 100.
da_max = 0.1
tau = np.array([200.0, 200.0, 0.0]).astype(np.double)
Q = np.diag([10., 10., 30.]).astype(np.double)
Ohm = np.diag([0., 0.]).astype(np.double)
tic = time()
for k in range(200):
Ba = Azi_Drive.thrust_matrix(a_0).astype(np.double)
jBa_u = Azi_Drive.thrust_jacobian(a_0, u_0).astype(np.double)
soln = cvxbind.ksolve(
u_0.flatten(),
a_0.flatten(),
Ohm.flatten(),
Q.flatten(),
Ba.transpose().flatten(),
jBa_u.transpose().A1,
u_max,
u_min,
a_min,
a_max,
da_max,
tau.flatten(),
# Ba = Azi_Drive.thrust_matrix(a_0)
# jBa_u = Azi_Drive.thrust_jacobian(a_0, u_0)
a_min = -3 * np.pi / 2
a_max = 3 * np.pi / 2
u_min = -100.0
u_max = 100.
da_max = 0.1
tau = np.array([200.0, 200.0, 0.0]).astype(np.double)
Q = np.diag([10., 10., 30.]).astype(np.double)
Ohm = np.diag([0., 0.]).astype(np.double)
tic = time()
for k in range(200):
Ba = Azi_Drive.thrust_matrix(a_0).astype(np.double)
jBa_u = Azi_Drive.thrust_jacobian(a_0, u_0).astype(np.double)
soln = cvxbind.ksolve(
u_0.flatten(),
a_0.flatten(),
Ohm.flatten(),
Q.flatten(),
Ba.transpose().flatten(),
jBa_u.transpose().A1,
u_max,
u_min,
a_min,
a_max,
da_max,
tau.flatten(),
def test_singularity(self):
cost = Azi_Drive.singularity_avoidance((0.0, 0.0))
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.controller_max_rotation = self.servo_max_rotation / self.rate
rospy.Subscriber("control_arbiter", Bool, self.control_callback)
rospy.Subscriber("pwm1_alias", Float64, self.pwm1_cb)
rospy.Subscriber("pwm2_alias", Float64, self.pwm2_cb)
rospy.init_node('azi_drive', log_level=rospy.DEBUG)
# rospy.init_node('azi_drive', log_level=rospy.WARN)
rospy.logwarn("Setting maximum rotation speed to {} rad/s".format(
self.controller_max_rotation))
Azi_Drive.set_delta_alpha_max(self.controller_max_rotation)
# These should not be queued! Old commands are garbage.
# Unfortunately, we have to queue these, because the subscriber cannot process two sequential
# thrust messages as quickly as they are sent
self.thrust_pub = rospy.Publisher('thruster_config',
thrusterNewtons,
queue_size=4)
self.servo_pub = rospy.Publisher('dynamixel/dynamixel_full_config',
DynamixelFullConfig,
queue_size=4)
rospy.Subscriber('wrench',
WrenchStamped,
self._wrench_cb,
queue_size=1)
# Thrust topic id's for each thruster
self.left_id = 3
self.right_id = 2
self.control_kill = False
# Time between messages before azi_drive shuts off
# self.control_timeout = 2 # secs
self.control_timeout = np.inf
self.default_angles = np.array([0.0, 0.0], dtype=np.float32)
self.default_forces = np.array([0.0, 0.0], dtype=np.float32)
self.pwm_forces = np.array([0.0, 0.0], dtype=np.float64)
# Left, Right
self.cur_angles = self.default_angles[:]
self.cur_forces = self.default_forces[:]
self.prev_angles = None
self.set_servo_angles(self.cur_angles)
self.set_forces(self.cur_forces)
# Initializations
self.last_msg_time = time()
self.des_fx, self.des_fy, self.des_torque = 0.0, 0.0, 0.0
# Prepare a shutdown service
rospy.loginfo(
"----------Waiting for control_manager service-------------")
rospy.wait_for_service('azi_drive/stop')
self.stop_boat_proxy = rospy.ServiceProxy('azi_drive/stop', AziStop)
self.MAX_NEWTONS = 100.0 #Full forward Jacksons motors
self.MIN_NEWTONS = -100.0 #Full reverse Jacksons
self.ABS_MAX_PW = .002
self.ABS_MIN_PW = .001
self.ZERO_NEWTONS = 0
self.REV_CONV = (self.ZERO_NEWTONS -
self.MIN_NEWTONS) / (0 - self.ABS_MIN_PW)
self.FWD_CONV = (self.MAX_NEWTONS -
self.ZERO_NEWTONS) / self.ABS_MAX_PW
def test_net_force(self):
net = Azi_Drive.net_force((0.0, 0.0), (10.0, 10.0))
self.assertTrue(np.allclose(net.T, [-20, 0, 0]))
def test_singularity(self):
cost = Azi_Drive.singularity_avoidance((0.0, 0.0))
def test_net_force(self):
net = Azi_Drive.net_force((0.0, 0.0), (10.0, 10.0))
self.assertTrue(np.allclose(net.T, [-20, 0, 0]))
class Controller(object):
def __init__(self, rate=100):
'''I can't swim on account of my ass-mar'''
# Register azi_drive as a controller
rospy.wait_for_service('/controller/register_controller', 30.0)
rospy.ServiceProxy('/controller/register_controller',
register_controller)('azi_drive')
self.rate = rate
self.servo_max_rotation = 6.0
self.controller_max_rotation = self.servo_max_rotation / self.rate
# rospy.init_node('azi_drive', log_level=rospy.WARN)
# Set up kill handling
self.kill_listener = KillListener(self.on_kill, self.on_unkill)
self.kill_broadcaster = KillBroadcaster(
id='Azi_drive', description='Azi drive node shutdown')
try:
self.kill_broadcaster.clear()
except rospy.service.ServiceException, e:
rospy.logwarn(str(e))
self.killed = False
rospy.logwarn("Setting maximum rotation speed to {} rad/s".format(
self.controller_max_rotation))
Azi_Drive.set_delta_alpha_max(self.controller_max_rotation)
#Azi_Drive.set_thrust_bound((-50,50))
# These should not be queued! Old commands are garbage.
# Unfortunately, we have to queue these, because the subscriber cannot process two sequential
# thrust messages as quickly as they are sent
self.thrust_pub = rospy.Publisher('thruster_config',
thrusterNewtons,
queue_size=4)
self.servo_pub = rospy.Publisher('controller/dynamixel_full_config',
ControlDynamixelFullConfig,
queue_size=4)
self.next_wrench = WrenchStamped().wrench
rospy.Subscriber('wrench',
WrenchStamped,
self._wrench_cb,
queue_size=1)
# Thrust topic id's for each thruster
self.left_id = 3
self.right_id = 2
# Time between messages before azi_drive shuts off
# self.control_timeout = 2 # secs
self.control_timeout = np.inf
self.default_angles = np.array([0.0, 0.0], dtype=np.float32)
self.default_forces = np.array([0.0, 0.0], dtype=np.float32)
self.pwm_forces = np.array([0.0, 0.0], dtype=np.float64)
# Left, Right
self.cur_angles = self.default_angles[:]
self.cur_forces = self.default_forces[:]
self.prev_angles = None
self.set_servo_angles(self.cur_angles)
self.set_forces(self.cur_forces)
# Initializations
self.last_msg_time = time()
self.des_fx, self.des_fy, self.des_torque = 0.0, 0.0, 0.0
self.MAX_NEWTONS = 100.0 #Full forward Jacksons motors
self.MIN_NEWTONS = -100.0 #Full reverse Jacksons
self.ABS_MAX_PW = .002
self.ABS_MIN_PW = .001
self.ZERO_NEWTONS = 0
self.REV_CONV = (self.ZERO_NEWTONS -
self.MIN_NEWTONS) / (0 - self.ABS_MIN_PW)
self.FWD_CONV = (self.MAX_NEWTONS -
self.ZERO_NEWTONS) / self.ABS_MAX_PW
def draw(self, display):
# Update positions given current angles
for _id, position in self.thruster_positions.items():
self.ray(display,
start=position,
angle=saneify_angle(self.thruster_cur_angles[_id]) - (np.pi / 2),
length=10,
scale=0.25,
color=(50, 30, 255),
width=5
)
angle = saneify_angle(self.thruster_goal_angles[_id]) - (np.pi / 2)
length = self.thruster_forces[_id]
self.ray(display,
start=position,
angle=angle,
length=length,
scale=0.1,
text="{}Force: {}\nAngle: {}\n".format(
"\n\n" * (_id - 2),
round(length, 4),
round(angle, 4),
),
)
desired_net = Azi_Drive.net_force(
alpha=[saneify_angle(self.thruster_goal_angles[3]), saneify_angle(self.thruster_goal_angles[2])],
u=[self.thruster_forces[3], self.thruster_forces[2]],
)
# Swapped x and y, draw desired net force
self.vector(display, start=(0, 0), direction=(desired_net[0], desired_net[1]), scale=0.1)
# Current net force
real_net = Azi_Drive.net_force(
alpha=[saneify_angle(self.thruster_cur_angles[3]), saneify_angle(self.thruster_cur_angles[2])],
u=[self.thruster_forces[3], self.thruster_forces[2]],
)
Text_Box.draw(display,
pos=(475, 200),
color=(60, 200, 30),
text="Fx: {} (Target)\nFy: {} (Target)\nTorque: {} (Target)\n".format(
round(desired_net[0], 4),
round(desired_net[1], 4),
round(desired_net[2], 4),
),
)
Text_Box.draw(display,
pos=(475, 200),
color=(60, 30, 200),
text="\n\n\nFx: {} (Actual)\nFy: {} (Actual)\nTorque: {} (Actual)\n".format(
round(real_net[0], 4),
round(real_net[1], 4),
round(real_net[2], 4),
),
)
Text_Box.draw(display,
pos=(475, 200),
color=(250, 30, 250),
text="\n\n\n\n\n\nOdom X: {}\nOdom Y: {}\nYaw: {}\n".format(
round(self.pos_x, 4),
round(self.pos_y, 4),
round(self.yaw, 4),
),
)
def test_thrust_jacobian(self):
'''Test thrust_jacobian
i.e. test that
jac(thrust_matrix(alpha) * u) * delta_alpha
+ thrust_matrix(alpha_0) * u_0
[jacobian evaluated at alpha = alpha_0, u = u_0]
is close to thrust_matrix(delta_alpha + alpha_0)
In English: That the implemented jacobian suffices to do a higher dimensional
first-order Taylor approximation of the nonlinear behavior of the thrust_matrix
'''
max_error = 0.2
tests = [
{
'u_0': (10, 10),
'alpha_0': (0.01, 0.01),
'delta_angle': (0.1, 0.1),
},
{
'u_0': (10, 10),
'alpha_0': (0.1, 0.1),
'delta_angle': (0.05, 0.05),
},
{
'u_0': (25, 10),
'alpha_0': (0.5, 0.3),
'delta_angle': (0.08, 0.05),
},
]
for test in tests:
u_0 = np.array(test['u_0'])
alpha_0 = np.array(test['alpha_0'])
delta_angle = np.array(test['delta_angle'])
jacobian = Azi_Drive.thrust_jacobian(alpha_0, u_0)
initial_thrust_matrix = Azi_Drive.thrust_matrix(alpha_0)
linear_estimate = np.dot(initial_thrust_matrix, u_0) + np.dot(
jacobian, delta_angle)
true_thrust_matrix = Azi_Drive.thrust_matrix(alpha_0 + delta_angle)
nonlinear_estimate = np.dot(true_thrust_matrix, u_0)
error = np.linalg.norm(nonlinear_estimate - linear_estimate)
self.assertLess(
error,
max_error,
msg=
'error of {} exceeds maximum of {}\n Estimates: nonlinear {}, linear {}'
.format(
error,
max_error,
nonlinear_estimate,
linear_estimate,
))
