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 )