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)
