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
shaft_1.SetRot(chrono.ChQuaternionD(1, 0, 0, 0)) # Add visualization assets to represent the shaft (a box) and the arm of the # universal joint's cross associated with this shaft (a cylinder) box_1 = chrono.ChBoxShape() box_1.GetBoxGeometry().Size = chrono.ChVectorD(0.15, 0.15, 0.9 * hl) shaft_1.AddAsset(box_1) cyl_2 = chrono.ChCylinderShape() cyl_2.GetCylinderGeometry().p1 = chrono.ChVectorD(-0.2, 0, hl) cyl_2.GetCylinderGeometry().p2 = chrono.ChVectorD(0.2, 0, hl) cyl_2.GetCylinderGeometry().rad = 0.05 shaft_1.AddAsset(cyl_2) col = chrono.ChColorAsset() col.SetColor(chrono.ChColor(0.9, 0.4, 0.1)) shaft_1.AddAsset(col) # Create the second shaft body # ---------------------------- # The second shaft is identical to the first one, but initialized at an angle # equal to the specified bend angle. shaft_2 = chrono.ChBody() mysystem.AddBody(shaft_2) shaft_2.SetIdentifier(1) shaft_2.SetBodyFixed(False) shaft_2.SetCollide(False) shaft_2.SetMass(1) shaft_2.SetInertiaXX(chrono.ChVectorD(1, 1, 0.2))
# Create the terrain
terrain = veh.RigidTerrain(my_bus.GetSystem())
if (contact_method == chrono.ChContactMethod_NSC):
patch_mat = chrono.ChMaterialSurfaceNSC()
patch_mat.SetFriction(0.9)
patch_mat.SetRestitution(0.01)
elif (contact_method == chrono.ChContactMethod_SMC):
patch_mat = chrono.ChMaterialSurfaceSMC()
patch_mat.SetFriction(0.9)
patch_mat.SetRestitution(0.01)
patch_mat.SetYoungModulus(2e7)
patch = terrain.AddPatch(patch_mat, chrono.ChVectorD(0, 0, 0),
chrono.ChVectorD(0, 0, 1), terrainLength,
terrainWidth)
patch.SetTexture(veh.GetDataFile("terrain/textures/tile4.jpg"), 200, 200)
patch.SetColor(chrono.ChColor(0.8, 0.8, 0.5))
terrain.Initialize()
# Create the vehicle Irrlicht interface
app = veh.ChWheeledVehicleIrrApp(my_bus.GetVehicle(), 'Citybus',
irr.dimension2du(1000, 800))
app.SetSkyBox()
app.AddTypicalLights(irr.vector3df(30, -30, 100), irr.vector3df(30, 50, 100),
250, 130)
app.AddTypicalLogo(chrono.GetChronoDataFile('logo_pychrono_alpha.png'))
app.SetChaseCamera(trackPoint, 15.0, 0.5)
app.SetTimestep(step_size)
app.AssetBindAll()
app.AssetUpdateAll()
# Create the driver system
def LoadFromObj(filename, density=1000, color=[1,0.1,0.1]):
body = chrono.ChBodyEasyMesh(filename, density, True, True)
body.AddAsset(chrono.ChColorAsset(chrono.ChColor(color[0], color[1], color[2])))
return body
def stepShape(position_front, direction_front, position_back, direction_back, width, height, clr = [0.5,0.5,0.5]):
################# TRAPPSTEG ###############
# position_front: chrono.ChVectorD, the position of the inner lower front corner
# direction_front: chrono.ChVectorD, normal direction to staircase center that aligns with front of the step
# position_back: chrono.ChVectorD, the position of the inner lower front corner
# direction_back: chrono.ChVectorD, normal direction that aligns with back of the step
# width: double, Width of the step as seen when walking in the staircase
# height: double, Thickness of the stepposition_front = ChVecify(position_front)
position_front = ChVecify(position_front)
direction_front = ChVecify(direction_front)
position_back = ChVecify(position_back)
direction_back = ChVecify(direction_back)
direction_front.SetLength(width)
direction_back.SetLength(width)
# Notation: I = Inner/O = Outer, U = Upper/L = Lower, F = Front/B = Back
# Ex: Step_ILF is the Inner Lower Front corner of the step
Step_ILF = position_front
Step_IUF = position_front + chrono.ChVectorD(0, height, 0)
Step_ILB = position_back
Step_IUB = position_back + chrono.ChVectorD(0, height, 0)
Step_OLF = position_front + direction_front
Step_OUF = position_front + direction_front + chrono.ChVectorD(0, height, 0)
Step_OLB = position_back + direction_back
Step_OUB = position_back + direction_back + chrono.ChVectorD(0, height, 0)
Step_mesh = chrono.ChTriangleMeshConnected()
# inner side
Step_mesh.addTriangle(Step_ILF, Step_ILB, Step_IUF)
Step_mesh.addTriangle(Step_IUB, Step_IUF, Step_ILB)
# outer side
Step_mesh.addTriangle(Step_OLF, Step_OUB, Step_OLB)
Step_mesh.addTriangle(Step_OLF, Step_OUF, Step_OUB)
# top side
Step_mesh.addTriangle(Step_IUF, Step_OUB, Step_OUF)
Step_mesh.addTriangle(Step_IUF, Step_IUB, Step_OUB)
# bottom side
Step_mesh.addTriangle(Step_ILF, Step_OLF, Step_OLB)
Step_mesh.addTriangle(Step_ILF, Step_OLB, Step_ILB)
# back side
Step_mesh.addTriangle(Step_ILB, Step_OLB, Step_IUB)
Step_mesh.addTriangle(Step_OUB, Step_IUB, Step_OLB)
# front side
Step_mesh.addTriangle(Step_ILF, Step_IUF, Step_OLF)
Step_mesh.addTriangle(Step_OUF, Step_OLF, Step_IUF)
Step_mesh.RepairDuplicateVertexes()
Step = chrono.ChBody()
Step.SetBodyFixed(True)
Step_shape = chrono.ChTriangleMeshShape()
Step_shape.SetMesh(Step_mesh)
Step_shape.SetColor(chrono.ChColor(clr[0], clr[1], clr[2]))
Step.GetCollisionModel().ClearModel()
Step.GetCollisionModel().AddTriangleMesh(Step_mesh, True, False)
Step.GetCollisionModel().BuildModel()
Step.SetCollide(True)
# Step_texture = chrono.ChTexture()
# Step_texture.SetTextureFilename(chrono.GetChronoDataFile('textures/red_dot.png'))
# Step_texture.SetTextureScale(10, 10)
# Step.GetAssets().push_back(Step_texture)
Step.GetAssets().push_back(Step_shape)
return Step
print("Brake type: " + gator.GetVehicle().GetBrake(1, veh.LEFT).GetTemplateName())
print("Tire type: " + gator.GetVehicle().GetTire(1, veh.LEFT).GetTemplateName())
print("\n")
# ------------------
# Create the terrain
# ------------------
terrain = veh.RigidTerrain(gator.GetSystem())
patch_mat = chrono.ChMaterialSurfaceNSC()
patch_mat.SetFriction(0.9)
patch_mat.SetRestitution(0.01)
patch = terrain.AddPatch(patch_mat,
chrono.ChVectorD(0, 0, 0), chrono.ChVectorD(0, 0, 1),
600, 600)
patch.SetColor(chrono.ChColor(0.8, 0.8, 1.0))
patch.SetTexture(veh.GetDataFile("terrain/textures/tile4.jpg"), 1200, 1200)
terrain.Initialize()
# -------------------------------------
# Create the vehicle Irrlicht interface
# Create the driver system
# -------------------------------------
app = veh.ChWheeledVehicleIrrApp(gator.GetVehicle(), 'Gator', irr.dimension2du(1000,800))
app.SetSkyBox()
app.AddTypicalLights(irr.vector3df(+130, +130, 150), irr.vector3df(-130, +130, 150), 120,
120, irr.SColorf(0.7, 0.7, 0.7, 1.0), irr.SColorf(0.7, 0.7, 0.7, 1.0))
app.AddTypicalLights(irr.vector3df(+130, -130, 150), irr.vector3df(-130, -130, 150), 120,
120, irr.SColorf(0.7, 0.7, 0.7, 1.0), irr.SColorf(0.7, 0.7, 0.7, 1.0))
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 iy % 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)
# Create the room floor: a simple fixed rigid body with a collision shape
# and a visualization shape
body_floor = chrono.ChBody()
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
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]
# Create the ground body
ground = chrono.ChBodyEasyBox(3, 2, 0.1, 10, True, False)
ground.SetBodyFixed(True)
mysystem.Add(ground)
# Create the sliding body
# Give an initial angular velocity
body = chrono.ChBodyEasyBox(0.5, 0.5, 0.5, 10, True, False)
body.SetBodyFixed(False)
mysystem.Add(body)
body.SetPos(chrono.ChVectorD(-1.25, -0.75, 0.1))
body.SetWvel_loc(chrono.ChVectorD(0.1, 0.1, 0.1))
body_col = chrono.ChColorAsset()
body_col.SetColor(chrono.ChColor(0.9, 0.4, 0.1))
body.AddAsset(body_col)
# Create the plane-plane constraint
# Constrain the sliding body to move and rotate in the x-y plane
# (i.e. the plane whose normal is the z-axis of the specified coord sys)
plane_plane = chrono.ChLinkLockPlanePlane()
plane_plane.Initialize(ground,
body,
chrono.ChCoordsysD(chrono.ChVectorD(-1.25, -0.75, 0), chrono.ChQuaternionD(1, 0, 0, 0)))
mysystem.AddLink(plane_plane)
# Create a linear spring (with default sprint & damping coefficients)
spring = chrono.ChLinkTSDA()
spring.SetSpringCoefficient(100)
spring.SetDampingCoefficient(5)
