def vehicleEOM(self, y, t0):
# Equations of motion for vehicle
# Assumes that angular momentum from spinning rotors is negligible
# Assumes that forces and moments are constant during a timestep
# (should be reasonable for realistic timesteps, like 1ms)
#
# position derivatives equal rotated body velocities
xDot = AQ.rotateVector(self.v_Q_i.inv(), y[3:6])
# acceleration = F/m
vDot = 1 / self.mass * self.externalForce.T[0, :] - np.cross(
np.array(y[10:]), np.array(y[3:6])).T
# update quaternnions
att = AQ.Quaternion(np.array(y[6:10]))
#qDot = att.inv()*AQ.vectorAsQuat(.5*np.array([y[10:]]).T)
qDot = AQ.qDotFromOmega(att, np.array(y[10:]))
# omegadots
effectiveMoment = self.externalMoment - np.array([
np.cross(np.array(y[10:]), np.dot(self.Inertia, np.array(y[10:])))
]).T
omegadots = np.dot(self.invInertia, effectiveMoment).T[0, :]
#print 'xdot: ',xDot.T[0]
#print 'vdot: ',vDot
#print 'qdot: ',qDot
#print 'wdot: ',omegadots
yDot = np.concatenate((xDot.T[0], vDot, qDot, omegadots))
return yDot
def vehicleEOM(self, y, t0):
# Equations of motion for vehicle
# Assumes that angular momentum from spinning rotors is negligible
# Assumes that forces and moments are constant during a timestep
# (should be reasonable for realistic timesteps, like 1ms)
#
# position derivatives equal rotated body velocities
xDot = AQ.rotateVector(self.v_Q_i.inv(), y[3:6])
# acceleration = F/m
vDot = 1 / self.mass * self.externalForce.T[0, :] - np.cross(np.array(y[10:]), np.array(y[3:6])).T
# update quaternnions
att = AQ.Quaternion(np.array(y[6:10]))
# qDot = att.inv()*AQ.vectorAsQuat(.5*np.array([y[10:]]).T)
qDot = AQ.qDotFromOmega(att, np.array(y[10:]))
# omegadots
effectiveMoment = (
self.externalMoment - np.array([np.cross(np.array(y[10:]), np.dot(self.Inertia, np.array(y[10:])))]).T
)
omegadots = np.dot(self.invInertia, effectiveMoment).T[0, :]
# print 'xdot: ',xDot.T[0]
# print 'vdot: ',vDot
# print 'qdot: ',qDot
# print 'wdot: ',omegadots
yDot = np.concatenate((xDot.T[0], vDot, qDot, omegadots), 1)
return yDot
