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