def test_make_wrench_stamped(self): '''Test that wrenchmaking works''' for k in range(10): force = np.random.random(3) * 10 torque = np.random.random(3) * 10 wrench_msg = make_wrench_stamped(force, torque, frame='/enu') msg_force = rosmsg_to_numpy(wrench_msg.wrench.force) # noqa msg_torque = rosmsg_to_numpy(wrench_msg.wrench.torque) # noqa self.assertIsNotNone(msg_force) self.assertIsnotNone(msg_torque)
def request_wrench_cb(self, msg): '''Callback for requesting a wrench''' time_now = rospy.Time.now() if (time_now - self.last_command_time) < self._min_command_time: return else: self.last_command_time = rospy.Time.now() force = rosmsg_to_numpy(msg.wrench.force) torque = rosmsg_to_numpy(msg.wrench.torque) wrench = np.hstack([force, torque]) success = False while not success: u, success = self.map(wrench) if not success: # If we fail to compute, shrink the wrench wrench = wrench * 0.75 continue thrust_cmds = [] # Assemble the list of thrust commands to send for name, thrust in zip(self.thruster_name_map, u): # > Can speed this up by avoiding appends if name in self.dropped_thrusters: continue # Ignore dropped thrusters if np.fabs(thrust) < self.min_commandable_thrust: thrust = 0 thrust_cmds.append(ThrusterCmd(name=name, thrust=thrust)) # TODO: Make this account for dropped thrusters actual_wrench = self.B.dot(u) self.actual_wrench_pub.publish( msg_helpers.make_wrench_stamped(actual_wrench[:3], actual_wrench[3:])) self.thruster_pub.publish(thrust_cmds)
def request_wrench_cb(self, msg): '''Callback for requesting a wrench''' time_now = rospy.Time.now() if (time_now - self.last_command_time) < self._min_command_time: return else: self.last_command_time = rospy.Time.now() force = rosmsg_to_numpy(msg.wrench.force) torque = rosmsg_to_numpy(msg.wrench.torque) wrench = np.hstack([force, torque]) success = False while not success: u, success = self.map(wrench) if not success: # If we fail to compute, shrink the wrench wrench = wrench * 0.75 continue thrust_cmds = [] # Assemble the list of thrust commands to send for name, thrust in zip(self.thruster_name_map, u): # > Can speed this up by avoiding appends if name in self.dropped_thrusters: continue # Ignore dropped thrusters if np.fabs(thrust) < self.min_commandable_thrust: thrust = 0 thrust_cmds.append(ThrusterCmd(name=name, thrust=thrust)) # TODO: Make this account for dropped thrusters actual_wrench = self.B.dot(u) self.actual_wrench_pub.publish( msg_helpers.make_wrench_stamped(actual_wrench[:3], actual_wrench[3:]) ) self.thruster_pub.publish(thrust_cmds)
def turbuloop(self): ''' The idea is to create a smooth application of force to emulate underwater motion. The Turbuloop applies a wrench with a magnitude that varies like a squared function with zeros on both sides so that there are no sudden changes in the force. ''' model_name = 'sub8::base_link' # Used to gently apply a force on the sub, time_step times per 1 / freq time_step = 5.0 while not rospy.is_shutdown(): turbulence_mag_step = self.turbulence_mag / time_step sleep_step = 1 / (self.turbulence_freq * time_step) # Random unit vector scaled by the desired magnitude f = np.random.uniform(-1, 1, size=3) * self.turbulence_mag r = np.random.uniform(-1, 1, size=3) r[:2] = 0 # C3 doesn't like variation in x or y rotation :( for i in range(int(time_step)): # Square function: -(x - a/2)^2 + (a/2)^2 mag_multiplier = -((i - time_step / 2) ** 2 - (time_step / 2) ** 2 - 1) * turbulence_mag_step # Create service call body_wrench = ApplyBodyWrenchRequest() body_wrench.body_name = model_name body_wrench.reference_frame = model_name body_wrench.wrench = msg_helpers.make_wrench_stamped(f * mag_multiplier, r * mag_multiplier).wrench body_wrench.start_time = rospy.Time() body_wrench.duration = rospy.Duration(sleep_step) #rospy.loginfo("{}: Wrench Applied: {}".format(i, body_wrench.wrench)) self.set_wrench(body_wrench) rospy.sleep(sleep_step)