def __init__(self, name, MBD_folder_abs_path=None, density=0, volume=0, mass=0, J_zz=0, CM_CAD_LCS=np.zeros(3), CAD_LCS_GCS=np.zeros(3), theta=np.zeros(3), dR=np.zeros(3), dtheta=np.zeros(3), color_GL=np.ones(3), transparent_GL=1, visible=True, display_style="filled", connected_to_ground=False, parent=None):
"""
Constructor of body class
:param name: body name (string)
:param MBD_folder_abs_path: absolute path to MBD system project folder
:param density: density of the material of the body
:param volume: volume of the body (as float) in m^3
:param mass: mass of the body (as float) in kg
:param J_zz: mass moment of inertia of a body against z-axis (as float) in kg*m^2
:param CM_CAD_LCS: a vector to mass center of a body in body CAD CS (as array) in m
:param CAD_LCS_GCS: a vector to body CAD CS in GCS of a system (as array) in m
:param theta: orientation angles (numpy array) in degrees
:param dR: a vector of velocities (numpy array) in m/s
:param dtheta: a vector of angular velocities (numpy array) in deg/s
:param color_GL: a color vector (RGB)
:param transparent_GL: transparency (float) a value from 0 to 1
:param visible: true or false
:param display_style: a display style (as string) options: filled, wireframe, points-check last option
:param properties_file: a path to mass and geometry properties data in .dat file (todo)
:param geometry_data_file: a path to geometry .stl or .obj file (todo)
"""
super(Body, self).__init__(name, parent)
# parent
self._parent = parent
# set working directory
if name.lower() == "ground":
self.body_id = -1
else:
# body id
self.body_id = self.__id.next()
# self.body_id = (len(self._parent._children) - 1)
if MBD_folder_abs_path != None:
os.chdir(MBD_folder_abs_path)
else:
print "MBS data folder not found. Define MBS data folder."
# raise IOError, "MBS data folder not found. Define MBS data folder."
# self.body_id = (len(bodies_list) - 1) + 1
# name as string
self._name = name
# check if body is ground
if not name == "ground":
self.mass = mass
self.J_zz = J_zz
else:
# override default geometry and physical properties
self.mass = 0
self.J_zz = 0
# material and geometry properties
self.density = density
self.volume = volume
self.CM_CAD_LCS = CM_CAD_LCS
self.CAD_LCS_GCS = CAD_LCS_GCS
# dynamic properties
# transform with respect to selected CS
# options: CAD, LCS
self.transformCS = "CAD"
self.R = self.CM_CAD_LCS + self.CAD_LCS_GCS
# (initial) coordinates and angles (in degrees)
self.theta = theta
# (initial) translational and rotational velocities
self.dR = dR
self.dtheta = dtheta
# connected to ground
self._connected_to_ground = connected_to_ground
# list of forces
self.forces = []
# list for markers
self.markers = []
# list of contact geometry data (nodes, edges) objects that are created for contact detection
self.contact_geometries = []
# geometry properties
self._geometry_type = None
self._geometry_types = ["line",
".stl",
"lines"] # todo: lines - for complex parametric models
self.geometry = None
self.geometry_filename = None
self.geometry_file_extension = ".stl"
self.z_dim = 0
# opengl visualization properties
self._visible = visible
self.geometry = None
self.VBO_created = False
self._idVisible = False
# AABB tree object assigned to body object as attribute if body is specified to be in contact with other body
self.AABBtree = None
self.AABBtree_created = False
# file properties
if hasattr(self._parent, "_typeInfo"):
if self._parent._typeInfo == "MBDsystem": # ground body object has parent MBD system
self.properties_file_extension = self._parent._project_filetype
elif hasattr(self._parent._parent, "_typeInfo"):
if self._parent._typeInfo == "group": # ground body object has parent MBD system
self.properties_file_extension = self._parent._parent._data_filetype
else:
print 'Data file not found for body %s' % self._name
self.properties_file_with_extension = self._name + self.properties_file_extension
# opengl properties
# create coordinate systems
# local coordinate system at center of gravity of a body
self.LCS = CoordinateSystem(parent=self)
self.LCS._visible = False
if name.lower() == "ground":
pass
else:
self.color_GL = color_GL
self.transparent_GL = transparent_GL
self.display_style = display_style
# contact properties
self.max_penetration_depth = 1E-7
self.uP_i_max = None
# if body is not ground read body properties data
if name.lower() == "ground":
self._visible = False
self.actual_body_name = "ground"
else:
# read body properties file
if os.path.isfile(self.properties_file_with_extension):
# self.actual_body_name, self.density, self.volume, self.mass, self.J_zz, self.CM_CAD_LCS, self.CAD_LCS_GCS, self.theta, self.dR, self.dtheta, self.body_geometry_filename, self.color_GL, self.transparent_GL, self.display_style = read_body_data_file.read_data(self.properties_file_with_extension)
_dict = read_body_data_file.read_data(self.properties_file_with_extension)
self.add_attributes_from_dict(_dict)
# check if both files exist
if not os.path.isfile(self.properties_file_with_extension):
raise IOError, "Properties file not found!"
if self._geometry_type == self.geometry_file_extension and self.geometry_filename is not None:
if not os.path.isfile(self.geometry_filename):
raise IOError, "Geometry file %s not found!"%self.geometry_filename
# self.CM_CAD_LCS_ = self.CM_CAD_LCS
# print "self.CM_CAD_LCS ="
# print self.CM_CAD_LCS
#
# print "self.CAD_LCS_GCS ="
# print self.CAD_LCS_GCS
# additional translation due to rotation with respect to CAD CS
self.CM_CAD_LCS[0:2] = Ai_ui_P_vector(self.CM_CAD_LCS[0:2], 0)#self.theta[2]
# print "self.CAD_LCS_GCS =", self.CAD_LCS_GCS
# print "self.theta[2] =", self.theta[2]
__dR = self.CM_CAD_LCS - Ai_ui_P_vector(self.CM_CAD_LCS, self.theta[2])#np.zeros(3)#
# print "__dR =", __dR
if all(self.CM_CAD_LCS == np.zeros(3)) and all(self.CAD_LCS_GCS == np.zeros(3)):
pass
else:
self.R = self.CM_CAD_LCS + self.CAD_LCS_GCS - __dR
# print "self.R ="
# print self.R
# cad coordinate system of geometry
self.CAD_CS = Marker(-self.CM_CAD_LCS, parent=self)#-self.CM_CAD_LCS-self.CAD_LCS_GCS
self.CAD_CS._visible = False
# create geometry object
# read geometry file and save vertices and normals
if self.geometry_filename is not None:
if os.path.isfile(self.geometry_filename):
self.geometry = Geometry(self.geometry_filename, parent=self)
# get extension
self._geometry_type = os.path.splitext(self.geometry_filename)[1]
elif self._geometry_type == "line":
self.geometry = Line(parent=self)
else:
if self.geometry is None:
print "Body geometry file %s not found! Attribute self.geometry for body %s not created."%(self.geometry_filename, self._name)
# add additional attributes to geometry object
_dict_geometry = extract_from_dictionary_by_string_in_key(_dict, "geometry.")
if self.geometry is not None and _dict_geometry:
self.geometry.add_attributes_from_dict(_dict_geometry)
class Body(BodyItem):
"""
classdocs
"""
__id = itertools.count(0)
def __init__(self, name, MBD_folder_abs_path=None, density=0, volume=0, mass=0, J_zz=0, CM_CAD_LCS=np.zeros(3), CAD_LCS_GCS=np.zeros(3), theta=np.zeros(3), dR=np.zeros(3), dtheta=np.zeros(3), color_GL=np.ones(3), transparent_GL=1, visible=True, display_style="filled", connected_to_ground=False, parent=None):
"""
Constructor of body class
:param name: body name (string)
:param MBD_folder_abs_path: absolute path to MBD system project folder
:param density: density of the material of the body
:param volume: volume of the body (as float) in m^3
:param mass: mass of the body (as float) in kg
:param J_zz: mass moment of inertia of a body against z-axis (as float) in kg*m^2
:param CM_CAD_LCS: a vector to mass center of a body in body CAD CS (as array) in m
:param CAD_LCS_GCS: a vector to body CAD CS in GCS of a system (as array) in m
:param theta: orientation angles (numpy array) in degrees
:param dR: a vector of velocities (numpy array) in m/s
:param dtheta: a vector of angular velocities (numpy array) in deg/s
:param color_GL: a color vector (RGB)
:param transparent_GL: transparency (float) a value from 0 to 1
:param visible: true or false
:param display_style: a display style (as string) options: filled, wireframe, points-check last option
:param properties_file: a path to mass and geometry properties data in .dat file (todo)
:param geometry_data_file: a path to geometry .stl or .obj file (todo)
"""
super(Body, self).__init__(name, parent)
# parent
self._parent = parent
# set working directory
if name.lower() == "ground":
self.body_id = -1
else:
# body id
self.body_id = self.__id.next()
# self.body_id = (len(self._parent._children) - 1)
if MBD_folder_abs_path != None:
os.chdir(MBD_folder_abs_path)
else:
print "MBS data folder not found. Define MBS data folder."
# raise IOError, "MBS data folder not found. Define MBS data folder."
# self.body_id = (len(bodies_list) - 1) + 1
# name as string
self._name = name
# check if body is ground
if not name == "ground":
self.mass = mass
self.J_zz = J_zz
else:
# override default geometry and physical properties
self.mass = 0
self.J_zz = 0
# material and geometry properties
self.density = density
self.volume = volume
self.CM_CAD_LCS = CM_CAD_LCS
self.CAD_LCS_GCS = CAD_LCS_GCS
# dynamic properties
# transform with respect to selected CS
# options: CAD, LCS
self.transformCS = "CAD"
self.R = self.CM_CAD_LCS + self.CAD_LCS_GCS
# (initial) coordinates and angles (in degrees)
self.theta = theta
# (initial) translational and rotational velocities
self.dR = dR
self.dtheta = dtheta
# connected to ground
self._connected_to_ground = connected_to_ground
# list of forces
self.forces = []
# list for markers
self.markers = []
# list of contact geometry data (nodes, edges) objects that are created for contact detection
self.contact_geometries = []
# geometry properties
self._geometry_type = None
self._geometry_types = ["line",
".stl",
"lines"] # todo: lines - for complex parametric models
self.geometry = None
self.geometry_filename = None
self.geometry_file_extension = ".stl"
self.z_dim = 0
# opengl visualization properties
self._visible = visible
self.geometry = None
self.VBO_created = False
self._idVisible = False
# AABB tree object assigned to body object as attribute if body is specified to be in contact with other body
self.AABBtree = None
self.AABBtree_created = False
# file properties
if hasattr(self._parent, "_typeInfo"):
if self._parent._typeInfo == "MBDsystem": # ground body object has parent MBD system
self.properties_file_extension = self._parent._project_filetype
elif hasattr(self._parent._parent, "_typeInfo"):
if self._parent._typeInfo == "group": # ground body object has parent MBD system
self.properties_file_extension = self._parent._parent._data_filetype
else:
print 'Data file not found for body %s' % self._name
self.properties_file_with_extension = self._name + self.properties_file_extension
# opengl properties
# create coordinate systems
# local coordinate system at center of gravity of a body
self.LCS = CoordinateSystem(parent=self)
self.LCS._visible = False
if name.lower() == "ground":
pass
else:
self.color_GL = color_GL
self.transparent_GL = transparent_GL
self.display_style = display_style
# contact properties
self.max_penetration_depth = 1E-7
self.uP_i_max = None
# if body is not ground read body properties data
if name.lower() == "ground":
self._visible = False
self.actual_body_name = "ground"
else:
# read body properties file
if os.path.isfile(self.properties_file_with_extension):
# self.actual_body_name, self.density, self.volume, self.mass, self.J_zz, self.CM_CAD_LCS, self.CAD_LCS_GCS, self.theta, self.dR, self.dtheta, self.body_geometry_filename, self.color_GL, self.transparent_GL, self.display_style = read_body_data_file.read_data(self.properties_file_with_extension)
_dict = read_body_data_file.read_data(self.properties_file_with_extension)
self.add_attributes_from_dict(_dict)
# check if both files exist
if not os.path.isfile(self.properties_file_with_extension):
raise IOError, "Properties file not found!"
if self._geometry_type == self.geometry_file_extension and self.geometry_filename is not None:
if not os.path.isfile(self.geometry_filename):
raise IOError, "Geometry file %s not found!"%self.geometry_filename
# self.CM_CAD_LCS_ = self.CM_CAD_LCS
# print "self.CM_CAD_LCS ="
# print self.CM_CAD_LCS
#
# print "self.CAD_LCS_GCS ="
# print self.CAD_LCS_GCS
# additional translation due to rotation with respect to CAD CS
self.CM_CAD_LCS[0:2] = Ai_ui_P_vector(self.CM_CAD_LCS[0:2], 0)#self.theta[2]
# print "self.CAD_LCS_GCS =", self.CAD_LCS_GCS
# print "self.theta[2] =", self.theta[2]
__dR = self.CM_CAD_LCS - Ai_ui_P_vector(self.CM_CAD_LCS, self.theta[2])#np.zeros(3)#
# print "__dR =", __dR
if all(self.CM_CAD_LCS == np.zeros(3)) and all(self.CAD_LCS_GCS == np.zeros(3)):
pass
else:
self.R = self.CM_CAD_LCS + self.CAD_LCS_GCS - __dR
# print "self.R ="
# print self.R
# cad coordinate system of geometry
self.CAD_CS = Marker(-self.CM_CAD_LCS, parent=self)#-self.CM_CAD_LCS-self.CAD_LCS_GCS
self.CAD_CS._visible = False
# create geometry object
# read geometry file and save vertices and normals
if self.geometry_filename is not None:
if os.path.isfile(self.geometry_filename):
self.geometry = Geometry(self.geometry_filename, parent=self)
# get extension
self._geometry_type = os.path.splitext(self.geometry_filename)[1]
elif self._geometry_type == "line":
self.geometry = Line(parent=self)
else:
if self.geometry is None:
print "Body geometry file %s not found! Attribute self.geometry for body %s not created."%(self.geometry_filename, self._name)
# add additional attributes to geometry object
_dict_geometry = extract_from_dictionary_by_string_in_key(_dict, "geometry.")
if self.geometry is not None and _dict_geometry:
self.geometry.add_attributes_from_dict(_dict_geometry)
# construct and display body geometry from stl data file
# if body_name != "ground":
# # check if opengl is running without errors and creates VBOs
# try:
# #if glGetError() == GL_NO_ERROR:
# self.create_VBO()
#
# except:
# self.create_VBO()
# ValueError
# logging.getLogger("DyS_logger").error("OpenGL error - is geometry created and displayed?")
# raise
def add_attributes_from_dict(self, dict):
"""
:return:
"""
for key in dict:
val = dict[key]
if key == "theta" and self._parent._parent._angle_units == "deg":
val = np.deg2rad(val)
setattr(self, key, val)
def _evaluate_uP_i_max(self):
"""
Function finds max coordinate of a point of body geometry in x and y axis
:return:
"""
self.uP_i_max = np.zeros(3)
if self.geometry is not None:
if self._geometry_type == ".stl":
self.uP_i_max = np.amax(abs(self.geometry.geom_data.vertices), axis=0)
elif self._geometry_type == "line":
self.uP_i_max = np.amax(abs(self.geometry.vertices), axis=0)
else:
print "uPmax not evaluated"
def get_uP_i_max(self):
"""
:return:
"""
self._evaluate_uP_i_max()
return self.uP_i_max
def _show(self):
"""
"""
if self._visible:
self._visible = False
else:
self._visible = True
def _showID(self):
"""
:return:
"""
if self._idVisible:
self._idVisible = False
else:
self._idVisible = True
def _show_AABB(self):
"""
:return:
"""
if self.AABBtree._visible:
self.AABBtree._visible = False
else:
self.AABBtree._visible = True
def get_q(self):
"""
:return:
"""
theta = self.theta[2]
# check angle units for display
if self._parent._parent._angle_units == "deg":
theta = np.rad2deg(theta)
print np.append(self.R[0:2], theta)
def get_dq(self):
"""
:return:
"""
print np.append(self.dR[0:2], self.dtheta[2])
def get_qdq(self):
"""
:return:
"""
print np.array([np.append(self.R[0:2], self.theta[2]), np.append(self.dR[0:2], self.dtheta[2])]).flatten()
def _update_VBO(self):
"""
"""
self.geometry._update_VBO_color(self.color_GL, self.transparent_GL)
def create_VBO(self):
"""
Method is called after the opengl is initialised for each body to construct opengl VBO.
"""
# print "=================================="
# print "create_VBO() =", self._name, "id =", self.body_id
# construct a body shape OpenGL object - VBO
if self.geometry is not None:
self.geometry.create_VBO()
# is buffer - status if VBO is created and can be displayed
self.VBO_created = True
# create body LCS as vbo
self.LCS._create_VBO()
# # create body CAD LCS vbo
self.CAD_CS._create_VBO()
# print "self.markers =", self.markers
# for marker in self.markers:
# pprint(vars(marker))
# marker._create_VBO()
# print "node =", marker.node
# marker = self.markers[1]
# if self.body_id == 0:
# print "node =", marker.node
# marker._create_VBO()
for contact_geometry in self.contact_geometries:
contact_geometry.create_VBO()
#
if not self.AABBtree_created and self.AABBtree is not None:
self.AABBtree.create_VBO_tree()
self.AABBtree_created = True
self.AABBtree._visible = False
self.AABBtree.create_VBO()
def _paintGL_VBO_AABBtree(self, shader_program):
"""
"""
if self.AABBtree is not None and self.AABBtree._visible and self.AABBtree._VBO_created:
shader_program.bind()
color_location = shader_program.uniformLocation("vertex_color")
shader_program.setUniformValue(color_location, QtGui.QVector3D(self.AABBtree.color_GL[0], self.AABBtree.color_GL[1], self.AABBtree.color_GL[2])) # cg.cgtypes.vec3(body.AABBtree.color_GL) ctypes.c_floatQVector3D(1, 0, 0)
self.AABBtree.paintGL_VBO_tree()
glUseProgram(0)
def paintGL_VBO(self, step=None, shader=None):
"""
Paint body VBO
"""
# translations
glTranslatef(self.R[0], self.R[1], self.R[2])
# rotations
glRotatef(np.rad2deg(self.theta[2]), 0, 0, 1)
glRotatef(np.rad2deg(self.theta[0]), 1, 0, 0)
glRotatef(np.rad2deg(self.theta[1]), 0, 1, 0)
self._paintGL_VBO_AABBtree(shader)
# for contact_geometry in self.contact_geometries:
# contact_geometry.paintGL_VBO()
if self._visible:
for force in self.forces:
force._paint_GL(step)
if self.LCS._visible and self.LCS._VBO_created:
self.LCS._paintGL_VBO()
if hasattr(self, "CAD_CS"):
if self.CAD_CS._visible and self.CAD_CS._VBO_created:
self.CAD_CS._paintGL_VBO()
# display markers of a body
for marker in self.markers:
if marker._VBO_created and marker._visible:
try:
marker._paintGL_VBO()
except:
pass
else:
marker._create_VBO()
if self.VBO_created:
try:
self.geometry._paint_VBO()
except:
raise UserWarning, "VBO object created but not displayed!"
# print "buffer check(body) =", glIsBuffer(self.geometry.vbo_id)
# for contact_geometry in self.contact_geometries:
# contact_geometry.paintGL_VBO()
# for marker in self.markers:
# if marker._VBO_created and marker._visible:
# marker._paintGL_VBO()
# else:
# marker._create_VBO()
def update_coordinates_and_angles_2D(self, q_i):
"""
Function updates coordinates Rx, Ry and angle theta and can be displayed in opengl widget
Args:
q_i - vector of coordinates of i-th body
Returns:
none
"""
self.R[0:2] = q_i[0:2]
self.theta[2] = q_i[2]
def update_velocities_2D(self, dq):
"""
Function updates velocities (translational and rotational) dRx, dRy and angle dtheta and can be displayed in opengl widget
Args:
dq - vector of velocities
Returns:
none
"""
self.dR[0:2] = dq[0:2]
self.dtheta[2] = dq[2]
def delete_VBO(self):
self.geometry.__del__()
# if self.AABBtree != None:
# for _AABBtree in self.AABBtree.children:
# _AABBtree.__del__()
def mechanical_energy(self, q=None, dq=None, gravity=None):
"""
Function evaluates mechanical energy of the body
:return:
"""
if q is None:
R = self.R[0:2]
else:
R = q[0:2]
if dq is None:
dR = self.dR[0:2]
dtheta = self.dR[2]
else:
dR = dq[0:2]
dtheta = dq[2]
if gravity is None:
g = 0
else:
g = gravity
# mechanical energy (kinematic and potential)
_energy = 0.5 * (self.mass * (np.linalg.norm(dR)**2) + self.J_zz * (dtheta**2)) + (self.mass * gravity * R[1])
return _energy
