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
