def evaluate_CM(self, q):
"""
:param q:
:return:
"""
e_i = q2q_body(q, self.body_id)
self.CM = self.mesh.evaluate_CM(e_i)
return self.CM
def e_node(self, q, node_id):
"""
:param node_id:
:return:
"""
e_i = q2q_body(q, self.body_id)
e_node = self.mesh.e_node(e_i, node_id)
return e_node
def _update_time_dependent_variables(self, t, q):
"""
Returns:
"""
self.q = q2q_body(q, self.body_id)
# update variables of a mesh
self.mesh._update_time_dependent_variables(self.q)
def evaluate_K(self, e=None):
"""
:return:
"""
# vector of ANC of a flexible body
if e is not None:
e = q2q_body(e, self.body_id)
else:
e = self.mesh.evaluate_q()
self.K = self.mesh.evaluate_K(e)
return self.K
def evaluate_q2q_body(self, q):
"""
Check if input vector of q is equal to mesh vector
:param q:
:return:
"""
#
if len(q) == self.q_i_size:
q_b = q
else:
q_b = q2q_body(q, self.body_id)
return q_b
def evaluate_Q_s(self, q):
"""
Function evaluates elastic strain forces of flexible body (mesh, structure)
:param q_i:
:return:
"""
# vector of absolute nodal coordinates of a flexible body
e_i = q2q_body(q, self.body_id)
# Q_s_i = np.zeros_like(e_i)
# stiffness matrix
# self.K = self.evaluate_K(e_i)
# elastic strain forces of body flexible body i
# Q_s_i = np.dot(self.K, e_i)
Q_s_i = self.mesh.evaluate_Q_s(e_i)
return Q_s_i
def evaluate_Q_g(self, q=None, gravity=None):
"""
Function evaluates vector of generalized distributed gravity forces of a flexible body
:return:
"""
if gravity is None:
gravity = self._parent._parent.gravity
else:
gravity = gravity[0:2]
if q is None:
q_i = self.evaluate_q()
else:
q_i = q2q_body(q, self.body_id)
self.Q_g = self.mesh.evaluate_Q_g(q=q_i, gravity=gravity)
return self.Q_g
def _evaluate_Q_e_flexible_M(self, t, q, M):
"""
Evaluate generalized external force for ANCF flexible body
:param t:
:param q:
:param M:
:return:
"""
if self._element is None:
self._element = self.set_element()
# predefine empty vector
Q_e = np.zeros(self._element.mesh.n_NC)
# get body generalized coordinates
e_b = q2q_body(q, self.body_id)
for i, element in enumerate(self._body.mesh.elements):
if self.node_id in element.node_id_list or self.node_id == -1:
if self.node_id != -1:
ksi = element.node_id_list.index(self.node_id)
else:
ksi = 1
e_i = element.evaluate_e_i(e_b=e_b)
Q_e_i = element.evaluate_Q_e_M(e_i, M, ksi)
# transformed force vector
if element.B is None:
element.evaluate_B()
if element.T is None:
element.evaluate_T()
_Q_e_i = reduce(np.dot, [element.B.T, element.T.T, Q_e_i])
# sum
Q_e += _Q_e_i
return Q_e
def _evaluate_Q_e_flexible_F(self, t, q, F):
"""
:param t:
:param q:
:return:
"""
if self._element is None:
self._element = self.set_element()
# predefine empty vector
Q_e = np.zeros(self._element.mesh.n_NC)
for i, element in enumerate(self._body.mesh.elements):
# this case is executed if force at node id is at boundary nodes of finite element or is the last node
# in the mesh and therefore parameter of finite element parametirization is not needed
if self.node_id in element.node_id_list or self.node_id == -1 and self.element_ksi is None:
# undeformed position
# self.r_P_GCS = self._element.mesh.nodes[self.node_id]
# deformed position (for display I think)
# in case of last node in mesh of nodes
if self.node_id == -1:
node = self._element.geometry_nodes[-1, :]
# in case of any other node except last in a mesh
else:
if self._element.node_id_list.index(self.node_id) == 0:
node = self._element.geometry_nodes[0, :]
elif self._element.node_id_list.index(self.node_id) == 1:
node = self._element.geometry_nodes[-1, :]
else:
node = np.zeros(2)
self.r_P_GCS = node
if self.node_id != -1:
ksi = element.node_id_list.index(self.node_id)
elif self.element_ksi is not None:
ksi = self.element_ksi
else:
ksi = 1
S = element._evaluate_S(ksi)
if (self.node_id == 0) or (self.node_id == len(
self._body.mesh.nodes)):
pass
else:
F = 0.5 * F
Q_e_i = np.dot(S.T, F)
# in other cases
else:
# vector of absolute nodal coordinates of a deformable body - mesh
e_b = q2q_body(q, self.body_id)
# vector of absolute nodal coordinates of a beam element
e_i = self._element.evaluate_e_i(e_b)
# position vector of beam element
self.r_P_GCS = self._element.evaluate_r(self.element_ksi,
e_i=e_i)
if element.element_id == self.element_id:
S = self._element._evaluate_S(self.element_ksi)
Q_e_i = np.dot(S.T, F)
else:
Q_e_i = np.zeros(element.get_e_size())
# transformed force vector
if element.B is None:
element.evaluate_B()
if element.T is None:
element.evaluate_T()
_Q_e_i = reduce(np.dot, [element.B.T, element.T.T, Q_e_i])
# sum
Q_e += _Q_e_i
return Q_e
def _contact_geometry_GCS(self, q):
"""
Function calculates distance between centerpoints and indentation based on radius of pin/hole
:param q:
:return distance_obj: distance object of calculated data in GCS
"""
self.u_CP_GCS_list = self._contact_geometry_CP_GCS(q)
print "self.u_CP_GCS_list =", self.u_CP_GCS_list
# calculate distance between axis of both bodies in revolute joint
self._distance_obj = DistanceRevoluteClearanceJoint(self.u_CP_GCS_list[0], self.u_CP_GCS_list[1], parent=self)
# penetration depth is difference between nominal radial clearance and actual calculated clearance at time t
_distance = self._distance_obj._distance
_delta = self._radial_clearance - _distance
# contact is present and actual contact point has to be evaluated
# if _distance >= self._radial_clearance and abs(_delta) >= self.distance_TOL:
# print "contact is present"
# unit vector in direction from one center to enother (pin to hole)
self._n_GCS = self._distance_obj._distance_vector / self._distance_obj._distance
# print "--------------------------------"
# print "step =", self._step
# print "n =", self._n
# if _delta < 0 and abs(_delta) > self.distance_TOL:
# print "body i =", self.u_CP_list_GCS[0]
# print "body j =", self.u_CP_list_GCS[1]
# print "n =", self._n
# create normal list in LCS
self._n_GCS_list = [-self._n_GCS, +self._n_GCS]
self._n_LCS_list = []
for body_id, _normal in zip(self.body_id_list, self._n_GCS_list):
# normal in LCS
_theta = q2theta_i(q, body_id)
_normal_LCS = uP_gcs2lcs(u_P=_normal, theta=_theta)
# append normal to list
self._n_LCS_list.append(_normal_LCS)
# calculate a actual contact point in revolute clearance joint of each body in GCS
# and vector of contact point in LCS
self.u_P_GCS_list = []
self.u_P_LCS_list = []
# plot = False#[False, self.status==2] #False
# if plot:
# print "*************************"
# fig = plt.gcf()
# plt.xlim([0.0, 0.01])
# plt.ylim([0.0, 0.01])
# ax = plt.gca()
# # ax.relim()
# ax.autoscale_view(True,True,True)
# ax.set_aspect('equal')
self.u_P_LCS_list = []
# evaluate actual contact point in LCS of each body and in GCS
for body_id, _u_CP_GCS, _u_CP_LCS, _R0 in zip(self.body_id_list, self.u_CP_GCS_list, self.u_CP_LCS_list, self.R0_list):
# print "body_id =", body_id
# q of a body
q_body = q2q_body(q, body_id)
# contact point in GCS
_u_P_GCS = _u_CP_GCS + _R0 * self._n_GCS
# contact point in body LCS
_u_P_LCS = gcs2cm_lcs(_u_P_GCS, q_body[0:2], q_body[2])
self.u_P_LCS_list.append(_u_P_LCS)
# if plot:
# print "----------------------------------"
# print "body =", self._parent._parent.bodies[body_id]._name
#
# R = q_body[0:2]
# print "q_body[0:2] =", q_body[0:2]
# print "joint center =", _u_CP_LCS
# print "contact point GCS =", _u_P_GCS
# print "contact point LCS =", _u_P_LCS
# _color = np.random.rand(3)
# circle=plt.Circle((_u_CP_LCS[0]+R[0],_u_CP_LCS[1]+R[1]),_R,color=_color, fill=False)
# # center of axis
# ax.plot(_u_CP_LCS[0], _u_CP_LCS[1], "o", color=_color)
# # contact point
# ax.plot(_u_P_LCS[0]+R[0], _u_P_LCS[1]+R[1], "x", color=_color)
# # LCS
# ax.plot(R[0], R[1], "s", color=_color)
# fig.gca().add_artist(circle)
# transform to 32bit float to display in opengl
self.u_P_GCS_list = np.array(self.u_P_GCS_list, dtype="float32")
self.u_P_LCS_list = np.array(self.u_P_LCS_list, dtype="float32")
# self.u_P_GCS = np.array(_u_P_GCS, dtype="float32")
# if self._step_solution_accepted:
# self._u_P_solution_container.append(self.u_P_list_LCS.flatten())
# if plot:
# plt.grid()
# plt.show()
# fig.savefig('testing.png')
return _distance, _delta