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 is defined in subclass
:param q:
:return:
"""
Q_d = np.zeros(self.n_CNC)
# get uPi in LCS
_u_P_i = self.u_P_LCS_list[self.body_id_list.index(self.body_id_i)]
# rigid body i data
theta_i = q2theta_i(q, self.body_id_i)
omega_i = q2dtheta_i(q, self.body_id_i)
# evaluate uPi in GCS
u_P_i = Ai_ui_P_vector(_u_P_i, theta_i)
# vector Qd
Q_d[0:2] = u_P_i * (omega_i ** 2)
# Q_d[2] = (omega_i ** 2) * np.dot(u_P_i, self.e_j_grad)
e_x = q2e_x_jk(q, self.body_id_j, self.node_id_j)
Ai_uiR = Ai_ui_P_vector(self.u_iR, theta_i)
de_x = q2de_x_jk(q, self.body_id_j, self.node_id_j)
Ai_theta_uiR = Ai_theta_ui_P_vector(self.u_iP_LCS, q2theta_i(q, self.body_id_i), 0)
# Q_d[2] = np.dot(u_i, e_x) * omega_i + np.dot(u_P_i, de_x)
# Q_d[2] = (np.dot(Ai_uiR, e_x) * (omega_i ** 2)) - (2. * omega_i * np.dot(Ai_theta_ui_P_vector(self.u_iR, theta_i, 0), de_x))
# Q_d[2] = (np.sum(Ai_uiR) * (omega_i ** 2)) - (2. * omega_i * np.dot(Ai_theta_ui_P_vector(self.u_iR, theta_i, 0), de_x))
Q_d[2] = (omega_i ** 2) * (np.dot(Ai_uiR, e_x) - np.dot(Ai_theta_uiR, de_x)) - (omega_i * np.dot(Ai_theta_uiR, e_x))
return Q_d
def _contact_velocity(self, q):
"""
Function evaluates relative contact velocity vectors in normal and tangent direction
"""
dr_P = []
for body_id, _R, _n, _t in zip(self.body_id_list, self.R0_list, self._n_list, self._t_list):
# contact point in body LCS
_uP = _R * _n
# body velocity, R, theta
_dR = q2dR_i(q, body_id)
_theta = q2theta_i(q, body_id)
# dtheta - omega
_dtheta = q2dtheta_i(q, body_id)
# velocity at contact point (in GCS) on a body
_dr_P = dr_contact_point_uP(_dR, _theta, _dtheta, _uP)
# appned to list
dr_P.append(_dr_P)
# relative contact velocity vector
_dq = dr_P[1] - dr_P[0]
# relative contact velocity (scalar value)
# normal direction
_dq_n = np.dot(_dq, self._n)
# tangent direction
_dq_t = np.dot(_dq, self._t)
return _dq_n, _dq_t
def contact_velocity(self, q):
"""
positive value - bodies are approaching
negative values - bodies are moving apart
:return:
"""
dr_P = [np.zeros(2, dtype="float32"), np.zeros(2, dtype="float32")]
for i, (body_id,
u_P) in enumerate(zip(self.body_id_list, self.u_P_LCS_list)):
# body velocity, R, theta
dR = q2dR_i(q, body_id)
theta = q2theta_i(q, body_id)
# dtheta - omega
dtheta = q2dtheta_i(q, body_id)
# point velocity
dr_P_i = dr_contact_point_uP(dR, theta, dtheta, u_P)
# update list
dr_P[i] = dr_P_i
# relative contact velocity vector
_dq = dr_P[1] - dr_P[0]
# relative contact velocity
# normal direction
self._n_GCS = self.n_list[self.index]
self._dq_n = np.dot(_dq, self._n_GCS)
# tangent direction
self._t_GCS = self.t_list[self.index]
self._dq_t = np.dot(_dq, self._t_GCS)
return self._dq_n, self._dq_t
def evaluate_kinetic_energy(self, q):
"""
:return:
"""
# vector of translational velocity
if q is None:
dR = self.dR[0:2]
else:
if len(q) == self.q_i_size:
dR = q[0:2]
else:
dR = q2dR_i(q, self.body_id)
# float of angular velocity
if q is None:
dtheta = self.dtheta[2]
else:
if len(q) == self.q_i_size:
dtheta = q[2]
else:
dtheta = q2dtheta_i(q, self.body_id)
# kinetic energy
self._kinetic_energy = 0.5 * self.mass * (np.linalg.norm(dR)**2) + .5 * self.J_zz * (dtheta**2)
return self._kinetic_energy
def _mechanical_energy(self, q):
"""
"""
# predefine zero array
_energy = np.zeros(self.MBD_system.number_of_bodies)
# energy of all bodies
for i, body in enumerate(self.MBD_system.bodies):
_q = q2R_i(q, body.body_id)
_dq = np.append(q2dR_i(q, body.body_id), q2dtheta_i(q, body.body_id))
# body energy
# body_energy = 0.5 * (body.mass * (_dq**2) + body.J_zz * (_omega**2)) + (body.mass * self.MBD_system.gravity * _q[1])
body_energy = body.mechanical_energy(q=_q, dq=_dq, gravity=self.MBD_system.gravity)
_energy[i] = body_energy
# energy of normal contact forces
_energy_of_contacts = np.zeros(len(self.MBD_system.contacts))
# for i, contact in enumerate(self.MBD_system.contacts):
# _energy_of_contacts[i] = contact.mechanical_energy()
# total mechanical energy
energy = np.sum(_energy) + np.sum(_energy_of_contacts)
return energy
def contact_velocity(self, q):
"""
Function evaluates relative contact velocity vectors in normal and tangent direction
:param q:
:return:
"""
dr_P = [np.zeros(2, dtype="float32"), np.zeros(2, dtype="float32")]
for i, (body_id,
u_P) in enumerate(zip(self.body_id_list, self.u_P_LCS_list)):
# body velocity, R, theta
dR = q2dR_i(q, body_id)
theta = q2theta_i(q, body_id)
# dtheta - omega
dtheta = q2dtheta_i(q, body_id)
# point velocity
dr_P_i = dr_contact_point_uP(dR, theta, dtheta, u_P)
# update list
dr_P[i] = dr_P_i
# relative contact velocity vector
_dq = dr_P[1] - dr_P[0]
# relative contact velocity
# normal direction
self._n_GCS = self.normal
self._dq_n = np.dot(_dq, self._n_GCS)
# tangent direction
self._t_GCS = self.tangent
self._dq_t = np.dot(_dq, self._t_GCS)
return self._dq_n, self._dq_t
def _contact_velocity(self, q):
"""
Function evaluates relative contact velocity at contact point in revolute clearance joint.
:param q: array of coordinates (r, theta) and velocities (dR, dhete=omega) of MBD system
"""
dr_P = []
print "self.u_P_LCS_list =", self.u_P_LCS_list, type(self.u_P_LCS_list)
for body_id, u_P in zip(self.body_id_list, self.u_P_LCS_list):
dR = q2dR_i(q, body_id)
theta = q2theta_i(q, body_id)
# dtheta - omega
dtheta = q2dtheta_i(q, body_id)
# point velocity
dr_P_body = dr_contact_point_uP(dR, theta, dtheta, u_P)
# add to list
dr_P.append(dr_P_body)
_dq = dr_P[0] - dr_P[1]
# relative contact velocity
# normal direction
_dq_n = np.dot(_dq, self._n_GCS)
# tangent direction
_dq_t = np.dot(_dq, self._t_GCS)
return _dq_n, _dq_t
def _contact_velocity(self, q):
"""
Args:
q:
Returns:
"""
dr_P = []
for body_id, u_P in zip(self.body_id_list, self.u_P_LCS_list):
# body velocity, R, theta
dR = q2dR_i(q, body_id)
theta = q2theta_i(q, body_id)
# dtheta - omega
dtheta = q2dtheta_i(q, body_id)
# point velocity
dr_P_body = dr_contact_point_uP(dR, theta, dtheta, u_P)
# print 'dr_P_body=', dr_P_body, 'body_id=', body_id
# add to list
dr_P.append(dr_P_body)
_dq = dr_P[1] - dr_P[0]
self._dq_n = np.dot(_dq, self.normal)
# print 'dq_n=', self._dq_n, 'normal=', self.normal, _dq
self._dq_t = np.dot(_dq, self.tangent)
return self._dq_n, self._dq_t
def _contact_velocity(self, q):
"""
Function evaluates relative contact velocity vectors in normal and tangent direction
"""
dr_P = []
for body_id, u_P in zip(self.body_id_list, self.u_P_LCS_list):
# body velocity, R, theta
_dR = q2dR_i(q, body_id)
_theta = q2theta_i(q, body_id)
# dtheta - omega
_dtheta = q2dtheta_i(q, body_id)
# velocity at contact point (in GCS) on a body
_dr_P = dr_contact_point_uP(_dR, _theta, _dtheta, u_P)
# appned to list
dr_P.append(_dr_P)
# relative contact velocity vector
_dq = dr_P[1] - dr_P[0]
# relative contact velocity (scalar value)
# normal direction
_dq_n = np.dot(_dq, self._n_GCS)
# tangent direction
_dq_t = np.dot(_dq, self._t_GCS)
return _dq_n, _dq_t
def _contact_velocity(self, q):
"""
Function evaluates relative contact velocity at contact point in revolute clearance joint.
:param q: array of coordinates (r, theta) and velocities (dR, dheta=omega) of MBD system
"""
dr_P = []
for body_id, u_P in zip(self.body_id_list, self.u_P_LCS_list):
dR = q2dR_i(q, body_id)
theta = q2theta_i(q, body_id)
# dtheta - omega
dtheta = q2dtheta_i(q, body_id)
# point velocity
dr_P_body = dr_contact_point_uP(dR, theta, dtheta, u_P)
# add to list
dr_P.append(dr_P_body)
_dq = dr_P[1] - dr_P[0]
# relative contact velocity
# normal direction
_dq_n = np.dot(_dq, self._n_GCS)
# tangent direction
_dq_t = np.dot(_dq, self._t_GCS)
return _dq_n, _dq_t
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
"""
Q_d = np.zeros(self.n_CNC)
# get uPi in LCS
_u_P_j = self.u_P_LCS_list[self.body_id_list.index(self.body_id_j)]
# rigid body i data
theta_j = q2theta_i(q, self.body_id_j)
omega_j = q2dtheta_i(q, self.body_id_j)
# evaluate uPi in GCS
u_P_i = -Ai_ui_P_vector(_u_P_j, theta_j)
# vector Qd
Q_d[0:2] = u_P_i * (omega_j**2)
# # 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_dl(self, q, I_r_ij_P):
"""
:return:
"""
# vector of dq_i, dq_j
dq = np.array([np.append(q2dR_i(q, self.body_id_i), q2dtheta_i(q, self.body_id_i)),
np.append(q2dR_i(q, self.body_id_j), q2dtheta_i(q, self.body_id_j))]).flatten()
# dr_ij_P_dq
dr_ij_P_dq_ = dr_ij_P_dq(body_id_i=self.body_id_i,
theta_i=q2theta_i(q, self.body_id_i),
u_iP=self.u_iP_LCS,
body_id_j=self.body_id_j,
theta_j=q2theta_i(q, self.body_id_j),
u_jP=self.u_jP_LCS)
# velocity of deformation of spring length
dl = np.dot(I_r_ij_P, np.dot(dr_ij_P_dq_, dq))
return dl
def evaluate_Q_d(self, q):
"""
Function evaluates Q_d vector of a joint
Q_d equation according to eq. 3.133 (example 3.14) 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
"""
Q_d = np.zeros(2)
# get theta i, j
theta_i = q2theta_i(q, self.body_id_i)
theta_j = q2theta_i(q, self.body_id_j)
# get translational absolute velocity of body - R i, j
dR_i = q2dR_i(q, self.body_id_i)
dR_j = q2dR_i(q, self.body_id_j)
# get rotational absolute velocity of body i, j
dtheta_i = q2dtheta_i(q, self.body_id_i)
dtheta_j = q2dtheta_i(q, self.body_id_j)
# evaluate h_i
hi = self.evaluate_hi(q)
# evaluate distance between
rijP = self.evaluate_rijP(q)
Q_d_21 = 2 * theta_i * np.dot(np.dot(A_theta_matrix(theta_i), hi), (dR_i - dR_j))
Q_d_22 = (dtheta_i ** 2) * (np.dot(self.u_iP_LCS, self.u_iQ_LCS) - np.dot(rijP, hi))
Q_d_23 = 2 * dtheta_i * dtheta_j * np.dot(np.dot(A_theta_matrix(theta_i), hi), np.dot(A_theta_matrix(theta_j), self.u_jP_LCS))
Q_d_24 = (dtheta_j ** 2) * np.dot(hi, self.u_jP_LCS)
Q_d[0] = -Q_d_21 - Q_d_22 + Q_d_23 - Q_d_24
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 (example 3.14) 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
"""
Q_d = np.zeros(2)
# get theta i, j
theta_i = q2theta_i(q, self.body_id_i)
theta_j = q2theta_i(q, self.body_id_j)
# get translational absolute velocity of body - R i, j
dR_i = q2dR_i(q, self.body_id_i)
dR_j = q2dR_i(q, self.body_id_j)
# get rotational absolute velocity of body i, j
dtheta_i = q2dtheta_i(q, self.body_id_i)
dtheta_j = q2dtheta_i(q, self.body_id_j)
# evaluate h_i
hi = self.evaluate_hi(q)
# evaluate distance between
rijP = self.evaluate_rijP(q)
Q_d_21 = 2 * theta_i * np.dot(np.dot(A_theta_matrix(theta_i), hi), (dR_i - dR_j))
Q_d_22 = (dtheta_i ** 2) * (np.dot(self.u_iP_LCS, self.u_iQ_LCS) - np.dot(rijP, hi))
Q_d_23 = 2 * dtheta_i * dtheta_j * np.dot(np.dot(A_theta_matrix(theta_i), hi), np.dot(A_theta_matrix(theta_j), self.u_jP_LCS))
Q_d_24 = (dtheta_j ** 2) * np.dot(hi, self.u_jP_LCS)
Q_d[0] = -Q_d_21 - Q_d_22 + Q_d_23 - Q_d_24
return Q_d
def evaluate_Q_d(self, q):
"""
Function is defined in subclass
:param q:
:return:
"""
Q_d = np.zeros(self.n_CNC)
u_P_i = self.u_P_LCS_list[self.body_id_list.index(self.body_id_i)]
theta_i = q2theta_i(q, self.body_id_i)
Q_d_i = Ai_ui_P_vector(u_P_i, theta_i) * (q2dtheta_i(
q, self.body_id_i)**2)
Q_d_j = np.zeros(self.n_CNC)
Q_d = Q_d_i - Q_d_j
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