def getForcesAndMoments(self):
# vehicleVelocity and vehicleOmega should be I_v_bcm and I_w_veh, in vehicle coords.
# windvelocity should have already been rotated into vehicle coords.
# returns forces and moments in vehicle reference frame
localWind = -(self.localVelAirRel)
omega = self.motorState[1]
FandM = self.prop.calculateForcesAndMoments(omega,localWind,self.direction)
output=[]
output.append(AQ.rotateVector(self.m_Q_v.inv(),FandM[0]))
output.append(AQ.rotateVector(self.m_Q_v.inv(),FandM[1]))
return FandM
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
def updateState(self,dT,vehicleVelocity = np.zeros((3,1)),vehicleOmega = np.zeros((3,1)),
windVelocity = np.zeros((3,1))):
# vehicle velocity, vehicle omega, and wind velocity are all given in vehicle frame
# first, calculate local velocity relative to the air
localVelAirRelVehFrame = vehicleVelocity + np.cross(vehicleOmega.T,self.position.T).T - windVelocity
# rotate into motor frame (in case we decide to tilt motors
self.localVelAirRel = AQ.rotateVector(self.m_Q_v,localVelAirRelVehFrame)
# solve ODEs
#y = sc.integrate.odeint(self.motorEOM,self.motorState,np.array([self.lastTime, self.lastTime + dT]))
#self.motorState = y[-1][:]
# faster?
#print 'motorState: ',self.motorState,'motorDeriv: ',self.motorEOM(self.motorState,dT)
self.motorState = self.motorState + dT*self.motorEOM(self.motorState,dT)
# then, calculate forces and moments
self.lastTime = self.lastTime + dT;
return self.motorState
def updateState(self,
dT,
motorCommands,
windVelocity=np.zeros((3, 1)),
disturbance=0,
externalForces=[]):
# Initialize force and moment vectors
Force = np.zeros((3, 1))
Moment = np.zeros((3, 1))
vel = np.array([self.stateVector[0, 3:6]]).T
omega = np.array([self.stateVector[0, 10:]]).T
self.v_Q_i = AQ.Quaternion(np.array(self.stateVector[0, 6:10]))
windVelBody = AQ.rotateVector(self.v_Q_i, windVelocity)
state = 'flying'
if (self.stateVector[0, 2] >= 0): # on ground?
if (np.dot(
AQ.rotateVector(self.v_Q_i.inv(), vel).T,
np.array([[0, 0, 1]]).T) > 2.): # falling fast
state = 'ground' #'crashed'
else:
state = 'ground'
#print state
if (state == 'crashed'):
return self.stateVector
# update all submodules (motors) and retrieve forces and moments
for ii in range(len(self.motorList)):
# update command
self.motorList[ii].commandMotor(motorCommands[ii])
# integrate state
self.motorList[ii].updateState(dT,
vehicleVelocity=vel,
vehicleOmega=omega,
windVelocity=windVelBody +
disturbance * np.random.randn(3, 1))
# get aero forces and moments from motor
FandM = self.motorList[ii].getForcesAndMoments()
# Sum all forces
Force = Force + FandM[0]
# Sum all moments + r_motor X F_motor
Moment = Moment + FandM[1] + np.cross(
self.motorList[ii].position.T, FandM[0].T).T
# add external forces, if any
# each entry in externalForces must be a tuple, the first entry of which is the applied force, and the second of which is the application location.
# both of these should be in body frame
for exFor in externalForces:
Force += exFor[0]
Moment += np.cross(exFor[1].T, exFor[0].T).T
# add drag force
relWindVector = vel - windVelBody
windMagnitude = np.sqrt(np.dot(relWindVector.T, relWindVector))
eWind = relWindVector / (windMagnitude + .00000000001)
dragForce = -0.5 * airDensity * self.cD * windMagnitude**2 * eWind
Force = Force + dragForce
# add gravity
Force = Force + self.mass * np.dot(self.v_Q_i.asRotMat, self.gravity)
#Force = Force + self.mass*self.gravity
# add ground force
if (self.stateVector[0, 2] >= 0):
# Add normal force
Kground = 1000
Kdamp = 50
Kangle = .001
groundForceMag = Kground * self.stateVector[0, 2] + Kdamp * np.dot(
self.v_Q_i.asRotMat, vel)[2, 0]
roll, pitch, yaw = self.v_Q_i.asEuler
groundPitchMoment = max(
min(
Kangle *
(-Kdamp * pitch - 6 * Kdamp * self.stateVector[0, 11]), 1),
-1)
groundRollMoment = max(
min(
Kangle *
(-Kdamp * roll - 6 * Kdamp * self.stateVector[0, 10]), 1),
-1)
groundYawMoment = Kangle * (-6 * Kdamp * self.stateVector[0, 12])
Force = Force + AQ.rotateVector(
self.v_Q_i,
np.array([[0, 0, -groundForceMag]]).T) - Kdamp * vel
Moment = Moment + np.array(
[[0., groundPitchMoment, 0.]]).T + np.array([[
groundRollMoment, 0., 0.
]]).T + np.array([[0., 0., groundYawMoment]]).T
self.externalForce = Force
self.externalMoment = Moment
#print state, self.mass*np.dot(self.v_Q_i.asRotMat,self.gravity).T
# solve eoms
#y = sc.integrate.odeint(self.vehicleEOM,self.stateVector[0,:],np.array([self.lastTime,self.lastTime + dT]))
attitude = AQ.Quaternion(np.array(self.stateVector[0, 6:10]))
magOmega = np.linalg.norm(omega)
axis = 1. / (magOmega + 1e-15) * omega
updateQuat = AQ.quatFromAxisAngle(axis, dT * magOmega)
self.stateVector = self.stateVector + dT * np.array(
[self.vehicleEOM(self.stateVector[0, :], 0)])
newAtt = updateQuat * attitude
self.stateVector[0, 6:10] = newAtt.q
#self.stateVector[0,:] = y[-1][:]
# Experiments with stupid Euler integration:
#y = self.stateVector[0,:] + dT*self.vehicleEOM(self.stateVector[0,:],0)
#self.stateVector[0,6:10] = AQ.normalize(self.stateVector[0,6:10])
self.lastTime = self.lastTime + dT
# update rotation matrix
#print 'euler angles: ',self.v_Q_i.asEuler, 'rates: ',self.stateVector[0,10:]
#print 'position: ', self.stateVector[0,0:3]
measuredAcceleration = 1. / self.mass * (
self.externalForce -
self.mass * np.dot(self.v_Q_i.asRotMat, self.gravity))
return [self.stateVector, measuredAcceleration]
import AeroQuaternion as AQ import numpy as np Q1 = AQ.Quaternion(np.array([0, 0, 0])) Q2 = AQ.Quaternion(np.array([0, 0, 90])) Q3 = AQ.Quaternion(np.array([0, 90, 0])) Q4 = AQ.Quaternion(np.array([0, 0, 180])) Q5 = AQ.Quaternion(np.array([90, 0, 0])) Q6 = AQ.Quaternion(np.array([0, 45, 0])) Q7 = AQ.Quaternion(np.array([45, 0, 0])) v = np.array([[0, 1, 0]]) print 'printing rotations of ', v.T print '0 : ', AQ.rotateVector(Q1, v).T print '90 yaw: ', AQ.rotateVector(Q2, v).T print 'with matrix: ', np.dot(Q2.asRotMat, v.T).T print '90 pitch: ', AQ.rotateVector(Q3, v).T print 'with matrix: ', np.dot(Q3.asRotMat, v.T).T print '90 roll: ', AQ.rotateVector(Q5, v).T, print 'with matrix: ', np.dot(Q5.asRotMat, v.T).T print '45 roll: ', AQ.rotateVector(Q7, v).T print 'with matrix: ', np.dot(Q7.asRotMat, v.T).T print '45 pitch: ', AQ.rotateVector(Q6, v).T print 'with matrix: ', np.dot(Q6.asRotMat, v.T).T print '180 yaw: ', AQ.rotateVector(Q4, v).T print 'with matrix: ', np.dot(Q4.asRotMat, v.T).T print 'Q7 as euler', Q7.asEuler
def updateState(self, dT, motorCommands, windVelocity=np.zeros((3, 1)), disturbance=0, externalForces=[]):
# Initialize force and moment vectors
Force = np.zeros((3, 1))
Moment = np.zeros((3, 1))
vel = np.array([self.stateVector[0, 3:6]]).T
omega = np.array([self.stateVector[0, 10:]]).T
self.v_Q_i = AQ.Quaternion(np.array(self.stateVector[0, 6:10]))
windVelBody = AQ.rotateVector(self.v_Q_i, windVelocity)
state = "flying"
if self.stateVector[0, 2] >= 0: # on ground?
if np.dot(AQ.rotateVector(self.v_Q_i.inv(), vel).T, np.array([[0, 0, 1]]).T) > 2.0: # falling fast
state = "ground" #'crashed'
else:
state = "ground"
# print state
if state == "crashed":
return self.stateVector
# update all submodules (motors) and retrieve forces and moments
for ii in range(len(self.motorList)):
# update command
self.motorList[ii].commandMotor(motorCommands[ii])
# integrate state
self.motorList[ii].updateState(
dT,
vehicleVelocity=vel,
vehicleOmega=omega,
windVelocity=windVelBody + disturbance * np.random.randn(3, 1),
)
# get aero forces and moments from motor
FandM = self.motorList[ii].getForcesAndMoments()
# Sum all forces
Force = Force + FandM[0]
# Sum all moments + r_motor X F_motor
Moment = Moment + FandM[1] + np.cross(self.motorList[ii].position.T, FandM[0].T).T
# add external forces, if any
# each entry in externalForces must be a tuple, the first entry of which is the applied force, and the second of which is the application location.
# both of these should be in body frame
for exFor in externalForces:
Force += exFor[0]
Moment += np.cross(exFor[1].T, exFor[0].T).T
# add drag force
relWindVector = vel - windVelBody
windMagnitude = np.sqrt(np.dot(relWindVector.T, relWindVector))
eWind = relWindVector / (windMagnitude + 0.00000000001)
dragForce = -0.5 * airDensity * self.cD * windMagnitude ** 2 * eWind
Force = Force + dragForce
# add gravity
Force = Force + self.mass * np.dot(self.v_Q_i.asRotMat, self.gravity)
# Force = Force + self.mass*self.gravity
# add ground force
if self.stateVector[0, 2] >= 0:
# Add normal force
Kground = 1000
Kdamp = 50
Kangle = 0.001
groundForceMag = Kground * self.stateVector[0, 2] + Kdamp * np.dot(self.v_Q_i.asRotMat, vel)[2, 0]
roll, pitch, yaw = self.v_Q_i.asEuler
groundPitchMoment = max(min(Kangle * (-Kdamp * pitch - 6 * Kdamp * self.stateVector[0, 11]), 1), -1)
groundRollMoment = max(min(Kangle * (-Kdamp * roll - 6 * Kdamp * self.stateVector[0, 10]), 1), -1)
groundYawMoment = Kangle * (-6 * Kdamp * self.stateVector[0, 12])
Force = Force + AQ.rotateVector(self.v_Q_i, np.array([[0, 0, -groundForceMag]]).T) - Kdamp * vel
Moment = (
Moment
+ np.array([[0.0, groundPitchMoment, 0.0]]).T
+ np.array([[groundRollMoment, 0.0, 0.0]]).T
+ np.array([[0.0, 0.0, groundYawMoment]]).T
)
self.externalForce = Force
self.externalMoment = Moment
# print state, self.mass*np.dot(self.v_Q_i.asRotMat,self.gravity).T
# solve eoms
# y = sc.integrate.odeint(self.vehicleEOM,self.stateVector[0,:],np.array([self.lastTime,self.lastTime + dT]))
attitude = AQ.Quaternion(np.array(self.stateVector[0, 6:10]))
magOmega = np.linalg.norm(omega)
axis = 1.0 / (magOmega + 1e-15) * omega
updateQuat = AQ.quatFromAxisAngle(axis, dT * magOmega)
self.stateVector = self.stateVector + dT * np.array([self.vehicleEOM(self.stateVector[0, :], 0)])
newAtt = updateQuat * attitude
self.stateVector[0, 6:10] = newAtt.q
# self.stateVector[0,:] = y[-1][:]
# Experiments with stupid Euler integration:
# y = self.stateVector[0,:] + dT*self.vehicleEOM(self.stateVector[0,:],0)
# self.stateVector[0,6:10] = AQ.normalize(self.stateVector[0,6:10])
self.lastTime = self.lastTime + dT
# update rotation matrix
# print 'euler angles: ',self.v_Q_i.asEuler, 'rates: ',self.stateVector[0,10:]
# print 'position: ', self.stateVector[0,0:3]
measuredAcceleration = (
1.0 / self.mass * (self.externalForce - self.mass * np.dot(self.v_Q_i.asRotMat, self.gravity))
)
return [self.stateVector, measuredAcceleration]
Q1 = AQ.Quaternion(np.array([0,0,0])) Q2 = AQ.Quaternion(np.array([0,0,90])) Q3 = AQ.Quaternion(np.array([0,90,0])) Q4 = AQ.Quaternion(np.array([0,0,180])) Q5 = AQ.Quaternion(np.array([90,0,0])) Q6 = AQ.Quaternion(np.array([0,45,0])) Q7 = AQ.Quaternion(np.array([45,0,0])) v = np.array([[0,1,0]]) print 'printing rotations of ',v.T print '0 : ', AQ.rotateVector(Q1,v).T print '90 yaw: ', AQ.rotateVector(Q2,v).T print 'with matrix: ', np.dot(Q2.asRotMat,v.T).T print '90 pitch: ', AQ.rotateVector(Q3,v).T print 'with matrix: ', np.dot(Q3.asRotMat,v.T).T print '90 roll: ', AQ.rotateVector(Q5,v).T, print 'with matrix: ', np.dot(Q5.asRotMat,v.T).T print '45 roll: ', AQ.rotateVector(Q7,v).T print 'with matrix: ', np.dot(Q7.asRotMat,v.T).T print '45 pitch: ', AQ.rotateVector(Q6,v).T print 'with matrix: ', np.dot(Q6.asRotMat,v.T).T print '180 yaw: ', AQ.rotateVector(Q4,v).T print 'with matrix: ', np.dot(Q4.asRotMat,v.T).T print 'Q7 as euler',Q7.asEuler
