def AddFallingItems(sys):
# Shared contact materials for falling objects
mat = chrono.ChMaterialSurfaceSMC()
# Create falling rigid bodies (spheres and boxes etc.)
for ix in range(-2, 3):
for iz in range(-2, 3):
# add spheres
mass = 1
radius = 1.1
body = chrono.ChBody()
comp = (2.0 / 5.0) * mass * radius**2
body.SetInertiaXX(chrono.ChVectorD(comp, comp, comp))
body.SetMass(mass)
body.SetPos(chrono.ChVectorD(4.0 * ix + 0.1, 4.0, 4.0 * iz))
body.GetCollisionModel().ClearModel()
body.GetCollisionModel().AddSphere(mat, radius)
body.GetCollisionModel().BuildModel()
body.SetCollide(True)
sphere = chrono.ChSphereShape()
sphere.GetSphereGeometry().rad = radius
body.AddAsset(sphere)
texture = chrono.ChTexture()
texture.SetTextureFilename(
chrono.GetChronoDataFile("textures/bluewhite.png"))
body.AddAsset(texture)
sys.AddBody(body)
# add boxes
mass = 1
hsize = chrono.ChVectorD(0.75, 0.75, 0.75)
body = chrono.ChBody()
body.SetMass(mass)
body.SetPos(chrono.ChVectorD(4.0 * ix, 6.0, 4.0 * iz))
body.GetCollisionModel().ClearModel()
body.GetCollisionModel().AddBox(mat, hsize.x, hsize.y, hsize.z)
body.GetCollisionModel().BuildModel()
body.SetCollide(True)
box = chrono.ChBoxShape()
box.GetBoxGeometry().Size = hsize
body.AddAsset(box)
texture = chrono.ChTexture()
texture.SetTextureFilename(
chrono.GetChronoDataFile("textures/pinkwhite.png"))
body.AddAsset(texture)
sys.AddBody(body)
def add_ellisoidShape(MiroSystem, radius_x, radius_y, radius_z, pos, texture='test.jpg', density=1000, scale=[1,1], Collide=True, Fixed=True, rotX=0, rotY=0, rotZ=0, rotOrder=['x','y','z'], rotAngle=0, rotAxis=[1,0,0], rotDegrees=True, color=[0.5, 0.5, 0.5]):
# Convert position to chrono vector, supports using chvector as input as well
ChPos = ChVecify(pos)
ChRotAxis = ChVecify(rotAxis)
# Create a cylinder
body_ball = chrono.ChBodyEasyEllipsoid(chrono.ChVectorD(radius_x, radius_y, radius_z), density)
body_ball.SetBodyFixed(Fixed)
body_ball.SetPos(ChPos)
rotateBody(body_ball, rotX, rotY, rotZ, rotOrder, rotAngle, ChRotAxis, rotDegrees)
# Collision shape
if(Collide):
body_ball.GetCollisionModel().ClearModel()
body_ball.GetCollisionModel().AddEllipsoid(radius_x, radius_y, radius_z) # hemi sizes
body_ball.GetCollisionModel().BuildModel()
body_ball.SetCollide(Collide)
# Body texture
if texture:
# Filter 'textures/' out of the texture name, it's added later
if len(texture) > len('textures/'):
if texture[0:len('textures/')] == 'textures/':
texture = texture[len('textures/'):]
body_texture = chrono.ChTexture()
body_texture.SetTextureFilename(chrono.GetChronoDataFile('textures/'+texture))
body_texture.SetTextureScale(scale[0], scale[1])
body_ball.GetAssets().push_back(body_texture)
if MiroSystem:
ChSystem = MiroSystem.Get_APIsystem()[0]
ChSystem.Add(body_ball)
return body_ball
def AddFallingItems(sys):
# Shared contact materials for falling objects
sph_mat = chrono.ChMaterialSurfaceNSC()
sph_mat.SetFriction(0.2)
box_mat = chrono.ChMaterialSurfaceNSC()
cyl_mat = chrono.ChMaterialSurfaceNSC()
# Create falling rigid bodies (spheres and boxes etc.)
for bi in range(29):
msphereBody = chrono.ChBodyEasySphere(1.1, # radius size
1000, # density
True, # visualization?
True, # collision?
sph_mat) # contact material
msphereBody.SetPos(chrono.ChVectorD(-5 + chrono.ChRandom() * 10, 4 + bi * 0.05, -5 + chrono.ChRandom() * 10))
sys.Add(msphereBody)
mtexture = chrono.ChTexture()
mtexture.SetTextureFilename(chrono.GetChronoDataFile("textures/bluewhite.png"))
msphereBody.AddAsset(mtexture)
mboxBody = chrono.ChBodyEasyBox(1.5, 1.5, 1.5, # x,y,z size
100, # density
True, # visualization?
True, # collision?
box_mat) # contact material
mboxBody.SetPos(chrono.ChVectorD(-5 + chrono.ChRandom() * 10, 4 + bi * 0.05, -5 + chrono.ChRandom() * 10))
sys.Add(mboxBody)
mtexturebox = chrono.ChTexture()
mtexturebox.SetTextureFilename(chrono.GetChronoDataFile("textures/cubetexture_bluewhite.png"))
mboxBody.AddAsset(mtexturebox)
mcylBody = chrono.ChBodyEasyCylinder(0.75, 0.5, # radius, height
100, # density
True, # visualization?
True, # collision?
cyl_mat) # contact material
mcylBody.SetPos(chrono.ChVectorD(-5 + chrono.ChRandom() * 10, 4 + bi * 0.05, -5 + chrono.ChRandom() * 10))
sys.Add(mcylBody)
# optional, attach a texture for better visualization
mtexturecyl = chrono.ChTexture()
mtexturecyl.SetTextureFilename(chrono.GetChronoDataFile("textures/pinkwhite.png"))
mcylBody.AddAsset(mtexturecyl)
def add_boxShape(MiroSystem, size_x, size_y, size_z, pos, texture='test.jpg', scale=[4,3], Collide=True, Fixed=True, rotX=0, rotY=0, rotZ=0, rotOrder=['x','y','z'], rotAngle=0, rotAxis=[1,0,0], rotDegrees=True, mass=False, density=1000, dynamic=False, color=[0.5, 0.5, 0.5]):
'''system, size_x, size_y, size_z, pos, texture, scale = [5,5], hitbox = True/False'''
# Convert position to chrono vector, supports using chvector as input as well
ChPos = ChVecify(pos)
ChRotAxis = ChVecify(rotAxis)
# Create a box
body_box = chrono.ChBody()
body_box.SetBodyFixed(Fixed)
body_box.SetPos(ChPos)
if not mass:
mass = density * size_x * size_y * size_z
inertia_brick_xx = (size_y**2 + size_z**2)*mass/3
inertia_brick_yy = (size_x**2 + size_z**2)*mass/3
inertia_brick_zz = (size_x**2 + size_y**2)*mass/3
body_box.SetMass(mass)
body_box.SetInertiaXX(chrono.ChVectorD(inertia_brick_xx, inertia_brick_yy, inertia_brick_zz))
# Collision shape
body_box.GetCollisionModel().ClearModel()
body_box.GetCollisionModel().AddBox(size_x/2, size_y/2, size_z/2) # hemi sizes
body_box.GetCollisionModel().BuildModel()
body_box.SetCollide(Collide)
# Visualization shape
body_box_shape = chrono.ChBoxShape()
body_box_shape.GetBoxGeometry().Size = chrono.ChVectorD(size_x/2, size_y/2, size_z/2)
body_box_shape.SetColor(chrono.ChColor(0.4,0.4,0.5))
body_box.GetAssets().push_back(body_box_shape)
# Body texture
if texture:
# Filter 'textures/' out of the texture name, it's added later
if len(texture) > len('textures/'):
if texture[0:len('textures/')] == 'textures/':
texture = texture[len('textures/'):]
body_box_texture = chrono.ChTexture()
body_box_texture.SetTextureFilename(chrono.GetChronoDataFile('textures/'+texture))
body_box_texture.SetTextureScale(scale[0], scale[1])
body_box.GetAssets().push_back(body_box_texture)
rotateBody(body_box, rotX, rotY, rotZ, rotOrder, rotAngle, rotAxis, rotDegrees)
if MiroSystem:
ChSystem = MiroSystem.Get_APIsystem()[0]
ChSystem.Add(body_box)
return body_box
def enableContact(self):
self.mbody1.GetCollisionModel().ClearModel()
self.mbody1.GetCollisionModel().AddBox(0.2, 0.5, 0.1) # hemi sizes
self.mbody1.GetCollisionModel().BuildModel()
self.mbody1.SetCollide(True)
self.mbody1.SetMass(20)
self.Body_wheel_L.GetCollisionModel().ClearModel()
self.Body_wheel_L.GetCollisionModel().AddCylinder(0.2, 0.5,
0.1) # hemi sizes
self.Body_wheel_L.GetCollisionModel().BuildModel()
self.Body_wheel_L.SetCollide(True)
self.Body_wheel_L.SetMaterialSurface(self.wheelMaterial)
self.Body_wheel_L.SetMass(1)
self.Body_wheel_L.AddAsset(chrono.ChTexture("Wheel_texture.png"))
self.Body_wheel_R.GetCollisionModel().ClearModel()
self.Body_wheel_R.GetCollisionModel().AddCylinder(0.2, 0.5,
0.1) # hemi sizes
self.Body_wheel_R.GetCollisionModel().BuildModel()
self.Body_wheel_R.SetCollide(True)
self.Body_wheel_R.SetMaterialSurface(self.wheelMaterial)
self.Body_wheel_R.SetMass(1)
self.Body_wheel_R.AddAsset(chrono.ChTexture("Wheel_texture.png"))
def ChangeBodyTexture(body, texture_file, scale=[1,1]):
texture = chrono.ChTexture()
texture.SetTextureFilename(chrono.GetChronoDataFile(texture_file))
texture.SetTextureScale(scale[0], scale[1])
body.AddAsset(texture)
vshape_3 = chrono.ChBoxShape()
vshape_3.GetBoxGeometry().SetLengths(chrono.ChVectorD(0.2, 2, 10.2))
vshape_3.GetBoxGeometry().Pos = chrono.ChVectorD(5, 0, 0)
ground.AddAsset(vshape_3)
vshape_4 = chrono.ChBoxShape()
vshape_4.GetBoxGeometry().SetLengths(chrono.ChVectorD(10.2, 2, 0.2))
vshape_4.GetBoxGeometry().Pos = chrono.ChVectorD(0, 0, -5)
ground.AddAsset(vshape_4)
vshape_5 = chrono.ChBoxShape()
vshape_5.GetBoxGeometry().SetLengths(chrono.ChVectorD(10.2, 2, 0.2))
vshape_5.GetBoxGeometry().Pos = chrono.ChVectorD(0, 0, 5)
ground.AddAsset(vshape_5)
ground.AddAsset(chrono.ChTexture(
chrono.GetChronoDataFile("textures/blue.png")))
# Add obstacle visualization (in a separate level with a different color).
ground.AddAsset(obstacle.GetVisualization())
# Create the falling ball
ball = chrono.ChBody()
sys.AddBody(ball)
ball.SetMass(10)
comp = 4 * ball_radius * ball_radius
ball.SetInertiaXX(chrono.ChVectorD(comp, comp, comp))
ball.SetPos(chrono.ChVectorD(-3, 1.2 * ball_radius, -3))
ball.SetPos_dt(chrono.ChVectorD(5, 0, 5))
ball.SetCollide(True)
ball.GetCollisionModel().ClearModel()
x = xl + (_L1l / 2) * np.cos(θ1l) y = yl + (_L1l / 2) * np.sin(θ1l) L1l.SetPos(chrono.ChVectorD(x, y, .01)) L1l.SetRot(chrono.ChMatrix33D(θ1l, chrono.ChVectorD(0, 0, 1))) #add visualization mesh_for_visualization = chrono.ChTriangleMeshConnected() mesh_for_visualization.LoadWavefrontMesh(assetsPath + '_L1l.obj') meshRotation = chrono.ChMatrix33D(np.pi / 2, chrono.ChVectorD(0, 1, 0)) mesh_for_visualization.Transform(chrono.ChVectorD(0, 0, 0), meshRotation) visualization_shape = chrono.ChTriangleMeshShape() visualization_shape.SetMesh(mesh_for_visualization) L1l.AddAsset(visualization_shape) texture = chrono.ChTexture() texture.SetTextureFilename(assetsPath + 'blue.png') L1l.GetAssets().push_back(texture) #----------- left link 2 ------------------ #add body L2l = chrono.ChBodyAuxRef() L2l.SetBodyFixed(False) mysystem.Add(L2l) #add mass properties //improve these based on actual data... m = .266 + .274 L2l.SetMass(m) #L2l.SetInertiaXX(chrono.ChVectorD(.00005,.02053,.02057)) #from solidworks #set position,orientation with FK
mboxasset = chrono.ChBoxShape()
mboxasset.GetBoxGeometry().Size = chrono.ChVectorD(0.2, 0.5, 0.1)
mbody1.AddAsset(mboxasset)
# Create a swinging rigid body
mbody2 = chrono.ChBody()
mbody2.SetBodyFixed(False)
mysystem.Add(mbody2)
mboxasset = chrono.ChBoxShape()
mboxasset.GetBoxGeometry().Size = chrono.ChVectorD(0.2, 0.5, 0.1)
mbody2.AddAsset(mboxasset)
mboxtexture = chrono.ChTexture()
mboxtexture.SetTextureFilename('../../../data/textures/concrete.jpg')
mbody2.GetAssets().push_back(mboxtexture)
# Create a revolute constraint
mlink = chrono.ChLinkRevolute()
# the coordinate system of the constraint reference in abs. space:
mframe = chrono.ChFrameD(chrono.ChVectorD(0.1, 0.5, 0))
# initialize the constraint telling which part must be connected, and where:
mlink.Initialize(mbody1, mbody2, mframe)
mysystem.Add(mlink)
def AddContainer(sys):
# The fixed body (5 walls)
fixedBody = chrono.ChBody()
fixedBody.SetMass(1.0)
fixedBody.SetBodyFixed(True)
fixedBody.SetPos(chrono.ChVectorD())
fixedBody.SetCollide(True)
# Contact material for container
fixed_mat = chrono.ChMaterialSurfaceSMC()
fixedBody.GetCollisionModel().ClearModel()
AddContainerWall(fixedBody, fixed_mat, chrono.ChVectorD(20, 1, 20),
chrono.ChVectorD(0, -5, 0))
AddContainerWall(fixedBody, fixed_mat, chrono.ChVectorD(1, 10, 20.99),
chrono.ChVectorD(-10, 0, 0))
AddContainerWall(fixedBody, fixed_mat, chrono.ChVectorD(1, 10, 20.99),
chrono.ChVectorD(10, 0, 0))
AddContainerWall(fixedBody, fixed_mat, chrono.ChVectorD(20.99, 10, 1),
chrono.ChVectorD(0, 0, -10), False)
AddContainerWall(fixedBody, fixed_mat, chrono.ChVectorD(20.99, 10, 1),
chrono.ChVectorD(0, 0, 10))
fixedBody.GetCollisionModel().BuildModel()
texture = chrono.ChTexture()
texture.SetTextureFilename(
chrono.GetChronoDataFile("textures/concrete.jpg"))
fixedBody.AddAsset(texture)
sys.AddBody(fixedBody)
# The rotating mixer body
rotatingBody = chrono.ChBody()
rotatingBody.SetMass(10.0)
rotatingBody.SetInertiaXX(chrono.ChVectorD(50, 50, 50))
rotatingBody.SetPos(chrono.ChVectorD(0, -1.6, 0))
rotatingBody.SetCollide(True)
# Contact material for mixer body
rot_mat = chrono.ChMaterialSurfaceSMC()
hsize = chrono.ChVectorD(5, 2.75, 0.5)
rotatingBody.GetCollisionModel().ClearModel()
rotatingBody.GetCollisionModel().AddBox(rot_mat, hsize.x, hsize.y, hsize.z)
rotatingBody.GetCollisionModel().BuildModel()
box = chrono.ChBoxShape()
box.GetBoxGeometry().Size = hsize
rotatingBody.AddAsset(box)
rotatingBody.AddAsset(texture)
sys.AddBody(rotatingBody)
# A motor between the two
my_motor = chrono.ChLinkMotorRotationSpeed()
my_motor.Initialize(
rotatingBody, fixedBody,
chrono.ChFrameD(chrono.ChVectorD(0, 0, 0),
chrono.Q_from_AngAxis(chrono.CH_C_PI_2,
chrono.VECT_X)))
mfun = chrono.ChFunction_Const(chrono.CH_C_PI / 2.0) # speed w=90°/s
my_motor.SetSpeedFunction(mfun)
sys.AddLink(my_motor)
return rotatingBody
def AddContainer(sys):
# Contact material for container
ground_mat = chrono.ChMaterialSurfaceNSC()
# Create the five walls of the rectangular container, using fixed rigid bodies of 'box' type
floorBody = chrono.ChBodyEasyBox(20, 1, 20, 1000, True, True, ground_mat)
floorBody.SetPos(chrono.ChVectorD(0, -5, 0))
floorBody.SetBodyFixed(True)
sys.Add(floorBody)
wallBody1 = chrono.ChBodyEasyBox(1, 10, 20.99, 1000, True, True, ground_mat)
wallBody1.SetPos(chrono.ChVectorD(-10, 0, 0))
wallBody1.SetBodyFixed(True)
sys.Add(wallBody1)
wallBody2 = chrono.ChBodyEasyBox(1, 10, 20.99, 1000, True, True, ground_mat)
wallBody2.SetPos(chrono.ChVectorD(10, 0, 0))
wallBody2.SetBodyFixed(True)
sys.Add(wallBody2)
wallBody3 = chrono.ChBodyEasyBox(20.99, 10, 1, 1000, False, True, ground_mat)
wallBody3.SetPos(chrono.ChVectorD(0, 0, -10))
wallBody3.SetBodyFixed(True)
sys.Add(wallBody3)
wallBody4 = chrono.ChBodyEasyBox(20.99, 10, 1, 1000, True, True, ground_mat)
wallBody4.SetPos(chrono.ChVectorD(0, 0, 10))
wallBody4.SetBodyFixed(True)
sys.Add(wallBody4)
# optional, attach textures for better visualization
mtexturewall = chrono.ChTexture()
mtexturewall.SetTextureFilename(chrono.GetChronoDataFile("textures/concrete.jpg"))
wallBody1.AddAsset(mtexturewall) # note: most assets can be shared
wallBody2.AddAsset(mtexturewall)
wallBody3.AddAsset(mtexturewall)
wallBody4.AddAsset(mtexturewall)
floorBody.AddAsset(mtexturewall)
# Add the rotating mixer
mixer_mat = chrono.ChMaterialSurfaceNSC()
mixer_mat.SetFriction(0.4)
rotatingBody = chrono.ChBodyEasyBox(10, 5, 1, # x,y,z size
4000, # density
True, # visualization?
True, # collision?
mixer_mat) # contact material
rotatingBody.SetPos(chrono.ChVectorD(0, -1.6, 0))
sys.Add(rotatingBody)
# .. a motor between mixer and truss
my_motor = chrono.ChLinkMotorRotationSpeed()
my_motor.Initialize(rotatingBody,
floorBody,
chrono.ChFrameD(chrono.ChVectorD(0, 0, 0),
chrono.Q_from_AngAxis(chrono.CH_C_PI_2, chrono.VECT_X)))
mfun = chrono.ChFunction_Const(chrono.CH_C_PI / 4.0) # speed 45 deg/s
my_motor.SetSpeedFunction(mfun)
sys.AddLink(my_motor)
# NOTE: Instead of creating five separate 'box' bodies to make
# the walls of the container, you could have used a single body
# made of five box shapes, which build a single collision description,
# as in the alternative approach:
"""
# create a plain ChBody (no colliding shape nor visualization mesh is used yet)
mrigidBody = chrono.ChBody()
# set as fixed body, and turn collision ON, otherwise no collide by default
mrigidBody.SetBodyFixed(True)
mrigidBody.SetCollide(True)
# Clear model. The colliding shape description MUST be between ClearModel() .. BuildModel() pair.
mrigidBody.GetCollisionModel().ClearModel()
# Describe the (invisible) colliding shape by adding five boxes (the walls and floor)
mrigidBody.GetCollisionModel().AddBox(ground_mat, 20, 1, 20, chrono.ChVectorD(0, -10, 0))
mrigidBody.GetCollisionModel().AddBox(ground_mat, 1, 40, 20, chrono.ChVectorD(-11, 0, 0))
mrigidBody.GetCollisionModel().AddBox(ground_mat, 1, 40, 20, chrono.ChVectorD(11, 0, 0))
mrigidBody.GetCollisionModel().AddBox(ground_mat, 20, 40, 1, chrono.ChVectorD(0, 0, -11))
mrigidBody.GetCollisionModel().AddBox(ground_mat, 20, 40, 1, chrono.ChVectorD(0, 0, 11))
# Complete the description of collision shape.
mrigidBody.GetCollisionModel().BuildModel()
# Attach some visualization shapes if needed:
vshape = chrono.ChBoxShape()
vshape.GetBoxGeometry().SetLengths(chrono.ChVectorD(20, 1, 20))
vshape.GetBoxGeometry().Pos = chrono.ChVectorD(0, -5, 0)
this.AddAsset(vshape)
# etc. for other 4 box shapes..
"""
return rotatingBody
body_floor.SetBodyFixed(True)
body_floor.SetPos(chrono.ChVectorD(0, -2, 0))
body_floor.SetMaterialSurface(brick_material)
# Collision shape
body_floor.GetCollisionModel().ClearModel()
body_floor.GetCollisionModel().AddBox(3, 1, 3) # hemi sizes
body_floor.GetCollisionModel().BuildModel()
body_floor.SetCollide(True)
# Visualization shape
body_floor_shape = chrono.ChBoxShape()
body_floor_shape.GetBoxGeometry().Size = chrono.ChVectorD(3, 1, 3)
body_floor.GetAssets().push_back(body_floor_shape)
body_floor_texture = chrono.ChTexture()
body_floor_texture.SetTextureFilename(chrono.GetChronoDataPath() +
'concrete.jpg')
body_floor.GetAssets().push_back(body_floor_texture)
my_system.Add(body_floor)
# Create the shaking table, as a box
size_table_x = 1
size_table_y = 0.2
size_table_z = 1
body_table = chrono.ChBody()
body_table.SetPos(chrono.ChVectorD(0, -size_table_y / 2, 0))
body_table.SetMaterialSurface(brick_material)
def buildALEXR(system,
Xee = .5,
Yee = -.5,
eeMass = 1,
side = "right"):
"""
public interface for building the ALEXR
:param system: the Chrono system to which the ALEXR robot is added
"""
#--------------- state infromation for initialization ---------------------
#initial location of the end effector (EE) of the ALEX Robot
#Xee = .5
#Yee = -.5
#specify mass properties of the payload at the end effector.
#eeMass = 1
#set the elbow discrete vars this will flip for right vs. left side useage
#side = "right" # "left"
#----------- Calculate IK angles using custom Library ---------------------
a = ALEXR() #encodes robot state info
system.refs = {} #setup the dictionary of object references
#ALEXR link lengths and origins
xl = a.xl ; yl = a.yl
xr = a.xr ; yr = a.yr
#left robot
_L1l = a.L1l
_L2l = a.L2l
_L3l = a.L3l
_L23l= a.L23l
#right robot
_L1r = a.L1r
_L2r = a.L2r
#calculate IK robot angles
θ1l , θ2l, θ1r , θ2r = a.IK_2DOF(Xee,Yee,side)
#perform a check here, and throw an error if the original configuration can't be solved.
if not a.feasable_point(Xee, Yee,side):
raise ValueError("there is no solution to IK for the end-effector location specified (Xee,Yee)")
#--------------- create each link as a rigid body -------------------------
# chrono uses a right handed coordinate system, and the model is constructed to look like the plotly model,
# but the irrelict visualizer operates in the mirror world - with a left handed coordinate system
# the long axis of the link to the right is the x axis, up is y, z is link rotation direction out of the page
# R is the rotation matrix between the default link frame orientation, and the solidworks coordinate system
# R (solidworks - > link frame)
#------------- ground body ------------
GB = chrono.ChBodyAuxRef()
GB.SetPos(chrono.ChVectorD(0,(yl+yr)/2,0))
GB.SetBodyFixed(True)
#set mesh visualization
mesh_for_visualization = chrono.ChTriangleMeshConnected()
mesh_for_visualization.LoadWavefrontMesh(assetsPath +'ground.obj')
# Optionally: you can scale/shrink/rotate/translate the mesh using this:
meshRotation = chrono.ChMatrix33D(np.pi/2,chrono.ChVectorD(0,1,0))
mesh_for_visualization.Transform(chrono.ChVectorD(0,0,0), meshRotation)
# Now the triangle mesh is inserted in a ChTriangleMeshShape visualization asset,
# and added to the body
visualization_shape = chrono.ChTriangleMeshShape()
visualization_shape.SetMesh(mesh_for_visualization)
GB.AddAsset(visualization_shape)
system.Add(GB)
system.refs["GB"] = GB
#--------- coordinate frame ---------------
coord = chrono.ChBodyAuxRef()
coord.SetPos(chrono.ChVectorD(0,0,0))
coord.SetBodyFixed(True)
mesh_for_visualization = chrono.ChTriangleMeshConnected()
mesh_for_visualization.LoadWavefrontMesh(assetsPath +'coords.obj')
mesh_for_visualization.Transform(chrono.ChVectorD(0,0,0), chrono.ChMatrix33D(.01))
visualization_shape = chrono.ChTriangleMeshShape()
visualization_shape.SetMesh(mesh_for_visualization)
coord.AddAsset(visualization_shape)
system.Add(coord)
#----------- left link 1 ------------------
#add body
L1l = chrono.ChBodyAuxRef()
L1l.SetBodyFixed(False)
system.Add(L1l)
system.refs["L1l"] = L1l
#set position,orientation with FK (while REF frame and COG frame are coincident)
x = xl + (_L1l/2)*np.cos(θ1l)
y = yl + (_L1l/2)*np.sin(θ1l)
L1l.SetPos(chrono.ChVectorD(x,y,.01))
L1l.SetRot(chrono.ChMatrix33D(θ1l,chrono.ChVectorD(0,0,1)))
#add visualization
mesh_for_visualization = chrono.ChTriangleMeshConnected()
mesh_for_visualization.LoadWavefrontMesh(assetsPath +'_L1l.obj')
meshRotation = chrono.ChMatrix33D(np.pi/2,chrono.ChVectorD(0,1,0))
mesh_for_visualization.Transform(chrono.ChVectorD(0,0,0), meshRotation)
visualization_shape = chrono.ChTriangleMeshShape()
visualization_shape.SetMesh(mesh_for_visualization)
L1l.AddAsset(visualization_shape)
texture = chrono.ChTexture()
texture.SetTextureFilename(assetsPath + 'blue.png')
L1l.GetAssets().push_back(texture)
#set the mass and inertial properties
# xs ys zs
R1 = np.array([[ 0, 0, -1], #xl #found by hand
[ 0, -1, 0], #yl
[-1, 0, 0]]) #zl
L1l.SetMass(3.642)
Is = np.array([[ 0.13286460, -0.00001280, 0.03326759], # centroidal moment of inertia
[-0.00001280, 0.15328523,-0.00014996],
[ 0.03326759, -0.00014996, 0.03071782]])
Il = R1 @ Is @ np.linalg.inv(R1) # rotate the inertia tensor into the link frame
Ilch = chrono.ChMatrix33D()
Ilch.SetMatr(Il.tolist())
L1l.SetInertia(Ilch)
# move the COG frame
c = R1 @ np.array([[.0904],[-.0004],[.1461]])
L1l.SetFrame_COG_to_REF(chrono.ChFrameD(chrono.ChVectorD(c[0,0],c[1,0],c[2,0])))
#----------- left link 2 ------------------
#add body
L2l = chrono.ChBodyAuxRef()
L2l.SetBodyFixed(False)
system.Add(L2l)
system.refs["L2l"] = L2l
#set position,orientation with FK
x = xl + (_L1l)*np.cos(θ1l) + (_L23l/2)*np.cos(θ1l + θ2l)
y = yl + (_L1l)*np.sin(θ1l) + (_L23l/2)*np.sin(θ1l + θ2l)
L2l.SetPos(chrono.ChVectorD(x,y,.02))
L2l.SetRot(chrono.ChMatrix33D(θ1l + θ2l,chrono.ChVectorD(0,0,1)))
#add visualization
mesh_for_visualization = chrono.ChTriangleMeshConnected()
mesh_for_visualization.LoadWavefrontMesh(assetsPath +'_L2l.obj')
meshRotation = chrono.ChMatrix33D(np.pi/2,chrono.ChVectorD(0,1,0))
#mesh origin was slightly off, so I hand tuned it
mesh_for_visualization.Transform(chrono.ChVectorD(-.00775,0,0), meshRotation)
visualization_shape = chrono.ChTriangleMeshShape()
visualization_shape.SetMesh(mesh_for_visualization)
L2l.AddAsset(visualization_shape)
texture = chrono.ChTexture()
texture.SetTextureFilename(assetsPath + 'blue.png')
L2l.GetAssets().push_back(texture)
#set the mass and inertial properties
# xs ys zs
R2 = np.array([[ 0, 0, 1], #xl #found by hand
[ 0, 1, 0], #yl
[-1, 0, 0]]) #zl
L2l.SetMass(1.158)
Is = np.array([[ 0.04061717, 0.00000000, 0.00000000], # centroidal moment of inertia
[ 0.00000000, 0.04040908, 0.00000000],
[ 0.00000000, 0.00000000, 0.00072961]])
Il = R2 @ Is @ np.linalg.inv(R2) # rotate the inertia tensor into the link frame
Ilch = chrono.ChMatrix33D()
Ilch.SetMatr(Il.tolist())
L2l.SetInertia(Ilch)
# move the COG frame
c = R2 @ np.array([[0],[0],[-.1192]])
L2l.SetFrame_COG_to_REF(chrono.ChFrameD(chrono.ChVectorD(c[0,0],c[1,0],c[2,0])))
#----------- right link 1 -----------------
#add body
L1r = chrono.ChBodyAuxRef()
L1r.SetBodyFixed(False)
system.Add(L1r)
system.refs["L1r"] = L1r
#set position,orientation with FK
x = xr + (_L1r/2)*np.cos(θ1r)
y = yr + (_L1r/2)*np.sin(θ1r)
L1r.SetPos(chrono.ChVectorD(x,y,.02))
L1r.SetRot(chrono.ChMatrix33D(θ1r,chrono.ChVectorD(0,0,1)))
#add visualization
mesh_for_visualization = chrono.ChTriangleMeshConnected()
mesh_for_visualization.LoadWavefrontMesh(assetsPath +'_L1r.obj')
meshRotation = chrono.ChMatrix33D(np.pi/2,chrono.ChVectorD(0,1,0))
mesh_for_visualization.Transform(chrono.ChVectorD(0,0,0), meshRotation)
visualization_shape = chrono.ChTriangleMeshShape()
visualization_shape.SetMesh(mesh_for_visualization)
L1r.AddAsset(visualization_shape)
texture = chrono.ChTexture()
texture.SetTextureFilename(assetsPath + 'red.png')
L1r.GetAssets().push_back(texture)
#set the mass and inertial properties
L1r.SetMass(4.1637)
Is = np.array([[ 0.05261769, -0.00006255, 0.02546226], # centroidal moment of inertia
[-0.00006255, 0.09428792,-0.00007718],
[ 0.02546226, -0.00007718, 0.05243999]])
Il = R1 @ Is @ np.linalg.inv(R1) # rotate the inertia tensor into the link frame
Ilch = chrono.ChMatrix33D()
Ilch.SetMatr(Il.tolist())
L1r.SetInertia(Ilch)
# move the COG frame
c = R1 @ np.array([[0.1222],[-0.0004],[.0927]])
L1r.SetFrame_COG_to_REF(chrono.ChFrameD(chrono.ChVectorD(c[0,0],c[1,0],c[2,0])))
#----------- right link 2 -----------------
#add body
L2r = chrono.ChBodyAuxRef()
L2r.SetBodyFixed(False)
system.Add(L2r)
system.refs["L2r"] = L2r
#set position,orientation with FK
x = xr + (_L1r)*np.cos(θ1r) + (_L2r/2)*np.cos(θ1r + θ2r)
y = yr + (_L1r)*np.sin(θ1r) + (_L2r/2)*np.sin(θ1r + θ2r)
L2r.SetPos(chrono.ChVectorD(x,y,.03))
L2r.SetRot(chrono.ChMatrix33D(θ1r + θ2r,chrono.ChVectorD(0,0,1)))
#add visualization
mesh_for_visualization = chrono.ChTriangleMeshConnected()
mesh_for_visualization.LoadWavefrontMesh(assetsPath +'_L2r.obj')
meshRotation = chrono.ChMatrix33D(np.pi/2,chrono.ChVectorD(0,1,0))
mesh_for_visualization.Transform(chrono.ChVectorD(0,0,0), meshRotation)
visualization_shape = chrono.ChTriangleMeshShape()
visualization_shape.SetMesh(mesh_for_visualization)
L2r.AddAsset(visualization_shape)
texture = chrono.ChTexture()
texture.SetTextureFilename(assetsPath + 'red.png')
L2r.GetAssets().push_back(texture)
#set the mass and inertial properties
L2r.SetMass(1.1947)
Is = np.array([[ 0.06453132, 0.00000000, 0.00101029], # centroidal moment of inertia
[ 0.00000000, 0.06454599, 0.00000000],
[ 0.00101029, 0.00000000, 0.00093856]])
Il = R1 @ Is @ np.linalg.inv(R1) #R1 is correct here, I checked, rotate the inertia tensor into the link frame
Ilch = chrono.ChMatrix33D()
Ilch.SetMatr(Il.tolist())
L2r.SetInertia(Ilch)
# move the COG frame
c = R1 @ np.array([[-0.0041],[0.0000],[-0.0499]])
L2r.SetFrame_COG_to_REF(chrono.ChFrameD(chrono.ChVectorD(c[0,0],c[1,0],c[2,0])))
#----------- end effector payload ---------
#add body
ee = chrono.ChBodyAuxRef()
ee.SetBodyFixed(False)
system.Add(ee)
system.refs["EE"] = ee
#add mass properties //improve these based on actual data...
ee.SetMass(eeMass)
#can leave the inertia large, as this frame doesn't rotate (it can be thought of as on a bearing)
#set position,orientation with FK
x = xl + (_L1l)*np.cos(θ1l) + (_L23l)*np.cos(θ1l + θ2l)
y = yl + (_L1l)*np.sin(θ1l) + (_L23l)*np.sin(θ1l + θ2l)
ee.SetPos(chrono.ChVectorD(x,y,.03))
ee.SetRot(chrono.ChMatrix33D(0,chrono.ChVectorD(0,0,1)))
#add visualization
mesh_for_visualization = chrono.ChTriangleMeshConnected()
mesh_for_visualization.LoadWavefrontMesh(assetsPath +'_EE.obj')
meshRotation = chrono.ChMatrix33D(np.pi/2,chrono.ChVectorD(0,1,0))
mesh_for_visualization.Transform(chrono.ChVectorD(0,0,0), meshRotation)
visualization_shape = chrono.ChTriangleMeshShape()
visualization_shape.SetMesh(mesh_for_visualization)
ee.AddAsset(visualization_shape)
#----------------------- create the revolute joints ---------------------------
# joint frame naming conventions
# X_c_a
# X - body the frame is attached to in the joint
# c_a - c frame represented in the a frame
# potential frames
# j - the joint frame - where the joint marker and frame is located
# ref - the reference frame located at the center of the link
# cog - the cog of the link, which is offset from the reference frame in an ChAuxRefBody()
# example L1l_j_cog
# this refers to a frame on body L1l, attached at the joint location, represented
# relative to the COG of L1l. this is the objective, as joints must be formed relative to
# a bodies COG frame, not it's auxillary reference frame
#------------- GB <-> L1l --------------
# jt = chrono.ChLinkRevolute() #set higher up
# add_θ1l_joint(system,jt)
#
##------------- L1l <-> L2l --------------
jt = chrono.ChLinkRevolute() # create revolute joint object
local = True # we will use the local frame
L1l_j_r = chrono.ChFrameD(chrono.ChVectorD(_L1l/2,0,0.01)) # local frame of attachment
L2l_j_r = chrono.ChFrameD(chrono.ChVectorD(-1*_L23l/2,0,0)) # local frame of attachment
L1l_j_COG = L1l_j_r >> L1l.GetFrame_REF_to_COG() # express L1l <-> L2l joint relative to L1l COG frame
L2l_j_COG = L2l_j_r >> L2l.GetFrame_REF_to_COG() # express L1l <-> L2l joint relative to L12 COG frame
jt.Initialize(L1l,L2l,local,L1l_j_COG,L2l_j_COG) # init joint
system.Add(jt) # add to system
system.refs["L1l<->L2l"] = jt # maintain a reference to the joint
##------------- GB <-> L1r --------------
# jt = chrono.ChLinkRevolute()
# add_θ1r_joint(system,jt)
##------------- L1r <-> L2r --------------
jt = chrono.ChLinkRevolute() # create revolute joint object
local = True # we will use the local frame
L1r_j_r = chrono.ChFrameD(chrono.ChVectorD(_L1r/2,0,.01)) # local frame of attachment
L2r_j_r = chrono.ChFrameD(chrono.ChVectorD(-1*_L2r/2,0,0)) # local frame of attachment
L1r_j_COG = L1r_j_r >> L1r.GetFrame_REF_to_COG() # express L1l <-> L2l joint relative to L1l COG frame
L2r_j_COG = L2r_j_r >> L2r.GetFrame_REF_to_COG() # express L1l <-> L2l joint relative to L12 COG frame
jt.Initialize(L1r,L2r,local,L1r_j_COG,L2r_j_COG) # init joint
system.Add(jt) # add to system
system.refs["L1r<->L2r"] = jt # maintain a reference to the joint
##------------- L2l <-> L2r --------------
jt = chrono.ChLinkRevolute() # create revolute joint object
local = True # we will use the local frame
dj = -1*(_L23l/2 - _L2l) # distance from center to joint point
L2l_j_r = chrono.ChFrameD(chrono.ChVectorD(dj,0,.01)) # local frame of attachment
L2r_j_r = chrono.ChFrameD(chrono.ChVectorD(_L2r/2,0,0)) # local frame of attachment
L2l_j_COG = L2l_j_r >> L2l.GetFrame_REF_to_COG() # express L2l <-> L2r joint relative to L2l COG frame
L2r_j_COG = L2r_j_r >> L2r.GetFrame_REF_to_COG() # express L2l <-> L2r joint relative to L2r COG frame
jt.Initialize(L2l,L2r,local,L2l_j_COG,L2r_j_COG) # init joint
system.Add(jt) # add to system
system.refs["L2l<->L2r"] = jt # maintain a reference to the joint
#
##------------- ee <-> L2l --------------
jt = chrono.ChLinkRevolute() # create revolute joint object
local = True # we will use the local frame
L2l_j_r = chrono.ChFrameD(chrono.ChVectorD(_L23l/2,0,.01)) # local frame of attachment
ee_j_r = chrono.ChFrameD(chrono.ChVectorD(0,0,0)) # local frame of attachment # COG isn't displaced in GB
L2l_j_COG = L2l_j_r >> L2l.GetFrame_REF_to_COG() # express ee <-> L2l joint relative to L1l COG frame
ee_j_COG = ee_j_r # COG isn't displaced in ee frame
jt.Initialize(L2l,ee,local,L2l_j_COG,ee_j_COG) # init joint
system.Add(jt) # add to system
system.refs["EE<->L2l"] = jt # maintain a reference to the joint
#no need to return, as system is passed by reference and then modified.
return system
# If running from a different directory, you must change the path to the data directory with:
# chrono.SetChronoDataPath('relative/path/to/data/directory/')
# ---------------------------------------------------------------------
#
# Create the simulation system and add items
#
mphysicalSystem = chrono.ChSystemNSC()
# Create all the rigid bodies.
mradius = 0.5
density = 1000
# Create a texture asset. It can be shared between bodies.
textureasset = chrono.ChTexture(
chrono.GetChronoDataFile("textures/bluewhite.png"))
# Create some spheres that roll horizontally, with increasing rolling friction values
for bi in range(10):
mat = chrono.ChMaterialSurfaceNSC()
mat.SetFriction(0.4)
mat.SetRollingFriction((float(bi) / 10.) * 0.05)
msphereBody = chrono.ChBodyEasySphere(
mradius, # radius size
1000, # density
True, # visualization?
True, # collision?
mat) # contact material
# Set some properties
def run_sim(traits, trial_num, gen_num, difficulty_level):
my_system = chrono.ChSystemNSC()
# Set the default outward/inward shape margins for collision detection
chrono.ChCollisionModel.SetDefaultSuggestedEnvelope(0.001)
chrono.ChCollisionModel.SetDefaultSuggestedMargin(0.001)
# Sets simulation precision
my_system.SetMaxItersSolverSpeed(70)
# Create a contact material (surface property)to share between all objects.
rollfrict_param = 0.5 / 10.0 * 0.05
brick_material = chrono.ChMaterialSurfaceNSC()
brick_material.SetFriction(0.5)
brick_material.SetDampingF(0.2)
brick_material.SetCompliance(0.0000001)
brick_material.SetComplianceT(0.0000001)
brick_material.SetRollingFriction(rollfrict_param)
brick_material.SetSpinningFriction(0.00000001)
brick_material.SetComplianceRolling(0.0000001)
brick_material.SetComplianceSpinning(0.0000001)
# Create the set of bricks in a vertical stack, along Y axis
block_bodies = [] # visualizes bodies
block_shapes = [] # geometry purposes
current_y = 0
for block_index in range(0, 4):
size_brick_x = traits[block_index][0]
size_brick_z = traits[block_index][1]
size_brick_y = traits[block_index][2]
if size_brick_y < settings.MIN_DIMENSIONS_THRESHOLD or size_brick_x < settings.MIN_DIMENSIONS_THRESHOLD or size_brick_z < settings.MIN_DIMENSIONS_THRESHOLD:
return [-50, traits]
mass_brick = settings.BLOCK_MASS
inertia_brick_xx = 1 / 12 * mass_brick * (pow(size_brick_z, 2) + pow(size_brick_y, 2))
inertia_brick_yy = 1 / 12 * mass_brick * (pow(size_brick_x, 2) + pow(size_brick_z, 2))
inertia_brick_zz = 1 / 12 * mass_brick * (pow(size_brick_x, 2) + pow(size_brick_y, 2))
body_brick = chrono.ChBody()
body_brick.SetPos(chrono.ChVectorD(0, current_y + 0.5 * size_brick_y, 0)) # set initial position
current_y += size_brick_y # set tower block positions
# setting mass properties
body_brick.SetMass(mass_brick)
body_brick.SetInertiaXX(chrono.ChVectorD(inertia_brick_xx, inertia_brick_yy, inertia_brick_zz))
# set collision surface properties
body_brick.SetMaterialSurface(brick_material)
# Collision shape
body_brick.GetCollisionModel().ClearModel()
body_brick.GetCollisionModel().AddBox(size_brick_x / 2, size_brick_y / 2,
size_brick_z / 2) # must set half sizes
body_brick.GetCollisionModel().BuildModel()
body_brick.SetCollide(True)
# Visualization shape, for rendering animation
body_brick_shape = chrono.ChBoxShape()
body_brick_shape.GetBoxGeometry().Size = chrono.ChVectorD(size_brick_x / 2, size_brick_y / 2,
size_brick_z / 2)
if block_index % 2 == 0:
body_brick_shape.SetColor(chrono.ChColor(0.65, 0.65, 0.6)) # set gray color only for odd bricks
body_brick.GetAssets().push_back(body_brick_shape)
my_system.Add(body_brick)
block_bodies.append(body_brick)
block_shapes.append(body_brick_shape);
# Create the room floor
body_floor = chrono.ChBody()
body_floor.SetBodyFixed(True)
body_floor.SetPos(chrono.ChVectorD(0, -2, 0))
body_floor.SetMaterialSurface(brick_material)
# Floor's collision shape
body_floor.GetCollisionModel().ClearModel()
body_floor.GetCollisionModel().AddBox(3, 1, 3) # hemi sizes default: 3,1,3
body_floor.GetCollisionModel().BuildModel()
body_floor.SetCollide(True)
# Visualization shape
body_floor_shape = chrono.ChBoxShape()
body_floor_shape.GetBoxGeometry().Size = chrono.ChVectorD(3, 1, 3)
body_floor.GetAssets().push_back(body_floor_shape)
body_floor_texture = chrono.ChTexture()
# body_floor_texture.SetTextureFilename(chrono.GetChronoDataPath() + 'concrete.jpg')
body_floor.GetAssets().push_back(body_floor_texture)
my_system.Add(body_floor)
# Create the shaking table, as a box
size_table_x = 1
size_table_y = 0.2
size_table_z = 1
body_table = chrono.ChBody()
body_table.SetPos(chrono.ChVectorD(0, -size_table_y / 2, 0))
body_table.SetMaterialSurface(brick_material)
# Collision shape
body_table.GetCollisionModel().ClearModel()
body_table.GetCollisionModel().AddBox(size_table_x / 2, size_table_y / 2, size_table_z / 2) # hemi sizes
body_table.GetCollisionModel().BuildModel()
body_table.SetCollide(True)
# Visualization shape
body_table_shape = chrono.ChBoxShape()
body_table_shape.GetBoxGeometry().Size = chrono.ChVectorD(size_table_x / 2, size_table_y / 2, size_table_z / 2)
body_table_shape.SetColor(chrono.ChColor(0.4, 0.4, 0.5))
body_table.GetAssets().push_back(body_table_shape)
body_table_texture = chrono.ChTexture()
# body_table_texture.SetTextureFilename(chrono.GetChronoDataPath() + 'concrete.jpg')
body_table.GetAssets().push_back(body_table_texture)
my_system.Add(body_table)
# Makes the table shake
link_shaker = chrono.ChLinkLockLock()
# link_shaker.SetMotion_X()
link_shaker.Initialize(body_table, body_floor, chrono.CSYSNORM)
my_system.Add(link_shaker)
# ..create the function for imposed y vertical motion, etc.
mfunY = chrono.ChFunction_Sine(0, settings.TABLE_FREQ_Y, settings.TABLE_AMP_Y) # phase, frequency, amplitude
# ..create the function for imposed z horizontal motion, etc.
mfunZ = chrono.ChFunction_Sine(0, settings.TABLE_FREQ_Z, settings.TABLE_AMP_Z) # phase, frequency, amplitude
# ..create the function for imposed x horizontal motion, etc.
mfunX = chrono.ChFunction_Sine(2, 0, 0) # phase, frequency, amplitude
print("Sim env_level " + str(difficulty_level))
if difficulty_level == settings.SHAKE_IN_X_AXIS_LEVEL:
mfunX = chrono.ChFunction_Sine(2, settings.TABLE_FREQ_X, settings.TABLE_AMP_X) # phase, frequency, amplitude
elif difficulty_level >= settings.SHAKE_IN_X_AND_Z_AXIS_LEVEL:
increment = 0.25 * difficulty_level
mfunX = chrono.ChFunction_Sine(2, settings.TABLE_FREQ_X + increment, settings.TABLE_AMP_X) # phase, frequency, amplitude
mfunZ = chrono.ChFunction_Sine(0, settings.TABLE_FREQ_Z + increment, settings.TABLE_AMP_Z) # phase, frequency, amplitude
link_shaker.SetMotion_Y(mfunY)
link_shaker.SetMotion_Z(mfunZ)
link_shaker.SetMotion_X(mfunX)
# ---------------------------------------------------------------------
#
# Create an Irrlicht application to visualize the system
window_name = "Tower Trial: " + str(trial_num) + " Gen: " + str(gen_num)
if trial_num == -1:
window_name = "Initializing Population..."
app = chronoirr.ChIrrApp(my_system, window_name, chronoirr.dimension2du(settings.SCREEN_WIDTH, settings.SCREEN_HEIGHT))
app.AddTypicalSky()
app.AddTypicalLogo(chrono.GetChronoDataPath() + 'logo_pychrono_alpha.png')
app.AddTypicalCamera(chronoirr.vector3df(settings.CAMERA_X, settings.CAMERA_Y, settings.CAMERA_Z))
app.AddLightWithShadow(chronoirr.vector3df(2, 4, 2), # point
chronoirr.vector3df(0, 0, 0), # aimpoint
9, # radius (power)
1, 9, # near, far
30)
# Committing visualization
app.AssetBindAll()
app.AssetUpdateAll();
app.AddShadowAll();
# ---------------------------------------------------------------------
#
# Run the simulation. This is where all of the constraints are set
#
app.SetTimestep(settings.SPEED)
app.SetTryRealtime(True)
app.GetDevice().run()
fitness = 0
brick1_init = block_bodies[0].GetPos().y
brick2_init = block_bodies[1].GetPos().y
brick3_init = block_bodies[2].GetPos().y
brick4_init = block_bodies[3].GetPos().y # Highest
while True:
brick1_curr_height = block_bodies[0].GetPos().y
brick2_curr_height = block_bodies[1].GetPos().y
brick3_curr_height = block_bodies[2].GetPos().y
brick4_curr_height = block_bodies[3].GetPos().y # Highest
# Break conditions
if my_system.GetChTime() > settings.SIMULATION_RUNTIME:
break
if my_system.GetChTime() > settings.SIMULATION_RUNTIME / 2:
mfunX = chrono.ChFunction_Sine(2, 0, 0)
mfunZ = chrono.ChFunction_Sine(2, 0, 0)
link_shaker.SetMotion_Z(mfunZ)
link_shaker.SetMotion_X(mfunX)
# If the blocks fall out of line
if brick1_init - brick1_curr_height > settings.CANCEL_SIM_THRESHOLD or \
brick2_init - brick2_curr_height > settings.CANCEL_SIM_THRESHOLD or \
brick3_init - brick3_curr_height > settings.CANCEL_SIM_THRESHOLD or \
brick4_init - brick4_curr_height > settings.CANCEL_SIM_THRESHOLD:
break
# Record fitness every 1/1000 of the runtime
if 0.01 > my_system.GetChTime() % ((1 / 1000) * settings.SIMULATION_RUNTIME) > 0:
if settings.FITNESS_FUNCTION == settings.Fitness.SumLengths: # Sum of size_y
fitness += block_shapes[0].GetBoxGeometry().GetLengths().y + \
block_shapes[1].GetBoxGeometry().GetLengths().y + \
block_shapes[2].GetBoxGeometry().GetLengths().y + \
block_shapes[3].GetBoxGeometry().GetLengths().y
elif settings.FITNESS_FUNCTION == settings.Fitness.MaxPosition: # Max of y positions
fitness += max(block_bodies[0].GetPos().y,
block_bodies[1].GetPos().y,
block_bodies[2].GetPos().y,
block_bodies[3].GetPos().y)
elif settings.FITNESS_FUNCTION == settings.Fitness.MaxPositionSumLengths: # Max * sum of sizes
fitness += max(block_bodies[0].GetPos().y,
block_bodies[1].GetPos().y,
block_bodies[2].GetPos().y,
block_bodies[3].GetPos().y) * \
block_shapes[0].GetBoxGeometry().GetLengths().y + \
block_shapes[1].GetBoxGeometry().GetLengths().y + \
block_shapes[2].GetBoxGeometry().GetLengths().y + \
block_shapes[3].GetBoxGeometry().GetLengths().y
app.BeginScene()
app.DrawAll()
for substep in range(0, 5):
app.DoStep()
app.EndScene()
app.GetDevice().closeDevice()
print("Fitness: " + str(fitness) + " Gen: " + str(gen_num))
return [fitness, traits]
