from numpy import * from DI_Bounded_2 import DI_controller import collections wn = 1 xi = sqrt(2)/2.0 kv = 2.0*xi*wn sigma_v = 1.0 kp = wn**2 sigma_p = 1.0 eps = 0.01 PAR = collections.namedtuple('DI_paramteres',['kv','sigma_v','kp','sigma_p','eps']) par = PAR(kv,sigma_v,kp,sigma_p,eps) # print(par) Controller = DI_controller(par) # Controller = DI_controller() p = numpy.array([-0.53,-1.23,0.98]) v = numpy.array([0.69,1.09,-0.79]) out = Controller._DI_Bounded(p,v) numpy.set_printoptions(precision=15) for i in range(12): # print("{:.15f}".format(out[i])) print(out[i]) print('\n')
#--------------------------------------------------------------------------# # solving differential equation # setting controller wn = 1 xi = sqrt(2)/2.0 kv = 2.0*xi*wn sigma_v = 1.0 kp = wn**2 sigma_p = 1.0 eps = 0.01 PAR = collections.namedtuple('DI_paramteres',['kv','sigma_v','kp','sigma_p','eps']) par = PAR(kv,sigma_v,kp,sigma_p,eps) Controller = DI_controller(par) # setting differetial equation r = ode(f).set_integrator('dop853') t0 = 0 y0 = states0 r.set_initial_value(y0, t0).set_f_params(Controller) t1 = 5 dt = 0.01 Time = [] Pvalue = [] Vvalue = [] # k = 1 or k = 2
#--------------------------------------------------------------------------# # solving differential equation # setting controller wn = 1 xi = sqrt(2) / 2.0 kv = 2.0 * xi * wn sigma_v = 1.0 kp = wn**2 sigma_p = 1.0 eps = 0.01 PAR = collections.namedtuple('DI_paramteres', ['kv', 'sigma_v', 'kp', 'sigma_p', 'eps']) par = PAR(kv, sigma_v, kp, sigma_p, eps) Controller = DI_controller(par) # setting differetial equation r = ode(f).set_integrator('dop853') t0 = 0 y0 = states0 r.set_initial_value(y0, t0).set_f_params(Controller) t1 = 5 dt = 0.01 Time = [] Pvalue = [] Vvalue = [] # k = 1 or k = 2 dV = 0
class Vector_Thrust_Controller(object): wn = 2.0 xi = sqrt(2) / 2.0 kv = 2.0 * xi * wn sigma_v = 1.0 kp = wn**2 sigma_p = 1.0 eps = 0.01 # kp = 6.0 # kv = 4.7 PAR = collections.namedtuple('DI_paramteres', ['kv', 'sigma_v', 'kp', 'sigma_p', 'eps']) par = PAR(kv, sigma_v, kp, sigma_p, eps) # print(par) DI_Ctrll = DI_controller(par) # ktt = 20 # ktt2 = 0.1*4 # kw = 20 # kw2 = 0.1*1 # factor = 0.05 # ktt = 20.0 # ktt2 = factor*kp # kw = 20.0 # kw2 = factor*kv ktt = 100.0 ktt2 = 0.05 kw = 100.0 kw2 = 0.05 # The class "constructor" - It's actually an initializer # def __init__(self): # self.M = 1.1 def output(self, x, gravity): return self._VectorThrustController(x, gravity) def _Vtheta(self, x): # ee = self.ee # ktt = self.ktt # Vtheta0 = ktt*x*(ee**2 - x**2)**(-1.0/2.0) # Vtheta1 = ktt*ee**2*(ee**2 - x**2)^(-3.0/2.0) # Vtheta2 = ktt*3*x*ee**2*(ee**2 - x**2)**(-5.0/2.0) ktt = self.ktt Vtheta0 = ktt * x Vtheta1 = ktt Vtheta2 = 0 return (Vtheta0, Vtheta1, Vtheta2) def _VectorThrustController(self, x, gravity): ad = gravity[0:3] jd = gravity[3:6] sd = gravity[6:9] # state # x = [p;v;n;w] p = x[0:3] v = x[3:6] n = x[6:9] w = x[9:12] u, u_p, u_v, u_p_p, u_v_v, u_p_v, Vpv, VpvD, V_p, V_v, V_v_p, V_v_v = self.DI_Ctrll.output( p, v) Td = ad + u Td_t = jd Td_p = u_p Td_v = u_v nTd = Td / numpy.linalg.norm(Td) # normTd = numpy.linalg.norm(g*e3 + ad + u) normTd = numpy.linalg.norm(Td) normTd_t = dot(nTd, jd) normTd_p = dot(u_p.T, nTd) normTd_v = dot(u_v.T, nTd) # TESTED: # normTdDot = normTd_t + normTd_p*v + normTd_v*(u - OP(n)*Td) # block.OutputPort(2).Data = [normTdnormTdDot] # TESTED: # uDot = u_p*v + u_v*(u - OP(n)*Td) # block.OutputPort(2).Data = [u(1)uDot(1)] nTd = Td / normTd nTd_t = dot(OP(nTd), jd / normTd) nTd_p = dot(OP(nTd), u_p / normTd) nTd_v = dot(OP(nTd), u_v / normTd) # TESTED: # nTdDot = nTd_t + nTd_p*v + nTd_v*(u - OP(n)*Td) # block.OutputPort(2).Data = [nTd(1)nTdDot(1)] # normTd_t_t = nTd'*sd + nTd_t'*jd # normTd_p_p = diag(u_p)*nTd_p + diag(nTd)*diag(u_p_p) # normTd_p_v = diag(u_p)*nTd_v + diag(nTd)*diag(u_p_v) # normTd_v_p = diag(u_v)*nTd_p + diag(nTd)*diag(u_p_v) # normTd_v_v = diag(u_v)*nTd_v + diag(nTd)*diag(u_v_v) # nTd_p_p = -(nTd_p*n' + nTd*nTd_p)*diag(u_p_p)/normTd + OP(nTd)*diag(u_p_p)/normTd - OP(nTd)*diag(u_p)/normTd**2*normTd_p # nTd_p_v = OP(nTd)*diag(u_p)/normTd # nTd_v_p = OP(nTd)*diag(u_v)/normTd # nTd_v_v = OP(nTd)*diag(u_v)/normTd xi = 1 - dot(n, nTd) Vtt0, Vtt1, Vtt2 = self._Vtheta(xi) xi_t = -dot(n, nTd_t) xi_p = -dot(n, nTd_p) xi_v = -dot(n, nTd_v) xi_n = -nTd # TESTED: # xiDot = xi_t + xi_p*v + xi_v*(u - OP(n)*Td) + xi_n*skew(w)*n # block.OutputPort(2).Data = [xixiDot] # TESTED: # block.OutputPort(2).Data = [Vtt1Vtt2*xiDot] aux_w_star = jd + dot(u_p, v) + dot(u_v, (u - dot(OP(n), Td))) aux_w_star_t = sd - dot(u_v, dot(OP(n), Td_t)) aux_w_star_p = dot(u_p_p.T, v).T + dot( u_v, u_p - dot(OP(n), Td_p)) + dot(u_p_v.T, u - dot(OP(n), Td)).T aux_w_star_v = u_p + dot(u_p_v.T, v).T + dot( u_v, u_v - dot(OP(n), Td_v)) + dot(u_v_v.T, u - dot(OP(n), Td)).T aux_w_star_n = u_v * dot(n, Td) + dot(u_v, outer(n, Td)) # TESTED: # aux_w_starDot = aux_w_star_t + aux_w_star_p*v + aux_w_star_v*(u - OP(n)*Td) + aux_w_star_n*skew(w)*n # block.OutputPort(2).Data = [aux_w_star(1)aux_w_starDot(1)] w_star = dot(skew(nTd), aux_w_star / normTd) w_star_t = dot(-skew(aux_w_star/normTd),nTd_t) + \ dot(skew(nTd),aux_w_star_t/normTd) + \ dot((-1)*skew(nTd),aux_w_star/normTd**2*normTd_t) w_star_p = dot(-skew(aux_w_star/normTd),nTd_p) + \ dot(skew(nTd),aux_w_star_p/normTd) + \ dot((-1)*skew(nTd),outer(aux_w_star,normTd_p)/normTd**2) w_star_v = dot(-skew(aux_w_star/normTd),nTd_v) + \ dot(skew(nTd),aux_w_star_v/normTd) + \ dot((-1)*skew(nTd),outer(aux_w_star,normTd_v)/normTd**2) w_star_n = dot(skew(nTd), aux_w_star_n / normTd) # TESTED: # w_starDot = w_star_t + w_star_p*v + w_star_v*(u - OP(n)*Td) + w_star_n*skew(w)*n # block.OutputPort(2).Data = [w_star(1)w_starDot(1)] # Thrust Thrust = dot(Td, n) # gains for angular control ktt2 = self.ktt2 # desired angular velocity wd = ktt2*dot(skew(n),nTd) + \ w_star + \ (-1)*dot(skew(n),V_v)*normTd*1/Vtt1 # aux1 = ktt2*skew(n)*nTd # aux1Dot = ktt2*skew(n)*nTd_t + ktt2*skew(n)*nTd_p*v + ktt2*skew(n)*nTd_v*(u - OP(n)*Td) - ktt2*skew(nTd)*skew(w)*n # block.OutputPort(2).Data = [aux1(1)aux1Dot(1)] # aux1 = skew(n)*V_v # aux1Dot = skew(n)*diag(V_v_p)*v + skew(n)*diag(V_v_v)*(u - OP(n)*Td) - skew(V_v)*skew(w)*n # block.OutputPort(2).Data = [aux1(2)aux1Dot(2)] wd_t = ktt2*dot(skew(n),nTd_t) + \ w_star_t + \ (-1)*dot(skew(n),V_v)*1/Vtt1*normTd_t + \ dot(skew(n),V_v)*normTd*1/Vtt1**2*Vtt2*xi_t wd_p = ktt2*dot(skew(n),nTd_p) + \ w_star_p + \ (-1)*dot(skew(n),normTd*1/Vtt1*V_v_p) + \ (-1)*dot(skew(n),outer(V_v,normTd_p)*1/Vtt1) + \ dot(skew(n),outer(V_v,xi_p)*normTd*1/Vtt1**2*Vtt2) wd_v = ktt2*dot(skew(n),nTd_v) + \ w_star_v + \ (-1)*dot(skew(n),outer(V_v,normTd_v)*1/Vtt1) + \ (-1)*dot(skew(n),normTd*1/Vtt1*V_v_v) + \ dot(skew(n),outer(V_v,xi_v)*normTd*1/Vtt1**2*Vtt2) wd_n = -ktt2*skew(nTd) + \ w_star_n + \ skew(V_v)*normTd*1/Vtt1 + \ dot(skew(n),outer(V_v,xi_n)*normTd*1/Vtt1**2*Vtt2) # TESTED: wdDot = wd_t + dot(wd_p, v) + dot(wd_v, (u - dot(OP(n), Td))) + dot( wd_n, dot(skew(w), n)) # block.OutputPort(2).Data = [wd(1)wdDot(1)] kw = self.kw kw2 = self.kw2 ew = dot(skew(n), w - wd) Tau = dot( skew(n), -wdDot - 1 / kw * Vtt1 * dot(skew(n), nTd) - dot(skew(n), wd) * dot(n, wd)) + kw2 * ew ## Lyapunov check V = Vpv + Vtt0 + 1 / 2 * kw * dot(ew, ew) VD = VpvD - ktt2 * Vtt1 * numpy.linalg.norm(dot( skew(n), nTd))**2 - kw2 * kw * dot(ew, ew) V_dT = dot( V_v - Vtt1 * dot(nTd_v.T, n) + kw * dot(wd_v.T, dot(skew(n), ew)), n) V_dTau = -kw * dot(OP(n), ew) p_dot = v v_dot = Thrust * n - gravity[0:3] n_dot = dot(skew(w), n) w_dot = dot(skew(n), Tau) derivatives = concatenate([p_dot, v_dot, n_dot, w_dot]) Td_dot = aux_w_star # Thrust: Thrust = dot(Td,n) Thrust_dot = dot(Td_dot, n) + dot(Td, n_dot) Tau_dot = numpy.zeros(3) return (Thrust, Tau, V, VD, V_dT, V_dTau, derivatives, Thrust_dot, Tau_dot)
from DI_Bounded_2 import DI_controller import collections wn = 1 xi = sqrt(2) / 2.0 kv = 2.0 * xi * wn sigma_v = 1.0 kp = wn**2 sigma_p = 1.0 eps = 0.01 PAR = collections.namedtuple('DI_paramteres', ['kv', 'sigma_v', 'kp', 'sigma_p', 'eps']) par = PAR(kv, sigma_v, kp, sigma_p, eps) # print(par) Controller = DI_controller(par) # Controller = DI_controller() p = numpy.array([-0.53, -1.23, 0.98]) v = numpy.array([0.69, 1.09, -0.79]) out = Controller._DI_Bounded(p, v) numpy.set_printoptions(precision=15) for i in range(12): # print("{:.15f}".format(out[i])) print(out[i]) print('\n')