def compute_manipulator_matrix_derivatives(x,u):
"compute_manipulator_matrix_derivatives begin"
p, theta, psi, phi, p_dot, theta_dot, psi_dot, phi_dot, q, q_dot = unpack_state(x)
x_and_u_vals = hstack( [ q.A1, q_dot.A1, u.A1 ] )
dqdotdot_dq = matrix(sympyutils.evaluate_matrix_anon_funcs( dqdotdot_dq_anon_funcs_ufuncify, x_and_u_vals[newaxis,:] ))
dqdotdot_dqdot = matrix(sympyutils.evaluate_matrix_anon_funcs( dqdotdot_dqdot_anon_funcs_ufuncify, x_and_u_vals[newaxis,:] ))
return dqdotdot_dq, dqdotdot_dqdot
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_matrix_derivatives(x, u):
"compute_manipulator_matrix_derivatives begin"
p, theta, psi, phi, p_dot, theta_dot, psi_dot, phi_dot, q, q_dot = unpack_state(
x)
x_and_u_vals = hstack([q.A1, q_dot.A1, u.A1])
dqdotdot_dq = matrix(
sympyutils.evaluate_matrix_anon_funcs(dqdotdot_dq_anon_funcs_ufuncify,
x_and_u_vals[newaxis, :]))
dqdotdot_dqdot = matrix(
sympyutils.evaluate_matrix_anon_funcs(
dqdotdot_dqdot_anon_funcs_ufuncify, x_and_u_vals[newaxis, :]))
return dqdotdot_dq, dqdotdot_dqdot
def compute_manipulator_matrix_derivatives(x, u):
p, theta, p_dot, theta_dot, q, q_dot = unpack_state(x)
x_and_u_vals = hstack([q.A1, q_dot.A1, u.A1])
# slow
# x_and_u_subs = dict(zip(x_and_u_syms,x_and_u_vals))
# dqdotdot_dq = dqdotdot_dq_expr.subs(x_and_u_subs).evalf()
# dqdotdot_dqdot = dqdotdot_dqdot_expr.subs(x_and_u_subs).evalf()
# fast
dqdotdot_dq = matrix(
sympyutils.evaluate_matrix_anon_funcs(dqdotdot_dq_anon_funcs_ufuncify,
x_and_u_vals[newaxis, :]))
dqdotdot_dqdot = matrix(
sympyutils.evaluate_matrix_anon_funcs(
dqdotdot_dqdot_anon_funcs_ufuncify, x_and_u_vals[newaxis, :]))
return dqdotdot_dq, dqdotdot_dqdot
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
