Exemplo n.º 1
0
	def __init__(self, maxLinearSpeed, maxAngularSpeed, correctionMatrix):
		self.dest = PoseModel() 	#Destination point in World.
		self.destENU = PoseModel()	#Destination in ENU frame.
		self.camera = PoseModel()	#Camera position in World.
		self.body = PoseModel()		#Body position in World. Y is aligned with Pixhawk front. Corrected position of camera.
		self.enu = PoseModel()		#ENU frame. Origin of this frame is body position. Y is aligned with geographical North
		self.localEnuPose = PoseModel()
		
		self.headingAngle = None	#angle from pixhawk front to North in CCW(right-hand). The number is represented in radians 
		
		self.maxLinearSpeed = maxLinearSpeed
		self.maxAngularSpeed = maxAngularSpeed 
		self.correctionMatrix = correctionMatrix
		
		self.twist = MavTwist(self)
Exemplo n.º 2
0
class MavModel(object):
	
	
	def __init__(self, maxLinearSpeed, maxAngularSpeed, correctionMatrix):
		self.dest = PoseModel() 	#Destination point in World.
		self.destENU = PoseModel()	#Destination in ENU frame.
		self.camera = PoseModel()	#Camera position in World.
		self.body = PoseModel()		#Body position in World. Y is aligned with Pixhawk front. Corrected position of camera.
		self.enu = PoseModel()		#ENU frame. Origin of this frame is body position. Y is aligned with geographical North
		self.localEnuPose = PoseModel()
		
		self.headingAngle = None	#angle from pixhawk front to North in CCW(right-hand). The number is represented in radians 
		
		self.maxLinearSpeed = maxLinearSpeed
		self.maxAngularSpeed = maxAngularSpeed 
		self.correctionMatrix = correctionMatrix
		
		self.twist = MavTwist(self)
	#eof
	
	
	
	def runPoseCorrection(self):
		self.body.setMatrix(self.camera.getMatrix())
		
		if(self.correctionMatrix is not None):
			self.body.transformByMatrix(self.correctionMatrix)
		
		return self
	#eof
	
	
	
	def runCalculation(self):
		if(self.calENU() and self.calDestENU()):
			self.twist.calAngular().calLinear()
		else:
			self.twist.reset()
		
		return self
	#eof
	
	
	
	def calDestENU(self):
		if(self.enu.isNone() or self.dest.isNone() or self.localEnuPose.isNone()):
			return False
		else:
			enuInverse = self.localEnuPose.getInverseMatrix()
			self.destENU.setMatrix(numpy.dot(enuInverse, self.dest.getMatrix()))
			return True
	#eof
	
	
	
	def calENU(self):
		if(self.body.isNone() or self.headingAngle is None):
			return False
		else:
			br = self.body.getRotationAroundZ()
			
			q = rotation_matrix(br+self.headingAngle, (0,0,1))
			
			t = translation_from_matrix(self.body.getMatrix())
			t = translation_matrix(t)
			
			self.enu.setMatrix(t).transformByMatrix(q)
			return True
#eoc