def __init__(self, _type, body, area=None, filename=None, uP1=None, uP2=None, parent=None):
"""
Constructor of contact geometry object
This object has data to be used when performing evaluation if contact between two geometries is present
The type of geometry is divided into three types:
(sub)area - if type is area than only a subarea part of entire body geometry is processed and nodes and normals are stored for contact calculation
edge - if type ie edge than only two points on a body have to be defined an extra option is to simulate a surface roughness of an edge
body - if type is body than the entire body geometry (data) is used to calculate contact data
all of this is valid at predefined z dimension (value) as we only work with 2D planar dynamics
_type - area, edge, body
"""
# parent is contact object
self._parent = parent
# type
self._type = _type
# pointer to body
self.body = body
# predefined empty list of
# nodes
self.contact_nodes = []
# normals
self.contact_normals = []
# edge objects
self.contact_profile = []
# position of contact (plane) in z dimension
self.z_dim = self._parent.z_dim
# in body coordinates
self.z_dim_lcs = gcs2lcs_z_axis(body.R[2], self.z_dim)
# max penetration depth
self.max_penetration_depth = self.body.max_penetration_depth
if self._type == "area":
self._area_contact_profile(area)
if self._type == "edge":
self._edge_contact_profile(uP1, uP2)
if self._type == "body":
self._body_contour_contact_profile()
# # create 2D profile
# self.construct_contact_profile_2D(self.z_dim_lcs, self._min, self._max)
#
# if filename is not None and uP1 is not None and uP2 is not None:
# self._load_contact_profile(filename)
#
# # calculate direction of edge vector
# self._edge = uP2 - uP1
# opengl properties
self.VBO_created = False
self._visible = True
def __init__(self, nodes_LCS=[], normals_LCS=[], min_LCS=None, max_LCS=None, z_dim=None, parent_body=None, id_sub=1, parent=None, visibility=False, _type=None):
# super(AABBItem, self).__init__(name="AABB" + parent._name + "_body=", parent=parent)
"""
Constructor of BB (bounding box) from nodes of a geometry
Args:
nodes_LCS - nodes body matrix (columns = 3, rows = N, dtype = numpy array) in local coordinate system of a body
normals_LCS - normals body matrix (columns = 3, rows = N, dtype = numpy array)
min_LCS -
max_LCS -
z_dim - dimension in z direction (a plane) where can be contact between two bodies
parent_body - a pointer to body object that this AABB item is part of AABB tree of a body
id_sub -
parent -
visibility -
Constructs:
tree of BB objects
"""
self._parent = parent
self.children = []
self._typeInfo = "aabb"
self._parent_body = parent_body
self.id_sub = id_sub
self.id = self.__id.next()
self._type = _type
# nodes in AABB as empty list - predefined empty list
# self.len_nodes, self.nD = np.shape(nodes_LCS)
# # 2D
# if self.nD != 3:
# # add zeros to third column - z component
# self.nodes_LCS = np.c_[nodes_LCS, np.zeros([self.len_nodes, 1])]
# if normals_LCS != []:
# print np.shape(normals_LCS)
# self.normals_LCS = np.c_[normals_LCS, np.zeros([self.len_nodes, 1])]
# else:
# self.normals_LCS = normals_LCS
#
# # 3D
# else:
self.nodes_LCS = nodes_LCS
self.normals_LCS = normals_LCS
self.min_LCS = min_LCS
self.max_LCS = max_LCS
self.nodes_LCS_in_AABB = []
self.normals_LCS_in_AABB = []
self.nodes_GCS_in_AABB = []
self.normals_GCS_in_AABB = []
self._visible = False
self._VBO_created = False
# print "self._parent._typeInfo =", self._parent._typeInfo
# print "self._parent_body._typeInfo =", self._parent_body._typeInfo
# this if is only for the example with matplotlib display of AABB
if self._parent==None and self._parent_body==None:
self.level = self.__level
self.id_path = [1]
self.body_id = 0
self.max_nodes_LCS_in_AABB = 3
self.min_dimensions_of_AABB = .0001
self.max_level = 6
self.z_dim = 0
# when building AABBtree from contact - at first step
elif self._parent._typeInfo == "contact" and self._parent_body._typeInfo == "body":
self.level = self.__level
self.id_path = [1]
self.body_id = self._parent_body.body_id
self.min_dimensions_of_AABB = 0.0001
self.max_level = 6
self._parent_body = parent_body
self.max_nodes_LCS_in_AABB = 3
# self.z_dim = self._parent.z_dim
# print "************************************************"
# print self.nodes_LCS
# print "self._parent_body._name =", self._parent_body._name
# print "self._parent.z_dim =", self._parent.z_dim
# print "self._parent_body.R[2] =", self._parent_body.R[2]
# print "self._parent_body.CM_CAD_LCS[2] =", self._parent_body.CM_CAD_LCS[2]
# print "self._parent_body.CAD_LCS_GCS[2] =", self._parent_body.CAD_LCS_GCS[2]
self.z_dim = gcs2lcs_z_axis(+self._parent_body.CM_CAD_LCS[2], self._parent.z_dim)
# self.z_dim = self._parent.z_dim
# print "self.z_dim =", self.z_dim
# np.savetxt(self._parent_body._name+"_LCS_2D.txt", self.nodes_LCS)
# when building AABBtree from contact - each step after first in recursion
elif self._parent._typeInfo == "aabb" and self._parent_body == None:
self.level = self._parent.level + 1
__id_path = copy.copy(self._parent.id_path)
self.id_path = __id_path
self.id_path.append(id_sub)
self.max_nodes_LCS_in_AABB = self._parent.max_nodes_LCS_in_AABB
self.min_dimensions_of_AABB = self._parent.min_dimensions_of_AABB
self.max_level = self._parent.max_level
self._parent_body = self._parent
self.id_sub = id_sub
self._divided_parent_list = self._parent.children
self._parent_body = self._parent._parent_body
self.z_dim = self._parent.z_dim
# self.z_dim = gcs2lcs_z_axis(self._parent_body.CM_CAD_LCS[2], self._parent.z_dim)
else:
raise ValueError, "None of the conditions are met!"
# print "level-id =", self.level,"-",self.id
# first AABB root object of each geometry is first constructed and than additional parameters are assigned to this object
# if additional parameters are defined in data files they are assigned to this root object at level 1
# at level 1 method construct() is called explicitly, when building lower AABBs (higher level) method construct() is called implicitly in __init__()
if self.level == 1:
pass
else:
self.construct()
def __init__(self,
_type,
body,
area=None,
filename=None,
uP1=None,
uP2=None,
parent=None):
"""
Constructor of contact geometry object
This object has data to be used when performing evaluation if contact between two geometries is present
The type of geometry is divided into three types:
(sub)area - if type is area than only a subarea part of entire body geometry is processed and nodes and normals are stored for contact calculation
edge - if type ie edge than only two points on a body have to be defined an extra option is to simulate a surface roughness of an edge
body - if type is body than the entire body geometry (data) is used to calculate contact data
all of this is valid at predefined z dimension (value) as we only work with 2D planar dynamics
_type - area, edge, body
"""
# parent is contact object
self._parent = parent
# type
self._type = _type
# pointer to body
self.body = body
# geometry type
self._type = _type
self._types = ["2D", "3D"]
# predefined empty list of
# nodes
self.contact_nodes = []
# normals
self.contact_normals = []
# edge objects
self.contact_profile = []
# position of contact (plane) in z dimension
self.z_dim = self._parent.z_dim
# in body coordinates
self.z_dim_lcs = gcs2lcs_z_axis(body.R[2], self.z_dim)
# max penetration depth
self.max_penetration_depth = self.body.max_penetration_depth
if self._type == "area":
self._area_contact_profile(area)
if self._type == "edge":
self._edge_contact_profile(uP1, uP2)
if self._type == "body":
self._body_contour_contact_profile()
# # create 2D profile
# self.construct_contact_profile_2D(self.z_dim_lcs, self._min, self._max)
#
# if filename is not None and uP1 is not None and uP2 is not None:
# self._load_contact_profile(filename)
#
# # calculate direction of edge vector
# self._edge = uP2 - uP1
# visualization properties
self._visible = True
self.vtk_actor = None
self.scale = 1
def __init__(self, nodes_LCS=[], normals_LCS=[], min_LCS=None, max_LCS=None, z_dim=None, parent_body=None, id_sub=1, parent=None, visibility=False, _type=None):
# super(AABBItem, self).__init__(name="AABB" + parent._name + "_body=", parent=parent)
"""
Constructor of BB (bounding box) from nodes of a geometry
Args:
nodes_LCS - nodes body matrix (columns = 3, rows = N, dtype = numpy array) in local coordinate system of a body
normals_LCS - normals body matrix (columns = 3, rows = N, dtype = numpy array)
min_LCS -
max_LCS -
z_dim - dimension in z direction (a plane) where can be contact between two bodies
parent_body - a pointer to body object that this AABB item is part of AABB tree of a body
id_sub -
parent -
visibility -
Constructs:
tree of BB objects
"""
self._parent = parent
self.children = []
self._typeInfo = "aabb"
self._parent_body = parent_body
self.id_sub = id_sub
self.id = self.__id.next()
self._type = _type
# nodes in AABB as empty list - predefined empty list
self.len_nodes, self.nD = np.shape(nodes_LCS)
if self.nD != 3:
# add zeros to third column - z component
self.nodes_LCS = np.c_[nodes_LCS, np.zeros([self.len_nodes, 1])]
if normals_LCS != []:
self.normals_LCS = np.c_[normals_LCS, np.zeros([self.len_nodes, 1])]
else:
self.normals_LCS = normals_LCS
else:
self.nodes_LCS = nodes_LCS
self.normals_LCS = normals_LCS
self.min_LCS = min_LCS
self.max_LCS = max_LCS
self.nodes_LCS_in_AABB = []
self.normals_LCS_in_AABB = []
self.nodes_GCS_in_AABB = []
self.normals_GCS_in_AABB = []
self._visible = False
self._VBO_created = False
# this if is only for the example with matplotlib display of AABB
if self._parent == None and self._parent_body == None:
self.level = self.__level
self.id_path = [1]
self.body_id = 0
self.max_nodes_LCS_in_AABB = 3
self.min_dimensions_of_AABB = .001
self.max_level = 6
self.z_dim = 0
# when building AABBtree from contact - at first step
elif self._parent._typeInfo == "contact" and self._parent_body._typeInfo == "body":
self.level = self.__level
self.id_path = [1]
self.body_id = self._parent_body.body_id
self.min_dimensions_of_AABB = None
self.max_level = None
self._parent_body = parent_body
# self.z_dim = self._parent.z_dim
# print "************************************************"
# print self.nodes_LCS
# print "self._parent_body._name =", self._parent_body._name
# print "self._parent.z_dim =", self._parent.z_dim
# print "self._parent_body.R[2] =", self._parent_body.R[2]
# print "self._parent_body.CM_CAD_LCS[2] =", self._parent_body.CM_CAD_LCS[2]
# print "self._parent_body.CAD_LCS_GCS[2] =", self._parent_body.CAD_LCS_GCS[2]
self.z_dim = gcs2lcs_z_axis(+self._parent_body.CM_CAD_LCS[2], self._parent.z_dim)
# self.z_dim = self._parent.z_dim
# print "self.z_dim =", self.z_dim
np.savetxt(self._parent_body._name+"_LCS_2D.txt", self.nodes_LCS)
# when building AABBtree from contact - each step after first in recursion
elif self._parent._typeInfo == "aabb" and self._parent_body == None:
self.level = self._parent.level + 1
__id_path = copy.copy(self._parent.id_path)
self.id_path = __id_path
self.id_path.append(id_sub)
self.max_nodes_LCS_in_AABB = self._parent.max_nodes_LCS_in_AABB
self.min_dimensions_of_AABB = self._parent.min_dimensions_of_AABB
self.max_level = self._parent.max_level
self._parent_body = self._parent
self.id_sub = id_sub
self._divided_parent_list = self._parent.children
self._parent_body = self._parent._parent_body
self.z_dim = self._parent.z_dim
# self.z_dim = gcs2lcs_z_axis(self._parent_body.CM_CAD_LCS[2], self._parent.z_dim)
else:
raise ValueError, "None of the conditions are met!"
# print "level-id =", self.level,"-",self.id
# first AABB root object of each geometry is first constructed and than additional parameters are assigned to this object
# if additional parameters are defined in data files they are assigned to this root object at level 1
# at level 1 method construct() is called explicitly, when building lower AABBs (higher level) method construct() is called implicitly in __init__()
if self.level == 1:
pass
else:
self.construct()
def __init__(self, _name, spring_type, body_id_i, body_id_j, u_iP_CAD, u_jP_CAD, k=0, c=0, l_0=None, parent = None):
"""
Constructor of a spring class
:param :
_name - string
spring_type - translational, rotational
k - spring stiffness
c - damping coefficient
body_i -
body_j -
u_iP - in CAD LCS of a body
u_jP - in CAD LCS of a body
l_0 - undeformed length of a spring
body_id - number of body on which the force is applied
F_x - x component of force
F_y . y component of force
u_iP_f - vector of acting force in CAD LCS of a body
"""
super(Spring, self).__init__(_name, parent)
# parent
self._parent = parent
# number
self.spring_id = self.__id.next()
# name as string
self._name = _name
# spring type
self.spring_type = spring_type
# position of spring in GCS
self.z_dim = 0
# physical properties
self.k = k
self.c = c
if self.spring_type == "translational":
# spring linear - params
self.l_0 = l_0
elif self.spring_type == "torsional":
# spring torsional - params
# self.k_t = k_t
# self.c_t = c_t
pass
# swap body_id that if body is connected to ground that ground is always the last item in list
if body_id_i == "ground":
self.body_id_i = body_id_j
self.body_id_j = body_id_i
self.u_iP_CAD = u_jP_CAD
self.u_jP_CAD = u_iP_CAD
else:
self.body_id_i = body_id_i
self.body_id_j = body_id_j
self.u_iP_CAD = u_iP_CAD
self.u_jP_CAD = u_jP_CAD
self.body_id_list = [self.body_id_i, self.body_id_j]
# list of point vectors to joint constraint in CAD lCS of a body
self.u_P_CAD_list = [self.u_iP_CAD, self.u_jP_CAD]
# predefined empty list to store point vectors of joint constraint in LCS (center of gravity)
# of each body
self.u_P_list = []
self.signs = np.array([-1, +1])
# predefined empty list of spring force matrix to store one force matrix for each body connected with a spring
self.spring_force_matrix_list = []
# pair of contact force list
self.force_list = []
self.markers = []
for body_id, _u_P in zip(self.body_id_list, self.u_P_CAD_list):
if body_id == "ground" or body_id == -1:
u_P_LCS = u_P_cad2cm_lcs(body_id, self._parent._parent.ground, _u_P=_u_P)
_body = self._parent._parent.ground
else:
# create pointer to body
_body = self._parent._parent.bodies[body_id]
# calculate point vector in body LCS (center of gravity)
u_P_LCS = u_P_cad2cm_lcs(body_id, _body, _u_P=_u_P)
# create force object
_force = Force(body_id, force_name=self._name + "_on_body_" + str(body_id))
# add pair of contact forces to forces list of MBD system
self._parent._parent.forces.append(_force)
# add pair of contact forces to forces list of spring
self.force_list.append(_force)
self._parent._parent.bodies[body_id].forces.append(_force)
# create markers for u_P points on every body in kinematic constraint
z_dim_lcs = gcs2lcs_z_axis(_body.R[2], self.z_dim)
_node = np.array(np.append(u_P_LCS, z_dim_lcs), dtype='float32')
_marker = Marker(_node, visible=True, parent=_body)
if body_id == "ground" or body_id == -1:
self._parent._parent.ground.markers.append(_marker)
else:
self._parent._parent.bodies[body_id].markers.append(_marker)
self.markers.append(_marker)
self.u_P_list.append(u_P_LCS)
[self.u_iP, self.u_jP] = self.u_P_list
self.N_b_ = len(self._parent._parent.bodies)
self.additional_params_calulated = True
