def gravity_function(t): g0 = numpy.array([c(t), s(t), 9.0]) g1 = numpy.array([-s(t), c(t), 0.0]) g2 = numpy.array([-c(t), -s(t), 0.0]) return numpy.concatenate([g0, g1, g2]) gravity = gravity_function(0) t0 = 0 y0 = x_t start = timeit.default_timer() Controller = Vector_Thrust_Controller(gravity_function) out = Controller.output(x_t, gravity) stop = timeit.default_timer() print "time = " + str(stop - start) numpy.set_printoptions(precision=15) for i in range(6): print(out[i]) delta_t = 0.001 print '\n' start = timeit.default_timer() x_t_dt = Controller.predict_x(x_t, t0, delta_t) stop = timeit.default_timer() print "time = " + str(stop - start)
def __init__(self, gravity_function_handle): self.VT_Ctrll = Vector_Thrust_Controller(gravity_function_handle)
class Load_Transport_Controller(object): # quadrotor mass m = 1.56779 # load mass M = 0.250 # cable length L = 0.6 # gravity g = 9.81 # PAR = collections.namedtuple('VT_paramteres',['...','...']) # par = PAR(...,...) # VT_Ctrll = Vector_Thrust_Controller() # VT_Ctrll = Vector_Thrust_Controller() # The class "constructor" - It's actually an initializer def __init__(self, gravity_function_handle): self.VT_Ctrll = Vector_Thrust_Controller(gravity_function_handle) def output(self, state, stated): x, gravity = self._state_transform(state, stated) Thrust, Tau, V, VD, V_dT, V_dTau = self.VT_Ctrll.output(x, gravity) print(Thrust) print(Tau) U = self._input_transform(Thrust, Tau) return U def _state_transform(self, state, stated): # masses and cable length m = self.m M = self.M L = self.L # gravity g = self.g e3 = numpy.array([0.0, 0.0, 1.0]) # current LOAD position pM = state[0:3] # current LOAD velocity vM = state[3:6] # current QUAD position pm = state[6:9] # current QUAD velocity vm = state[9:12] # DESIRED LOAD position pd = stated[0:3] # DESIRED LOAD velocity vd = stated[3:6] # transformation of state p = pM - pd v = vM - vd # direction of cable n = (pm - pM) / numpy.linalg.norm(pm - pM) # angular velocity of cable w = dot(skew(n), (vm - vM) / numpy.linalg.norm(pM - pm)) x = concatenate([p, v, n, w]) print x self.x = x #---------------------------------------------------# # DESIRED LOAD acceleration, jerk and snap ad = stated[6:9] jd = stated[9:12] sd = stated[12:15] gravity = concatenate([g * e3 + ad, jd, sd]) return (x, gravity) def _input_transform(self, Thrust, Tau): # masses and cable length m = self.m M = self.M L = self.L n = self.x[6:9] w = self.x[9:12] U = n * (Thrust * (m + M) - dot(w, w) * m * L) + Tau * m * L return U def _input_transform2(self, x, Thrust, Tau): # masses and cable length m = self.m M = self.M L = self.L n = x[6:9] w = x[9:12] U = n * (Thrust * (m + M) - dot(w, w) * m * L) + Tau * m * L return U def blabla(): x_t_1dt = self.VT_Ctrll.predict_x(x_t, t, delta_t) Thrust_t_1dt, Tau_t_1dt, V, VD, V_dT, V_dTau = self.VT_Ctrll.output( x_t_1dt, gravity_function(t + 1.0 * delta_t)) U_t_1dt = self._input_transform2(x_t_1dt, Thrust_t_1dt, Tau_t_1dt) x_t_2dt = self.VT_Ctrll.predict_x(x_t, t0, 2 * delta_t) Thrust_t_2dt, Tau_t_2dt, V, VD, V_dT, V_dTau = self.VT_Ctrll.output( x_t_2dt, gravity_function(t + 2.0 * delta_t)) U_t_2dt = self._input_transform2(x_t_2dt, Thrust_t_2dt, Tau_t_2dt)
def __init__(self,gravity_function_handle): self.VT_Ctrll = Vector_Thrust_Controller(gravity_function_handle)
class Load_Transport_Controller(object): # quadrotor mass m = 1.56779 # load mass M = 0.250 # cable length L = 0.6 # gravity g = 9.81 # PAR = collections.namedtuple('VT_paramteres',['...','...']) # par = PAR(...,...) # VT_Ctrll = Vector_Thrust_Controller() # VT_Ctrll = Vector_Thrust_Controller() # The class "constructor" - It's actually an initializer def __init__(self,gravity_function_handle): self.VT_Ctrll = Vector_Thrust_Controller(gravity_function_handle) def output(self,state,stated): x,gravity = self._state_transform(state,stated) Thrust,Tau,V,VD,V_dT,V_dTau = self.VT_Ctrll.output(x,gravity) print(Thrust) print(Tau) U = self._input_transform(Thrust,Tau) return U def _state_transform(self,state,stated): # masses and cable length m = self.m; M = self.M; L = self.L; # gravity g = self.g; e3 = numpy.array([0.0,0.0,1.0]) # current LOAD position pM = state[0:3] # current LOAD velocity vM = state[3:6] # current QUAD position pm = state[6:9] # current QUAD velocity vm = state[9:12] # DESIRED LOAD position pd = stated[0:3] # DESIRED LOAD velocity vd = stated[3:6] # transformation of state p = pM - pd v = vM - vd # direction of cable n = (pm - pM)/numpy.linalg.norm(pm - pM) # angular velocity of cable w = dot(skew(n),(vm - vM)/numpy.linalg.norm(pM - pm)) x = concatenate([p,v,n,w]) print x self.x = x #---------------------------------------------------# # DESIRED LOAD acceleration, jerk and snap ad = stated[6:9] jd = stated[9:12] sd = stated[12:15] gravity = concatenate([g*e3 + ad,jd,sd]) return (x,gravity) def _input_transform(self,Thrust,Tau): # masses and cable length m = self.m M = self.M L = self.L n = self.x[6:9] w = self.x[9:12] U = n*(Thrust*(m+M) - dot(w,w)*m*L) + Tau*m*L; return U def _input_transform2(self,x,Thrust,Tau): # masses and cable length m = self.m M = self.M L = self.L n = x[6:9] w = x[9:12] U = n*(Thrust*(m+M) - dot(w,w)*m*L) + Tau*m*L; return U def blabla(): x_t_1dt = self.VT_Ctrll.predict_x(x_t,t, delta_t) Thrust_t_1dt,Tau_t_1dt,V,VD,V_dT,V_dTau = self.VT_Ctrll.output(x_t_1dt,gravity_function(t + 1.0*delta_t)) U_t_1dt = self._input_transform2(x_t_1dt,Thrust_t_1dt,Tau_t_1dt) x_t_2dt = self.VT_Ctrll.predict_x(x_t,t0,2*delta_t) Thrust_t_2dt,Tau_t_2dt,V,VD,V_dT,V_dTau = self.VT_Ctrll.output(x_t_2dt,gravity_function(t + 2.0*delta_t)) U_t_2dt = self._input_transform2(x_t_2dt,Thrust_t_2dt,Tau_t_2dt)
# g2 = numpy.array([0.1,0.2,0.3]) # gravity = concatenate([g0,g1,g2]) def gravity_function(t): g0 = numpy.array([ c(t), s(t),9.0]) g1 = numpy.array([-s(t), c(t),0.0]) g2 = numpy.array([-c(t),-s(t),0.0]) return numpy.concatenate([g0,g1,g2]) gravity = gravity_function(0) t0 = 0 y0 = x_t start = timeit.default_timer() Controller = Vector_Thrust_Controller(gravity_function) out = Controller.output(x_t,gravity) stop = timeit.default_timer() print "time = " + str(stop - start) numpy.set_printoptions(precision=15) for i in range(6): print(out[i]) delta_t = 0.001 print '\n' start = timeit.default_timer() x_t_dt = Controller.predict_x(x_t,t0,delta_t) stop = timeit.default_timer() print "time = " + str(stop - start)