def DP1_phi(X, C, f_t):
body_1 = C.body_i_name
body_2 = C.body_j_name
for i in X:
if i.name == body_1:
i_index = X.index(i)
if i.name == body_2:
j_index = X.index(i)
I = X[i_index]
J = X[j_index]
a_bar_i = C.a_bar_i
a_bar_j = C.a_bar_j
A_i = build_A(I.q[3:])
A_j = build_A(J.q[3:])
a_bar_i_T = np.transpose(a_bar_i)
A_i_T = np.transpose(A_i)
n1 = np.dot(a_bar_i_T, A_i_T)
n2 = np.dot(n1, A_j)
n3 = np.dot(n2, a_bar_j)
phi_dp1_value = n3 - f_t
return float(phi_dp1_value)
def DP1_PHI_partials(X, C):
body_1 = C.body_i_name
body_2 = C.body_j_name
for i in X:
if i.name == body_1:
i_index = X.index(i)
if i.name == body_2:
j_index = X.index(i)
I = X[i_index]
J = X[j_index]
#Set a and A for bodies i and j
a_bar_i = C.a_bar_i
a_bar_j = C.a_bar_j
A_i = build_A(I.q[3:])
A_j = build_A(J.q[3:])
#Performe some transposes
a_bar_i_T = np.transpose(a_bar_i)
a_bar_j_T = np.transpose(a_bar_j)
A_i_T = np.transpose(A_i)
A_j_T = np.transpose(A_j)
p_i = I.q[3:]
p_j = J.q[3:]
p_dot_i = I.p_dot
p_dot_j = J.p_dot
a_j = np.dot(A_j, a_bar_j)
a_i = np.dot(A_i, a_bar_i)
a_i_T = np.transpose(a_i)
a_j_T = np.transpose(a_j)
p_i.shape = (4, 1)
a_bar_i.shape = (3, 1)
B_i = build_B(p_i, a_bar_i)
B_j = build_B(p_j, a_bar_j)
PHI_DP1_p_i = np.dot(a_j_T, B_i)
PHI_DP1_p_j = np.dot(a_i_T, B_j)
PHI_DP1_p_i.shape = (1, 4)
PHI_DP1_p_j.shape = (1, 4)
PHI_DP1_r_i = np.array([0, 0, 0])
PHI_DP1_r_j = np.array([0, 0, 0])
PHI_DP1_r_i.shape = (1, 3)
PHI_DP1_r_j.shape = (1, 3)
if J.ground == 'True':
return np.concatenate((PHI_DP1_r_i, PHI_DP1_p_i), axis=1)
else:
# return np.concatenate((PHI_DP1_r_i,PHI_DP1_r_j,PHI_DP1_p_i,PHI_DP1_p_j),axis=1)
return np.concatenate(
(PHI_DP1_r_i, PHI_DP1_p_i, PHI_DP1_r_j, PHI_DP1_p_j), axis=1)
def gamma_DP1(X, C, ddf):
#Set bodys i and j
for i in X:
if i.name == "body i":
i_index = X.index(i)
if i.name == "body j":
j_index = X.index(i)
I = X[i_index]
J = X[j_index]
#Set a and A for bodies i and j
a_bar_i = C.a_bar_i
a_bar_j = C.a_bar_j
A_i = build_A(I.q[3:])
A_j = build_A(J.q[3:])
p_i = I.q[3:]
p_j = J.q[3:]
p_i.shape = (4, 1)
p_dot_i = I.p_dot
p_dot_j = J.p_dot
a_i = np.dot(A_i, a_bar_i)
a_j = np.dot(A_j, a_bar_j)
B_dot_i = build_B(p_dot_i, a_bar_i)
B_dot_j = build_B(p_dot_j, a_bar_j)
B_i = build_B(p_i, a_bar_i)
B_j = build_B(p_j, a_bar_j)
a_dot_i = np.dot(B_i, p_dot_i)
a_dot_j = np.dot(B_j, p_dot_j)
a_i_T = np.transpose(a_i)
a_j_T = np.transpose(a_j)
a_dot_i_T = np.transpose(a_dot_i)
if C.name == "driving":
ddt = ddf
else:
ddt = 0
gamma_DP1 = np.dot(np.dot(-a_i_T, B_dot_j), p_dot_j) - np.dot(
np.dot(a_j_T, B_dot_i), p_dot_i) - 2 * np.dot(a_dot_i_T, a_dot_j) + ddt
return float(gamma_DP1)
X=data_file()
import numpy as np
L=2
t=0
#%%
theta=np.pi/4
#
#p_j=build_p(theta)
#p_j=np.array([0.27059805,-0.6532815,-0.270505981,0.65328148])
p_j=np.array([0.7,0.1,0.7,0.1])
p_j.shape=(4,1)
X[1].A_rotation=build_A(p_j)
df=np.cos(-1/2**np.pi*np.sin(2*t))
ddf=np.cos(-np.pi*np.cos(2*t))
#%%
for i in range(0,len(X)):
if X[i].name == "body j":
X[i].q[3:]=p_j
X[1].q[:3,0]=np.array([0,1.4142,-1.4142])
X[1].A_rotation=build_A(X[1].q[3:])
phi_values=[]
for i in constraint_list:
J_bar[0, 0] = 1 / 12 * m * (b**2 + c**2)
J_bar[1, 1] = 1 / 12 * m * (L**2 + c**2)
J_bar[2, 2] = 1 / 12 * m * (L**2 + b**2)
F = np.array([0, 0, m * -9.81])
F.shape = (3, 1)
tau = np.array([0, 0, 0])
tau.shape = (3, 1)
X = data_file()
constraint_list = constraints_in()
p_j = np.array([0.7, 0.1, 0.7, 0.1])
p_j.shape = (4, 1)
X[1].A_rotation = build_A(p_j)
for i in range(0, len(X)):
if X[i].name == "body j":
X[i].q[3:] = p_j
X[1].q[:3, 0] = np.array([0, 1, -0.5])
tm = np.arange(0, 10, 0.01)
body_list = []
partial_list = []
torque_list = []
y = []
z = []
x = []
r_dot = []
tau=np.array([0,0,0]) tau.shape=(3,1) # set body j starting values r_start=np.array([0,L,0]) A_start=np.zeros([3,3]) A_start[0,2]=np.cos(0) A_start[1,0]=np.cos(0) A_start[2,1]=np.cos(0) p_start=build_p_from_A(A_start) r_start.shape=(3,1) p_start.shape=(4,1) X[1].q[0:3]=r_start X[1].q[3:]=p_start X[1].A_rotation=build_A(X[1].q[3:]) X[1].r_dot=np.array([0,0,0]) X[1].p_dot=np.array([0,0,0,0]) r_start=np.array([0,2*L,-L/2]) A_start=np.zeros([3,3]) A_start[0,2]=np.cos(0) A_start[1,1]=np.cos(0) A_start[2,0]=np.cos(np.pi) p_start=build_p_from_A(A_start) r_start.shape=(3,1) p_start.shape=(4,1) X[2].q[0:3]=r_start X[2].q[3:]=p_start X[2].A_rotation=build_A(X[2].q[3:]) X[2].r_dot=np.array([0,0,0])
def pendulum(X, constraint_list, t):
# f=np.cos(np.pi/4*np.cos(2*t))
# df=np.cos(-1/2**np.pi*np.sin(2*t))
# ddf=np.cos(-np.pi*np.cos(2*t))
df = np.pi * np.sin(2 * t) * np.cos((np.pi * np.cos(t * 2)) / 4) * -1 / 2
ddf = np.pi**2 * np.sin(t * 2)**2 * np.sin(
(np.pi * np.cos(t * 2)) / 4) * (-1 / 4) - np.pi * np.cos(
t * 2) * np.cos((np.pi * np.cos(t * 2)) / 4)
phi_values = []
for i in constraint_list:
if i.type.strip(" ' ") == 'CD':
phi_values.append(phi_cd(X, i, eval(i.f)))
elif i.type.strip(" ' ") == 'DP1':
phi_values.append(phi_dp1(X, i, eval(i.f)))
#Add euler parameter constraint
phi_values.append(
round(float((np.dot(np.transpose(X[1].q[3:]), X[1].q[3:]) - 1)), 12))
pi_values = []
for i in constraint_list:
if i.type.strip(" ' ") == 'CD':
PI_j = CD_PI(X, i)
pi_values.append(PI_j)
if i.type.strip(" ' ") == 'DP1':
PI_j = DP1_PI(X, i)
pi_values.append(PI_j)
phi_partials_values = []
for i in constraint_list:
if i.type.strip(" ' ") == 'CD':
dri, dpi, drj, dpj = CD_PHI_partials(X, i)
phi_partials_values.append([drj, dpj])
if i.type.strip(" ' ") == 'DP1':
dri, dpi, drj, dpj = DP1_PHI_partials(X, i)
phi_partials_values.append([drj, dpj])
#%% Assembel Jacobian
ja_list = []
for i in range(0, len(phi_partials_values)):
ca = np.zeros([7])
#order for [d_ri d_pi d_rj d_pj]
ca[0:3] = phi_partials_values[i][0]
ca[3:7] = phi_partials_values[i][1]
ca.shape = (1, 7)
ja_list.append(ca)
jacobian = np.zeros([7, 7])
for i in range(0, len(ja_list)):
jacobian[i, :] = ja_list[i]
#add euler parameter normalization constraint to jacobian
jacobian[6, 3:] = 2 * X[1].q[3:].T
#%%
#Newton Raphson method
q0 = np.zeros([7, 1])
q0[:] = X[1].q
phi = np.array(phi_values)
phi.shape = (7, 1)
error = 10
tol = 1e-4
counter = 1
while abs(error) > tol and counter < 30:
# print(counter)
# q_new=q0-np.linalg.lstsq(jacobian,phi)[0]
q_new = q0 - np.dot(np.linalg.inv(jacobian), phi)
# q_new=q0-jacobian@phi
error = np.linalg.norm(q0 - q_new)
X[1].q = q_new #Assing new q to body J
X[1].A_rotation = build_A(
q_new[3:]) #Build new rotation matrix from new p values
phi = np.array(calc_phi(X, constraint_list,
t)) #calculate new values of phi
phi.shape = (7, 1)
partials_new = calc_partials(X,
constraint_list) #calculate new partials
jacobian = build_ja(X, partials_new) #build new jacobian
q0 = q_new
counter = counter + 1
if counter == 30:
print("Failed to converge")
nue_values = []
for i in constraint_list:
if i.type.strip(" ' ") == 'CD':
nue_values.append(CD_nue(X, i, df))
if i.type.strip(" ' ") == 'DP1':
nue_values.append(DP1_nue(X, i, df))
nue_values.append(0)
#calculate p_dot
q_dot = np.dot(np.linalg.inv(jacobian), nue_values)
r_dot = q_dot[:3]
r_dot.shape = (3, 1)
X[1].r_dot = r_dot
p_dot = q_dot[3:]
p_dot.shape = (4, 1)
X[1].p_dot = p_dot
gamma_values = []
for i in constraint_list:
if i.type.strip(" ' ") == 'CD':
gamma_values.append(gamma_CD(X, i, ddf))
if i.type.strip(" ' ") == 'DP1':
gamma_values.append(gamma_DP1(X, i, ddf))
gamma_values.append(
float(-2 * np.dot(np.transpose(X[1].p_dot), X[1].p_dot)))
q_d_dot = np.dot(np.linalg.inv(jacobian), gamma_values)
r_d_dot = q_d_dot[:3]
r_d_dot.shape = (3, 1)
X[1].r_d_dot = r_d_dot
p_d_dot = q_d_dot[3:]
p_d_dot.shape = (4, 1)
X[1].p_d_dot = p_d_dot
return X, jacobian, gamma_values, pi_values