def calibrateR(self,motor,Mass):
I = self.fI/float(self.number)
#OI = [0.177, 0.177, 0.334]
#I = [(OI[1]-OI[2])/OI[0], -(OI[0]-OI[2])/OI[1], (OI[0]-OI[1])/OI[2]]
#This works if I is well known and simulation step is small enough so that the difference between oldomega and omega is small
Rx = (self.Ialpha[0][0] - self.Iomega[0][1]*self.Iomega[0][2]*I[0])/self.fP[0]#pow(self.fP[0],2)
#Rx = ((self.Ialpha[1][0]-self.Ialpha[0][0]) - I[0]*(self.Iomega[1][1]*self.Iomega[1][2] - self.Iomega[0][1]*self.Iomega[0][2]))/(pow(self.fP[1],2)-pow(self.fP[0], 2))
Ry = (self.Ialpha[0][1] - self.Iomega[0][0]*self.Iomega[0][2]*I[1])/self.fP[0]#pow(self.fP[0],2)
#Ry = ((self.Ialpha[1][1]-self.Ialpha[0][1]) - I[1]*(self.Iomega[1][0]*self.Iomega[1][2] - self.Iomega[0][0]*self.Iomega[0][2]))/(pow(self.fP[1],2)-pow(self.fP[0], 2))
Rz = (self.Ialpha[0][2] - self.Iomega[0][0]*self.Iomega[0][1]*I[2])/self.fP[0]#pow(self.fP[0],2)
#Rz = ((self.Ialpha[1][2]-self.Ialpha[0][2]) - I[2]*(self.Iomega[1][0]*self.Iomega[1][1] - self.Iomega[0][0]*self.Iomega[0][1]))/(pow(self.fP[1],2)-pow(self.fP[0], 2))
#print "Estimated Rx " + str(Rx)
#print "Estimated Ry " + str(Ry)
#print "Estimated Ry " + str(Rz)
q1 = self.Iq[0]
a1 = self.Ia[0]
a1 = a1.rotate(q1.conj())
q2 = self.Iq[1]
a2 = self.Ia[1]
a2 = a2.rotate(q2.conj())
g = Vector([0,0, 9.81])
g1 = g.rotate(q1.conj())
g2 = g.rotate(q2.conj())
#Ok but needs to be with very small velocity to neglect friction
Rt = Mass*((a1- a2) + (g1-g2))/(self.fP[0]-self.fP[1])
self.R[motor] = [Rx,Ry,Rz,Rt[2]]
return True
def __init__(self):
'''
Constructor
'''
#self.QRef = qmath.quaternion([0, 0, 0])
self.QRef = Quaternion([0,0,0])
self.POmega = 0
self.PQ = 0
self.PV = 0
self.PA = 0
self.I = Vector([0, 0, 0])
self.Torque = Vector([0, 0, 0])
self.Output = Vector([0,0,0,0])
self.VRef = Vector([0,0,0])
self.Thrust = Vector([0,0,0])
class PSquare:
'''
classdocs
'''
lastqRef = Quaternion()
lastvRef = Vector()
QRef = Quaternion()
VRef = Vector()
PQ = None
POmega = None
PInt = None
I = Matrix()
Torque = Vector()
Thrust = Vector()
Km1 = None
Km2 = None
Km3 = None
Km4 = None
Output = None
K = None
b = None
L = None
Mass = None
QInt = Quaternion()
def __init__(self):
'''
Constructor
'''
#self.QRef = qmath.quaternion([0, 0, 0])
self.QRef = Quaternion([0,0,0])
self.POmega = 0
self.PQ = 0
self.PV = 0
self.PA = 0
self.I = Vector([0, 0, 0])
self.Torque = Vector([0, 0, 0])
self.Output = Vector([0,0,0,0])
self.VRef = Vector([0,0,0])
self.Thrust = Vector([0,0,0])
def setI(self, I):
self.I = Matrix([[I[0],0,0],[0,I[1],0],[0,0,I[2]]])
def setPs(self, v):
self.PQ = v[0]
self.POmega = v[1]
self.PInt = v[2]
def setQRef(self, q, v):
self.QRef = q
self.VRef = v
def setMotorCoefficient(self, m1, m2, m3, m4):
self.Km1 = m1
self.Km2 = m2
self.Km3 = m3
self.Km4 = m4
def setParameters(self, k,L,b,m):
self.K = k
self.L = L
self.b = b
self.Mass = m
def controlSignal(self):
if self.QRef.vector().mag()==0:
aControl = 0
else:
aControl = 1
if self.VRef.mag()==0 and aControl!=0:
vControl = 0
else:
vControl = 1
return [vControl, aControl]
def velocityControl(self, qM, aM, vM, vControl):
if vControl:
vErr = self.VRef-vM
self.lastvRef = self.VRef
else:
vErr = Vector([aM[0], aM[1], 0])
self.lastvRef = Vector([0,0,0])
#TODO: try having a real PID with here rather than P with Kp=1
#print "AReq " + (-Params.Gravity)
#print "vErr " + vErr
#TODO: try multiplying by the mass here as this is actually computing the acceleration
Thrust = vErr - Params.Gravity
qRef = createRotation(Thrust, Vector([0,0,1]))
#This is required to keep the z component compensating for gravity.
#If not, the norm is kept when rotating and the z component is reduced accordingly.
#Maybe try by creating the rotation between z and the error and adding the gravity after!
#TODO: ou alors c'est fait plus loin en divisant par z' et celui-ci ne sert a rien?
qRef.w = 1
qRef.normalize()
return [Thrust, qRef]
def attitudeControl(self, qM, omegaM, qRef, dt):
qErr = qRef * qM.conj()
self.QInt = self.QInt + qErr*dt*self.PInt
#TODO: QInt here?
if qErr[0]<0:
intErr = -Vector(self.QInt)
else:
intErr = Vector(self.QInt)
if qErr[0]<0:
axisErr = -Vector(qErr)
else:
axisErr = Vector(qErr)
#PInt>>0 -> precision; PInt<<0 -> Reaction speed
Torque = self.I*(axisErr*self.PQ - omegaM*self.POmega - intErr)
#Iinv = Matrix([[self.I[0][0],0,0],[0,self.I[1][1],0],[0,0,self.I[2][2]]])
#Torque = Iinv*(axisErr*self.PQ - omegaM*self.POmega)
return Torque
def computePP(self, qM, omegaM, aM, vM, dt):
#print "qM " + qM
#print "omegaM " + omegaM
#print "aM " + aM
#print "vM " + vM
print "VRef " + self.VRef
print "QRef " + self.QRef
[vControl, aControl] = self.controlSignal()
print "Control signal : " + str(vControl) + " " + str(aControl)
[self.Thrust, vQRef] = self.velocityControl(qM, aM, vM, vControl)
#print "qm inv " + qM.conj()
#print "Thrust " + self.Thrust*self.Mass + " (" + str(sqrt(self.Thrust.mag())) + ")"
#print "Rotation " + qM
#TODO: remove the -qM as -qM==qM
#TODO: je pense qu'en ce qui concerne l'axe z ici, comme on rotate z et qu'on prends la composante z, q ou q* donne les meme resultat
#TODO: je pense que c'est ca qui rescale T pour contrer correctement la gravite.
T = self.Thrust*self.Mass*(1/(Vector([0,0,1]).rotate(-qM.conj())[2]))
#T = self.Thrust.rotate(-qM.conj())*self.Mass
#print "T " + T + " (" + str(sqrt(T.mag())) + ")"
T = T[2]
#print "T " + str(T)
if vControl or not aControl:
qRef = vQRef
else:
qRef = self.QRef
print "qRef for torque " + qRef
self.lastqRef = qRef
self.Torque = self.attitudeControl(qM, omegaM, qRef, dt)
#fit
if Params.TorqueIsSet:
return self.Torque
#print "Requested T " + str(T)
#print "Requested Torque " + self.Torque
#print self.Km1
#print self.Km2
#print self.Km3
#print self.Km4
deno = (self.Km1[0]*(self.Km2[1]*(self.Km3[2]*self.Km4[3]+self.Km4[2]*self.Km3[3])+self.Km4[1]*(self.Km2[2]*self.Km3[3]+self.Km3[2]*self.Km2[3]))+self.Km3[0]*(self.Km2[1]*(self.Km1[2]*self.Km4[3]+self.Km4[2]*self.Km1[3])+self.Km4[1]*(self.Km1[2]*self.Km2[3]+self.Km2[2]*self.Km1[3])))
#print deno
p1=((self.Km3[0])*((self.Km2[1])*((self.Km4[2])*T+(self.Torque[2])*(self.Km4[3]))+(self.Km4[1])*((self.Km2[2])*T+(self.Torque[2])*(self.Km2[3]))+(self.Torque[1])*((self.Km2[2])*(self.Km4[3])-(self.Km4[2])*(self.Km2[3])))+(self.Torque[0])*((self.Km2[1])*((self.Km3[2])*(self.Km4[3])+(self.Km4[2])*(self.Km3[3]))+(self.Km4[1])*((self.Km2[2])*(self.Km3[3])+(self.Km3[2])*(self.Km2[3]))))/deno
p2=-((self.Km1[0])*((self.Km4[1])*((self.Torque[2])*(self.Km3[3])-(self.Km3[2])*T)+(self.Torque[1])*(-(self.Km3[2])*(self.Km4[3])-(self.Km4[2])*(self.Km3[3])))+(self.Km3[0])*((self.Km4[1])*((self.Torque[2])*(self.Km1[3])-(self.Km1[2])*T)+(self.Torque[1])*(-(self.Km1[2])*(self.Km4[3])-(self.Km4[2])*(self.Km1[3])))+(self.Torque[0])*(self.Km4[1])*((self.Km3[2])*(self.Km1[3])-(self.Km1[2])*(self.Km3[3])))/deno
p3=-((self.Km1[0])*((self.Km2[1])*(-(self.Km4[2])*T-(self.Torque[2])*(self.Km4[3]))+(self.Km4[1])*(-(self.Km2[2])*T-(self.Torque[2])*(self.Km2[3]))+(self.Torque[1])*((self.Km4[2])*(self.Km2[3])-(self.Km2[2])*(self.Km4[3])))+(self.Torque[0])*((self.Km2[1])*((self.Km1[2])*(self.Km4[3])+(self.Km4[2])*(self.Km1[3]))+(self.Km4[1])*((self.Km1[2])*(self.Km2[3])+(self.Km2[2])*(self.Km1[3]))))/deno
p4=-((self.Km1[0])*((self.Km2[1])*((self.Torque[2])*(self.Km3[3])-(self.Km3[2])*T)+(self.Torque[1])*((self.Km2[2])*(self.Km3[3])+(self.Km3[2])*(self.Km2[3])))+(self.Km3[0])*((self.Km2[1])*((self.Torque[2])*(self.Km1[3])-(self.Km1[2])*T)+(self.Torque[1])*((self.Km1[2])*(self.Km2[3])+(self.Km2[2])*(self.Km1[3])))+(self.Torque[0])*(self.Km2[1])*((self.Km3[2])*(self.Km1[3])-(self.Km1[2])*(self.Km3[3])))/deno
self.Output = Vector([p1,p2,p3,p4])
#print "Torque0 = " + str(p1*self.Km1[0]-p3*self.Km3[0])
#print "Torque1 = " + str(p2*self.Km2[1]-p4*self.Km4[1])
#print "Torque2 = " + str(p1*self.Km1[2]-p2*self.Km2[2]+p3*self.Km3[2]-p4*self.Km4[2])
#print "T = " + str(p1*self.Km1[3]+p2*self.Km2[3]+p3*self.Km3[3]+p4*self.Km4[3])
return self.Output
def computeThrust(self):
if not self.TorqueIsSet:
self.Thrust = Vector([0, 0, self.m1.Thrust+self.m2.Thrust+self.m3.Thrust+self.m4.Thrust])
else:
self.Thrust = Vector([0,0,0])
class Body(object):
'''
classdocs
'''
oldomega = Vector()
m1 = Motor()
m2 = Motor()
m3 = Motor()
m4 = Motor()
'''internal state:
alpha -> computed from Torque
omega -> Sensor(gyro)
quat -> Sensor(DMZ)
acceleration -> Sensor(acc)
velocity
position
'''
ctrl = None
#set by controller
CtrlInput = [0,0,0,0]
#from motor
Torque = Vector() #vector
Thrust = Vector() #vector
#from physics
Friction = Vector() #vector
#state
Alpha = Vector() #vector
Omega = Vector() #vector
Quat = Quaternion() #quaternion
ThetaDot = Vector()
Angles = Vector()
Acceleration = Vector() #vector
Velocity = Vector() #vector
Position = Vector() #vector
#model parameters
L = None #longueur des bras #scalar
Mass = None #masse #scalar
I = Vector() #inertia matrix #vector
K_d = None #friction-linear velocity #scalar
#simulation parameters
TorqueIsSet = False #switch #scalar
MaxTorque = 100
UseController = False
def __init__(self, simu):
'''
Constructor
'''
if Params.ctrlType==0:
self.ctrl = PID()
print "Using controller PID"
elif Params.ctrlType==1:
self.ctrl = PSquare()
print "Using controller PSquare"
self.m1.setParams(1)
self.m2.setParams(-1)
self.m3.setParams(1)
self.m4.setParams(-1)
self.sensors = SensorValues()
self.calib = Calibration(self, simu)
def setModel(self, I, K_d, L, Mass):
self.I = I
self.K_d = K_d
self.L = L
self.Mass = Mass
#print "inertia factor x " + str((self.I[1]-self.I[2])/self.I[0])
def setParameters(self,TorqueIsSet, MaxTorque, UseController):
self.TorqueIsSet = TorqueIsSet
self.MaxTorque = MaxTorque
self.UseController = UseController
def setReference(self, qRef, vRef):
self.ctrl.setQRef(qRef, vRef)
def initController(self):
self.ctrl.setI(self.I)
if Params.ctrlType==0:
self.ctrl.setPs([10,15,2])
elif Params.ctrlType==1:
self.ctrl.setPs(Params.PCoeff)
def applyController(self):
t = self.ctrl.computePP(self.Quat, self.Omega, self.Acceleration, self.Velocity, Params.dt)
if self.TorqueIsSet:
if t[0]>self.MaxTorque:
t[0] = self.MaxTorque
if t[1]>self.MaxTorque:
t[1] = self.MaxTorque
if t[2]>self.MaxTorque:
t[2] = self.MaxTorque
if t[0]<-self.MaxTorque:
t[0] = -self.MaxTorque
if t[1]<-self.MaxTorque:
t[1] = -self.MaxTorque
if t[2]<-self.MaxTorque:
t[2] = -self.MaxTorque
self.CtrlInput = t
#print "Controller input " + t
def changeInput(self, motor, value):
self.CtrlInput[motor] = value;
print "CtrlInput is now " + str(self.CtrlInput)
def nextStep(self, dt):
#controller
if self.UseController:
self.applyController()
ooldomega = Vector(self.Omega)
oldaplha = Vector(self.Alpha)
#motor
self.computeMotor(dt)
#torque
self.computeTorque()
#thrust
self.computeThrust()
#fiction
self.computeFriction()
#acceleration
self.computeAcceleration()
#vitesse
self.computeVelocity(dt)
#position
self.computePosition(dt)
#alpha
self.computeAlpha(dt)
#omega
self.computeOmega(dt)
#thetaDot
self.computeThetaDot(dt)
#theta
self.computeAngles(dt)
#quaternion
self.computeQuaternion(dt)
self.sensors.setValues(self.Acceleration, self.Omega, self.Quat, self.Alpha)
''''I = (oldaplha[0]-self.Alpha[0])/(ooldomega[1]*ooldomega[2]-self.Omega[1]*self.Omega[2])
if I > 1:
#print "oldomega " + oldomega
print "oldomega " + ooldomega
print "omega " + self.Omega
print "oldalpha " + oldaplha
print "alpha " + self.Alpha
#print "self.oldomega + " + self.oldomega
print "diff " + str(ooldomega[1]*ooldomega[2]-self.Omega[1]*self.Omega[2])
print "I Body " + str(I)'''
#I = (self.I[2]-self.I[1])/self.I[0]
#rx = self.L*self.m1.K*pow(Params.MaxOmega,2)/(10000.*self.I[0])
'''print "true I " + str(I)
print "true rx " + str(rx)
print "true alpha " + str(self.Alpha)
print "omega motor " + str(self.m1.Omega)
print "omega ici " + str(self.m1.Power*Params.MaxOmega/100.)
print "torque " + str(self.Torque[0]/self.I[0])
print "torque ici " + str(rx*pow(self.m1.Power,2))
print "alpha second terme " + str(oldomega[1]*oldomega[2]*I)'''
#alpha = pow(self.m1.Power,2)*rx - ooldomega[1]*ooldomega[2]*I
#I = -0.887005649718
#Rx = (self.Alpha[0]+ooldomega[1]*ooldomega[2]*I)/pow(self.m1.Power,2)
#print Rx
#print "calxulated alpha " + str(alpha)
#print "Calculated rx " + str(Rx)
#self.oldomega = Vector(ooldomega)
def setMotorConstants(self, Rho, K_v, K_t, K_tau, I_M, A_swept, A_xsec, Radius, C_D):
self.m1.setConstants(Rho, K_v, K_t, K_tau, I_M, A_swept, A_xsec, Radius, C_D)
self.m2.setConstants(Rho, K_v, K_t, K_tau, I_M, A_swept, A_xsec, Radius, C_D)
self.m3.setConstants(Rho, K_v, K_t, K_tau, I_M, A_swept, A_xsec, Radius, C_D)
self.m4.setConstants(Rho, K_v, K_t, K_tau, I_M, A_swept, A_xsec, Radius, C_D)
if self.UseController:
Rx = self.L*self.m1.K*pow(Params.MaxOmega,2)/(10000)
Ry = self.L*self.m1.K*pow(Params.MaxOmega,2)/(10000)
Rz = self.m1.B*pow(Params.MaxOmega,2)/(10000)
RT = self.m1.K*pow(Params.MaxOmega,2)/(10000)
print "Real parameters :"
print "R1 " + str([Rx, 0, Rz, RT])
print "R2 " + str([0, Ry, Rz, RT])
print "R3 " + str([Rx, 0, Rz, RT])
print "R4 " + str([0, Ry, Rz, RT])
print Params.I
print "I_1 " + str((Params.I[1]-Params.I[2])/Params.I[0])
print "I_2 " + str((Params.I[0]-Params.I[2])/Params.I[1])
print "I_3 " + str((Params.I[0]-Params.I[1])/Params.I[2])
self.ctrl.setMotorCoefficient([Rx, 0, Rz, RT],
[0, Ry, Rz, RT],
[Rx, 0, Rz, RT],
[0, Ry, Rz, RT])
self.ctrl.setParameters(self.m1.K, self.L, self.m1.B, self.Mass)
def computeMotor(self, dt):
if not self.TorqueIsSet:
self.m1.setMeasure(self.CtrlInput[0], dt)
self.m2.setMeasure(self.CtrlInput[1], dt)
self.m3.setMeasure(self.CtrlInput[2], dt)
self.m4.setMeasure(self.CtrlInput[3], dt)
def computeTorque(self):
if not self.TorqueIsSet:
self.Torque = Vector([self.L*(self.m1.Thrust-self.m3.Thrust), self.L*(self.m2.Thrust-self.m4.Thrust), self.m1.Tau_z+self.m2.Tau_z+self.m3.Tau_z+self.m4.Tau_z])
'''print "M1 thrust " + str(self.L*self.m1.Thrust)
print "M2 thrust " + str(self.L*self.m2.Thrust)
print "M3 thrust " + str(self.L*self.m3.Thrust)
print "M4 thrust " + str(self.L*self.m4.Thrust)
print "M1 tauZ " + str(self.m1.Tau_z)
print "M2 tauZ " + str(self.m2.Tau_z)
print "M3 tauZ " + str(self.m3.Tau_z)
print "M4 tauZ " + str(self.m4.Tau_z)'''
#print "Body torque " + self.Torque
else:
self.Torque = self.CtrlInput
def computeFriction(self):
self.Friction = -self.Velocity*self.K_d
def computeAlpha(self, dt):
#print "Omega alpha: " + self.Omega
#oldalpha = self.Alpha
self.Alpha = Vector([(self.Torque[0] - (self.I[1]-self.I[2])*self.Omega[1]*self.Omega[2])/self.I[0],
(self.Torque[1] - (self.I[2]-self.I[0])*self.Omega[0]*self.Omega[2])/self.I[1],
(self.Torque[2] - (self.I[0]-self.I[1])*self.Omega[0]*self.Omega[1])/self.I[2]])
#print "I factor apha " + str((self.Alpha[0] - oldalpha[0])/(self.Omega[1]*self.Omega[2] - self.oldomega[1]*self.oldomega[2]))
#print self.Alpha
#print "alpha second term(true) " + str((self.I[2]-self.I[1])*self.Omega[1]*self.Omega[2]/self.I[0])
def computeOmega(self, dt):
self.Omega = self.Omega + self.Alpha*dt
#print self.Omega
def computeThetaDot(self, dt):
mat1 = Matrix([[1, sin(self.Angles[0])*tan(self.Angles[1]), cos(self.Angles[0])*tan(self.Angles[1])],
[0, cos(self.Angles[0]), -sin(self.Angles[0])],
[0, sin(self.Angles[0])/cos(self.Angles[1]), cos(self.Angles[0])/cos(self.Angles[1])]])
self.ThetaDot = mat1*self.Omega
def computeAngles(self, dt):
self.Angles = self.Angles + self.ThetaDot*dt
def computeQuaternion(self, dt):
self.Quat = Quaternion([self.Angles[0], self.Angles[1], self.Angles[2]])
def mass(self):
return self.Mass
def setOmega(self, v):
self.Omega = v
def setTorque(self, t):
self.Torque = t
def setCtrlInput(self, i):
self.CtrlInput = i
def computeThrust(self):
if not self.TorqueIsSet:
self.Thrust = Vector([0, 0, self.m1.Thrust+self.m2.Thrust+self.m3.Thrust+self.m4.Thrust])
else:
self.Thrust = Vector([0,0,0])
def computeAcceleration(self):
#rotate thrust
#print "Thrust before rotation " + self.Thrust
#print "Rotation " + self.Quat
T = self.Thrust.rotate(self.Quat)
#print "Thrust " + T
self.Acceleration = T/self.Mass
self.Acceleration = Params.Gravity+T*(1/self.Mass)+self.Friction*(1/self.Mass)
def computeVelocity(self, dt):
#print dt
self.Velocity += self.Acceleration*dt
#print self.Velocity
def computePosition(self, dt):
self.Position += self.Velocity*dt
if(Params.ground and self.Position[2]<0):
#On the ground. Reset all values
self.Position[2]=0
self.Velocity = Vector([0,0,0])
self.Acceleration = Vector([0,0,0])
self.Alpha = Vector([0,0,0])
self.Omega = Vector([0,0,0])
self.ThetaDot = Vector([0,0,0])
self.Angles = Vector([0,0,0])
self.Quat = Quaternion([self.Angles[0], self.Angles[1], self.Angles[2]])
def calibrate(self,dt):
#dt = 0.001
self.UseController = False
#minMotor = self.level(simu)
#print minMotor
minMotor = 886
self.calib.calibrateI(dt, minMotor)
self.calib.calibrateMotor(0, dt, minMotor)
self.calib.calibrateMotor(1, dt, minMotor)
self.calib.calibrateMotor(2, dt, minMotor)
self.calib.calibrateMotor(3, dt, minMotor)
print self.calib.cali.getAveragedI()
print self.calib.cali.getIAxis()
print self.calib.cali.getR(0)
print self.calib.cali.getR(1)
print self.calib.cali.getR(2)
print self.calib.cali.getR(3)
self.UseController = True
self.ctrl.setMotorCoefficient( self.calib.cali.getR(0), self.calib.cali.getR(1), self.calib.cali.getR(2), self.calib.cali.getR(3))
self.ctrl.setI( self.calib.cali.getIAxis())