def compute_manipulator_matrices(x): p, theta, psi, phi, p_dot, theta_dot, psi_dot, phi_dot, q, q_dot = unpack_state( x) R_world_from_body = matrix( transformations.euler_matrix(psi, theta, phi, "ryxz"))[0:3, 0:3] R_body_from_world = R_world_from_body.T x_vals = hstack([q.A1, q_dot.A1]) A = matrix( sympyutils.evaluate_matrix_anon_funcs(A_anon_funcs_ufuncify, x_vals[newaxis, :])) A_dot = matrix( sympyutils.evaluate_matrix_anon_funcs(A_dot_anon_funcs_ufuncify, x_vals[newaxis, :])) euler_dot = matrix([theta_dot, psi_dot, phi_dot]).T omega_in_body = R_body_from_world * A * euler_dot I_omega_in_body_X = linalgutils.cross_product_left_term_matrix_from_vector( I * omega_in_body) M_thrust_body_from_control = matrix([[0, 0, 0, 0], [1, 1, 1, 1], [0, 0, 0, 0]]) M_torque_body_from_control = matrix( [[-d * cos(alpha), d * cos(beta), d * cos(beta), -d * cos(alpha)], [gamma, -gamma, gamma, -gamma], [d * sin(alpha), d * sin(beta), -d * sin(beta), -d * sin(alpha)]]) # H H_00 = matrix(m * identity(3)) H_11 = I * R_body_from_world * A H_zeros = matrix(zeros((3, 3))) # C C_11 = I * R_body_from_world * A_dot - I_omega_in_body_X * R_body_from_world * A C_zeros = matrix(zeros((3, 3))) # G G_0 = -f_external_world G_1 = matrix(zeros((3, 1))) # B B_0 = R_world_from_body * M_thrust_body_from_control B_1 = M_torque_body_from_control H = bmat([[H_00, H_zeros], [H_zeros, H_11]]) C = bmat([[C_zeros, C_zeros], [C_zeros, C_11]]) G = bmat([[G_0], [G_1]]) B = bmat([[B_0], [B_1]]) return H, C, G, B
def compute_manipulator_matrices(x): p, theta, psi, phi, p_dot, theta_dot, psi_dot, phi_dot, q, q_dot = unpack_state(x) R_world_from_body = matrix(transformations.euler_matrix(psi,theta,phi,"ryxz"))[0:3,0:3] R_body_from_world = R_world_from_body.T x_vals = hstack( [ q.A1, q_dot.A1 ] ) A = matrix(sympyutils.evaluate_matrix_anon_funcs( A_anon_funcs_ufuncify, x_vals[newaxis,:] )) A_dot = matrix(sympyutils.evaluate_matrix_anon_funcs( A_dot_anon_funcs_ufuncify, x_vals[newaxis,:] )) euler_dot = matrix([theta_dot,psi_dot,phi_dot]).T omega_in_body = R_body_from_world*A*euler_dot I_omega_in_body_X = linalgutils.cross_product_left_term_matrix_from_vector(I*omega_in_body) M_thrust_body_from_control = matrix([[0,0,0,0],[1,1,1,1],[0,0,0,0]]) M_torque_body_from_control = matrix([[-d*cos(alpha),d*cos(beta),d*cos(beta),-d*cos(alpha)],[gamma,-gamma,gamma,-gamma],[d*sin(alpha),d*sin(beta),-d*sin(beta),-d*sin(alpha)]]) # H H_00 = matrix(m*identity(3)) H_11 = I*R_body_from_world*A H_zeros = matrix(zeros((3,3))) # C C_11 = I*R_body_from_world*A_dot - I_omega_in_body_X*R_body_from_world*A C_zeros = matrix(zeros((3,3))) # G G_0 = -f_external_world G_1 = matrix(zeros((3,1))) # B B_0 = R_world_from_body*M_thrust_body_from_control B_1 = M_torque_body_from_control H = bmat( [ [H_00, H_zeros], [H_zeros, H_11] ] ) C = bmat( [ [C_zeros, C_zeros], [C_zeros, C_11] ] ) G = bmat( [ [G_0], [G_1] ] ) B = bmat( [ [B_0], [B_1] ] ) return H, C, G, B
def compute_manipulator_matrices(x): front_prop_and_quad_positive_x_axis_angle = pi/4 rear_prop_and_quad_negative_x_axis_angle = pi/4 y_axis_torque_per_newton_per_prop = 1 alpha = front_prop_and_quad_positive_x_axis_angle beta = rear_prop_and_quad_negative_x_axis_angle gamma = y_axis_torque_per_newton_per_prop p_body, theta_body, psi_body, phi_body, theta_cam, psi_cam, phi_cam, p_body_dot, theta_body_dot, psi_body_dot, phi_body_dot, theta_cam_dot, psi_cam_dot, phi_cam_dot, q, q_dot = unpack_state(x) R_world_from_body = matrix(transformations.euler_matrix(psi_body,theta_body,phi_body,"ryxz"))[0:3,0:3] R_body_from_world = R_world_from_body.T R_body_from_cam = matrix(transformations.euler_matrix(psi_cam,theta_cam,phi_cam,"ryxz"))[0:3,0:3] R_cam_from_body = R_body_from_cam.T subs_body = array( [ theta_body, psi_body, phi_body, theta_body_dot, psi_body_dot, phi_body_dot ] ) A_body = matrix(sympyutils.evaluate_matrix_anon_funcs( A_anon_funcs_ufuncify, subs_body[newaxis,:] )) A_body_dot = matrix(sympyutils.evaluate_matrix_anon_funcs( A_dot_anon_funcs_ufuncify, subs_body[newaxis,:] )) subs_cam = array( [ theta_cam, psi_cam, phi_cam, theta_cam_dot, psi_cam_dot, phi_cam_dot ] ) A_cam = matrix(sympyutils.evaluate_matrix_anon_funcs( A_anon_funcs_ufuncify, subs_cam[newaxis,:] )) A_cam_dot = matrix(sympyutils.evaluate_matrix_anon_funcs( A_dot_anon_funcs_ufuncify, subs_cam[newaxis,:] )) euler_body_dot = matrix([theta_body_dot,psi_body_dot,phi_body_dot]).T omega_body_in_body = R_body_from_world*A_body*euler_body_dot I_body_omega_body_in_body_X = linalgutils.cross_product_left_term_matrix_from_vector(I_body*omega_body_in_body) euler_cam_dot = matrix([theta_cam_dot,psi_cam_dot,phi_cam_dot]).T omega_cam_in_cam = R_cam_from_body*A_cam*euler_cam_dot I_cam_omega_cam_in_cam_X = linalgutils.cross_product_left_term_matrix_from_vector(I_cam*omega_cam_in_cam) M_thrust_body_from_control = matrix([[0,0,0,0],[1,1,1,1],[0,0,0,0]]) M_torque_body_from_control = matrix([[-d*cos(alpha),d*cos(beta),d*cos(beta),-d*cos(alpha)],[gamma,-gamma,gamma,-gamma],[d*sin(alpha),d*sin(beta),-d*sin(beta),-d*sin(alpha)]]) # H H_00 = matrix(m*identity(3)) H_11 = I_body*R_body_from_world*A_body H_22 = A_cam H_zeros = matrix(zeros((3,3))) # C C_11 = I_body*R_body_from_world*A_body_dot - I_body_omega_body_in_body_X*R_body_from_world*A_body C_22 = A_cam_dot C_zeros = matrix(zeros((3,3))) # G G_0 = -f_external_world G_1 = matrix(zeros((3,1))) G_2 = matrix(zeros((3,1))) # B B_00 = R_world_from_body*M_thrust_body_from_control B_01 = matrix(zeros((3,3))) B_10 = M_torque_body_from_control B_11 = matrix(zeros((3,3))) B_20 = matrix(zeros((3,4))) B_21 = matrix(identity(3)) H = bmat( [ [H_00, H_zeros, H_zeros], [H_zeros, H_11, H_zeros], [H_zeros, H_zeros, H_22 ] ] ) C = bmat( [ [C_zeros, C_zeros, C_zeros], [C_zeros, C_11, C_zeros], [C_zeros, C_zeros, C_22 ] ] ) G = bmat( [ [G_0], [G_1],[G_2] ] ) B = bmat( [ [B_00, B_01], [B_10, B_11], [B_20, B_21] ] ) return H, C, G, B