def parm_f2c( P, dof, usefricdyn = False ):
from copy import copy
from sage.all import matrix
Pc = copy(P)
P = P.list()
for i in range(dof):
o = i * (10 + 2*usefricdyn)
oI = o+4
m = P[o+0]
ml = matrix([[P[o+1]],[P[o+2]],[P[o+3]]])
If = matrix([ [ P[oI+0], P[oI+1], P[oI+2] ],
[ P[oI+1], P[oI+3], P[oI+4] ],
[ P[oI+2], P[oI+4], P[oI+5] ] ])
l = ml / m
Ic = If - m * utils.skew(l).transpose() * utils.skew(l)
Pc[o+0,0] = m
Pc[o+1,0] = l[0,0]
Pc[o+2,0] = l[1,0]
Pc[o+3,0] = l[2,0]
Pc[oI+0,0] = Ic[0,0]
Pc[oI+1,0] = Ic[0,1]
Pc[oI+2,0] = Ic[0,2]
Pc[oI+3,0] = Ic[1,1]
Pc[oI+4,0] = Ic[1,2]
Pc[oI+5,0] = Ic[2,2]
return Pc
def parm_c2f( P, dof, usefricdyn = False ):
from copy import copy
from sage.all import matrix
Pf = copy(P)
P = P.list()
for i in range(dof):
o = i * (10 + 2*usefricdyn)
print i * (10 + 2*usefricdyn)
oI = o+4
m = P[o+0]
l = matrix([[P[o+1]],[P[o+2]],[P[o+3]]])
Ic = matrix([ [ P[oI+0], P[oI+1], P[oI+2] ],
[ P[oI+1], P[oI+3], P[oI+4] ],
[ P[oI+2], P[oI+4], P[oI+5] ] ])
ml = l * m
If = Ic + m * utils.skew(l).transpose() * utils.skew(l)
Pf[o+0,0] = m
Pf[o+1,0] = ml[0,0]
Pf[o+2,0] = ml[1,0]
Pf[o+3,0] = ml[2,0]
Pf[oI+0,0] = If[0,0]
Pf[oI+1,0] = If[0,1]
Pf[oI+2,0] = If[0,2]
Pf[oI+3,0] = If[1,1]
Pf[oI+4,0] = If[1,2]
Pf[oI+5,0] = If[2,2]
return Pf
def gen_kinem( self ):
self.Jpi = range(0 ,self.dof+1 )
self.Jpi[0] = zero_matrix(SR,3,self.dof)
for l in range(1 ,self.dof+1 ):
self.Jpi[l] = matrix(SR,3 ,self.dof)
for j in range(1 ,l+1 ):
self.Jpi[l][0 :3 ,j-1 ] = utils.skew(self.zi[j-1 ]) * ( self.pi[l] - self.pi[j-1 ] )
#Jpi[i] = Jpi[i].simplify_rational()
#Jpi[i] = trig_reduce(Jpi[i])
#Jpi[i] = Jpi[i].simplify()
self.Joi = range(0 ,self.dof+1 )
self.Joi[0] = zero_matrix(SR,3,self.dof)
for l in range(1 ,self.dof+1 ):
self.Joi[l] = matrix(SR,3 ,self.dof)
for j in range(1 ,l+1 ):
self.Joi[l][0 :3 ,j-1 ] = (self.zi[j-1 ])
#Joi[i] = Joi[i].simplify_rational()
#Joi[i] = Joi[i].simplify()
self.Jcpi = range(0 ,self.dof+1 )
self.Jcoi = self.Joi
for l in range(1 ,self.dof+1 ):
self.Jcpi[l] = self.Jpi[l] - utils.skew( self.Ri[l]*self.li[l] ) * self.Joi[l]
return self
def run_quadrotor(self,dt,fb,taub):
""" Dynamic/Differential equations for rigid body motion/flight """
# dpos/dt = ve
self.d_pos = self.ve
# q = [ s ] = [ s v1 v2 v3]^T
# [ v ]
# dq/dt = 0.5*[ -v ]
# [ sI3 + skew(v) ] * omegab
# version 1, unfolded - fastest, but still lower than rotm
self.d_q=np.array(
[(-self.q[1]*self.omegab[0]
-self.q[2]*self.omegab[1]-self.q[3]*self.omegab[2]),
(self.q[0]*self.omegab[0]
-self.q[3]*self.omegab[1]+self.q[2]*self.omegab[2]),
(self.q[3]*self.omegab[0]
+self.q[0]*self.omegab[1]-self.q[1]*self.omegab[2]),
(-self.q[2]*self.omegab[0]
+self.q[1]*self.omegab[1]+self.q[0]*self.omegab[2])
])*0.5
# version 2, more compact but slower than version 1
#d_q = 0.5*np.dot(
# np.block([[-self.q[1:3+1]],
# [self.q[0]*np.identity(3)+ut.skew(self.q[1:3+1])]]),
# self.omegab
# )
# dve/dt = 1/m*Rbe*fb + g
self.d_ve = 1/self.mass*utils.quat2rotm(self.q)@fb + np.array([0,0,-envir.g ])
# domegab/dt = I^(-1)*(-skew(omegab)*I*omegabb + taub)
self.d_omegab = (np.dot(
self.invI,
np.dot(
-utils.skew(self.omegab),
np.dot(self.I,self.omegab)
)
+ taub
)
)
# Integrate for over dt
# Simple Euler, the step dt must be small
X = np.concatenate([self.pos, self.q, self.ve, self.omegab])
dX = np.concatenate([ self.d_pos, self.d_q, self.d_ve, self.d_omegab ])
X = X + dt*dX
# unpack the vector state
self.pos = X[1-1:3]
self.q = X[4-1:7] / np.linalg.norm(X[4-1:7]) #update and normalize
self.rotmb2e = utils.quat2rotm(self.q)
self.rpy = utils.rotm2exyz(self.rotmb2e)
self.ve = X[8-1:10]
self.vb =np.transpose(self.rotmb2e)@self.ve
self.omegab = X[11-1:13]
def gen_kinetic_energy(robot, usemotordyn=True):
Ki = range(0, robot.dof + 1)
for i in range(1, robot.dof + 1):
Ki[i] = (
((1.0 / 2.0) * robot.mi[i] * robot.dq.transpose() *
robot.Jpi[i].transpose() * robot.Jpi[i] * robot.dq) +
(robot.dq.transpose() * robot.Jpi[i].transpose() * robot.Ri[i] *
utils.skew(robot.Ri[i].transpose() * robot.Joi[i] * robot.dq) *
robot.mli[i]) + (1.0 / 2.0) * robot.dq.transpose() *
robot.Joi[i].transpose() * robot.Ri[i] * robot.Ifi[i] *
robot.Ri[i].transpose() * robot.Joi[i] * robot.dq)[0, 0]
if usemotordyn:
Kmi = range(len(robot.motors))
for i, m in enumerate(robot.motors):
Im = m[0]
qm = m[1]
l = m[2]
zm = m[3]
dqm = qm.subs(robot.D_q_v2f).derivative(robot.t).subs(
robot.D_q_f2v)
ddqm = dqm.subs(robot.D_q_v2f).derivative(robot.t).subs(
robot.D_q_f2v)
wl = robot.Ri[l].transpose() * robot.Joi[l] * robot.dq
Kmi[i] = dqm * Im * (zm.transpose() *
wl)[0, 0] + (1.0 / 2.0) * dqm * dqm * Im
K = sum(Ki)
if usemotordyn: K += sum(Kmi)
return K
def quadrotor_dt(X, t, mass, I, invI, fb, taub):
q = X[3:7] / np.linalg.norm(X[3:7]) #update and normalize
ve = X[7:10]
omegab = X[10:13]
d_pos = ve
# q = [ s ] = [ s v1 v2 v3]^T
# [ v ]
# dq/dt = 0.5* [ -v^T ]
# [ sI3 + skew(v) ] * omegab
# version 1, unfolded - fastest, but still lower than rotm
d_q = np.array([(-q[1] * omegab[0] - q[2] * omegab[1] - q[3] * omegab[2]),
(q[0] * omegab[0] - q[3] * omegab[1] + q[2] * omegab[2]),
(q[3] * omegab[0] + q[0] * omegab[1] - q[1] * omegab[2]),
(-q[2] * omegab[0] + q[1] * omegab[1] + q[0] * omegab[2])
]) * 0.5
# dve/dt = 1/m*Rbe*fb + g
d_ve = 1 / mass * utils.quat2rotm(q) @ fb + np.array([0, 0, -envir.g])
# domegab/dt = I^(-1)*(-skew(omegab)*I*omegabb + taub)
d_omegab = (np.dot(invI,
np.dot(-utils.skew(omegab), np.dot(I, omegab)) + taub))
return np.concatenate([d_pos, d_q, d_ve, d_omegab])
def quadrotor_dt_rotm(X, t, velb, omegab):
Reb = X[3:12].reshape([3, 3])
d_pos = Reb @ velb
d_Reb = (Reb @ utils.skew(omegab)).flatten()
return np.concatenate([d_pos, d_Reb])
def V_operator(w):
w_skew = utils.skew(w)
theta = np.linalg.norm(w)
if(abs(theta) < 1e-7):
return np.identity(3) + w_skew / 2.
V = np.identity(3) + (1. - cos(theta)) / (theta * theta) * w_skew + \
+ (theta - sin(theta)) / (theta * theta * theta) * w_skew @ w_skew
return V
def dist_load(self, g, xi, eta, xi_dot, eta_dot, rod, q):
omega = xi[:3]
nu = xi[3:]
R = g[:3, :3]
A_bar = 0
B_bar = 0
for i in range(self.N):
r_i = self.r(i)
pa_der = R @ (nu - skew(r_i) @ omega)
P = R.T @ -skew(pa_der) * skew(pa_der) / np.linalg.norm(pa_der) ** 3 @ R
b = P @ skew(omega) @ (nu - skew(r_i) @ omega)
B_bar += q[i] * np.concatenate([skew(r_i) @ b, b])
A_bar += q[i] * np.concatenate([np.concatenate([-skew(r_i) @ P @ skew(r_i), skew(r_i) @ P], 1),
np.concatenate([-P @ skew(r_i), P], 1)])
return A_bar, B_bar
def log6( m ):
'''
Log: SE3 -> se3. Pseudo-inverse of exp from SE3 -> { v,w \in se3, ||w|| < 2pi }.
'''
if isinstance(m,se3.SE3): R = m.rotation; p = m.translation
else: R = m[:3,:3]; p = m[:3,3]
w = log3(R)
if npl.norm(w)>1e-15:
t = npl.norm(w)
S = skew(w)
V = eye(3) + (1-cos(t))/t**2 * S + (t-sin(t))/t**3 * S*S
v = npl.inv(V)*p
else:
v = p
return se3.Motion(v,w)
def exp6( nu ):
'''
Exp: se3 -> SE3. Return the integral of the input spatial velocity during time 1.
'''
if isinstance(nu,se3.Motion): w = nu.angular; v = nu.linear
else: w = nu[3:]; v = nu[:3]
if npl.norm(w)>1e-15:
R = exp3(w)
t = npl.norm(w)
S = skew(w)
V = eye(3) + (1-cos(t))/t**2 * S + (t-sin(t))/t**3 * S*S
p = V*v
return se3.SE3(R,p)
else:
return se3.SE3(eye(3),v)
def run_rate(self, ref_omegab, meas_omegab, J):
alpha_ref = np.zeros(3)
alpha_ref[0] = self.pid_rollrate.run(ref_omegab[0] - meas_omegab[0],
self.dt_ctrl_rate)
alpha_ref[0] = self.pid_rollrate.saturate()
self.pid_rollrate.antiwindup()
alpha_ref[1] = self.pid_pitchrate.run(ref_omegab[1] - meas_omegab[1],
self.dt_ctrl_rate)
alpha_ref[1] = self.pid_pitchrate.saturate()
self.pid_pitchrate.antiwindup()
alpha_ref[2] = self.pid_yawrate.run(ref_omegab[2] - meas_omegab[2],
self.dt_ctrl_rate)
alpha_ref[2] = self.pid_yawrate.saturate()
self.pid_yawrate.antiwindup()
tau_ref = J @ alpha_ref + utils.skew(meas_omegab) @ J @ meas_omegab
return tau_ref
def tip_load(self, g, xi, eta, xi_dot, eta_dot, rod, q):
W = 0
z = np.array([0, 0, 1])
for i in range(self.N):
W += q[i] * np.concatenate([-skew(self.r(i)) @ z, z])
return W
bg=WHITE)
img_fp = output_dir + '/' + token_type + '.' + token[:
20] + '.' + font_name + '.png'
trim_ = lambda im: trim(im, BORDER)
img = apply_modification_and_save(img, img_fp, 'trim', trim_, ret=True)
if do_rotate:
rot_ccw = lambda im: rotate(im, ROTATE_ANGLE)
apply_modification_and_save(img, img_fp, 'rot_ccw', rot_ccw)
rot_cw = lambda im: rotate(im, -ROTATE_ANGLE)
apply_modification_and_save(img, img_fp, 'rot_cw', rot_cw)
if do_skew:
skew_r = lambda im: skew(im, SKEW_ANGLE)
apply_modification_and_save(img, img_fp, 'skew_r', skew_r)
skew_l = lambda im: skew(im, -SKEW_ANGLE)
apply_modification_and_save(img, img_fp, 'skew_l', skew_l)
if do_blur:
blur_ = lambda im: blur(im, BLUR_RADIUS)
apply_modification_and_save(img, img_fp, 'blur', blur_)
if do_underline:
ul = lambda im: underline(im, FONT_SIZE, BORDER)
apply_modification_and_save(img, img_fp, 'ul', ul)
if do_complex:
skew_r_blur = lambda im: blur(skew(im, SKEW_ANGLE), BLUR_RADIUS)
def _gensymbols(self, usefricdyn=True) :
self.t = var('t',domain=RR)
self.q = matrix(SR,self.dof,1 )
self.dq = matrix(SR,self.dof,1 )
self.ddq = matrix(SR,self.dof,1 )
for i in range(1 ,self.dof+1 ):
self.q[i-1 ] = var('q'+str(i),domain=RR)
self.dq[i-1 ] = var('dq'+str(i),latex_name=r'\dot{q}_'+str(i),domain=RR)
self.ddq[i-1 ] = var('ddq'+str(i),latex_name=r'\ddot{q}_'+str(i),domain=RR)
self.qt = matrix(SR,self.dof,1 )
for i in range(1 ,self.dof+1 ):
self.qt[i-1 ,0 ] = function('q'+str(i)+'t',t,latex_name=r'q_'+str(i))
self.LoP_trig_f2v = []
for i in range(1 ,self.dof+1 ):
self.LoP_trig_f2v.append( ( cos(self.q[i-1 ,0 ]) , var('c'+str(i),domain=RR) ) )
self.LoP_trig_f2v.append( ( sin(self.q[i-1 ,0 ]) , var('s'+str(i),domain=RR) ) )
self.LoP_trig_v2f = zip(zip(*self.LoP_trig_f2v)[1],zip(*self.LoP_trig_f2v)[0])
self.D_q_v2f={} ; self.D_q_f2v={}
for i in range(0 ,self.dof):
self.D_q_v2f.update( { self.q[i,0 ]:self.qt[i,0 ], self.dq[i,0 ]:self.qt[i,0 ].derivative(t), self.ddq[i,0 ]:self.qt[i,0 ].derivative(t,2 ) } )
self.D_q_f2v.update( { self.qt[i,0 ]:self.q[i,0 ], self.qt[i,0 ].derivative(t):self.dq[i,0 ], self.qt[i,0 ].derivative(t,2 ):self.ddq[i,0 ] } )
self.mi = range(0 ,self.dof+1 )
self.li = range(0 ,self.dof+1 )
self.fvi = range(0 ,self.dof+1 )
self.fci = range(0 ,self.dof+1 )
self.Ici = range(0 ,self.dof+1 )
self.Ifi = range(0 ,self.dof+1 )
self.Ici_from_Ifi = range(0 ,self.dof+1 )
self.Ifi_from_Ici = range(0 ,self.dof+1 )
self.LoP_Ii_c2f = []
self.LoP_Ii_f2c = []
self.mli = range(0 ,self.dof+1 )
self.mli_e = range(0 ,self.dof+1 )
self.LoP_mli_v2e = []
self.D_mli_v2e = {}
for i in range(1 ,self.dof+1 ):
self.mi[i] = var('m'+str(i),domain=RR)
self.fvi[i] = var('fv'+str(i),latex_name=r'{fv}_{'+str(i)+'}',domain=RR)
self.fci[i] = var('fc'+str(i),latex_name=r'{fc}_{'+str(i)+'}',domain=RR)
aux1 = var('l_'+str(i)+'x',latex_name=r'l_{'+str(i)+',x}',domain=RR)
aux2 = var('l_'+str(i)+'y',latex_name=r'l_{'+str(i)+',y}',domain=RR)
aux3 = var('l_'+str(i)+'z',latex_name=r'l_{'+str(i)+',z}',domain=RR)
self.li[i] = matrix([[aux1],[aux2],[aux3]])
aux1 = var('ml_'+str(i)+'x',latex_name=r'\widehat{m_'+str(i)+'l_{'+str(i)+',x}}',domain=RR)
aux2 = var('ml_'+str(i)+'y',latex_name=r'\widehat{m_'+str(i)+'l_{'+str(i)+',y}}',domain=RR)
aux3 = var('ml_'+str(i)+'z',latex_name=r'\widehat{m_'+str(i)+'l_{'+str(i)+',z}}',domain=RR)
self.mli[i] = matrix([[aux1],[aux2],[aux3]])
self.mli_e[i] = self.mi[i] * self.li[i]
self.LoP_mli_v2e.append( ( self.mli[i][0 ,0 ] , self.mli_e[i][0 ,0 ] ) )
self.LoP_mli_v2e.append( ( self.mli[i][1 ,0 ] , self.mli_e[i][1 ,0 ] ) )
self.LoP_mli_v2e.append( ( self.mli[i][2 ,0 ] , self.mli_e[i][2 ,0 ] ) )
auxIcxx = var('I_'+str(i)+'xx',latex_name=r'I_{'+str(i)+',xx}',domain=RR)
auxIcyy = var('I_'+str(i)+'yy',latex_name=r'I_{'+str(i)+',yy}',domain=RR)
auxIczz = var('I_'+str(i)+'zz',latex_name=r'I_{'+str(i)+',zz}',domain=RR)
auxIcxy = var('I_'+str(i)+'xy',latex_name=r'I_{'+str(i)+',xy}',domain=RR)
auxIcxz = var('I_'+str(i)+'xz',latex_name=r'I_{'+str(i)+',xz}',domain=RR)
auxIcyz = var('I_'+str(i)+'yz',latex_name=r'I_{'+str(i)+',yz}',domain=RR)
self.Ici[i] = matrix([[auxIcxx,auxIcxy,auxIcxz],[auxIcxy,auxIcyy,auxIcyz],[auxIcxz,auxIcyz,auxIczz]])
auxIfxx = var('If_'+str(i)+'xx',latex_name=r'\hat{I}_{'+str(i)+',xx}',domain=RR)
auxIfyy = var('If_'+str(i)+'yy',latex_name=r'\hat{I}_{'+str(i)+',yy}',domain=RR)
auxIfzz = var('If_'+str(i)+'zz',latex_name=r'\hat{I}_{'+str(i)+',zz}',domain=RR)
auxIfxy = var('If_'+str(i)+'xy',latex_name=r'\hat{I}_{'+str(i)+',xy}',domain=RR)
auxIfxz = var('If_'+str(i)+'xz',latex_name=r'\hat{I}_{'+str(i)+',xz}',domain=RR)
auxIfyz = var('If_'+str(i)+'yz',latex_name=r'\hat{I}_{'+str(i)+',yz}',domain=RR)
self.Ifi[i] = matrix([[auxIfxx,auxIfxy,auxIfxz],[auxIfxy,auxIfyy,auxIfyz],[auxIfxz,auxIfyz,auxIfzz]])
self.Ifi_from_Ici[i] = self.Ici[i] + self.mi[i] * utils.skew(self.li[i]).transpose() * utils.skew(self.li[i])
self.Ici_from_Ifi[i] = self.Ifi[i] - self.mi[i] * utils.skew(self.li[i]).transpose() * utils.skew(self.li[i])
for e in [(a,b) for a in range(3) for b in range (3)]:
self.LoP_Ii_f2c.append( ( self.Ifi[i][e] , self.Ifi_from_Ici[i][e] ) )
self.LoP_Ii_c2f.append( ( self.Ici[i][e] , self.Ici_from_Ifi[i][e] ) )
self.D_mli_v2e = utils.LoP_to_D( self.LoP_mli_v2e )
self.D_Ii_f2c = utils.LoP_to_D( self.LoP_Ii_f2c )
self.D_Ii_c2f = utils.LoP_to_D( self.LoP_Ii_c2f )
self.D_parm_lin2elem = utils.LoP_to_D( self.LoP_mli_v2e + self.LoP_Ii_f2c )
Pi = [ matrix([ self.mi[i], self.mli[i][0,0], self.mli[i][1,0], self.mli[i][2,0], self.Ifi[i][0,0], self.Ifi[i][0,1], self.Ifi[i][0,2], self.Ifi[i][1,1], self.Ifi[i][1,2], self.Ifi[i][2,2] ] ).transpose() for i in range(1,self.dof+1) ]
return self
