vL0 = numpy.array([0.0,0.0,0.0]) # initial unit vector n0 = numpy.array([0.0,s(0.0),c(0.0)]) # initial QUAD position p0 = pL0 + L*n0 # initial angular velocity w0 = numpy.array([0.0,0.0,0.0]) # initial quad velocity v0 = vL0 + L*dot(skew(w0),n0) # collecting all initial states states0 = concatenate([pL0,vL0,p0,v0]) statesd0 = traj_des(time) Controller = Load_Transport_Controller() out = Controller.output(states0,statesd0) numpy.set_printoptions(precision=15) out = euler_desired(out,0.0) print('roll : ' + str(out[0]*180/3.14)) print('pitch: ' + str(out[1]*180/3.14)) print('\n') #--------------------------------------------------------------------------# print('Goal to the right') # initial LOAD position pL0 = numpy.array([0.0,1.0,0.0]) # initial LOAD velocity vL0 = numpy.array([0.0,0.0,0.0])
# initial LOAD velocity vL0 = numpy.array([0.0, 0.0, 0.0]) # initial unit vector n0 = numpy.array([0.0, s(0.0), c(0.0)]) # initial QUAD position p0 = pL0 + L * n0 # initial angular velocity w0 = numpy.array([0.0, 0.0, 0.0]) # initial quad velocity v0 = vL0 + L * dot(skew(w0), n0) # collecting all initial states states0 = concatenate([pL0, vL0, p0, v0]) statesd0 = traj_des(time) Controller = Load_Transport_Controller() out = Controller.output(states0, statesd0) numpy.set_printoptions(precision=15) out = euler_desired(out, 0.0) print('roll : ' + str(out[0] * 180 / 3.14)) print('pitch: ' + str(out[1] * 180 / 3.14)) print('\n') #--------------------------------------------------------------------------# print('Goal to the right') # initial LOAD position pL0 = numpy.array([0.0, 1.0, 0.0]) # initial LOAD velocity vL0 = numpy.array([0.0, 0.0, 0.0])
# acceleration of transported mass vMD = T*n/M - g*e3; # collecting derivatives derivatives = concatenate([vM,vMD,v,vD]) return derivatives #--------------------------------------------------------------------------# M = 0.200 m = 1.250 L = 0.5 g = 9.81 Load_Ctroll = Load_Transport_Controller() PAR = collections.namedtuple('DI_paramteres',['M','m','L','g','Controller']) parameters = PAR(M,m,L,g,Load_Ctroll) print Load_Ctroll.report() #--------------------------------------------------------------------------# # initial states # initial load position xM0 = numpy.array([0.0,0.0,0.0]) # initial load velocity vM0 = numpy.array([0.0,0.0,0.0]) # initial cable direction n0 = unit_vec(0*3.14/180,-90*3.14/180) # cable length
# acceleration of transported mass vMD = T * n / M - g * e3 # collecting derivatives derivatives = concatenate([vM, vMD, v, vD]) return derivatives #--------------------------------------------------------------------------# M = 0.200 m = 1.250 L = 0.5 g = 9.81 Load_Ctroll = Load_Transport_Controller() PAR = collections.namedtuple('DI_paramteres', ['M', 'm', 'L', 'g', 'Controller']) parameters = PAR(M, m, L, g, Load_Ctroll) #--------------------------------------------------------------------------# # initial states # initial load position xM0 = numpy.array([0.0, 0.0, 0.0]) # initial load velocity vM0 = numpy.array([0.0, 0.0, 0.0]) # initial cable direction n0 = unit_vec(0 * 3.14 / 180, -90 * 3.14 / 180) # cable length L = parameters.L
class Load_Transport_Controller_Disturbance_Removal(object): # PAR = collections.namedtuple('paramteres',['...','...']) # par = PAR(...,...) # VT_Ctrll = Vector_Thrust_Controller() Load_Ctrll = Load_Transport_Controller() # estimate of motor coefficient k_Motor_est = 0.85 # gain for disturbance estimate k_int = 0.005 # parameters for disturbance removal PAR = collections.namedtuple('estimator_paramteres', ['b', 'b_infty', 'delta', 'b_eps', 'n']) par = PAR(b=k_Motor_est, b_infty=0.1, delta=10, b_eps=0.1, n=1.0) # solving differential equation for disturbance estimate r = ode(b_est_D).set_integrator('dopri5') b_est_Dot = numpy.zeros(3) r.set_initial_value(k_Motor_est, 0.0).set_f_params(b_est_Dot, par) # dt = 1/FREQUENCY at which controller will be called dt = 0.01 # The class "constructor" - It's actually an initializer # def __init__(self): # self.M = 1.1 def output(self, state, stated): # output full actuation and # update estimate of motor constant x, gravity = self.Load_Ctrll._state_transform(state, stated) Thrust, Tau, V, VD, V_dT, V_dTau = self.Load_Ctrll.VT_Ctrll.output( x, gravity) U = self.Load_Ctrll._input_transform(Thrust, Tau) U_to_send = U / self.k_Motor_est #-----------------------------------------------------------# # updating estimate for motor constant # masses and cable length m = self.Load_Ctrll.m M = self.Load_Ctrll.M L = self.Load_Ctrll.L # cable unit vector n = x[6:9] # cable angular velocity w = x[9:12] # estimate k_Motor_est = self.k_Motor_est # gain for disturbance estimate dynamics k_int = self.k_int # estimate update unconstrained k_Motor_est_Dot = k_int * (V_dT * dot(n, U) / (m + M) + dot(V_dTau, U) / (m * L)) / k_Motor_est self.r.set_f_params(k_Motor_est_Dot, self.par) self.r.integrate(self.r.t + self.dt) self.k_Motor_est = self.r.y #-----------------------------------------------------------# return U_to_send