def __init__(self, profile=False):
self.profile = profile
self.update_frequency = 20.0
self.datum = coordinates.Simulation_Datum
# displacement in x (East), y (North), z (Altitude)
self.displacement = np.array((0.0, 0.0, -5))
self.velocity = np.array((0.0, 0.0, 0.0))
# Rotation applied from East,North,Altitude coordinate system to the vehicle
# euler angles = (pitch, roll, -yaw)
self.orientation = Quaternion.fromEuler(
(math.radians(0), math.radians(0), math.radians(0)))
#print math.degrees(calculatePitch(self.orientation))
#assert(44 < math.degrees(calculatePitch(self.orientation)) < 46)
#print math.degrees(calculateYaw(self.orientation))
#assert(89 < math.degrees(calculateYaw(self.orientation)) < 91)
# In the vehicle-local coordinate system:
self.angular_velocity = np.array(
(0.0, 0.0, 0.0)) # about x, about y, about z
self.motor_states = MotorStates()
self.update_lock = threading.Lock()
self.thread = threading.Thread(target=self.runLoopWrapper)
self.thread.daemon = True
self.keep_going = True
rospy.Subscriber("/control/motors", ctrl_msgs.MotorDemand,
self.onMotorStateMessage)
def processUpdate(self, s):
# state is stored in:
# self.displacement = (x,y,z) - global coordinates
# self.velocity = (x,y,z) - global coordinates
# self.orientation = Quat(...) - of vehicle relative to the
# world x=East,y=North,z=Up
# coords
# a_local_vec = orientation.inverse().rotate(global vec)
# a_global_vec = orientation.rotate(local vec)
# self.angular_velocity = (dYaw/dt, dRoll/dt, dPitch/dt)
#
# yaw is rotation about the z axis
# pitch is rotation about the x axis
# roll is rotation about the y axis
#
weight_vec = np.array((0,0,-1))
buoyancy_vec = np.array((0,0,1))
#debug(str(s))
now = self.relativeTime()
dt = now - self.last_t
if dt > 10:
warning('processUpdate called too infrequently! Maximum time step will be 10 seconds.')
dt = 10
if dt < 0:
warning('Clock skew!')
if dt < -10:
error('FATAL: Clock skewed by more than 10 seconds!')
self.stop()
return
# Update velocity with forces:
# Forces in vehicle-local coordinates:
hbow_force = HBow_Vec * s.HBow * Force_Per_Unit_Thrust
vbow_force = VBow_Vec * s.VBow * Force_Per_Unit_Thrust
hstern_force = HStern_Vec * s.HStern * Force_Per_Unit_Thrust
vstern_force = VStern_Vec * s.VStern * Force_Per_Unit_Thrust
prop_force = Prop_Vec * s.Prop * Force_Per_Unit_Thrust
drag_force = -Drag_F * self.orientation.inverse().rotate(self.velocity);
local_force = sum(
(hbow_force, vbow_force, hstern_force, vstern_force, prop_force, drag_force)
)
#debug('local force (R,F,U) = %s' % local_force)
# now in global coordinates:
global_force = self.orientation.rotate(local_force, mkVec)
#debug('global force (E,N,U) = %s' % global_force)
weight_force = weight_vec * Mass * 9.81
buoyancy_force = buoyancy_vec * Displacement * 9.81
if self.displacement[2] > 0:
p = min(self.displacement[2], 0.4)
debug('auv appears to be above the surface', 3)
buoyancy_force -= buoyancy_force * (p/0.4)
force = sum((global_force, weight_force, buoyancy_force))
self.velocity += force * dt / Mass
# and angular velocities with moments:
# these moments are (about x axis, about y axis, about z axis)
# in vehicle-local coordinates:
hbow_moment = np.cross(HBow_At, hbow_force)
vbow_moment = np.cross(VBow_At, vbow_force)
hstern_moment = np.cross(HStern_At, hstern_force)
vstern_moment = np.cross(VStern_At, vstern_force)
prop_moment = np.cross(Prop_At, prop_force) # expect this to be zero!
weight_local_force = self.orientation.inverse().rotate(weight_force, mkVec)
buoyancy_local_force = self.orientation.inverse().rotate(buoyancy_force, mkVec)
weight_moment = np.cross(Weight_At, weight_local_force)
buoyancy_moment = np.cross(Buoyancy_At, buoyancy_local_force)
local_moment = sum(
(hbow_moment, vbow_moment,
hstern_moment, vstern_moment,
prop_moment,
weight_moment,
buoyancy_moment)
)
#debug('local moment = %s' % local_moment)
# drag moments:
drag_moment = -Drag_J * self.angular_velocity
moment = sum((local_moment, drag_moment))
# apply moment to angular velocity:
domega = moment * dt / np.array((Ixx, Iyy, Izz))
self.angular_velocity += domega
# Update positions with velocities:
self.displacement += self.velocity * dt
dorientation = Quaternion.fromEuler(self.angular_velocity * dt)
self.orientation *= dorientation
self.last_t = now
# last thing: renormalise orientation:
if self.orientation.sxx() > 1.0001 or self.orientation.sxx() < 0.9999:
debug('orientation quat denormalised: it will be renormalised')
self.orientation = self.orientation.normalised()
# last last thing: send messages reflecting the new state: this has to
# be rate-limited since it can cause more motor state message to be
# sent from control - leading to a tight loop
if self.relativeTime() - self.last_state_sent > 0.05:
self.sendStateMessages()
self.last_state_sent = self.relativeTime()