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