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 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 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 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
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_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]))
a_max,
da_max,
tau.flatten(),
)
# du_1, du_2, da_1, da_2, s1, s2, s3 = soln
du_1, du_2, da_1, da_2 = soln
du = np.array([du_1, du_2]).astype(np.double)
da = np.array([da_1, da_2]).astype(np.double)
# s = np.array([s1, s2, s3])
# print 's', s
# print 'du:', du, '| da:', da
a_0 += da
u_0 += du
print 'u:', u_0, '| alpha:', a_0
print 'Net Force', Azi_Drive.net_force(a_0, u_0)
print
print 'Took {} seconds'.format(time() - tic)
# boost::python::list cvxsolve(
# boost::python::numeric::array u_0,
# boost::python::numeric::array a_0,
# boost::python::numeric::array Ohm,
# boost::python::numeric::array Q,
# boost::python::numeric::array Ba,
# boost::python::numeric::array jBa_u,
# double u_max,
# double u_min,
# double a_min,
# double a_max,
# double da_max,
da_max,
tau.flatten(),
)
# du_1, du_2, da_1, da_2, s1, s2, s3 = soln
du_1, du_2, da_1, da_2 = soln
du = np.array([du_1, du_2]).astype(np.double)
da = np.array([da_1, da_2]).astype(np.double)
# s = np.array([s1, s2, s3])
# print 's', s
# print 'du:', du, '| da:', da
a_0 += da
u_0 += du
print 'u:', u_0, '| alpha:', a_0
print 'Net Force', Azi_Drive.net_force(a_0, u_0)
print
print 'Took {} seconds'.format(time() - tic)
# boost::python::list cvxsolve(
# boost::python::numeric::array u_0,
# boost::python::numeric::array a_0,
# boost::python::numeric::array Ohm,
# boost::python::numeric::array Q,
# boost::python::numeric::array Ba,
# boost::python::numeric::array jBa_u,
# double u_max,
# double u_min,
# double a_min,
# double a_max,
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]))
