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 generialized_coordinate_inertia(I, phi, theta, psi):
W = mechanics.att_vel_to_ang_vel_jacobian(phi, theta)
J = np.dot(W.T, np.dot(I, W))
return J
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 generialized_coordinate_inertia(I, phi, theta, psi):
W = mechanics.att_vel_to_ang_vel_jacobian(phi, theta)
J = np.dot(W.T, np.dot(I, W))
return J
