def time_derivative_generialized_coordinate_inertia(I, phi, theta, phi_dot, theta_dot): W = mechanics.att_vel_to_ang_vel_jacobian(phi, theta) W_dot = mechanics.time_derivative_att_vel_to_ang_vel_jacobian( phi, theta, phi_dot, theta_dot) J_dot = 2 * np.dot(W.T, np.dot(I, W_dot)) return J_dot
def diffeomorphism_full_state_control(self): d = 1 xi_bar = np.array([[0, 0, 1]]).T zeta_bar = xi_bar + np.array([[0, 0, d]]).T m = self.M g = self.G R = self.rot_mat x = self.state xi = x[0:3] eta = x[3:6] xi_dot = x[6:9] eta_dot = x[9:12] phi = eta[0] theta = eta[1] psi = eta[2] phi_dot = eta_dot[0] theta_dot = eta_dot[1] psi_dot = eta_dot[2] zeta = (self * rigid.Vector(*(0, 0, -d))).array chi = np.vstack([zeta - zeta_bar, psi, xi_dot, psi_dot]) W = mechanics.att_vel_to_ang_vel_jacobian(phi, theta) W_inv = np.linalg.inv(W) W_dot = mechanics.time_derivative_att_vel_to_ang_vel_jacobian( phi, theta, phi_dot, theta_dot) A = np.hstack( [np.zeros((8, 4)), np.vstack([np.identity(4), np.zeros((4, 4))])]) b = np.array([[-1 / m * R[0, 2], d * R[0, 1], -d * R[0, 0]], [-1 / m * R[1, 2], d * R[1, 1], -d * R[1, 0]], [-1 / m * R[2, 2], d * R[2, 1], -d * R[2, 0]]]) B = np.vstack([ np.zeros((4, 4)), np.hstack([b, np.zeros((3, 1))]), np.array([[0, 0, 0, 1]]) ]) G = np.array([[0, 0, 0, 0, 0, 0, -g, 0]]).T QQ = np.diag([100, 100, 10, 1, 1, 1, 1, 1]) RR = np.diag(np.array([1, 1, 1, 1])) K, _, _ = lqr(A, B, QQ, RR) # mu_bar = np.array([[-g*m*R[2, 2], g/d*R[2, 1], -g/d*R[2, 0], 0]]).T mu_bar = np.array([[g * m, 0, 0, 0]]).T mu_delta = -np.dot(K, chi) mu = mu_bar + mu_delta F = mu[0, 0] alpha = mu[1:4] J = generialized_coordinate_inertia(self.I, phi, theta, psi) C = quad_coriolis(self.I, phi, theta, phi_dot, theta_dot, psi_dot) tau_eta = np.dot(J, np.dot(W_inv, (alpha - np.dot(W_dot, eta_dot)))) + np.dot( C, eta_dot) return np.vstack([F, tau_eta])
def diffeomorphism_full_state_control(self): d = 1 xi_bar = np.array([[0, 0, 1]]).T zeta_bar = xi_bar + np.array([[0, 0, d]]).T m = self.M g = self.G R = self.rot_mat x = self.state xi = x[0:3] eta = x[3:6] xi_dot = x[6:9] eta_dot = x[9:12] phi = eta[0] theta = eta[1] psi = eta[2] phi_dot = eta_dot[0] theta_dot = eta_dot[1] psi_dot = eta_dot[2] zeta = (self * rigid.Vector(*(0, 0, -d))).array chi = np.vstack([zeta-zeta_bar, psi, xi_dot, psi_dot]) W = mechanics.att_vel_to_ang_vel_jacobian(phi, theta) W_inv = np.linalg.inv(W) W_dot = mechanics.time_derivative_att_vel_to_ang_vel_jacobian(phi, theta, phi_dot, theta_dot) A = np.hstack([np.zeros((8, 4)), np.vstack([np.identity(4), np.zeros((4, 4))])]) b = np.array([[-1/m*R[0, 2], d*R[0, 1], -d*R[0, 0]], [-1/m*R[1, 2], d*R[1, 1], -d*R[1, 0]], [-1/m*R[2, 2], d*R[2, 1], -d*R[2, 0]]]) B = np.vstack([np.zeros((4, 4)), np.hstack([b, np.zeros((3, 1))]), np.array([[0, 0, 0, 1]])]) G = np.array([[0, 0, 0, 0, 0, 0, -g, 0]]).T QQ = np.diag([100, 100, 10, 1, 1, 1, 1, 1]) RR = np.diag(np.array([1, 1, 1, 1])) K,_,_ = lqr(A, B, QQ, RR) # mu_bar = np.array([[-g*m*R[2, 2], g/d*R[2, 1], -g/d*R[2, 0], 0]]).T mu_bar = np.array([[g*m, 0, 0, 0]]).T mu_delta = -np.dot(K, chi) mu = mu_bar + mu_delta F = mu[0, 0] alpha = mu[1:4] J = generialized_coordinate_inertia(self.I, phi, theta, psi) C = quad_coriolis(self.I, phi, theta, phi_dot, theta_dot, psi_dot) tau_eta = np.dot(J, np.dot(W_inv, (alpha - np.dot(W_dot, eta_dot)))) + np.dot(C, eta_dot) return np.vstack([F, tau_eta])
def time_derivative_generialized_coordinate_inertia(I, phi, theta, phi_dot, theta_dot): W = mechanics.att_vel_to_ang_vel_jacobian(phi, theta) W_dot = mechanics.time_derivative_att_vel_to_ang_vel_jacobian(phi, theta, phi_dot, theta_dot) J_dot = 2 * np.dot(W.T, np.dot(I, W_dot)) return J_dot