def evaluate_Q_d(self, q):
"""
Q_d equation according to eq. 3.133 in Computational Dynamics 3rd Ed. (A.A.Shabana)
expanded with z component of moment
"""
# if body i and j are not ground
if self.body_id_i != "ground" and self.body_id_j != "ground":
self.Q_d_list = []
for body_id in self.body_id_list:
Q_d_body = Joint_Q_d_vector(self.joint_type)
Q_d_body.Q_d[0:2] = Ai_ui_P_vector(
self.u_P_LCS_list[Q_d_body.id], q2theta_i(
q, body_id)) * (q2dtheta_i(q, body_id)**2)
# add calculated joint matrix of a body to list
self.Q_d_list.append(Q_d_body)
Q_d = +(self.Q_d_list[0].Q_d - self.Q_d_list[1].Q_d)
elif self.body_id_i == "ground":
Q_d_body = Joint_Q_d_vector(self.joint_type,
body_connected_to_ground=True)
Q_d_body.id = 1
body_id_ = self.body_id_list[Q_d_body.id]
# calculate Q_d vector of only non-ground body
Q_d_body.Q_d[0:2] = Ai_ui_P_vector(
u_P=self.u_P_list[Q_d_body.id], theta=q2theta_i(
q, body_id_)) * (q2dtheta_i(q, body_id_)**2)
Q_d = Q_d_body.Q_d
elif self.body_id_j == "ground":
Q_d_body = Joint_Q_d_vector(self.joint_type,
body_connected_to_ground=True)
body_id_ = self.body_id_list[Q_d_body.id]
# calculate Q_d vector of only non-ground body
Q_d_body.Q_d[0:2] = Ai_ui_P_vector(
self.u_P_LCS_list[Q_d_body.id], q2theta_i(
q, body_id_)) * (q2dtheta_i(q, body_id_)**2)
Q_d = Q_d_body.Q_d
else:
raise ValueError, "Q_d vector for fixed joint not constructed. Check calculation process."
return Q_d
def evaluate_Q_d(self, q):
"""
Function evaluates Q_d vector of a joint
Q_d equation according to eq. 3.133 in Computational Dynamics 3rd Ed. (A.A.Shabana)
Args:
bodies - list of all bodies
q - array of ppositions and velosities
N - number of bodies
Returns:
Q_d - vector Q_d for joint
"""
# if body i and body j are not ground
if (self.body_id_i != "ground") and (self.body_id_j != "ground"):
self.Q_d_list = []
for body_id in self.body_id_list:
# vector Q_d object for each body
Q_d_body = Joint_Q_d_vector(self.joint_type)
# evaluated for each body
Q_d_body.Q_d = Ai_ui_P_vector(
self.u_P_LCS_list[Q_d_body.id], q2theta_i(
q, body_id)) * (q2dtheta_i(q, body_id)**2)
# add calculated joint matrix of a body to list
self.Q_d_list.append(Q_d_body)
Q_d = self.Q_d_list[0].Q_d - self.Q_d_list[1].Q_d
# construct Q_d vector if body i is ground
elif self.body_id_i == "ground":
Q_d_body = Joint_Q_d_vector(self.joint_type,
body_connected_to_ground=True)
Q_d_body.Q_d = Ai_ui_P_vector(
self.u_P_LCS_list[Q_d_body.id], q2theta_i(
q, self.body_id_j)) * (q2dtheta_i(q, self.body_id_j)**2)
Q_d = Q_d_body.Q_d
# construct Q_d vector if body j is ground
elif self.body_id_j == "ground":
Q_d_body = Joint_Q_d_vector(self.joint_type,
body_connected_to_ground=True)
Q_d_body.Q_d = Ai_ui_P_vector(
self.u_P_LCS_list[Q_d_body.id], q2theta_i(
q, self.body_id_i)) * (q2dtheta_i(q, self.body_id_i)**2)
Q_d = Q_d_body.Q_d
else:
raise ValueError, "Q_d vector for revolute joint not constructed. Check calculation process."
return Q_d
def evaluate_Q_d(self, q):
"""
Q_d equation according to eq. 3.133 in Computational Dynamics 3rd Ed. (A.A.Shabana)
expanded with z component of moment
"""
# if body i and j are not ground
if self.body_id_i != "ground" and self.body_id_j != "ground":
self.Q_d_list = []
for body_id in self.body_id_list:
Q_d_body = Joint_Q_d_vector(self.joint_type)
Q_d_body.Q_d[0:2] = Ai_ui_P_vector(self.u_P_LCS_list[Q_d_body.id], q2theta_i(q, body_id)) * (q2dtheta_i(q, body_id) ** 2)
# add calculated joint matrix of a body to list
self.Q_d_list.append(Q_d_body)
Q_d = +(self.Q_d_list[0].Q_d - self.Q_d_list[1].Q_d)
elif self.body_id_i == "ground":
Q_d_body = Joint_Q_d_vector(self.joint_type, body_connected_to_ground=True)
Q_d_body.id = 1
body_id_ = self.body_id_list[Q_d_body.id]
# calculate Q_d vector of only non-ground body
Q_d_body.Q_d[0:2] = Ai_ui_P_vector(u_P=self.u_P_list[Q_d_body.id], theta=q2theta_i(q, body_id_)) * (q2dtheta_i(q, body_id_) ** 2)
Q_d = Q_d_body.Q_d
elif self.body_id_j == "ground":
Q_d_body = Joint_Q_d_vector(self.joint_type, body_connected_to_ground=True)
body_id_ = self.body_id_list[Q_d_body.id]
# calculate Q_d vector of only non-ground body
Q_d_body.Q_d[0:2] = Ai_ui_P_vector(self.u_P_LCS_list[Q_d_body.id], q2theta_i(q, body_id_)) * (q2dtheta_i(q, body_id_) ** 2)
Q_d = Q_d_body.Q_d
else:
raise ValueError, "Q_d vector for fixed joint not constructed. Check calculation process."
return Q_d
def evaluate_Q_d(self, q):
"""
Function evaluates Q_d vector of a joint
Q_d equation according to eq. 3.133 in Computational Dynamics 3rd Ed. (A.A.Shabana)
Args:
bodies - list of all bodies
q - array of ppositions and velosities
N - number of bodies
Returns:
Q_d - vector Q_d for joint
"""
# if body i and body j are not ground
if (self.body_id_i != "ground") and (self.body_id_j != "ground"):
self.Q_d_list = []
for body_id in self.body_id_list:
# vector Q_d object for each body
Q_d_body = Joint_Q_d_vector(self.joint_type)
# evaluated for each body
Q_d_body.Q_d = Ai_ui_P_vector(self.u_P_LCS_list[Q_d_body.id], q2theta_i(q, body_id)) * (q2dtheta_i(q, body_id) ** 2)
# add calculated joint matrix of a body to list
self.Q_d_list.append(Q_d_body)
Q_d = self.Q_d_list[0].Q_d - self.Q_d_list[1].Q_d
# construct Q_d vector if body i is ground
elif self.body_id_i == "ground":
Q_d_body = Joint_Q_d_vector(self.joint_type, body_connected_to_ground=True)
# Q_d_body.id = 1
#
# _body_id = self.body_id_list[Q_d_body.id]
#
# # calculate Q_d vector of only non-ground body
# Ai_ui_P_vector_ = Ai_ui_P_vector(u_P=self.u_P_list[Q_d_body.id], _theta=q2theta_i(q_, _body_id))
#
#
# dtheta_body = q2dtheta_i(q_, _body_id)
#
# Q_d_body.Q_d = Ai_ui_P_vector_ * (dtheta_body ** 2)
Q_d_body.Q_d = Ai_ui_P_vector(self.u_P_LCS_list[Q_d_body.id], q2theta_i(q, self.body_id_j)) * (q2dtheta_i(q, self.body_id_j) ** 2)
Q_d = Q_d_body.Q_d
# construct Q_d vector if body j is ground
elif self.body_id_j == "ground":
Q_d_body = Joint_Q_d_vector(self.joint_type, body_connected_to_ground=True)
# _body_id = self.body_id_list[Q_d_body.id]
#
# # get evaluated vector to point of kinematic constraint u_P
# _u_P = self.u_P_list[Q_d_body.id]
# # theta of body from vector q
# _theta = q2theta_i(q, _body_id)
# # calculate Q_d vector of only non-ground body
# Ai_ui_P_vector_ = Ai_ui_P_vector(_u_P, _theta)
# # omega of body from vector q
# dtheta_body = q2dtheta_i(q, _body_id)
#
# Q_d_body.Q_d = Ai_ui_P_vector_ * dtheta_body ** 2
Q_d_body.Q_d = Ai_ui_P_vector(self.u_P_LCS_list[Q_d_body.id], q2theta_i(q, self.body_id_i)) * (q2dtheta_i(q, self.body_id_i) ** 2)
Q_d = Q_d_body.Q_d
else:
raise ValueError, "Q_d vector for revolute joint not constructed. Check calculation process."
return Q_d
