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