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 createWheels(self):
# Left Wheel
self.Body_wheel_L = chrono.ChBody()
self.Body_wheel_L.SetBodyFixed(False)
self.Body_wheel_L.SetPos(chrono.ChVectorD(0, -0.5, 0))
self.Body_wheel_L.SetRot(chrono.ChQuaternionD(0.707, -0.707, 0, 0))
self.mysystem.Add(self.Body_wheel_L)
self.Shape_Wheel_L = chrono.ChCylinderShape()
self.Shape_Wheel_L.GetCylinderGeometry().radius = 0.03
self.Shape_Wheel_L.GetCylinderGeometry().p1 = chrono.ChVectorD(
0, 0.1, 0)
self.Shape_Wheel_L.GetCylinderGeometry().p2 = chrono.ChVectorD(
0, -0.01, 0)
self.Body_wheel_L.AddAsset(self.Shape_Wheel_L)
self.motor_L = chrono.ChLinkMotorRotationSpeed()
self.motorpos_L = chrono.ChFrameD(chrono.ChVectorD(0, -0.5, 0))
self.motor_L.Initialize(self.mbody1, self.Body_wheel_L,
self.motorpos_L)
self.mysystem.Add(self.motor_L)
# Right Wheel
self.Body_wheel_R = chrono.ChBody()
self.Body_wheel_R.SetBodyFixed(False)
self.Body_wheel_R.SetPos(chrono.ChVectorD(0, -0.5, -0.3))
self.Body_wheel_R.SetRot(chrono.ChQuaternionD(0.707, -0.707, 0, 0))
self.mysystem.Add(self.Body_wheel_R)
self.Shape_Wheel_R = chrono.ChCylinderShape()
self.Shape_Wheel_R.GetCylinderGeometry().radius = 0.03
self.Shape_Wheel_R.GetCylinderGeometry().p1 = chrono.ChVectorD(
0, 0.1, 0)
self.Shape_Wheel_R.GetCylinderGeometry().p2 = chrono.ChVectorD(
0, -0.01, 0)
self.Body_wheel_R.AddAsset(self.Shape_Wheel_R)
self.motor_R = chrono.ChLinkMotorRotationSpeed()
self.motorpos_R = chrono.ChFrameD(chrono.ChVectorD(0, -0.5, 0))
self.motor_R.Initialize(self.mbody1, self.Body_wheel_R,
self.motorpos_R)
self.mysystem.Add(self.motor_R)
def createGround(self):
# Create ground
self.ground = chrono.ChBody()
self.mysystem.AddBody(self.ground)
self.ground.SetIdentifier(-1)
self.ground.SetPos(chrono.ChVectorD(0,-1,0))
self.ground.SetName("ground")
self.ground.SetBodyFixed(True)
self.box_ground = chrono.ChBoxShape()
self.box_ground.GetBoxGeometry().Size = chrono.ChVectorD(5,0.01,5)
self.ground.AddAsset(self.box_ground)
def createBody(self):
# Create a fixed rigid body
self.mbody1 = chrono.ChBody()
self.mbody1.SetBodyFixed(False)
self.mbody1.SetIdentifier(1)
self.mbody1.SetPos(chrono.ChVectorD(0, 0, -0.2))
self.mbody1.SetRot(chrono.ChQuaternionD(0, 0, 0, 1))
self.mysystem.Add(self.mbody1)
self.mboxasset = chrono.ChBoxShape()
self.mboxasset.GetBoxGeometry().Size = chrono.ChVectorD(0.2, 0.5, 0.1)
self.mbody1.AddAsset(self.mboxasset)
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
# Global parameters for tire
tire_rad = 0.8
tire_vel_z0 = -3
tire_center = chrono.ChVectorD(0, 0.02 + tire_rad, -1.5)
tire_w0 = tire_vel_z0 / tire_rad
# ----------------------------
# Create the mechanical system
# ----------------------------
mysystem = chrono.ChSystemSMC()
# Create the ground
ground = chrono.ChBody()
ground.SetBodyFixed(True)
mysystem.Add(ground)
# Create the rigid body with contact mesh
body = chrono.ChBody()
mysystem.Add(body)
body.SetMass(500)
body.SetInertiaXX(chrono.ChVectorD(20, 20, 20))
body.SetPos(tire_center + chrono.ChVectorD(0, 0.3, 0))
# Load mesh
mesh = chrono.ChTriangleMeshConnected()
mesh.LoadWavefrontMesh(
chrono.GetChronoDataFile('models/tractor_wheel/tractor_wheel.obj'))
nbricks_on_x = 4
nbricks_on_y = 6
size_brick_x = 0.15
size_brick_y = 0.12
size_brick_z = 0.12
density_brick = 1000
# kg/m^3
mass_brick = density_brick * size_brick_x * size_brick_y * size_brick_z
inertia_brick = 2 / 5 * (pow(size_brick_x, 2)) * mass_brick
# to do: compute separate xx,yy,zz inertias
for ix in range(0, nbricks_on_x):
for iy in range(0, nbricks_on_y):
# create it
body_brick = chrono.ChBody()
# set initial position
body_brick.SetPos(
chrono.ChVectorD(ix * size_brick_x, (iy + 0.5) * size_brick_y, 0))
# set mass properties
body_brick.SetMass(mass_brick)
body_brick.SetInertiaXX(
chrono.ChVectorD(inertia_brick, inertia_brick, inertia_brick))
# 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
# Optionally, set a function that gets the curvilinear
# abscyssa s of the line, as a function of time s(t).
# By default it was simply s=t.
mspacefx = chrono.ChFunction_Ramp(0, 0.5)
mtrajectory.Set_space_fx(mspacefx)
mysystem.Add(mtrajectory)
#
# EXAMPLE 2:
#
# Create a ChBody that contains the trajectory
mwheel = chrono.ChBody()
mwheel.SetPos(chrono.ChVectorD(-3, 2, 0))
mysystem.Add(mwheel)
# Create a motor that spins the wheel
my_motor = chrono.ChLinkMotorRotationSpeed()
my_motor.Initialize(mwheel, mfloor, chrono.ChFrameD(chrono.ChVectorD(-3, 2,
0)))
my_angularspeed = chrono.ChFunction_Const(chrono.CH_C_PI / 4.0)
my_motor.SetMotorFunction(my_angularspeed)
mysystem.Add(my_motor)
# Create a ChLinePath geometry, and insert sub-paths:
mglyph = chrono.ChLinePath()
ms1 = chrono.ChLineSegment(chrono.ChVectorD(-0.5, -0.5, 0),
chrono.ChVectorD(0.5, -0.5, 0))
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
# Recall that Irrlicht uses a left-hand frame, so everything is rendered with # left and right flipped. #----------------------------------------------------------------------------- import pychrono.core as chrono import pychrono.irrlicht as chronoirr import math system = chrono.ChSystemNSC() system.Set_G_acc(chrono.ChVectorD(0, -9.81, 0)) # Create the ground body with two visualization cylinders ground = chrono.ChBody() system.Add(ground) ground.SetIdentifier(-1) ground.SetBodyFixed(True) ground.SetCollide(False) cyl_1 = chrono.ChCylinderShape() cyl_1.GetCylinderGeometry().p1 = chrono.ChVectorD(0, 0, 1.2) cyl_1.GetCylinderGeometry().p2 = chrono.ChVectorD(0, 0, 0.8) cyl_1.GetCylinderGeometry().rad = 0.2 ground.AddAsset(cyl_1) cyl_2 = chrono.ChCylinderShape() cyl_2.GetCylinderGeometry().p1 = chrono.ChVectorD(0, 0, -1.2) cyl_2.GetCylinderGeometry().p2 = chrono.ChVectorD(0, 0, -0.8) cyl_2.GetCylinderGeometry().rad = 0.2
msphereBody.AddAsset(textureasset) # assets can be shared
# Set initial speed: spinning in vertical direction
msphereBody.SetWvel_par(chrono.ChVectorD(0, 20, 0))
# Add to the system
mphysicalSystem.Add(msphereBody)
# Notes:
# - setting nonzero spinning friction and/or setting nonzero rolling friction
# affects the speed of the solver (each contact eats 2x of CPU time repsect to the
# case of simple sliding/staic contact)
# - avoid using zero spinning friction with nonzero rolling friction.
# Create a container fixed to ground
bin = chrono.ChBody()
bin.SetPos(chrono.ChVectorD(0, -1, 0))
bin.SetBodyFixed(True)
bin.SetCollide(True)
# Set rolling and spinning friction coefficients for the container.
# By default, the composite material will use the minimum value for an interacting collision pair.
bin_mat = chrono.ChMaterialSurfaceNSC()
bin_mat.SetRollingFriction(1)
bin_mat.SetSpinningFriction(1)
# Add collision geometry and visualization shapes for the floor and the 4 walls
bin.GetCollisionModel().ClearModel()
bin.GetCollisionModel().AddBox(bin_mat, 20. / 2., 1. / 2., 20. / 2.,
chrono.ChVectorD(0, 0, 0))
bin.GetCollisionModel().AddBox(bin_mat, 1. / 2., 2. / 2., 20.99 / 2.,
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]
mysystem = chrono.ChSystemNSC() cpi = ChronoPandaInterface.OnscreenRender() # Create a fixed rigid body chrono.ChCollisionModel.SetDefaultSuggestedEnvelope(0.005) chrono.ChCollisionModel.SetDefaultSuggestedMargin(0.005) mysystem.SetMaxItersSolverSpeed(70) brick_material = chrono.ChMaterialSurfaceNSC() brick_material.SetFriction(0.5) brick_material.SetDampingF(0.2) brick_material.SetCompliance(0.0000001) brick_material.SetComplianceT(0.0000001) mbody1 = chrono.ChBody() mbody1.SetBodyFixed(True) mbody1.SetPos(chrono.ChVectorD(0, 0, 0)) q = chrono.ChQuaternionD() q.Q_from_AngAxis(math.pi / 8, chrono.ChVectorD(0, 1, 0)) mbody1.SetRot(q) mbody1.SetMass(1) mbody1.SetMaterialSurface(brick_material) mbody1.GetCollisionModel().ClearModel() mbody1.GetCollisionModel().AddBox(1, 1, 1) # must set half sizes mbody1.GetCollisionModel().BuildModel() mbody1.SetCollide(True) ChronoPandaInterface.PandaCube(cpi, mbody1, 2, WHITE) mysystem.Add(mbody1)
b_mat.SetFriction(0.5)
o_mat = chrono.ChMaterialSurfaceSMC()
o_mat.SetRestitution(0.9)
o_mat.SetFriction(0.4)
ground_mat = g_mat
ball_mat = b_mat
obst_mat = o_mat
time_step = 1e-4
frame_skip = 100
sys.Set_G_acc(chrono.ChVectorD(0, -9.8, 0))
# Create the ground body with a plate and side walls (both collision and visualization).
ground = chrono.ChBody()
sys.AddBody(ground)
ground.SetCollide(True)
ground.SetBodyFixed(True)
ground.GetCollisionModel().ClearModel()
ground.GetCollisionModel().AddBox(ground_mat, 5.0, 1.0, 5.0,
chrono.ChVectorD(0, -1, 0))
ground.GetCollisionModel().AddBox(ground_mat, 0.1, 1.0, 5.1,
chrono.ChVectorD(-5, 0, 0))
ground.GetCollisionModel().AddBox(ground_mat, 0.1, 1.0, 5.1,
chrono.ChVectorD(5, 0, 0))
ground.GetCollisionModel().AddBox(ground_mat, 5.1, 1.0, 0.1,
chrono.ChVectorD(0, 0, -5))
ground.GetCollisionModel().AddBox(ground_mat, 5.1, 1.0, 0.1,
chrono.ChVectorD(0, 1, 5))
print("Example: create a system and visualize it in realtime 3D")
# Change this path to asset path, if running from other working dir.
# It must point to the data folder, containing GUI assets (textures, fonts, meshes, etc.)
chrono.SetChronoDataPath("../../../data/")
# ---------------------------------------------------------------------
#
# Create the simulation system and add items
#
mysystem = chrono.ChSystemNSC()
# Create a fixed rigid body
mbody1 = chrono.ChBody()
mbody1.SetBodyFixed(True)
mbody1.SetPos(chrono.ChVectorD(0, 0, -0.2))
mysystem.Add(mbody1)
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()
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
def main():
# -----------------
# Create the system
# -----------------
mphysicalSystem = chrono.ChSystemNSC()
# -----------------------------------
# add a mesh to be sensed by a camera
# -----------------------------------
mmesh = chrono.ChTriangleMeshConnected()
mmesh.LoadWavefrontMesh(
chrono.GetChronoDataFile("vehicle/hmmwv/hmmwv_chassis.obj"), False,
True)
# scale to a different size
mmesh.Transform(chrono.ChVectorD(0, 0, 0), chrono.ChMatrix33D(2))
trimesh_shape = chrono.ChTriangleMeshShape()
trimesh_shape.SetMesh(mmesh)
trimesh_shape.SetName("HMMWV Chassis Mesh")
trimesh_shape.SetStatic(True)
mesh_body = chrono.ChBody()
mesh_body.SetPos(chrono.ChVectorD(0, 0, 0))
mesh_body.AddAsset(trimesh_shape)
mesh_body.SetBodyFixed(True)
mphysicalSystem.Add(mesh_body)
# -----------------------
# Create a sensor manager
# -----------------------
manager = sens.ChSensorManager(mphysicalSystem)
intensity = 1.0
manager.scene.AddPointLight(
chrono.ChVectorF(2, 2.5, 100),
chrono.ChVectorF(intensity, intensity, intensity), 500.0)
manager.scene.AddPointLight(
chrono.ChVectorF(9, 2.5, 100),
chrono.ChVectorF(intensity, intensity, intensity), 500.0)
manager.scene.AddPointLight(
chrono.ChVectorF(16, 2.5, 100),
chrono.ChVectorF(intensity, intensity, intensity), 500.0)
manager.scene.AddPointLight(
chrono.ChVectorF(23, 2.5, 100),
chrono.ChVectorF(intensity, intensity, intensity), 500.0)
# manager.SetKeyframeSizeFromTimeStep(.001,1/exposure_time)
# ------------------------------------------------
# Create a camera and add it to the sensor manager
# ------------------------------------------------
offset_pose = chrono.ChFrameD(
chrono.ChVectorD(-5, 0, 2),
chrono.Q_from_AngAxis(2, chrono.ChVectorD(0, 1, 0)))
cam = sens.ChCameraSensor(
mesh_body, # body camera is attached to
update_rate, # update rate in Hz
offset_pose, # offset pose
image_width, # image width
image_height, # image height
fov # camera's horizontal field of view
)
cam.SetName("Camera Sensor")
cam.SetLag(lag)
cam.SetCollectionWindow(exposure_time)
# ------------------------------------------------------------------
# Create a filter graph for post-processing the data from the camera
# ------------------------------------------------------------------
if noise_model == "CONST_NORMAL":
cam.PushFilter(sens.ChFilterCameraNoiseConstNormal(0.0, 0.02))
elif noise_model == "PIXEL_DEPENDENT":
cam.PushFilter(sens.ChFilterCameraNoisePixDep(0, 0.02, 0.03))
elif noise_model == "NONE":
# Don't add any noise models
pass
# Renders the image at current point in the filter graph
if vis:
cam.PushFilter(
sens.ChFilterVisualize(image_width, image_height,
"Before Grayscale Filter"))
# Provides the host access to this RGBA8 buffer
cam.PushFilter(sens.ChFilterRGBA8Access())
# Save the current image to a png file at the specified path
if save:
cam.PushFilter(sens.ChFilterSave(out_dir + "rgb/"))
# Filter the sensor to grayscale
cam.PushFilter(sens.ChFilterGrayscale())
# Render the buffer again to see the new grayscaled image
if vis:
cam.PushFilter(
sens.ChFilterVisualize(int(image_width / 2), int(image_height / 2),
"Grayscale Image"))
# Save the grayscaled image at the specified path
if save:
cam.PushFilter(sens.ChFilterSave(out_dir + "gray/"))
# Resizes the image to the provided width and height
cam.PushFilter(
sens.ChFilterImageResize(int(image_width / 2), int(image_height / 2)))
# Access the grayscaled buffer as R8 pixels
cam.PushFilter(sens.ChFilterR8Access())
# add sensor to manager
manager.AddSensor(cam)
# ---------------
# Simulate system
# ---------------
orbit_radius = 10
orbit_rate = 0.5
ch_time = 0.0
t1 = time.time()
while (ch_time < end_time):
cam.SetOffsetPose(
chrono.ChFrameD(
chrono.ChVectorD(
-orbit_radius * math.cos(ch_time * orbit_rate),
-orbit_radius * math.sin(ch_time * orbit_rate), 1),
chrono.Q_from_AngAxis(ch_time * orbit_rate,
chrono.ChVectorD(0, 0, 1))))
# Access the RGBA8 buffer from the camera
rgba8_buffer = cam.GetMostRecentRGBA8Buffer()
if (rgba8_buffer.HasData()):
rgba8_data = rgba8_buffer.GetRGBA8Data()
print('RGBA8 buffer recieved from cam. Camera resolution: {0}x{1}'\
.format(rgba8_buffer.Width, rgba8_buffer.Height))
print('First Pixel: {0}'.format(rgba8_data[0, 0, :]))
# Update sensor manager
# Will render/save/filter automatically
manager.Update()
# Perform step of dynamics
mphysicalSystem.DoStepDynamics(step_size)
# Get the current time of the simulation
ch_time = mphysicalSystem.GetChTime()
print("Sim time:", end_time, "Wall time:", time.time() - t1)
mysystem = chrono.ChSystemNSC() mysystem.Set_G_acc(chrono.ChVectorD(0, 0, 0)) # Set the half-length of the two shafts hl = 2 # Set the bend angle between the two shafts (positive rotation # about the global X axis) angle = math.pi / 6. cosa = math.cos(angle) sina = math.sin(angle) rot = chrono.Q_from_AngX(angle) # Create the ground body # ---------------------- ground = chrono.ChBody() ground.SetIdentifier(-1) ground.SetBodyFixed(True) ground.SetCollide(False) mysystem.Add(ground) # attach visualization assets to represent the revolute and cylindrical # joints that connect the two shafts to ground cyl_1 = chrono.ChCylinderShape() cyl_1.GetCylinderGeometry().p1 = chrono.ChVectorD(0, 0, -hl - 0.2) cyl_1.GetCylinderGeometry().p2 = chrono.ChVectorD(0, 0, -hl + 0.2) cyl_1.GetCylinderGeometry().rad = 0.3 ground.AddAsset(cyl_1) cyl_2 = chrono.ChCylinderShape() cyl_2.GetCylinderGeometry().p1 = chrono.ChVectorD(0, -(hl - 0.2) * sina,
def main():
chrono.SetChronoDataPath("../../../data/")
# -----------------
# Create the system
# -----------------
mphysicalSystem = chrono.ChSystemNSC()
# ----------------------------------
# add a mesh to be sensed by a lidar
# ----------------------------------
mmesh = chrono.ChTriangleMeshConnected()
mmesh.LoadWavefrontMesh(
chrono.GetChronoDataFile("vehicle/hmmwv/hmmwv_chassis.obj"), False,
True)
mmesh.Transform(chrono.ChVectorD(0, 0, 0),
chrono.ChMatrix33D(2)) # scale to a different size
trimesh_shape = chrono.ChTriangleMeshShape()
trimesh_shape.SetMesh(mmesh)
trimesh_shape.SetName("HMMWV Chassis Mesh")
trimesh_shape.SetStatic(True)
mesh_body = chrono.ChBody()
mesh_body.SetPos(chrono.ChVectorD(0, 0, 0))
mesh_body.AddAsset(trimesh_shape)
mesh_body.SetBodyFixed(True)
mphysicalSystem.Add(mesh_body)
# -----------------------
# Create a sensor manager
# -----------------------
manager = sens.ChSensorManager(mphysicalSystem)
manager.SetKeyframeSizeFromTimeStep(.001, 1 / collection_time)
# ------------------------------------------------
# Create a lidar and add it to the sensor manager
# ------------------------------------------------
offset_pose = chrono.ChFrameD(
chrono.ChVectorD(-8, 0, 1),
chrono.Q_from_AngAxis(0, chrono.ChVectorD(0, 1, 0)))
lidar = sens.ChLidarSensor(
mesh_body, # body lidar is attached to
update_rate, # scanning rate in Hz
offset_pose, # offset pose
horizontal_samples, # number of horizontal samples
vertical_samples, # number of vertical channels
horizontal_fov, # horizontal field of view
max_vert_angle, # vertical field of view
min_vert_angle,
100.0, #max lidar range
sample_radius, # sample radius
divergence_angle, # divergence angle
return_mode, # return mode for the lidar
lens_model # method/model to use for generating data
)
lidar.SetName("Lidar Sensor")
lidar.SetLag(lag)
lidar.SetCollectionWindow(collection_time)
# -----------------------------------------------------------------
# Create a filter graph for post-processing the data from the lidar
# -----------------------------------------------------------------
if noise_model == "CONST_NORMAL_XYZI":
lidar.PushFilter(sens.ChFilterLidarNoiseXYZI(0.01, 0.001, 0.001, 0.01))
elif noise_model == "NONE":
# Don't add any noise models
pass
if vis:
# Visualize the raw lidar data
lidar.PushFilter(
sens.ChFilterVisualize(horizontal_samples, vertical_samples,
"Raw Lidar Depth Data"))
# Provides the host access to the Depth,Intensity data
lidar.PushFilter(sens.ChFilterDIAccess())
# Convert Depth,Intensity data to XYZI point cloud data
lidar.PushFilter(sens.ChFilterPCfromDepth())
if vis:
# Visualize the point cloud
lidar.PushFilter(
sens.ChFilterVisualizePointCloud(640, 480, 1.0,
"Lidar Point Cloud"))
# Provides the host access to the XYZI data
lidar.PushFilter(sens.ChFilterXYZIAccess())
# Add the lidar to the sensor manager
manager.AddSensor(lidar)
# ---------------
# Simulate system
# ---------------
orbit_radius = 5
orbit_rate = 0.2
ch_time = 0.0
render_time = 0
t1 = time.time()
while (ch_time < end_time):
lidar.SetOffsetPose(
chrono.ChFrameD(
chrono.ChVectorD(
-orbit_radius * math.cos(ch_time * orbit_rate),
-orbit_radius * math.sin(ch_time * orbit_rate), 1),
chrono.Q_from_AngAxis(ch_time * orbit_rate,
chrono.ChVectorD(0, 0, 1))))
# Access the XYZI buffer from lidar
xyzi_buffer = lidar.GetMostRecentXYZIBuffer()
if xyzi_buffer.HasData():
xyzi_data = xyzi_buffer.GetXYZIData()
print('XYZI buffer recieved from lidar. Lidar resolution: {0}x{1}'\
.format(xyzi_buffer.Width, xyzi_buffer.Height))
print('Max Value: {0}'.format(np.max(xyzi_data)))
# Update sensor manager
# Will render/save/filter automatically
manager.Update()
# Perform step of dynamics
mphysicalSystem.DoStepDynamics(step_size)
# Get the current time of the simulation
ch_time = mphysicalSystem.GetChTime()
print("Sim time:", end_time, "Wall time:", time.time() - t1)
