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 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 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 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 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 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 _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 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 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 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 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 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 single_layer_backward_progation(dA_curr, W_curr, b_curr, Z_curr, A_prev, activation='relu'):
m = A_prev.shape[1]
if activation is 'relu':
backward_activation_func = relu_backward
elif activation is 'sigmoid':
backward_activation_func = sigmoid_backward
else:
raise Exception('Non-supported activation function')
dZ_curr = backward_activation_func(dA_curr, Z_curr)
dW_curr = dot(dZ_curr, A_prev.T) / m
db_curr = s(dZ_curr, axis=1, keepdims=True) / m
dA_prev = dot(W_curr, dW_curr, db_curr)
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 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 cameraTransform(a): #a is of type numpy array of double
c = np.array([5.0, 0, 0])
theta = np.array([0.0, 0, 0])
e = np.array([0.0, 0, 0])
d = a - c
d = np.dot(
np.array([[co(theta[2]), s(theta[2]), 0],
[-s(theta[2]), co(theta[2]), 0], [0, 0, 1]]), d)
print(d)
d = np.dot(
np.array([[co(theta[1]), 0, -s(theta[1])], [0, 1, 0],
[s(theta[1]), 0, co(theta[1])]]), d)
d = np.dot(
np.array([[1, 0, 0], [0, co(theta[0]), s(theta[0])],
[0, -s(theta[0]), co(theta[0])]]), d)
print(d)
b = np.array([((e[2] / d[2]) * d[0] - e[0]),
((e[2] / d[2]) * d[1] - e[1])])
return b
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 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 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 getMatrixForForwardKinematic(*q):
'''
Forward kinematic from robot base to camera.
DH table
=================================================
| index | theta | d | r | alpha |
=================================================
| 1 | 0 | H | L | -pi/2 |
-------------------------------------------------
| 2 | phi | 0 | 0 | pi/2 |
=================================================
DH table from robot base to pan-tilt base.^
-------------------------------------------------
DH table from pan-tilt base to camera. v
=================================================
| 3 | q1 | h1 | l1 | -pi/2 |
-------------------------------------------------
| 4 | q2-pi/2 | 0 | h2 | -pi/2 |
-------------------------------------------------
| 5 | pi/2 | l2 | lx | 0 |
=================================================
'''
global H, L, h1, l1, h2, l2, Phi, lx
q1 = q[0]
q2 = q[1]
q3 = q2 - (np.pi / 2)
H_ = _prefix * _H
L_ = _prefix * _L
h1_ = _prefix * _h1
l1_ = _prefix * _l1
h2_ = _prefix * _h2
l2_ = _prefix * _l2
lx_ = _prefix * _lx
T_R_j3 = [[
c(Phi) * s(q1), -c(q2) * s(Phi) - c(Phi) * c(q1) * s(q2),
c(Phi) * c(q1) * c(q2) - s(Phi) * s(q2),
L_ + h1_ * s(Phi) + l1_ * c(Phi) * c(q1) + h2_ * c(q2) * s(Phi) -
l2_ * s(Phi) * s(q2) + l2_ * c(Phi) * c(q1) * c(q2) +
h2_ * c(Phi) * c(q1) * s(q2) - c(Phi) * s(q1) * lx_
],
[
-c(q1), -s(q1) * s(q2),
c(q2) * s(q1),
s(q1) * (l1_ + l2_ * c(q2) + h2_ * s(q2))
],
[
-s(Phi) * s(q1),
c(q1) * s(Phi) * s(q2) - c(Phi) * c(q2),
-c(Phi) * s(q2) - c(q1) * c(q2) * s(Phi),
H_ + h1_ * c(Phi) + h2_ * c(Phi) * c(q2) -
l1_ * c(q1) * s(Phi) - l2_ * c(Phi) * s(q2) -
l2_ * c(q1) * c(q2) * s(Phi) - h2_ * c(q1) * s(Phi) * s(q2)
], [0, 0, 0, 1]]
T_R_j3 = np.array(T_R_j3, dtype=np.float64)
return T_R_j3
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 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]
])
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)
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
# the following expressions for new x an y accelerations account for the forces:
# the total thrust imparted by the rotors
# aerodynamic drag
# pid control
'''
QUESTION : WILL THE PID CONTROL HAVE TO GO INTO A DIFFERENT EXPRESSION IN A REAL IMPLEMENTATION?
DOES IT AMOUNT TO A PHYSICAL FORCE IN THIS SETUP?
HAVE I INTRODUCED A LAG OF ONE TIME-STEP?; THE PID INPUT TO THE SYSTEM AT TIME K INFLUENCES THE THRUST AND TORQUES AT TIME K+1?
'''
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 * Xint(x_des,h) )
)
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 * Yint(y_des,h) )
)
# 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 )
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
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 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 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
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
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 )
#-------------------------------- 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) ,
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 = GetEulerAngles(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 Ry(tt):
return numpy.array([[c(tt), 0.0, s(tt)], [0.0, 1, 0.0],
[-s(tt), 0.0, c(tt)]])
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 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
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 Rx(tt):
return numpy.array([[1.0, 0.0, 0.0], [0.0, c(tt), -s(tt)],
[0.0, s(tt), c(tt)]])
#--------------------------------------------------------------------------#
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 Rz(tt):
return numpy.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 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 Ry(tt):
return numpy.array([[c(tt),0.0,s(tt)],[0.0,1,0.0],[-s(tt),0.0,c(tt)]])
def rot_z(tt):
return np.array([[c(tt),-s(tt),0.0],[s(tt),c(tt),0.0],[0.0,0.0,1]])
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_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 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 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 )
