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
