refType = 'xyah'
yawCmd = 0.
zCmd = 10.
cutMotors = True
gpsGood = False
###################################
# Create Quadrotor object
# and initialize sim stuff
###################################
MotorHeightOffset = 0.
motorPos = [np.array([[.19,-.19,MotorHeightOffset]]).T,
np.array([[.19,.19,MotorHeightOffset]]).T,
np.array([[-.19,.19,MotorHeightOffset]]).T,
np.array([[-.19,-.19,MotorHeightOffset]]).T]
print "initializing"
Quad = Multirotor.Multirotor(fuselageMass = .5,motorPositions = motorPos) # default is quadrotor
print "line 45: ",len(Quad.motorList)
initialAtt = AQ.Quaternion(attitude)
Quad.stateVector[0,6] = initialAtt.q[0]
Quad.stateVector[0,7] = initialAtt.q[1]
Quad.stateVector[0,8] = initialAtt.q[2]
Quad.stateVector[0,9] = initialAtt.q[3]
#while(True):
# pass
idx = 0
dt = 0.002
dtControl = .008
T = 1.3
numsteps = 3
maxInd = int(math.ceil(T/dt))
stateHist = np.zeros((numsteps*maxInd,13))
xform = Q
#pose.rotation.x = state[0,6]
#pose.rotation.y = state[0,7]
#pose.rotation.z = state[0,8]
#pose.rotation.w = state[0,9]
pose.rotation.x = xform.q[0]
pose.rotation.y = xform.q[1]
pose.rotation.z = xform.q[2]
pose.rotation.w = xform.q[3]
socketEKF.send( pose.SerializeToString() )
# create quadrotor
Quad = Multirotor.Multirotor() # default is quadrotor
idx = 0
dt = 0.005
T = 1.3
numsteps = 3
maxInd = int(math.ceil(T/dt))
stateHist = np.zeros((numsteps*maxInd,13))
# command motor
commands = [.5,.5,.5,.5]
#commands = [0,0,0,0]
# add wind
windvel = np.zeros((3,1))
# run for a while
controller = QuadrotorController.Controller()
EKF = KinematicEKF.kinematicQuadEKF()
AttEstimator = KinematicEKF.AttitudeComplementaryFilter()
name = 'Quadrotor Visualizer' yaw = 0 height = 0; position = np.zeros((3,1)) attitude = [0,0,0] attEst = [0,0,0] cameraMode = 'CHASE_CAM' refType = 'xyah' yawCmd = 0. zCmd = 10. cutMotors = True ################################### # Create Quadrotor object # and initialize sim stuff ################################### Quad = Multirotor.Multirotor(fuselageMass = 0.5) # default is quadrotor idx = 0 dt = 0.002 T = 1.3 numsteps = 3 maxInd = int(math.ceil(T/dt)) stateHist = np.zeros((numsteps*maxInd,13)) commands = [0.5,0.5,0.5,0.5] # add wind #windvel = np.array([[30.,30.,0]]).T windvel = np.zeros((3,1)) # run for a whil controller = QuadrotorController.Controller() EKF = KinematicEKF.kinematicQuadEKF() AttEstimator = KinematicEKF.AttitudeComplementaryFilter(initialAttitude=AQ.Quaternion(np.array(attEst)), timeConstant = 10.,Kbias = .00) reference = [0.,0.,5.,0.]