def greatCircleDistance(coord1, coord2, r):
Φ1 = np.deg2rad(coord1[0])
Φ2 = np.deg2rad(coord2[0])
λ1 = np.deg2rad(coord1[1])
λ2 = np.deg2rad(coord2[1])
Δσ = np.arccos(s(Φ1) * s(Φ2) + c(Φ1) * c(Φ2) * c(abs(λ2 - λ1)))
return r * Δσ
def GetEulerAngles(R):
#phi = atan2(R(3,2),R(3,3));
#theta = asin(-R(3,1));
#psi = atan2(R(2,1),R(1,1));
EULER = numpy.array([0.0, 0.0, 0.0])
sin_theta = -R[2, 0]
sin_theta = bound(sin_theta, 1, -1)
theta = numpy.arcsin(sin_theta)
EULER[1] = theta
sin_phi = R[2, 1] / c(theta)
sin_phi = bound(sin_phi, 1, -1)
cos_phi = R[2, 2] / c(theta)
cos_phi = bound(cos_phi, 1, -1)
phi = numpy.arctan2(sin_phi, cos_phi)
EULER[0] = phi
sin_psi = R[1, 0] / c(theta)
sin_psi = bound(sin_psi, 1, -1)
cos_psi = R[0, 0] / c(theta)
cos_psi = bound(cos_psi, 1, -1)
psi = numpy.arctan2(sin_psi, cos_psi)
EULER[2] = psi
# EULER[0] = numpy.arctan2(bound(R[2,1],1,-1),bound(R[2,2],1,-1));
# EULER[1] = numpy.arcsin(-bound(R[2,0],1,-1));
# EULER[2] = numpy.arctan2(bound(R[1,0],1,-1),bound(R[0,0],1,-1));
return EULER
def GetEulerAngles(R):
#phi = atan2(R(3,2),R(3,3));
#theta = asin(-R(3,1));
#psi = atan2(R(2,1),R(1,1));
EULER = numpy.array([0.0,0.0,0.0])
sin_theta = -R[2,0]
sin_theta = numpy.clip(sin_theta,1,-1)
theta = numpy.arcsin(sin_theta)
EULER[1] = theta
sin_phi = R[2,1]/c(theta)
sin_phi = numpy.clip(sin_phi,1,-1)
cos_phi = R[2,2]/c(theta)
cos_phi = numpy.clip(cos_phi,1,-1)
phi = numpy.arctan2(sin_phi,cos_phi)
EULER[0] = phi
sin_psi = R[1,0]/c(theta)
sin_psi = numpy.clip(sin_psi,1,-1)
cos_psi = R[0,0]/c(theta)
cos_psi = numpy.clip(cos_psi,1,-1)
psi = numpy.arctan2(sin_psi,cos_psi)
EULER[2] = psi
# EULER[0] = numpy.arctan2(numpy.clip(R[2,1],1,-1),numpy.clip(R[2,2],1,-1));
# EULER[1] = numpy.arcsin(-numpy.clip(R[2,0],1,-1));
# EULER[2] = numpy.arctan2(numpy.clip(R[1,0],1,-1),numpy.clip(R[0,0],1,-1));
return EULER
def J(ph,th):
global Ixx
global Iyy
global Izz
return array([
[Ixx , 0 , -Ixx * s(th) ],
[0 , Iyy*(c(ph)**2) + Izz * s(ph)**2 , (Iyy-Izz)*c(ph)*s(ph)*c(th) ],
[-Ixx*s(th) , (Iyy-Izz)*c(ph)*s(ph)*c(th) , Ixx*(s(th)**2) + Iyy*(s(th)**2)*(c(th)**2) + Izz*(c(ph)**2)*(c(th)**2)]
])
def projected_distance(dist,ra_center,de_center,ra,de,dtype="rad"):
'''
args:
ra_center,de_center,ra,de: radian unit
note:
calculate the projected distance from the center position:
dist = dist*sin(theta),
cos_theta = c(de_center)*c(de)*(c(ra-ra_center))+s(de_center)*s(de)
'''
t = np.deg2rad if dtype == "deg" else lambda x:x
cos_theta = c(t(de_center))*c(t(de))*(c(t(ra-ra_center)))+s(t(de_center))*s(t(de))
return dist*np.sqrt(1-cos_theta*cos_theta)
def projected_angle(ra_center,de_center,ra,de,dtype="rad"):
'''
args:
ra_center,de_center,ra,de: radian unit
note:
calculate the projected distance from the center position:
dist = dist*sin(theta),
cos_theta = c(de_center)*c(de)*(c(ra-ra_center))+s(de_center)*s(de)
'''
t,t_inv = (np.deg2rad,np.rad2deg) if dtype == "deg" else (lambda x:x,lambda x:x)
cos_theta = c(t(de_center))*c(t(de))*(c(t(ra-ra_center)))+s(t(de_center))*s(t(de))
return t_inv(np.arccos(cos_theta))
def rot2(nu_az, nu_zen, gen_az, gen_zen):
new_x = c(nu_az)*c(gen_az)*s(gen_zen) - s(nu_az)*s(gen_az)*s(gen_zen)
new_y = s(nu_az)*c(gen_az)*s(gen_zen) + c(nu_az)*s(gen_az)*s(gen_zen)
new_z = c(gen_zen)
new_az = np.zeros(len(new_x))
for i in range(len(new_x)):
if new_x[i] > 0:
new_az[i] = np.arctan(new_y[i] / new_x[i]) % (2*np.pi)
else:
new_az[i] = np.arctan(new_y[i] / new_x[i]) + np.pi
new_zen = np.arccos(new_z)
return new_az, new_zen
def J(self):
ixx = self.Ixx
iyy = self.Iyy
izz = self.Izz
th = self.theta[-1]
ph = self.phi[-1]
j11 = ixx
j12 = 0
j13 = -ixx * s(th)
j21 = 0
j22 = iyy*(c(ph)**2) + izz * s(ph)**2
j23 = (iyy-izz)*c(ph)*s(ph)*c(th)
j31 = -ixx*s(th)
j32 = (iyy-izz)*c(ph)*s(ph)*c(th)
j33 = ixx*(s(th)**2) + iyy*(s(th)**2)*(c(th)**2) + izz*(c(ph)**2)*(c(th)**2)
return array([
[j11, j12, j13],
[j21, j22, j23],
[j31, j32, j33]
])
def J(self):
ixx = self.Ixx
iyy = self.Iyy
izz = self.Izz
th = self.theta[-1]
ph = self.phi[-1]
j11 = ixx
j12 = 0
j13 = -ixx * s(th)
j21 = 0
j22 = iyy * (c(ph)**2) + izz * s(ph)**2
j23 = (iyy - izz) * c(ph) * s(ph) * c(th)
j31 = -ixx * s(th)
j32 = (iyy - izz) * c(ph) * s(ph) * c(th)
j33 = ixx * (s(th)**2) + iyy * (s(th)**2) * (c(th)**2) + izz * (
c(ph)**2) * (c(th)**2)
return array([[j11, j12, j13], [j21, j22, j23], [j31, j32, j33]])
def jet_engine(engine, cg, altitude, throttle):
"""returns jet engine forces and moments."""
rho = Atmosphere(altitude).air_density()
rho_sl = Atmosphere(0).air_density()
t_slo = engine['thrust'] * throttle
phi = deg2rad(engine['thrust_angle'])
psi = deg2rad(engine['toe_angle'])
r = array([-engine['station'], engine['buttline'], -engine['waterline']
]) + array(cg)
t = t_slo * rho / rho_sl # [lbs]
t = t * array([c(phi) * c(psi), s(psi), -s(phi)])
m = cross(r, t)
c_f_m = array([t[0], t[1], t[2], m[0], m[1], m[2]])
return c_f_m
def traj_des(self, t):
from numpy import cos as c
from numpy import sin as s
r = self.r
w = self.w
p = r * w**0 * numpy.array([c(w * t), -s(w * t), 0.0])
v = r * w**1 * numpy.array([-s(w * t), -c(w * t), 0.0])
a = r * w**2 * numpy.array([-c(w * t), s(w * t), 0.0])
j = r * w**3 * numpy.array([s(w * t), c(w * t), 0.0])
s = r * w**4 * numpy.array([c(w * t), -s(w * t), 0.0])
return numpy.concatenate([p, v, a, j, s])
def Euler2Quat(euler):
p = euler[0] / 2.
t = euler[1] / 2.
g = euler[2] / 2.
cp = c(p)
ct = c(t)
cg = c(g)
sp = s(p)
st = s(t)
sg = s(g)
return [
cp * ct * cg + sp * st * sg, sp * ct * cg - cp * st * sg,
cp * st * cg + sp * ct * sg, cp * ct * sg - sp * st * cg
]
def traj_des(self,t):
from numpy import cos as c
from numpy import sin as s
r = self.r
w = self.w
p = r*w**0*numpy.array([ c(w*t),-s(w*t),0.0]);
v = r*w**1*numpy.array([-s(w*t),-c(w*t),0.0]);
a = r*w**2*numpy.array([-c(w*t), s(w*t),0.0]);
j = r*w**3*numpy.array([ s(w*t), c(w*t),0.0]);
s = r*w**4*numpy.array([ c(w*t),-s(w*t),0.0]);
return numpy.concatenate([p,v,a,j,s])
def euler_desired(U, psi):
# desired roll and pitch angles
n_des = U / numpy.linalg.norm(U)
n_des_rot = Rz(-psi).dot(n_des)
sin_phi = -n_des_rot[1]
sin_phi = bound(sin_phi, 1, -1)
phi = numpy.arcsin(sin_phi)
sin_theta = n_des_rot[0] / c(phi)
sin_theta = bound(sin_theta, 1, -1)
cos_theta = n_des_rot[2] / c(phi)
cos_theta = bound(cos_theta, 1, -1)
pitch = numpy.arctan2(sin_theta, cos_theta)
return (phi, pitch)
def euler_desired(U,psi):
# desired roll and pitch angles
n_des = U/numpy.linalg.norm(U)
n_des_rot = Rz(-psi).dot(n_des)
sin_phi = -n_des_rot[1]
sin_phi = bound(sin_phi,1,-1)
phi = numpy.arcsin(sin_phi)
sin_theta = n_des_rot[0]/c(phi)
sin_theta = bound(sin_theta,1,-1)
cos_theta = n_des_rot[2]/c(phi)
cos_theta = bound(cos_theta,1,-1)
pitch = numpy.arctan2(sin_theta,cos_theta)
return (phi,pitch)
def feed(self, X):
X = array(X)
if len(X) != self._inputs_num:
raise Exception(
"vector 'X' must be list of length equal to inputs_num=%d" %
self._inputs_num)
return self._feed(c(([1], X)) if self._shift else X)
def main(argv):
'''Take in 3 joint angles and calculate the end effector position
for the modified DH parameters through transformation.
Also find the Jacobian matrix and its determinant for the 6th
transformation to calculate the angular torque using input forces in XYZ directions.
Using the torque then find the actual end effector force
'''
# Case 1 : DH-Parameters
dhp = np.mat([[0, 0, 18.5, 0.1381482112], [0, 22.3, 0, 1.54],
[-np.pi / 2, 25.3, 0, -0.1841976149],
[np.pi / 2, 6.5, -1.5, 0.08186560661], [0, 6, -15.5, 0]])
# Copy joint angles that were provided as command line arguments
# into the table of DH parameters and forces into a 1x3 Matrix.
z0 = np.empty((dhp.shape[0], 4, 4)) # Z-axis matrices for each joint
(fig, ax, ep) = visualizeArm(np.empty((1, 1, 1)))
plt.show()
# for j in range(10):
# t0 = np.empty((dhp.shape[0], 4,4))
# a1, a2, a3, a4 = inverse_jacobian(dhp[0, -1], dhp[1, -1], dhp[2, -1], dhp[3, -1], 0, -1, 0, 0)
# dhp[:, 3] = dhp[:, 3] + (np.matrix([a1, a2, a3, a4, 0])).T
# # Compute the forward kinematics of the arm, finding the orientation
# # and position of each joint's frame in the base frame (X0, Y0, Z0).
# for i in range(dhp.shape[0]):
# # Create joint transformation from DH Parameters
#
# t0[i] = np.mat([(c(dhp[i,3]), -s(dhp[i,3]), 0, dhp[i,1]),
# (s(dhp[i,3]) * c(dhp[i,0]), c(dhp[i,3]) * c(dhp[i,0]), -s(dhp[i,0]),-dhp[i,2] * s(dhp[i, 0])),
# (s(dhp[i,3]) * s(dhp[i,0]), c(dhp[i,3]) * s(dhp[i,0]), c(dhp[i,0]), dhp[i,2] * c(dhp[i, 0])),
# (0, 0, 0, 1)])
#
# # Put each transformation in the frame 0
# if i != 0: t0[i] = np.matmul(t0[i-1],t0[i])
#
# # Compute Z axis in frame 0 for each joint
# z0[i] = np.matmul(t0[i],np.matrix([[1, 0, 0, 0],
# [0, 1, 0, 0],
# [0, 0, 1, 15],
# [0, 0, 0, 1]]))
# np.savetxt(sys.stdout.buffer, np.mat(t0[-1,:-1,-1]), fmt="%.5f")
# print(a1, a2, a3, a4)
# (fig, ax) = visualizeArm(t0, ax=ax, fig=fig)
# # print (jacobian(dhp[0,-1],dhp[1,-1],dhp[2,-1],dhp[3,-1],0,0,0,0))
t0 = np.empty((dhp.shape[0], 4, 4))
for i in range(dhp.shape[0]):
# Create joint transformation from DH Parameters
t0[i] = np.mat([
(c(dhp[i, 3]), -s(dhp[i, 3]), 0, dhp[i, 1]),
(s(dhp[i, 3]) * c(dhp[i, 0]), c(dhp[i, 3]) * c(dhp[i, 0]),
-s(dhp[i, 0]), -dhp[i, 2] * s(dhp[i, 0])),
(s(dhp[i, 3]) * s(dhp[i, 0]), c(dhp[i, 3]) * s(dhp[i, 0]),
c(dhp[i, 0]), dhp[i, 2] * c(dhp[i, 0])), (0, 0, 0, 1)
])
step_x(10, dhp, z0, ax, fig, ep)
print()
step_y(-2, dhp, z0, ax, fig, ep)
step_x(-10, dhp, z0, ax, fig, ep)
step_y(2, dhp, z0, ax, fig, ep)
def _get_untransformed_point(self, time):
t = time
r = self.radius
w = self.angular_velocity
rot = self.rotation
off = self.offset
p = numpy.zeros(4)
v = numpy.zeros(4)
a = numpy.zeros(4)
j = numpy.zeros(4)
sn = numpy.zeros(4)
cr = numpy.zeros(4)
p[0:3] = r*(numpy.array([c(w * t), -s(w * t), 0.0])-numpy.array([1.0, 0.0, 0.0]))
v[0:3] = r * w**1 * numpy.array([-s(w * t), -c(w * t), 0.0])
a[0:3] = r * w**2 * numpy.array([-c(w * t), s(w * t), 0.0])
j[0:3] = r * w**3 * numpy.array([s(w * t), c(w * t), 0.0])
sn[0:3] = r * w**4 * numpy.array([c(w * t), -s(w * t), 0.0])
cr[0:3] = r * w**5 * numpy.array([-s(w * t), -c(w * t), 0.0])
p[3] = -numpy.arctan2(s(w*t),c(w*t))
v[3] = -w
a[3] = 0.0
j[3] = 0.0
sn[3] = 0.0
cr[3] = 0.0
return p, v, a, j, sn, cr
def traj_des(t):
# p = numpy.array([0.6,0.0,0.0]);
# v = numpy.array([0.0,0.0,0.0]);
# a = numpy.array([0.0,0.0,0.0]);
# j = numpy.array([0.0,0.0,0.0]);
# s = numpy.array([0.0,0.0,0.0]);
from numpy import cos as c
from numpy import sin as s
r = 0.0
w = 0.0
p = r * w**0 * numpy.array([c(w * t), s(w * t), 0.0])
v = r * w**1 * numpy.array([-s(w * t), c(w * t), 0.0])
a = r * w**2 * numpy.array([-c(w * t), -s(w * t), 0.0])
j = r * w**3 * numpy.array([s(w * t), -c(w * t), 0.0])
s = r * w**4 * numpy.array([c(w * t), s(w * t), 0.0])
return concatenate([p, v, a, j, s])
def traj_des(t):
# p = numpy.array([0.6,0.0,0.0]);
# v = numpy.array([0.0,0.0,0.0]);
# a = numpy.array([0.0,0.0,0.0]);
# j = numpy.array([0.0,0.0,0.0]);
# s = numpy.array([0.0,0.0,0.0]);
from numpy import cos as c
from numpy import sin as s
r = 0.0
w = 0.0
p = r*w**0*numpy.array([ c(w*t), s(w*t),0.0]);
v = r*w**1*numpy.array([-s(w*t), c(w*t),0.0]);
a = r*w**2*numpy.array([-c(w*t),-s(w*t),0.0]);
j = r*w**3*numpy.array([ s(w*t),-c(w*t),0.0]);
s = r*w**4*numpy.array([ c(w*t), s(w*t),0.0]);
return concatenate([p,v,a,j,s])
def eci_to_ned(mu, lam):
"""Earth Centered Inertial to North East Down direct cosine matrix."""
b = array([[c(mu), -s(mu) * s(lam), s(mu) * c(lam)], [0, c(lam),
s(lam)],
[-s(mu), -c(mu) * s(lam),
c(mu) * c(lam)]])
return b
def train1(self, X, T, train_rate=None, prints=False):
X_ = c(([1], X))
T = array(T)
n = self.n
p = self.p
def print_(*args, **kwargs):
print(*args, **kwargs) if prints else None
#print_("Weights BEFORE train:\n")
#self.print_weights() if prints else None
result = self.process_input(X, full=True)
Z_in = result['Z_in']
Z = result['Z']
Z_ = c(([1], Z))
Y_in = result['Y_in']
Y = result['Y']
sigma_Y = (T-Y)*sigmoid_deriv(Y_in)
delta_w = array([sigma_Y * Z_[i] for i in range(0, 1+p)]) *\
((1/Y) if train_rate is None else train_rate)
print_("\nDelta W:\n", abs(delta_w))
sigma_in = array([sum(sigma_Y * self.W[i] for i in range(1, p+1))]).T[0]
sigma_Z = sigma_in * sigmoid_deriv(Z_in)
delta_v = array([sigma_Z * X_[i] for i in range(0, 1+n)]) *\
((1/Z) if train_rate is None else train_rate)
#print_("Sigma Z:", sigma_Z)
print_("Delta V:\n", abs(delta_v), "\n")
self.W = self.W + delta_w
self.V = self.V + delta_v
#print_("Weights AFTER train:\n")
#self.print_weights() if prints else None
return delta_w
def matTrans(thetArr):
t1 = thetArr[0]
t2 = thetArr[1]
t3 = thetArr[2]
matRes = np.array([[0, s(t3), c(t3)], [0, c(t3) * c(t2), -s(t3) * c(t2)],
[c(t2), s(t2) * s(t3),
c(t3) * s(t2)]])
return matRes
def propeller(engine, cg, v, throttle):
"""returns propeller system forces and moments."""
# thrust coefficient table
c_t = array([[0.14, 0.08, 0, -0.07, -0.15, -0.21],
[0.16, 0.15, 0.1, 0.02, -0.052, -0.1],
[0.18, 0.172, 0.16, 0.12, 0.04, -0.025]])
pitch_c_t = [15, 25, 35]
j_c_t = [0, 0.4, 0.8, 1.2, 1.6, 2.0]
rpm = engine['rpm_max'] * throttle / 60
j = array([v / (engine['diameter'] * rpm)])
f = RectBivariateSpline(pitch_c_t, j_c_t, c_t, kx=1)
c_t_i = f(engine['pitch'], j)
rho = Atmosphere(0).air_density()
t = float(rho * (rpm**2) * (engine['diameter']**4) * c_t_i)
phi = deg2rad(engine['thrust_angle'])
psi = deg2rad(engine['toe_angle'])
r = array([-engine['station'], engine['buttline'], -engine['waterline']
]) + array(cg)
t = t * array([c(phi) * c(psi), s(psi), -s(phi)])
m = cross(r, t)
c_f_m = array([t[0], t[1], t[2], m[0], m[1], m[2]])
return c_f_m
def J(ph, th):
global Ixx
global Iyy
global Izz
return array([[Ixx, 0, -Ixx * s(th)],
[
0, Iyy * (c(ph)**2) + Izz * s(ph)**2,
(Iyy - Izz) * c(ph) * s(ph) * c(th)
],
[
-Ixx * s(th), (Iyy - Izz) * c(ph) * s(ph) * c(th),
Ixx * (s(th)**2) + Iyy * (s(th)**2) * (c(th)**2) + Izz *
(c(ph)**2) * (c(th)**2)
]])
def rotation_from_angle_axix(self, theta, v):
"""
theta
v = [vx, vy, vz]
"""
vx = v.item(0)
vy = v.item(1)
vz = v.item(2)
from numpy import cos as c
from numpy import sin as s
R = np.matrix([[(vx**2)*(1-c(theta))+c(theta), vx*vy*(1-c(theta))-vz*s(theta), vx*vz*(1-c(theta))+vy*s(theta)],
[vx*vy*(1-c(theta))+vz*s(theta), (vy**2)*(1-c(theta))+c(theta), vy*vz*(1-c(theta))-vx*s(theta)],
[vx*vz*(1-c(theta))-vy*s(theta), vy*vz*(1-c(theta))+vx*s(theta), (vz**2)*(1-c(theta))+c(theta)]])
return R
def train1(self, X, d, eps=1e-1, train_rate=None, maxiters=1e4, prints=False):
if eps <= Perzeptron.train_accuracy_threshold:
raise Exception("Accuracy parameter eps must be greater then %d" %
Perzeptron.train_accuracy_threshold)
def print_(*args, **kwargs):
if prints:
print(*args, **kwargs)
print_("Training parameters:\n",
"accuracy is ", eps, '\n',
"training rate is ", train_rate, '\n',
"Weights before train:\n",
self._weights,
sep='')
print_("\nNow training...\n")
X = c(([1], array(X))) if self._shift else array(X)
for i in range(0, int(maxiters)):
y = self._process_input(X)
delta = (d - y) * (1/y if train_rate is None else train_rate)
print_("Iteration", i)
print_("Y_i:")
print_(y)
print_("Delta:")
print_(abs(delta))
print_("\n")
if abs(delta) < eps:
print_("Train stoped after %d iteration" % i)
print_("Weights after train:\n", self._weights, sep='')
return delta
else:
self._weights = self._weights + X * delta
print_("Training process reached maxiters=%d number of iterations. Training stoped" % maxiters)
print_("Weights after train:\n", self._weights, sep='')
return abs(delta)
def set_thetas(self, position=(0, 0, 0, 0), wait=False):
a1, a2, a3, a4 = position
# print (position)
self.dhp[:, 3] = (np.mat([a1, a2, a3, a4, 0])).T
for i in range(self.dhp.shape[0]):
# Create joint transformation from DH Parameters
self.t0[i] = np.mat([
(c(self.dhp[i, 3]), -s(self.dhp[i, 3]), 0, self.dhp[i, 1]),
(s(self.dhp[i, 3]) * c(self.dhp[i, 0]),
c(self.dhp[i, 3]) * c(self.dhp[i, 0]), -s(self.dhp[i, 0]),
-self.dhp[i, 2] * s(self.dhp[i, 0])),
(s(self.dhp[i, 3]) * s(self.dhp[i, 0]),
c(self.dhp[i, 3]) * s(self.dhp[i, 0]), c(self.dhp[i, 0]),
self.dhp[i, 2] * c(self.dhp[i, 0])), (0, 0, 0, 1)
])
# Put each transformation in the frame 0
if i != 0: self.t0[i] = np.matmul(self.t0[i - 1], self.t0[i])
self.visualizeArm()
def step_x(dist, dhp, z0, ax, fig, ep):
steps = 100
a1, a2, a3, a4 = dhp[:4, 3]
step_x = dist / (steps)
x = 24.509846106016546
for j in range(steps):
# a2 += -0.65366736003 * np.sin(0.02077772917 * step_x)
x += step_x
a2 -= 2 * 0.0006346 * x * step_x
# a2 += -np.sin(0.00661375661 * np.pi * step_x)
a1 += (-0.0273428 * step_x) + 2 * 0.0006346 * x * step_x
# print (a1, 0.0012692 * step_x)
dhp[:, 3] = (np.mat([a1, a2, a3, a4, 0])).T
# print(a1,a2,a3,a4,x)
# print(*get_transformation(a1, a2, a3, a4)[:-1, -1].T)
t0 = np.empty((dhp.shape[0], 4, 4))
for i in range(dhp.shape[0]):
# Create joint transformation from DH Parameters
t0[i] = np.mat([
(c(dhp[i, 3]), -s(dhp[i, 3]), 0, dhp[i, 1]),
(s(dhp[i, 3]) * c(dhp[i, 0]), c(dhp[i, 3]) * c(dhp[i, 0]),
-s(dhp[i, 0]), -dhp[i, 2] * s(dhp[i, 0])),
(s(dhp[i, 3]) * s(dhp[i, 0]), c(dhp[i, 3]) * s(dhp[i, 0]),
c(dhp[i, 0]), dhp[i, 2] * c(dhp[i, 0])), (0, 0, 0, 1)
])
# Put each transformation in the frame 0
if i != 0: t0[i] = np.matmul(t0[i - 1], t0[i])
# Compute Z axis in frame 0 for each joint
z0[i] = np.matmul(
t0[i],
np.matrix([[1, 0, 0, 0], [0, 1, 0, 0], [0, 0, 1, 15],
[0, 0, 0, 1]]))
(fig, ax, ep) = visualizeArm(t0, ax=ax, fig=fig, ep=ep)
def Eul2DCM(ang, axis, deg):
if deg:
angs = r(ang)
else:
angs = ang
MatRes = np.eye(3)
for n, th in enumerate(angs):
if axis[n] == 1:
MatInt = np.array([[1, 0, 0], [0, c(th), s(th)],
[0, -s(th), c(th)]])
elif axis[n] == 2:
MatInt = np.array([[c(th), 0, -s(th)], [0, 1, 0],
[s(th), 0, c(th)]])
else:
MatInt = np.array([[c(th), s(th), 0], [-s(th), c(th), 0],
[0, 0, 1]])
MatRes = np.dot(MatInt, MatRes)
return MatRes
def step_y(dist, dhp, z0, ax, fig, ep):
steps = 100
a1, a2, a3, a4 = dhp[:4, 3]
step_x = dist / (steps)
for j in range(steps):
a1 += 0.046884 * step_x
a2 += -1.009377 * 0.046884 * step_x - 0.5627 * 0.046884 * step_x * a1
# a2 += - 2.854 * np.sin(0.0071 * np.pi * step_x)
dhp[:, 3] = (np.mat([a1, a2, a3, a4, 0])).T
# print(a1,a2,a3,a4,x)
# print(*get_transformation(a1, a2, a3, a4)[:-1, -1].T)
t0 = np.empty((dhp.shape[0], 4, 4))
for i in range(dhp.shape[0]):
# Create joint transformation from DH Parameters
t0[i] = np.mat([
(c(dhp[i, 3]), -s(dhp[i, 3]), 0, dhp[i, 1]),
(s(dhp[i, 3]) * c(dhp[i, 0]), c(dhp[i, 3]) * c(dhp[i, 0]),
-s(dhp[i, 0]), -dhp[i, 2] * s(dhp[i, 0])),
(s(dhp[i, 3]) * s(dhp[i, 0]), c(dhp[i, 3]) * s(dhp[i, 0]),
c(dhp[i, 0]), dhp[i, 2] * c(dhp[i, 0])), (0, 0, 0, 1)
])
# Put each transformation in the frame 0
if i != 0: t0[i] = np.matmul(t0[i - 1], t0[i])
# Compute Z axis in frame 0 for each joint
z0[i] = np.matmul(
t0[i],
np.matrix([[1, 0, 0, 0], [0, 1, 0, 0], [0, 0, 1, 15],
[0, 0, 0, 1]]))
(fig, ax, ep) = visualizeArm(t0, ax=ax, fig=fig, ep=ep)
def controller(states,states_d,parameters):
# mass of vehicles (kg)
m = parameters.m
# acceleration due to gravity (m/s^2)
g = parameters.g
# Angular velocities multiplication factor
# Dw = parameters.Dw
# third canonical basis vector
e3 = numpy.array([0.0,0.0,1.0])
#--------------------------------------#
# transported mass: position and velocity
x = states[0:3];
v = states[3:6];
# thrust unit vector and its angular velocity
R = states[6:15];
R = numpy.reshape(R,(3,3))
n = R.dot(e3)
# print(GetEulerAngles(R)*180/3.14)
#--------------------------------------#
# desired quad trajectory
xd = states_d[0:3];
vd = states_d[3:6];
ad = states_d[6:9];
jd = states_d[9:12];
sd = states_d[12:15];
#--------------------------------------#
# position error and velocity error
ep = xd - x
ev = vd - v
#--------------------------------------#
G = g*e3 + ad
Gdot = jd
G2dot = sd
G_all = numpy.concatenate([G,Gdot,G2dot])
#--------------------------------------#
TT,wd,nTd = UniThurstControlComplete(ep,ev,n,G_all,parameters)
U = numpy.array([0.0,0.0,0.0,0.0])
U[0] = TT*m
RT = numpy.transpose(R)
U[1:4] = RT.dot(wd)
#--------------------------------------#
# yaw control: gain
k_yaw = parameters.k_yaw;
# desired yaw: psi_star
psi_star = parameters.psi_star;
# current euler angles
euler = euler_rad_from_rot(R);
phi = euler[0];
theta = euler[1];
psi = euler[2];
psi_star_dot = 0;
psi_dot = psi_star_dot - k_yaw*s(psi - psi_star);
U[3] = 1/c(phi)*(c(theta)*psi_dot - s(phi)*U[2]);
U = Cmd_Converter(U,parameters)
return U
def system_model_block(self):
# BELOW ARE THE EQUATIONS THAT MODEL THE SYSTEM,
# FOR THE PURPOSE OF SIMULATION, GIVEN THE MOTOR SPEEDS WE CAN CALCULATE THE STATES OF THE SYSTEM
self.tao_qr_frame.append( array([
self.L*self.k*( -self.w2[-1]**2 + self.w4[-1]**2 ) ,
self.L*self.k*( -self.w1[-1]**2 + self.w3[-1]**2 ) ,
self.b* ( -self.w1[-1]**2 + self.w2[-1]**2 - self.w3[-1]**2 + self.w4[-1]**2 )
]) )
self.tao_phi.append(self.tao_qr_frame[-1][0])
self.tao_theta.append(self.tao_qr_frame[-1][1])
self.tao_psi.append(self.tao_qr_frame[-1][2])
self.T.append(self.k*( self.w1[-1]**2 + self.w2[-1]**2 + self.w3[-1]**2 + self.w4[-1]**2 ) )
# use the previous known angles and the known thrust to calculate the new resulting linear accelerations
# remember this would be measured ...
# for the purpose of modeling the measurement error and testing a kalman filter, inject noise here...
# perhaps every 1000ms substitute an artificial gps measurement (and associated uncertianty) for the double integrated imu value
self.xddot.append( (self.T[-1]/self.m)*( c(self.psi[-1])*s(self.theta[-1])*c(self.phi[-1])
+ s(self.psi[-1])*s(self.phi[-1]) )
- self.Ax * self.xdot[-1] / self.m )
self.yddot.append( (self.T[-1]/self.m)*( s(self.psi[-1])*s(self.theta[-1])*c(self.phi[-1])
- c(self.psi[-1])*s(self.phi[-1]) )
- self.Ay * self.ydot[-1] / self.m )
self.zddot.append( self.g + (self.T[-1]/self.m)*( c(self.theta[-1])*c(self.phi[-1]) ) - self.Az * self.zdot[-1] / self.m )
# calculate the new angular accelerations based on the known values
self.etadot.append( array( [self.phidot[-1], self.thetadot[-1], self.psidot[-1] ] ) )
self.etaddot.append( dot(inv( self.J() ), self.tao_qr_frame[-1] - dot(self.coriolis_matrix() , self.etadot[-1]) ) )
# parse the etaddot vector of the new accelerations into the appropriate time series'
self.phiddot.append(self.etaddot[-1][0])
self.thetaddot.append(self.etaddot[-1][1])
self.psiddot.append(self.etaddot[-1][2])
#------------------------------ integrate new acceleration values to obtain velocity values
self.xdot.append( self.xdot[-1] + self.xddot[-1] * self.h )
self.ydot.append( self.ydot[-1] + self.yddot[-1] * self.h )
self.zdot.append( self.zdot[-1] + self.zddot[-1] * self.h )
self.phidot.append( self.phidot[-1] + self.phiddot[-1] * self.h )
self.thetadot.append( self.thetadot[-1] + self.thetaddot[-1] * self.h )
self.psidot.append( self.psidot[-1] + self.psiddot[-1] * self.h )
#------------------------------ integrate new velocity values to obtain position / angle values
self.x.append( self.x[-1] + self.xdot[-1] * self.h )
self.y.append( self.y[-1] + self.ydot[-1] * self.h )
self.z.append( self.z[-1] + self.zdot[-1] * self.h )
self.phi.append( self.phi[-1] + self.phidot[-1] * self.h )
self.theta.append( self.theta[-1] + self.thetadot[-1] * self.h )
self.psi.append( self.psi[-1] + self.psidot[-1] * self.h )
def system_iteration(x_des,
y_des,
z_des,
h,
x,y,z,xdot,ydot,zdot,xddot,yddot,zddot,
phi,theta,psi,phidot,thetadot,psidot,phiddot,thetaddot,psiddot,
w1,w2,w3,w4,
tao_phi,tao_theta,tao_psi,
T,
max_total_thrust,
min_total_thrust,
ith_wind_x,
ith_wind_y,
ith_wind_z,
kpx = 10.0,
kpy = 10.0,
kpz = 8.0,
kdx = 5.0,
kdy = 5.0,
kdz = 5.0,
kix = 1,
kiy = 1,
kiz = 1,
xError,
yError,
zError
):
kpphi = 8.0
kptheta = 6.0
kppsi = 6.0
kdphi = 1.75
kdtheta = 1.75
kdpsi = 1.75
#--------------------------------nonlinear roll/pitch control law gains
sPh1 = 3
sPh2 = 3
sPh3 = 2
sPh4 = .1
sTh1 = 3
sTh2 = 3
sTh3 = 2
sTh4 = .1
#--------------------------------control expressions eq 23
# the following two expressions for total thrust and torque about z axis are from 'teppo_luukkenon'
T_k = (g + kdz*( zdot_des - zdot[-1] ) + kpz*( z_des - z[-1] ) + kiz*zError )* m/float( c(phi[-1]) * c(theta[-1]))
if T_k <= min_total_thrust:
T.append( min_total_thrust )
elif T_k >= max_total_thrust:
T.append( max_total_thrust )
elif T_k > 0 and T_k <= max_total_thrust :
T.append( T_k )
tao_psi.append( ( kdpsi*( psidot_des - psidot[-1] ) + kppsi*( psi_des - psi[-1] ) ) * Izz )
#equation 61 in 'real-time stablization and tracking'
tao_phi.append( - sPh1 * (phidot[-1] + sPh2 * (phi[-1] + phidot[-1] +
sPh3 * ( 2 * phi[-1] + phidot[-1] + ( ydot[-1]/g ) +
sPh4 * (phidot[-1] + 3 * phi[-1] + 3 * ( ydot[-1]/(g) ) + y[-1]/(g) )))) * Ixx
)
#equation 66 in 'real-time stablization and tracking'
tao_theta.append( - sTh1 * ( thetadot[-1] + sTh2 * ( theta[-1] + thetadot[-1] +
sTh3 * ( 2 * theta[-1] + thetadot[-1] - ( xdot[-1]/(g) ) +
sTh4 * ( thetadot[-1] + 3 * theta[-1] - 3 * ( thetadot[-1]/(g) ) - x[-1]/(g) )))) * Iyy
)
# original pd contol expressions for roll and pitch
# tao_phi.append( ( kdphi*( phidot_des - phidot[-1] ) + kpphi*( phi_des - phi[-1] ) ) * Ixx )
# tao_theta.append( ( kdtheta*( thetadot_des - thetadot[-1] ) + kptheta*( theta_des - theta[-1] ) )*Iyy)
#--------------------------------solve for motor speeds, eq 24
w1.append( sqrt( (T[-1] / (4.0*k)) - ( tao_theta[-1] / (2.0*k*L) ) - ( tao_psi[-1] / (4.0*b) ) ) )
w2.append( sqrt( (T[-1] / (4.0*k)) - ( tao_phi[-1] / (2.0*k*L) ) + ( tao_psi[-1] / (4.0*b) ) ) )
w3.append( sqrt( (T[-1] / (4.0*k)) + ( tao_theta[-1] / (2.0*k*L) ) - ( tao_psi[-1] / (4.0*b) ) ) )
w4.append( sqrt( (T[-1] / (4.0*k)) + ( tao_phi[-1] / (2.0*k*L) ) + ( tao_psi[-1] / (4.0*b) ) ) )
#---------------------------------calculate new linear accelerations
#note the "wind" is introduced as an acceleration which imparts exogenous forces in the position control law expressions
xddot.append( (T[-1]/m) * ( c(psi[-1]) * s(theta[-1]) * c(phi[-1]) + s(psi[-1]) * s(phi[-1]) )
- (Ax * xdot[-1]/m )
+ (-kdx * xdot[-1] - kpx * ( x[-1] - x_des ) + kix * xError )
+ ith_wind_x * Ax / m
)
yddot.append( ( T[-1] / m ) * ( s(psi[-1]) * s(theta[-1]) * c(phi[-1]) - c(psi[-1]) * s(phi[-1]) )
- ( Ay * ydot[-1] / m )
+ (-kdy * ydot[-1] - kpy * ( y[-1] - y_des ) + kiy * yError )
+ ith_wind_y * Ay / m
)
# xddot.append( ( T[-1] / m ) * ( c(psi[-1]) * s(theta[-1]) * c(phi[-1]) + s(psi[-1]) * s(phi[-1]) ) - Ax * xdot[-1] / m )
# yddot.append( ( T[-1] / m ) * ( s(psi[-1]) * s(theta[-1]) * c(phi[-1]) - c(psi[-1]) * s(phi[-1]) ) - Ay * ydot[-1] / m )
zddot.append( -g + ( T[-1] / m ) * ( c(theta[-1]) * c(phi[-1]) ) - Az * ( zdot[-1] / m ) + ith_wind_z * Az /m)
#-------------------------------- calculate new angular accelerations
tao = array( [tao_phi[-1], tao_theta[-1], tao_psi[-1] ] ) # must build vectors of kth timestep quantities for the matrix math evaluations
etadot = array( [phidot[-1], thetadot[-1], psidot[-1] ] )
etaddot = dot(
inv( J(phi[-1],theta[-1]) ),
tao - dot(
coriolis_matrix(phi[-1],theta[-1],phidot[-1],thetadot[-1],psidot[-1] ,Ixx,Iyy,Izz) ,
etadot
)
)
phiddot.append(etaddot[0]) # parse the etaddot vector of the new accelerations into the appropriate time series'
thetaddot.append(etaddot[1])
psiddot.append(etaddot[2])
#------------------------------ integrate new acceleration values to obtain velocity values
xdot.append( xdot[-1] + xddot[-1] * h )
ydot.append( ydot[-1] + yddot[-1] * h )
zdot.append( zdot[-1] + zddot[-1] * h )
phidot.append( phidot[-1] + phiddot[-1] * h )
thetadot.append( thetadot[-1] + thetaddot[-1] * h )
psidot.append( psidot[-1] + psiddot[-1] * h )
#------------------------------ integrate new velocity values to obtain position / angle values
x.append( x[-1] + xdot[-1] * h )
y.append( y[-1] + ydot[-1] * h )
z.append( z[-1] + zdot[-1] * h )
phi.append( phi[-1] + phidot[-1] * h )
theta.append( theta[-1] + thetadot[-1] * h )
psi.append( psi[-1] + psidot[-1] * h )
return [x,y,z,xdot,ydot,zdot,xddot,yddot,zddot,
phi,theta,psi,phidot,thetadot,psidot,phiddot,thetaddot,psiddot,
w1,w2,w3,w4,
tao_phi,tao_theta,tao_psi,
T
]
def traj_des_circle(t):
r = 0.0
w = 2 * 3.14 / 20.0
pp = r * w**0 * numpy.array([c(w * t), s(w * t), 0.0])
vv = r * w**1 * numpy.array([-s(w * t), c(w * t), 0.0])
aa = r * w**2 * numpy.array([-c(w * t), -s(w * t), 0.0])
jj = r * w**3 * numpy.array([s(w * t), -c(w * t), 0.0])
ss = r * w**4 * numpy.array([c(w * t), s(w * t), 0.0])
uu = r * w**5 * numpy.array([-s(w * t), c(w * t), 0.0])
pp = pp + numpy.array([0.0, 0.0, 0.01])
return numpy.concatenate([pp, vv, aa, jj, ss, uu])
# # Desired trajectory for LOAD
# def traj_des(t):
# if t <= 0.0:
# r = 1.0
# w = 2*3.14/20.0
# pp = r*w**0*numpy.array([ c(w*t), s(w*t),0.0]);
# vv = r*w**1*numpy.array([-s(w*t), c(w*t),0.0]);
# aa = r*w**2*numpy.array([-c(w*t),-s(w*t),0.0]);
# jj = r*w**3*numpy.array([ s(w*t),-c(w*t),0.0]);
# ss = r*w**4*numpy.array([ c(w*t), s(w*t),0.0]);
# pp = pp + numpy.array([0.0,0.0,0.01])
# else:
# pp = numpy.array([0.0,0.0,0.01]);
# vv = numpy.array([0.0,0.0,0.0 ]);
# aa = numpy.array([0.0,0.0,0.0 ]);
# jj = numpy.array([0.0,0.0,0.0 ]);
# ss = numpy.array([0.0,0.0,0.0 ]);
# return numpy.concatenate([pp,vv,aa,jj,ss])
# # Desired trajectory for LOAD
# def traj_des(t):
# T = 20.0
# if t <= T/2:
# Delta = 5.0
# w = 2*3.14/T
# pp = numpy.array([ -0.5*Delta*(w**0)*(c(w*t) - 1.0),0.0,0.0]);
# vv = numpy.array([ 0.5*Delta*(w**1)*s(w*t) ,0.0,0.0]);
# aa = numpy.array([ 0.5*Delta*(w**2)*c(w*t) ,0.0,0.0]);
# jj = numpy.array([ -0.5*Delta*(w**3)*s(w*t) ,0.0,0.0]);
# ss = numpy.array([ -0.5*Delta*(w**4)*c(w*t) ,0.0,0.0]);
# pp = pp + numpy.array([-Delta,0.0,0.01])
# pp = pp + numpy.array([0.0,0.0,0.01])
# else:
# pp = numpy.array([0.0,0.0,0.0 ]);
# vv = numpy.array([0.0,0.0,0.0 ]);
# aa = numpy.array([0.0,0.0,0.0 ]);
# jj = numpy.array([0.0,0.0,0.0 ]);
# ss = numpy.array([0.0,0.0,0.0 ]);
# return numpy.concatenate([pp,vv,aa,jj,ss])
# # Desired trajectory for LOAD
# def traj_des2(t,pp_real,vv_real,aa_real,jj_real,ss_real):
# pp_final,vv_final,aa_final,jj_final,ss_final
# if t <= 0.0:
# r = 1.0
# w = 2*3.14/10.0
# pp = r*w**0*numpy.array([ c(w*t), s(w*t),0.0]);
# vv = r*w**1*numpy.array([-s(w*t), c(w*t),0.0]);
# aa = r*w**2*numpy.array([-c(w*t),-s(w*t),0.0]);
# jj = r*w**3*numpy.array([ s(w*t),-c(w*t),0.0]);
# ss = r*w**4*numpy.array([ c(w*t), s(w*t),0.0]);
# pp = pp + numpy.array([0.0,0.0,0.01])
# else:
# pp = numpy.array([0.0,0.0,0.01]);
# vv = numpy.array([0.0,0.0,0.0 ]);
# aa = numpy.array([0.0,0.0,0.0 ]);
# jj = numpy.array([0.0,0.0,0.0 ]);
# ss = numpy.array([0.0,0.0,0.0 ]);
# return numpy.concatenate([pp,vv,aa,jj,ss])
def rot_y(tt):
return np.array([[c(tt),0.0,s(tt)],[0.0,1,0.0],[-s(tt),0.0,c(tt)]])
def rot_y(tt):
"""This function returns the rotation matrix corresponding to a rotation
of tt radians about the y-axis.
"""
return np.array(
[[c(tt), 0.0, s(tt)], [0.0, 1, 0.0], [-s(tt), 0.0, c(tt)]])
def Ry(tt):
return numpy.array([[c(tt), 0.0, s(tt)], [0.0, 1, 0.0],
[-s(tt), 0.0, c(tt)]])
def coriolis_matrix(self):
ph = self.phi[-1]
th = self.theta[-1]
phd = self.phidot[-1]
thd = self.thetadot[-1]
psd = self.psidot[-1]
ixx = self.Ixx
iyy = self.Iyy
izz = self.Izz
c11 = 0
# break up the large elements in to bite size chunks and then add each term ...
c12_term1 = (iyy-izz) * ( thd*c(ph)*s(ph) + psd*c(th)*s(ph)**2 )
c12_term2and3 = (izz-iyy)*psd*(c(ph)**2)*c(th) - ixx*psd*c(th)
c12 = c12_term1 + c12_term2and3
c13 = (izz-iyy) * psd * c(ph) * s(ph) * c(th)**2
c21_term1 = (izz-iyy) * ( thd*c(ph)*s(ph) + psd*s(ph)*c(th) )
c21_term2and3 = (iyy-izz) * psd * (c(ph)**2) * c(th) + ixx * psd * c(th)
c21 = c21_term1 + c21_term2and3
c22 = (izz-iyy)*phd*c(ph)*s(ph)
c23 = -ixx*psd*s(th)*c(th) + iyy*psd*(s(ph)**2)*s(th)*c(th)
c31 = (iyy-izz)*phd*(c(th)**2)*s(ph)*c(ph) - ixx*thd*c(th)
c32_term1 = (izz-iyy)*( thd*c(ph)*s(ph)*s(th) + phd*(s(ph)**2)*c(th) )
c32_term2and3 = (iyy-izz)*phd*(c(ph)**2)*c(th) + ixx*psd*s(th)*c(th)
c32_term4 = - iyy*psd*(s(ph)**2)*s(th)*c(th)
c32_term5 = - izz*psd*(c(ph)**2)*s(th)*c(th)
c32 = c32_term1 + c32_term2and3 + c32_term4 + c32_term5
c33_term1 = (iyy-izz) * phd *c(ph)*s(ph)*(c(th)**2)
c33_term2 = - iyy * thd*(s(ph)**2) * c(th)*s(th)
c33_term3and4 = - izz*thd*(c(ph)**2)*c(th)*s(th) + ixx*thd*c(th)*s(th)
c33 = c33_term1 + c33_term2 + c33_term3and4
return array([
[c11,c12,c13],
[c21,c22,c23],
[c31,c32,c33]
])
def control_block(self):
# calculate the integral of the error in position for each direction
self.x_integral_error.append( self.x_integral_error[-1] + (self.x_des - self.x[-1])*self.h )
self.y_integral_error.append( self.y_integral_error[-1] + (self.y_des - self.y[-1])*self.h )
self.z_integral_error.append( self.z_integral_error[-1] + (self.z_des - self.z[-1])*self.h )
# computte the comm linear accelerations needed to move the system from present location to the desired location
self.xacc_comm.append( self.kdx * (self.xdot_des - self.xdot[-1])
+ self.kpx * ( self.x_des - self.x[-1] )
+ self.kddx * (self.xdd_des - self.xddot[-1] )
+ self.kix * self.x_integral_error[-1] )
self.yacc_comm.append( self.kdy * (self.ydot_des - self.ydot[-1])
+ self.kpy * ( self.y_des - self.y[-1] )
+ self.kddy * (self.ydd_des - self.yddot[-1] )
+ self.kiy * self.y_integral_error[-1] )
self.zacc_comm.append( self.kdz * (self.zdot_des - self.zdot[-1])
+ self.kpz * ( self.z_des - self.z[-1] )
+ self.kddz * (self.zdd_des - self.zddot[-1] )
+ self.kiz * self.z_integral_error[-1] )
# need to limit the max linear acceleration that is perscribed by the control law
# as a meaningful place to start, just use the value '10m/s/s' , compare to g = -9.8 ...
max_latt_acc = 5
max_z_acc = 30
if abs(self.xacc_comm[-1]) > max_latt_acc: self.xacc_comm[-1] = max_latt_acc * sign(self.xacc_comm[-1])
if abs(self.yacc_comm[-1]) > max_latt_acc: self.yacc_comm[-1] = max_latt_acc * sign(self.yacc_comm[-1])
if abs(self.zacc_comm[-1]) > max_z_acc: self.zacc_comm[-1] = max_z_acc * sign(self.zacc_comm[-1])
min_z_acc = 12
if self.zacc_comm[-1] < min_z_acc: self.zacc_comm[-1] = min_z_acc
# using the comm linear accellerations, calc theta_c, phi_c and T_c
theta_numerator = (self.xacc_comm[-1] * c(self.psi[-1]) + self.yacc_comm[-1] * s(self.psi[-1]) )
theta_denominator = float( self.zacc_comm[-1] + self.g )
if theta_denominator <= 0:
theta_denominator = 0.1 # don't divide by zero !!!
self.theta_comm.append(arctan2( theta_numerator , theta_denominator ))
self.phi_comm.append(arcsin( (self.xacc_comm[-1] * s(self.psi[-1]) - self.yacc_comm[-1] * c(self.psi[-1]) ) / float(sqrt( self.xacc_comm[-1]**2 +
self.yacc_comm[-1]**2 +
(self.zacc_comm[-1] + self.g)**2 )) ))
self.T_comm.append(self.m * ( self.xacc_comm[-1] * ( s(self.theta[-1])*c(self.psi[-1])*c(self.phi[-1]) + s(self.psi[-1])*s(self.phi[-1]) ) +
self.yacc_comm[-1] * ( s(self.theta[-1])*s(self.psi[-1])*c(self.phi[-1]) - c(self.psi[-1])*s(self.phi[-1]) ) +
(self.zacc_comm[-1] + self.g) * ( c(self.theta[-1])*c(self.phi[-1]) )
))
if self.T_comm[-1] < 1.0:
self.T_comm = self.T_comm[:-1]
self.T_comm.append(1.0)
# we will need the derivatives of the comanded angles for the torque control laws.
self.phidot_comm = (self.phi_comm[-1] - self.phi_comm[-2])/self.h
self.thetadot_comm = (self.theta_comm[-1] - self.theta_comm[-2])/self.h
# solve for torques based on theta_c, phi_c and T_c , also psi_des , and previous values of theta, phi, and psi
tao_phi_comm_temp = ( self.kpphi*(self.phi_comm[-1] - self.phi[-1]) + self.kdphi*(self.phidot_comm - self.phidot[-1]) )*self.Ixx
tao_theta_comm_temp = ( self.kptheta*(self.theta_comm[-1] - self.theta[-1]) + self.kdtheta*(self.thetadot_comm - self.thetadot[-1]) )*self.Iyy
tao_psi_comm_temp = ( self.kppsi*(self.psi_des - self.psi[-1]) + self.kdpsi*( self.psidot_des - self.psidot[-1] ) )*self.Izz
self.tao_phi_comm.append(tao_phi_comm_temp )
self.tao_theta_comm.append(tao_theta_comm_temp )
self.tao_psi_comm.append(tao_psi_comm_temp )
#--------------------------------solve for motor speeds, eq 24
self.w1_arg.append( (self.T_comm[-1] / (4.0*self.k)) - ( self.tao_theta_comm[-1] / (2.0*self.k*self.L) ) - ( self.tao_psi_comm[-1] / (4.0*self.b) ) )
self.w2_arg.append( (self.T_comm[-1] / (4.0*self.k)) - ( self.tao_phi_comm[-1] / (2.0*self.k*self.L) ) + ( self.tao_psi_comm[-1] / (4.0*self.b) ) )
self.w3_arg.append( (self.T_comm[-1] / (4.0*self.k)) + ( self.tao_theta_comm[-1] / (2.0*self.k*self.L) ) - ( self.tao_psi_comm[-1] / (4.0*self.b) ) )
self.w4_arg.append( (self.T_comm[-1] / (4.0*self.k)) + ( self.tao_phi_comm[-1] / (2.0*self.k*self.L) ) + ( self.tao_psi_comm[-1] / (4.0*self.b) ) )
self.w1.append( sqrt( self.w1_arg[-1] ) )
self.w2.append( sqrt( self.w2_arg[-1] ) )
self.w3.append( sqrt( self.w3_arg[-1] ) )
self.w4.append( sqrt( self.w4_arg[-1] ) )
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])
def controller(states,states_load,states_d,parameters):
# mass of vehicles (kg)
m = parameters.m
ml = .136
# is there some onboard thrust control based on the vertical acceleration?
ml = .01
# acceleration due to gravity (m/s^2)
g = parameters.g
rospy.logwarn('g: '+str(g))
# Angular velocities multiplication factor
# Dw = parameters.Dw
# third canonical basis vector
e3 = numpy.array([0.0,0.0,1.0])
#--------------------------------------#
# transported mass: position and velocity
p = states[0:3];
v = states[3:6];
# thrust unit vector and its angular velocity
R = states[6:15];
R = numpy.reshape(R,(3,3))
r3 = R.dot(e3)
# load
pl = states_load[0:3]
vl = states_load[3:6]
Lmeas = norm(p-pl)
rospy.logwarn('rope length: '+str(Lmeas))
L = 0.66
#rospy.logwarn('param Throttle_neutral: '+str(parameters.Throttle_neutral))
rl = (p-pl)/Lmeas
omegal = skew(rl).dot(v-vl)/Lmeas
omegal = zeros(3)
#--------------------------------------#
# desired quad trajectory
pd = states_d[0:3] - L*e3;
vd = states_d[3:6];
ad = states_d[6:9];
jd = states_d[9:12];
sd = states_d[12:15];
# rospy.logwarn(numpy.concatenate((v,vl,vd)))
#--------------------------------------#
# position error and velocity error
ep = pl - pd
ev = vl - vd
rospy.logwarn('ep: '+str(ep))
#--------------------------------------#
gstar = g*e3 + ad
d_gstar = jd
dd_gstar = sd
#rospy.logwarn('rl: '+str(rl))
#rospy.logwarn('omegal: '+str(omegal))
#rospy.logwarn('ev: '+str(ev))
# temp override
#rl = array([0.,0.,1.])
#omegal = zeros(3)
#L = 1
#--------------------------------------#
# rospy.logwarn('ep='+str(ep)+' ev='+str(ev)+' rl='+str(rl)+' omegal='+str(omegal))
Tl, tau, _, _ = backstep_ctrl(ep, ev, rl, omegal, gstar, d_gstar, dd_gstar)
rospy.logwarn('g: '+str(g))
U = (eye(3)-outer(rl,rl)).dot(tau*m*L)+rl*(
# -m/L*inner(v-vl,v-vl)
+Tl*(m+ml))
U = R.T.dot(U)
n = rl
# -----------------------------------------------------------------------------#
# STABILIZE MODE:APM COPTER
# The throttle sent to the motors is automatically adjusted based on the tilt angle
# of the vehicle (i.e increased as the vehicle tilts over more) to reduce the
# compensation the pilot must fo as the vehicles attitude changes
# -----------------------------------------------------------------------------#
# Full_actuation = m*(ad + u + g*e3 + self.d_est)
Full_actuation = U
# -----------------------------------------------------------------------------#
U = numpy.zeros(4)
Throttle = numpy.dot(Full_actuation,r3)
# this decreases the throtle, which will be increased
Throttle = Throttle*numpy.dot(n,r3)
U[0] = Throttle
#--------------------------------------#
# current euler angles
euler = GetEulerAngles(R)
# current phi and theta
phi = euler[0];
theta = euler[1];
# current psi
psi = euler[2];
#--------------------------------------#
# Rz(psi)*Ry(theta_des)*Rx(phi_des) = r3_des
# desired roll and pitch angles
r3_des = Full_actuation/numpy.linalg.norm(Full_actuation)
r3_des_rot = Rz(-psi).dot(r3_des)
sin_phi = -r3_des_rot[1]
sin_phi = bound(sin_phi,1,-1)
phi = numpy.arcsin(sin_phi)
U[1] = phi
sin_theta = r3_des_rot[0]/c(phi)
sin_theta = bound(sin_theta,1,-1)
cos_theta = r3_des_rot[2]/c(phi)
cos_theta = bound(cos_theta,1,-1)
pitch = numpy.arctan2(sin_theta,cos_theta)
U[2] = pitch
#--------------------------------------#
# yaw control: gain
k_yaw = parameters.k_yaw;
# desired yaw: psi_star
psi_star = parameters.psi_star;
psi_star_dot = 0;
psi_dot = psi_star_dot - k_yaw*s(psi - psi_star);
U[3] = 1/c(phi)*(c(theta)*psi_dot - s(phi)*U[2]);
U = Cmd_Converter(U,parameters)
return U
w1 = []
w2 = []
w3 = []
w4 = []
etaddot = []
#----------------------------------
max_iterations = 5000
h = 0.001
for i in range(max_iterations):
#--------------------------------control expressions eq 23
T.append( (g + kdz*( zdot_des - zdot[-1] ) + kpz*( z_des - z[-1] ))* m/float( c(phi[-1]) * c(theta[-1])) )
tao_phi.append( ( kdphi*( phidot_des - phidot[-1] ) + kpphi*( phi_des - phi[-1] ) ) * Ixx )
tao_theta.append( ( kdtheta*( thetadot_des - thetadot[-1] ) + kptheta*( theta_des - theta[-1] ) )*Iyy)
tao_psi.append( ( kdpsi*( psidot_des - psidot[-1] ) + kppsi*( psi_des - psi[-1] ) ) * Izz )
#--------------------------------solve for motor speeds, eq 24
w1.append( sqrt( (T[-1] / (4.0*k)) - ( tao_theta[-1] / (2.0*k*L) ) - ( tao_psi[-1] / (4.0*b) ) ) )
w2.append( sqrt( (T[-1] / (4.0*k)) - ( tao_phi[-1] / (2.0*k*L) ) + ( tao_psi[-1] / (4.0*b) ) ) )
w3.append( sqrt( (T[-1] / (4.0*k)) + ( tao_theta[-1] / (2.0*k*L) ) - ( tao_psi[-1] / (4.0*b) ) ) )
w4.append( sqrt( (T[-1] / (4.0*k)) + ( tao_phi[-1] / (2.0*k*L) ) + ( tao_psi[-1] / (4.0*b) ) ) )
#---------------------------------calculate new linear accelerations
def coriolis_matrix(ph,th,phd,thd,psd ,ixx,iyy,izz):
c11 = 0
c12 = (iyy-izz) * ( thd*c(ph)*s(ph) + psd*c(th)*s(ph)**2 ) + (izz-iyy)*psd*(c(ph)**2)*c(th) - ixx*psd*c(th)
c13 = (izz-iyy) * psd * c(ph) * s(ph) * c(th)**2
c21 = (izz-iyy) * ( thd*c(ph)*s(ph) + psd*s(ph)*c(th) ) + (iyy-izz) * psd * (c(ph)**2) * c(th) + ixx * psd * c(th)
c22 = (izz-iyy)*phd*c(ph)*s(ph)
c23 = -ixx*psd*s(th)*c(th) + iyy*psd*(s(ph)**2)*s(th)*c(th)
c31 = (iyy-izz)*phd*(c(th)**2)*s(ph)*c(ph) - ixx*thd*c(th)
c32 = (izz-iyy)*( thd*c(ph)*s(ph)*s(th) + phd*(s(ph)**2)*c(th) ) + (iyy-izz)*phd*(c(ph)**2)*c(th) + ixx*psd*s(th)*c(th) - iyy*psd*(s(ph)**2)*s(th)*c(th) - izz*psd*(c(ph)**2)*s(th)*c(th)
c33 = (iyy-izz) * phd *c(ph)*s(ph)*(c(th)**2) - iyy * thd*(s(ph)**2) * c(th)*s(th) - izz*thd*(c(ph)**2)*c(th)*s(th) + ixx*thd*c(th)*s(th)
return n.array([
[c11,c12,c13],
[c21,c22,c23],
[c31,c32,c33]
])
def controller(states,states_d,parameters):
# mass of vehicles (kg)
m = parameters.m
# acceleration due to gravity (m/s^2)
g = parameters.g
# Angular velocities multiplication factor
# Dw = parameters.Dw
# third canonical basis vector
e3 = numpy.array([0.0,0.0,1.0])
#--------------------------------------#
# transported mass: position and velocity
x = states[0:3];
v = states[3:6];
# thrust unit vector and its angular velocity
R = states[6:15];
R = numpy.reshape(R,(3,3))
n = R.dot(e3)
# print(GetEulerAngles(R)*180/3.14)
#--------------------------------------#
# desired quad trajectory
xd = states_d[0:3];
vd = states_d[3:6];
ad = states_d[6:9];
#--------------------------------------#
# position error and velocity error
ep = xd - x
ev = vd - v
u = cmd_di_3D(ep,ev,parameters)
# -----------------------------------------------------------------------------#
# vector of commnads to be sent by the controller
U = numpy.zeros(4)
# -----------------------------------------------------------------------------#
# STABILIZE MODE:APM COPTER
# The throttle sent to the motors is automatically adjusted based on the tilt angle
# of the vehicle (i.e increased as the vehicle tilts over more) to reduce the
# compensation the pilot must fo as the vehicles attitude changes
# -----------------------------------------------------------------------------#
Full_actuation = m*(ad + u + g*e3)
Throttle = numpy.dot(Full_actuation,n)
# this decreases the throtle, which will be increased
Throttle = Throttle*numpy.dot(n,e3)
U[0] = Throttle
#--------------------------------------#
# current euler angles
euler = GetEulerAngles(R)
# current phi and theta
phi = euler[0];
theta = euler[1];
# current psi
psi = euler[2];
#--------------------------------------#
# Rz(psi)*Ry(theta_des)*Rx(phi_des) = n_des
# desired roll and pitch angles
n_des = Full_actuation/numpy.linalg.norm(Full_actuation)
n_des_rot = Rz(-psi).dot(n_des)
sin_phi = -n_des_rot[1]
sin_phi = bound(sin_phi,1,-1)
phi = numpy.arcsin(sin_phi)
U[1] = phi
sin_theta = n_des_rot[0]/c(phi)
sin_theta = bound(sin_theta,1,-1)
cos_theta = n_des_rot[2]/c(phi)
cos_theta = bound(cos_theta,1,-1)
pitch = numpy.arctan2(sin_theta,cos_theta)
U[2] = pitch
#--------------------------------------#
# yaw control: gain
k_yaw = parameters.k_yaw;
# desired yaw: psi_star
psi_star = parameters.psi_star;
psi_star_dot = 0;
psi_dot = psi_star_dot - k_yaw*s(psi - psi_star);
U[3] = 1/c(phi)*(c(theta)*psi_dot - s(phi)*U[2]);
U = Cmd_Converter(U,parameters)
return U
#--------------------------------------------------------------------------#
time = 0.0
# cable length
L = 0.5
#--------------------------------------------------------------------------#
print('Goal to the front')
# initial LOAD position
pL0 = numpy.array([-1.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)
def rot_z(tt):
"""This function returns the rotation matrix corresponding to a rotation
of tt radians about the z-axis.
"""
return np.array(
[[c(tt), -s(tt), 0.0], [s(tt), c(tt), 0.0], [0.0, 0.0, 1]])
def Rx(tt):
return numpy.array([[1.0,0.0,0.0],[0.0,c(tt),-s(tt)],[0.0,s(tt),c(tt)]])
def Rz(tt):
return numpy.array([[c(tt), -s(tt), 0.0], [s(tt), c(tt), 0.0],
[0.0, 0.0, 1]])
def Ry(tt):
return numpy.array([[c(tt),0.0,s(tt)],[0.0,1,0.0],[-s(tt),0.0,c(tt)]])
def rot_x(tt):
return np.array([[1.0,0.0,0.0],[0.0,c(tt),-s(tt)],[0.0,s(tt),c(tt)]])
def Rz(tt):
return numpy.array([[c(tt),-s(tt),0.0],[s(tt),c(tt),0.0],[0.0,0.0,1]])
def rot_z(tt):
return np.array([[c(tt),-s(tt),0.0],[s(tt),c(tt),0.0],[0.0,0.0,1]])
th = .1
ps = .1
phd = 1
thd = 1
psd = 1
m = 1
l = 0.4
ixx = (1/12.)*0.5 * m * l**2
iyy = (1/12.)*0.5 * m * l**2
izz = (1/12.)* m * l**2
matr = coriolis_matrix(ph,th,phd,thd,psd,ixx,iyy,izz)
print 'coriolis_matrix = ',matr
J = n.array([
[ixx , 0 , -ixx * s(th) ],
[0 , iyy*(c(ph)**2) + izz * s(ph)**2 , (iyy-izz)*c(ph)*s(ph)*c(th) ],
[-ixx*s(th) , (iyy-izz)*c(ph)*s(ph)*c(th) , ixx*(s(th)**2) + iyy*(s(th)**2)*(c(th)**2) + izz*(c(ph)**2)*(c(th)**2)]
])
eta = n.array([ph, th, ps])
etad = n.array([phd, thd, psd])
etadd = n.array([1,1,1])
LHS = n.dot( J , etadd ) + n.dot( coriolis_matrix(ph,th,phd,thd,psd ,ixx,iyy,izz) , etad )
print 'LHS = ',LHS
def rot_x(tt):
"""This function returns the rotation matrix corresponding to a rotation
of tt radians about the x-axis.
"""
return numpy.array(
[[1.0, 0.0, 0.0], [0.0, c(tt), -s(tt)], [0.0, s(tt), c(tt)]])
def Grad(w1,w2,w3,w4, th,ph,ps, phd,thd,psd, phdd,thdd,psdd, xdd,ydd,zdd):
#----------------------------------------------------------------------------------physical constants
k = 10**-6 #
m = 2
l = 0.4
g = - 9.81
b = 10**-7
beta = (1/12.0)*m*l**2
ixx = 0.5*beta
iyy = 0.5*beta
izz = beta
J = n.array([
[ixx , 0 , -ixx * s(th) ],
[0 , iyy*(c(ph)**2) + izz * s(ph)**2 , (iyy-izz)*c(ph)*s(ph)*c(th) ],
[-ixx*s(th) , (iyy-izz)*c(ph)*s(ph)*c(th) , ixx*(s(th)**2) + iyy*(s(th)**2)*(c(th)**2) + izz*(c(ph)**2)*(c(th)**2)]
])
#eta = n.array([ph, th, ps])
etad = n.array([phd, thd, psd])
etadd = n.array([phdd,thdd,psdd])
LHS = n.dot( J , etadd ) + n.dot( coriolis_matrix(ph,th,phd,thd,psd ,ixx,iyy,izz) , etad )
lhs_phi = LHS[0]
lhs_theta = LHS[1]
lhs_psi = LHS[2]
return [
#-----------------------------dw1
sum([
2 * (l * k * ( -w2**2 + w4**2 + w1**2 - w3**2 ) - lhs_phi + lhs_theta) * l * k * 2 * w1,
2 * (b*(w1**2 + w2**2 + w3**2 + w4**2) - lhs_psi) * b * 2 * w1,
2 * (-k * (w1**2 + w2**2 + w3**2 + w4**2) * (c(ps) * s(th) * c(ph) + s(ps) * s(ph)) - m * xdd ) * -k * (c(ps) * s(th) * c(ph) + s(ps) * s(ph)) * 2 * w1,
2 * (( k * (s(ps) * s(th) * c(ph) - c(ps) *s(ph)) - c(ph) * c(th) ) * (w1**2 + w2**2 + w3**2 + w4**2) - m * ( ydd - zdd + g )) * ( k * (s(ps) * s(th) * c(ph) - c(ps) *s(ph)) - c(ph) * c(th) ) * 2 * w1
]),
#-----------------------------dw2
sum([
2 * (l * k * ( -w2**2 + w4**2 + w1**2 - w3**2 ) - lhs_phi + lhs_theta) * l * k * 2 * -w2,
2 * (b*(w1**2 + w2**2 + w3**2 + w4**2) - lhs_psi) * b * 2 * w2,
2 * (-k * (w1**2 + w2**2 + w3**2 + w4**2) * (c(ps) * s(th) * c(ph) + s(ps) * s(ph)) - m * xdd ) * -k * (c(ps) * s(th) * c(ph) + s(ps) * s(ph)) * 2 * w2,
2 * (( k * (s(ps) * s(th) * c(ph) - c(ps) *s(ph)) - c(ph) * c(th) ) * (w1**2 + w2**2 + w3**2 + w4**2) - m * ( ydd - zdd + g )) * ( k * (s(ps) * s(th) * c(ph) - c(ps) *s(ph)) - c(ph) * c(th) ) * 2 * w2
] ),
#-----------------------------dw3
sum( [
2 * (l * k * ( -w2**2 + w4**2 + w1**2 - w3**2 ) - lhs_phi + lhs_theta) * l * k * 2 * -w3,
2 * (b*(w1**2 + w2**2 + w3**2 + w4**2) - lhs_psi) * b * 2 * w3,
2 * (-k * (w1**2 + w2**2 + w3**2 + w4**2) * (c(ps) * s(th) * c(ph) + s(ps) * s(ph)) - m * xdd ) * -k * (c(ps) * s(th) * c(ph) + s(ps) * s(ph)) * 2 * w3,
2 * (( k * (s(ps) * s(th) * c(ph) - c(ps) *s(ph)) - c(ph) * c(th) ) * (w1**2 + w2**2 + w3**2 + w4**2) - m * ( ydd - zdd + g )) * ( k * (s(ps) * s(th) * c(ph) - c(ps) *s(ph)) - c(ph) * c(th) ) * 2 * w3
] ),
#-----------------------------dw4
sum([
2 * (l * k * ( -w2**2 + w4**2 + w1**2 - w3**2 ) - lhs_phi + lhs_theta) * l * k * 2 * w4,
2 * (b*(w1**2 + w2**2 + w3**2 + w4**2) - lhs_psi) * b * 2 * w4,
2 * (-k * (w1**2 + w2**2 + w3**2 + w4**2) * (c(ps) * s(th) * c(ph) + s(ps) * s(ph)) - m * xdd ) * -k * (c(ps) * s(th) * c(ph) + s(ps) * s(ph)) * 2 * w4,
2 * (( k * (s(ps) * s(th) * c(ph) - c(ps) *s(ph)) - c(ph) * c(th) ) * (w1**2 + w2**2 + w3**2 + w4**2) - m * ( ydd - zdd + g )) * ( k * (s(ps) * s(th) * c(ph) - c(ps) *s(ph)) - c(ph) * c(th) ) * 2 * w4
])
]#close return list
def traj_des_circle(t):
r = 0.0
w = 2*3.14/20.0
pp = r*w**0*numpy.array([ c(w*t), s(w*t),0.0]);
vv = r*w**1*numpy.array([-s(w*t), c(w*t),0.0]);
aa = r*w**2*numpy.array([-c(w*t),-s(w*t),0.0]);
jj = r*w**3*numpy.array([ s(w*t),-c(w*t),0.0]);
ss = r*w**4*numpy.array([ c(w*t), s(w*t),0.0]);
uu = r*w**5*numpy.array([-s(w*t), c(w*t),0.0]);
pp = pp + numpy.array([0.0,0.0,0.01])
return numpy.concatenate([pp,vv,aa,jj,ss,uu])
# # Desired trajectory for LOAD
# def traj_des(t):
# if t <= 0.0:
# r = 1.0
# w = 2*3.14/20.0
# pp = r*w**0*numpy.array([ c(w*t), s(w*t),0.0]);
# vv = r*w**1*numpy.array([-s(w*t), c(w*t),0.0]);
# aa = r*w**2*numpy.array([-c(w*t),-s(w*t),0.0]);
# jj = r*w**3*numpy.array([ s(w*t),-c(w*t),0.0]);
# ss = r*w**4*numpy.array([ c(w*t), s(w*t),0.0]);
# pp = pp + numpy.array([0.0,0.0,0.01])
# else:
# pp = numpy.array([0.0,0.0,0.01]);
# vv = numpy.array([0.0,0.0,0.0 ]);
# aa = numpy.array([0.0,0.0,0.0 ]);
# jj = numpy.array([0.0,0.0,0.0 ]);
# ss = numpy.array([0.0,0.0,0.0 ]);
# return numpy.concatenate([pp,vv,aa,jj,ss])
# # Desired trajectory for LOAD
# def traj_des(t):
# T = 20.0
# if t <= T/2:
# Delta = 5.0
# w = 2*3.14/T
# pp = numpy.array([ -0.5*Delta*(w**0)*(c(w*t) - 1.0),0.0,0.0]);
# vv = numpy.array([ 0.5*Delta*(w**1)*s(w*t) ,0.0,0.0]);
# aa = numpy.array([ 0.5*Delta*(w**2)*c(w*t) ,0.0,0.0]);
# jj = numpy.array([ -0.5*Delta*(w**3)*s(w*t) ,0.0,0.0]);
# ss = numpy.array([ -0.5*Delta*(w**4)*c(w*t) ,0.0,0.0]);
# pp = pp + numpy.array([-Delta,0.0,0.01])
# pp = pp + numpy.array([0.0,0.0,0.01])
# else:
# pp = numpy.array([0.0,0.0,0.0 ]);
# vv = numpy.array([0.0,0.0,0.0 ]);
# aa = numpy.array([0.0,0.0,0.0 ]);
# jj = numpy.array([0.0,0.0,0.0 ]);
# ss = numpy.array([0.0,0.0,0.0 ]);
# return numpy.concatenate([pp,vv,aa,jj,ss])
# # Desired trajectory for LOAD
# def traj_des2(t,pp_real,vv_real,aa_real,jj_real,ss_real):
# pp_final,vv_final,aa_final,jj_final,ss_final
# if t <= 0.0:
# r = 1.0
# w = 2*3.14/10.0
# pp = r*w**0*numpy.array([ c(w*t), s(w*t),0.0]);
# vv = r*w**1*numpy.array([-s(w*t), c(w*t),0.0]);
# aa = r*w**2*numpy.array([-c(w*t),-s(w*t),0.0]);
# jj = r*w**3*numpy.array([ s(w*t),-c(w*t),0.0]);
# ss = r*w**4*numpy.array([ c(w*t), s(w*t),0.0]);
# pp = pp + numpy.array([0.0,0.0,0.01])
# else:
# pp = numpy.array([0.0,0.0,0.01]);
# vv = numpy.array([0.0,0.0,0.0 ]);
# aa = numpy.array([0.0,0.0,0.0 ]);
# jj = numpy.array([0.0,0.0,0.0 ]);
# ss = numpy.array([0.0,0.0,0.0 ]);
# return numpy.concatenate([pp,vv,aa,jj,ss])
s = r*w**4*numpy.array([ c(w*t), s(w*t),0.0]);
return concatenate([p,v,a,j,s])
#--------------------------------------------------------------------------#
time = 0.4
# cable length
L = 0.5
# initial LOAD position
pL0 = numpy.array([-1.4,3.4,5.0])
# initial LOAD velocity
vL0 = numpy.array([-4.0,-2.0,3.0])
# initial unit vector
n0 = numpy.array([0.0,s(0.3),c(0.3)])
# initial QUAD position
p0 = pL0 + L*n0
# initial angular velocity
w0 = numpy.array([0.1,0.2,-0.1])
# 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)