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,