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