def AddFixedObstacles(system):
# Create contact material, of appropriate type. Use default properties
material = None
if (NSC_SMC == chrono.ChContactMethod_NSC):
matNSC = chrono.ChMaterialSurfaceNSC()
#Change NSC material properties as desired
material = matNSC
elif (NSC_SMC == chrono.ChContactMethod_SMC):
matSMC = chrono.ChMaterialSurfaceSMC()
# Change SMC material properties as desired
material = matSMC
else:
raise ("Unvalid Contact Method")
radius = 3
length = 10
obstacle = chrono.ChBodyEasyCylinder(radius, length, 2000, True, True,
material)
obstacle.SetPos(chrono.ChVectorD(-20, 0, -2.7))
obstacle.SetBodyFixed(True)
system.AddBody(obstacle)
for i in range(8):
stoneslab = chrono.ChBodyEasyBox(0.5, 2.5, 0.25, 2000, True, True,
material)
stoneslab.SetPos(chrono.ChVectorD(-1.2 * i + 22, -1.5, -0.05))
stoneslab.SetRot(
chrono.Q_from_AngAxis(15 * chrono.CH_C_DEG_TO_RAD, chrono.VECT_Y))
stoneslab.SetBodyFixed(True)
system.AddBody(stoneslab)
def AddMovingObstacles(system):
# Create contact material, of appropriate type. Use default properties
material = None
if (NSC_SMC == chrono.ChContactMethod_NSC):
matNSC = chrono.ChMaterialSurfaceNSC()
#Change NSC material properties as desired
material = matNSC
elif (NSC_SMC == chrono.ChContactMethod_SMC):
matSMC = chrono.ChMaterialSurfaceSMC()
# Change SMC material properties as desired
material = matSMC
else:
raise ("Unvalid Contact Method")
sizeX = 300
sizeY = 300
height = 0
numObstacles = 10
for i in range(numObstacles):
o_sizeX = 1.0 + 3.0 * chrono.ChRandom()
o_sizeY = 0.3 + 0.2 * chrono.ChRandom()
o_sizeZ = 0.05 + 0.1 * chrono.ChRandom()
obstacle = chrono.ChBodyEasyBox(o_sizeX, o_sizeY, o_sizeZ, 2000.0,
True, True, material)
o_posX = (chrono.ChRandom() - 0.5) * 0.4 * sizeX
o_posY = (chrono.ChRandom() - 0.5) * 0.4 * sizeY
o_posZ = height + 4
rot = chrono.ChQuaternionD(chrono.ChRandom(), chrono.ChRandom(),
chrono.ChRandom(), chrono.ChRandom())
rot.Normalize()
obstacle.SetPos(chrono.ChVectorD(o_posX, o_posY, o_posZ))
obstacle.SetRot(rot)
system.AddBody(obstacle)
def __init__(
self,
pos: Tuple[float, float, float],
size: Tuple[float, float, float],
subdiv: Union[int, Tuple[int, int, int]],
mass: float = 0,
sphere_swept_thickness: Optional[float] = 0.2,
) -> None:
# use the same subdivision on all axis
if not isinstance(subdiv, tuple):
subdiv = (subdiv, ) * 3
self._nodes = None # store nodes in a 3d list, indeces _nodes[x][y][z]
self._elements = None
self._sphere_swept_thickness = sphere_swept_thickness
self._pos = pos
self._size = size
self._subdiv = subdiv
self._mass = mass
self._material = fea.ChContinuumElastic(
) # elastic: no permanet deformation
self._contact_surface = None # fea.ChContactSurfaceMesh()
self._contact_material = chrono.ChMaterialSurfaceSMC()
self._mesh = fea.ChMesh()
def main():
#print("Copyright (c) 2017 projectchrono.org\nChrono version: ", CHRONO_VERSION , "\n\n")
# Create the HMMWV vehicle, set parameters, and initialize
my_hmmwv = veh.HMMWV_Full()
my_hmmwv.SetContactMethod(contact_method)
my_hmmwv.SetChassisFixed(False)
my_hmmwv.SetInitPosition(
chrono.ChCoordsysD(initLoc, chrono.ChQuaternionD(1, 0, 0, 0)))
my_hmmwv.SetPowertrainType(powertrain_model)
my_hmmwv.SetDriveType(drive_type)
my_hmmwv.SetSteeringType(steering_type)
my_hmmwv.SetTireType(tire_model)
my_hmmwv.SetTireStepSize(tire_step_size)
my_hmmwv.Initialize()
my_hmmwv.SetChassisVisualizationType(chassis_vis_type)
my_hmmwv.SetSuspensionVisualizationType(suspension_vis_type)
my_hmmwv.SetSteeringVisualizationType(steering_vis_type)
my_hmmwv.SetWheelVisualizationType(wheel_vis_type)
my_hmmwv.SetTireVisualizationType(tire_vis_type)
# Create the terrain
terrain = veh.RigidTerrain(my_hmmwv.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), 300, 50)
patch.SetTexture(veh.GetDataFile("terrain/textures/tile4.jpg"), 200, 200)
patch.SetColor(chrono.ChColor(0.8, 0.8, 0.5))
terrain.Initialize()
# Create the path-follower, cruise-control driver
# Use a parameterized ISO double lane change (to left)
path = veh.DoubleLaneChangePath(initLoc, 13.5, 4.0, 11.0, 50.0, True)
driver = veh.ChPathFollowerDriver(my_hmmwv.GetVehicle(), path, "my_path",
target_speed)
driver.GetSteeringController().SetLookAheadDistance(5)
driver.GetSteeringController().SetGains(0.8, 0, 0)
driver.GetSpeedController().SetGains(0.4, 0, 0)
driver.Initialize()
# Create the vehicle Irrlicht interface
app = veh.ChWheeledVehicleIrrApp(my_hmmwv.GetVehicle(), 'HMMWV',
irr.dimension2du(1000, 800))
app.SetSkyBox()
app.AddTypicalLights(irr.vector3df(-60, -30, 100),
irr.vector3df(60, 30, 100), 250, 130)
app.AddTypicalLogo(chrono.GetChronoDataFile('logo_pychrono_alpha.png'))
app.SetChaseCamera(chrono.ChVectorD(0.0, 0.0, 1.75), 6.0, 0.5)
app.SetTimestep(step_size)
app.AssetBindAll()
app.AssetUpdateAll()
# Visualization of controller points (sentinel & target)
ballS = app.GetSceneManager().addSphereSceneNode(0.1)
ballT = app.GetSceneManager().addSphereSceneNode(0.1)
ballS.getMaterial(0).EmissiveColor = irr.SColor(0, 255, 0, 0)
ballT.getMaterial(0).EmissiveColor = irr.SColor(0, 0, 255, 0)
# Simulation loop
realtime_timer = chrono.ChRealtimeStepTimer()
while (app.GetDevice().run()):
time = my_hmmwv.GetSystem().GetChTime()
# End simulation
if (time >= t_end):
break
# Update sentinel and target location markers for the path-follower controller.
pS = driver.GetSteeringController().GetSentinelLocation()
pT = driver.GetSteeringController().GetTargetLocation()
ballS.setPosition(irr.vector3df(pS.x, pS.y, pS.z))
ballT.setPosition(irr.vector3df(pT.x, pT.y, pT.z))
# Draw scene
app.BeginScene(True, True, irr.SColor(255, 140, 161, 192))
app.DrawAll()
app.EndScene()
# Get driver inputs
driver_inputs = driver.GetInputs()
# Update modules (process inputs from other modules)
driver.Synchronize(time)
terrain.Synchronize(time)
my_hmmwv.Synchronize(time, driver_inputs, terrain)
app.Synchronize("", driver_inputs)
# Advance simulation for one timestep for all modules
driver.Advance(step_size)
terrain.Advance(step_size)
my_hmmwv.Advance(step_size)
app.Advance(step_size)
# Spin in place for real time to catch up
realtime_timer.Spin(step_size)
return 0
def main():
#print("Copyright (c) 2017 projectchrono.org\nChrono version: ", CHRONO_VERSION , "\n\n")
# Create the M113 vehicle
# ------------------------
vehicle = veh.M113_Vehicle(False, veh.TrackShoeType_SINGLE_PIN,
veh.BrakeType_SIMPLE,
chrono.ChContactMethod_SMC,
veh.CollisionType_NONE)
vehicle.Initialize(chrono.ChCoordsysD(initLoc, initRot))
vehicle.SetChassisVisualizationType(veh.VisualizationType_PRIMITIVES)
vehicle.SetSprocketVisualizationType(veh.VisualizationType_MESH)
vehicle.SetIdlerVisualizationType(veh.VisualizationType_MESH)
vehicle.SetRoadWheelAssemblyVisualizationType(veh.VisualizationType_MESH)
vehicle.SetRoadWheelVisualizationType(veh.VisualizationType_MESH)
vehicle.SetTrackShoeVisualizationType(veh.VisualizationType_MESH)
# Create the powertrain system
# ----------------------------
powertrain = veh.M113_SimpleCVTPowertrain("Powertrain")
vehicle.InitializePowertrain(powertrain)
# Create the terrain
# ------------------
terrain = veh.RigidTerrain(vehicle.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.5, 0.8, 0.5))
terrain.Initialize()
# Create the vehicle Irrlicht interface
# -------------------------------------
app = veh.ChTrackedVehicleIrrApp(vehicle, 'M113',
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, 6.0, 0.5)
app.SetTimestep(step_size)
app.AssetBindAll()
app.AssetUpdateAll()
# Create the interactive driver system
# ------------------------------------
driver = veh.ChIrrGuiDriver(app)
# Set the time response for steering and throttle keyboard inputs.
steering_time = 0.5 # time to go from 0 to +1 (or from 0 to -1)
throttle_time = 1.0 # time to go from 0 to +1
braking_time = 0.3 # time to go from 0 to +1
driver.SetSteeringDelta(render_step_size / steering_time)
driver.SetThrottleDelta(render_step_size / throttle_time)
driver.SetBrakingDelta(render_step_size / braking_time)
driver.Initialize()
# Simulation loop
# ---------------
# Inter-module communication data
shoe_forces_left = veh.TerrainForces(vehicle.GetNumTrackShoes(veh.LEFT))
shoe_forces_right = veh.TerrainForces(vehicle.GetNumTrackShoes(veh.RIGHT))
# Number of simulation steps between miscellaneous events
render_steps = m.ceil(render_step_size / step_size)
# Initialize simulation frame counter and simulation time
step_number = 0
realtime_timer = chrono.ChRealtimeStepTimer()
while (app.GetDevice().run()):
time = vehicle.GetSystem().GetChTime()
app.BeginScene(True, True, irr.SColor(255, 140, 161, 192))
app.DrawAll()
app.EndScene()
# Get driver inputs
driver_inputs = driver.GetInputs()
# Update modules (process inputs from other modules)
driver.Synchronize(time)
terrain.Synchronize(time)
vehicle.Synchronize(time, driver_inputs, shoe_forces_left,
shoe_forces_right)
app.Synchronize("", driver_inputs)
# Advance simulation for one timestep for all modules
driver.Advance(step_size)
terrain.Advance(step_size)
vehicle.Advance(step_size)
app.Advance(step_size)
# Increment frame number
step_number += 1
# Spin in place for real time to catch up
realtime_timer.Spin(step_size)
return 0
# 6. Add constraints # TO DO .... # 7. Add a collision mesh to the skin of the finite element mesh # - Create a ChMaterialSurfaceSMC , it must be assigned to FEA # meshes and rigid bodies. The ChSystemSMC requires it! # - Create a ChContactSurfaceNodeCloud and add to the FEA mesh. # This is the easiest representation of a FEA contact surface: it # simply creates contact spheres per each node. So, no edge-edge cases # can be detected between elements though, but it is enough for # dense finite elements meshes that collide with large objects. # Create a surface material to be shared with some objects mysurfmaterial = chrono.ChMaterialSurfaceSMC() mysurfmaterial.SetYoungModulus(6e4) mysurfmaterial.SetFriction(0.3) mysurfmaterial.SetRestitution(0.2) mysurfmaterial.SetAdhesion(0) # Create the contact surface and add to the mesh, using our SMC contact material mcontactcloud = fea.ChContactSurfaceNodeCloud(mysurfmaterial) mesh.AddContactSurface(mcontactcloud) # Must use this to 'populate' the contact surface use larger point size to match beam section radius mcontactcloud.AddAllNodes(0.01) # 8. Create a collision plane, as a huge box floor = chrono.ChBodyEasyBox(
def main():
#print("Copyright (c) 2017 projectchrono.org\nChrono version: ", CHRONO_VERSION , "\n\n")
# Create systems
# Create the HMMWV vehicle, set parameters, and initialize
my_hmmwv = veh.HMMWV_Full()
my_hmmwv.SetContactMethod(contact_method)
my_hmmwv.SetChassisCollisionType(chassis_collision_type)
my_hmmwv.SetChassisFixed(False)
my_hmmwv.SetInitPosition(chrono.ChCoordsysD(initLoc, initRot))
my_hmmwv.SetPowertrainType(powertrain_model)
my_hmmwv.SetDriveType(drive_type)
my_hmmwv.SetSteeringType(steering_type)
my_hmmwv.SetTireType(tire_model)
my_hmmwv.SetTireStepSize(tire_step_size)
my_hmmwv.Initialize()
my_hmmwv.SetChassisVisualizationType(chassis_vis_type)
my_hmmwv.SetSuspensionVisualizationType(suspension_vis_type)
my_hmmwv.SetSteeringVisualizationType(steering_vis_type)
my_hmmwv.SetWheelVisualizationType(wheel_vis_type)
my_hmmwv.SetTireVisualizationType(tire_vis_type)
# Create the terrain
terrain = veh.RigidTerrain(my_hmmwv.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_hmmwv.GetVehicle(), 'HMMWV',
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, 6.0, 0.5)
app.SetTimestep(step_size)
app.AssetBindAll()
app.AssetUpdateAll()
# Initialize output
try:
os.mkdir(out_dir)
except:
print("Error creating directory ")
# Set up vehicle output
my_hmmwv.GetVehicle().SetChassisOutput(True)
my_hmmwv.GetVehicle().SetSuspensionOutput(0, True)
my_hmmwv.GetVehicle().SetSteeringOutput(0, True)
my_hmmwv.GetVehicle().SetOutput(veh.ChVehicleOutput.ASCII, out_dir,
"output", 0.1)
# Generate JSON information with available output channels
my_hmmwv.GetVehicle().ExportComponentList(out_dir + "/component_list.json")
# Create the interactive driver system
driver = veh.ChIrrGuiDriver(app)
# Set the time response for steering and throttle keyboard inputs.
steering_time = 1.0 # time to go from 0 to +1 (or from 0 to -1)
throttle_time = 1.0 # time to go from 0 to +1
braking_time = 0.3 # time to go from 0 to +1
driver.SetSteeringDelta(render_step_size / steering_time)
driver.SetThrottleDelta(render_step_size / throttle_time)
driver.SetBrakingDelta(render_step_size / braking_time)
driver.Initialize()
# Simulation loop
# Number of simulation steps between miscellaneous events
render_steps = m.ceil(render_step_size / step_size)
debug_steps = m.ceil(debug_step_size / step_size)
# Initialize simulation frame counter and simulation time
step_number = 0
render_frame = 0
if (contact_vis):
app.SetSymbolscale(1e-4)
#app.SetContactsDrawMode(irr.eCh_ContactsDrawMode::CONTACT_FORCES);
realtime_timer = chrono.ChRealtimeStepTimer()
while (app.GetDevice().run()):
time = my_hmmwv.GetSystem().GetChTime()
#End simulation
if (time >= t_end):
break
app.BeginScene(True, True, irr.SColor(255, 140, 161, 192))
app.DrawAll()
app.EndScene()
#Debug logging
if (debug_output and step_number % debug_steps == 0):
print("\n\n============ System Information ============\n")
print("Time = " << time << "\n\n")
#my_hmmwv.DebugLog(OUT_SPRINGS | OUT_SHOCKS | OUT_CONSTRAINTS)
marker_driver = my_hmmwv.GetChassis().GetMarkers()[0].GetAbsCoord(
).pos
marker_com = my_hmmwv.GetChassis().GetMarkers()[1].GetAbsCoord(
).pos
print("Markers\n")
print(" Driver loc: ", marker_driver.x, " ", marker_driver.y,
" ", marker_driver.z)
print(" Chassis COM loc: ", marker_com.x, " ", marker_com.y, " ",
marker_com.z)
# Get driver inputs
driver_inputs = driver.GetInputs()
# Update modules (process inputs from other modules)
driver.Synchronize(time)
terrain.Synchronize(time)
my_hmmwv.Synchronize(time, driver_inputs, terrain)
app.Synchronize(driver.GetInputModeAsString(), driver_inputs)
# Advance simulation for one timestep for all modules
driver.Advance(step_size)
terrain.Advance(step_size)
my_hmmwv.Advance(step_size)
app.Advance(step_size)
# Increment frame number
step_number += 1
# Spin in place for real time to catch up
realtime_timer.Spin(step_size)
return 0
def main():
#print("Copyright (c) 2017 projectchrono.org\nChrono version: ", CHRONO_VERSION , "\n\n")
# Create systems
# Create the FEDA vehicle, set parameters, and initialize
my_feda = veh.FEDA()
my_feda.SetContactMethod(contact_method)
my_feda.SetChassisCollisionType(chassis_collision_type)
my_feda.SetChassisFixed(False)
my_feda.SetInitPosition(chrono.ChCoordsysD(initLoc, initRot))
my_feda.SetPowertrainType(powertrain_model)
my_feda.SetTireType(tire_model)
my_feda.SetTireStepSize(tire_step_size)
my_feda.Initialize()
my_feda.SetChassisVisualizationType(chassis_vis_type)
my_feda.SetSuspensionVisualizationType(suspension_vis_type)
my_feda.SetSteeringVisualizationType(steering_vis_type)
my_feda.SetWheelVisualizationType(wheel_vis_type)
my_feda.SetTireVisualizationType(tire_vis_type)
# Create the terrain
terrain = veh.RigidTerrain(my_feda.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_feda.GetVehicle(), 'HMMWV',
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, 6.0, 0.5)
app.SetTimestep(step_size)
app.AssetBindAll()
app.AssetUpdateAll()
# Create the interactive driver system
driver = veh.ChIrrGuiDriver(app)
# Set the time response for steering and throttle keyboard inputs.
steering_time = 1.0 # time to go from 0 to +1 (or from 0 to -1)
throttle_time = 1.0 # time to go from 0 to +1
braking_time = 0.3 # time to go from 0 to +1
driver.SetSteeringDelta(10 * step_size / steering_time)
driver.SetThrottleDelta(10 * step_size / throttle_time)
driver.SetBrakingDelta(10 * step_size / braking_time)
driver.Initialize()
# Simulation loop
realtime_timer = chrono.ChRealtimeStepTimer()
while (app.GetDevice().run()):
time = my_feda.GetSystem().GetChTime()
app.BeginScene(True, True, irr.SColor(255, 140, 161, 192))
app.DrawAll()
app.EndScene()
# Get driver inputs
driver_inputs = driver.GetInputs()
# Update modules (process inputs from other modules)
driver.Synchronize(time)
terrain.Synchronize(time)
my_feda.Synchronize(time, driver_inputs, terrain)
app.Synchronize(driver.GetInputModeAsString(), driver_inputs)
# Advance simulation for one timestep for all modules
driver.Advance(step_size)
terrain.Advance(step_size)
my_feda.Advance(step_size)
app.Advance(step_size)
# Spin in place for real time to catch up
realtime_timer.Spin(step_size)
return 0
# SEABED # create a box seabed = pychrono.ChBodyEasyBox(100., 0.2, 1., 1000, True) # move box seabed.SetPos(pychrono.ChVectorD(0., -0.1 - d * 2, 0.)) # fix boxed in space seabed.SetBodyFixed(True) # add box to system system.ChSystem.Add(seabed) # CONTACT MATERIAL # define contact material for collision detection material = pychrono.ChMaterialSurfaceSMC() material.SetKn(3e6) # normal stiffness material.SetGn(1.) # normal damping coefficient material.SetFriction(0.3) material.SetRestitution(0.2) material.SetAdhesion(0) # add material to objects seabed.SetMaterialSurface(material) m1.setContactMaterial(material) m2.setContactMaterial(material) # ____ _ ____ _ _ _ _ # | __ ) ___ _ _ _ __ __| | __ _ _ __ _ _ / ___|___ _ __ __| (_) |_(_) ___ _ __ ___ # | _ \ / _ \| | | | '_ \ / _` |/ _` | '__| | | | | / _ \| '_ \ / _` | | __| |/ _ \| '_ \/ __| # | |_) | (_) | |_| | | | | (_| | (_| | | | |_| | |__| (_) | | | | (_| | | |_| | (_) | | | \__ \
def main():
print("Copyright (c) 2017 projectchrono.org" + "\n\n")
# Create systems
# Create the HMMWV vehicle, set parameters, and initialize
my_hmmwv = veh.HMMWV_Full()
my_hmmwv.SetContactMethod(contact_method)
my_hmmwv.SetChassisCollisionType(chassis_collision_type)
my_hmmwv.SetChassisFixed(False)
my_hmmwv.SetInitPosition(chrono.ChCoordsysD(initLoc, initRot))
my_hmmwv.SetPowertrainType(powertrain_model)
my_hmmwv.SetDriveType(drive_type)
my_hmmwv.SetSteeringType(steering_type)
my_hmmwv.SetTireType(tire_model)
my_hmmwv.SetTireStepSize(tire_step_size)
my_hmmwv.Initialize()
my_hmmwv.SetChassisVisualizationType(chassis_vis_type)
my_hmmwv.SetSuspensionVisualizationType(suspension_vis_type)
my_hmmwv.SetSteeringVisualizationType(steering_vis_type)
my_hmmwv.SetWheelVisualizationType(wheel_vis_type)
my_hmmwv.SetTireVisualizationType(tire_vis_type)
# Create the terrain
terrain = veh.RigidTerrain(my_hmmwv.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_hmmwv.GetVehicle(), 'HMMWV',
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, 6.0, 0.5)
app.SetTimestep(step_size)
app.AssetBindAll()
app.AssetUpdateAll()
# Initialize output
try:
os.mkdir(out_dir)
except:
print("Error creating directory ")
# Set up vehicle output
my_hmmwv.GetVehicle().SetChassisOutput(True)
my_hmmwv.GetVehicle().SetSuspensionOutput(0, True)
my_hmmwv.GetVehicle().SetSteeringOutput(0, True)
my_hmmwv.GetVehicle().SetOutput(veh.ChVehicleOutput.ASCII, out_dir,
"output", 0.1)
# Generate JSON information with available output channels
my_hmmwv.GetVehicle().ExportComponentList(out_dir + "/component_list.json")
# Create the interactive driver system
driver = veh.ChIrrGuiDriver(app)
# Set the time response for steering and throttle keyboard inputs.
steering_time = 1.0 # time to go from 0 to +1 (or from 0 to -1)
throttle_time = 1.0 # time to go from 0 to +1
braking_time = 0.3 # time to go from 0 to +1
driver.SetSteeringDelta(render_step_size / steering_time)
driver.SetThrottleDelta(render_step_size / throttle_time)
driver.SetBrakingDelta(render_step_size / braking_time)
driver.Initialize()
# Simulation loop
# Number of simulation steps between miscellaneous events
render_steps = m.ceil(render_step_size / step_size)
debug_steps = m.ceil(debug_step_size / step_size)
# Initialize simulation frame counter and simulation time
step_number = 0
render_frame = 0
if (contact_vis):
app.SetSymbolscale(1e-4)
# app.SetContactsDrawMode(irr.eCh_ContactsDrawMode::CONTACT_FORCES);
# ---------------------------------------------
# Create a sensor manager and add a point light
# ---------------------------------------------
manager = sens.ChSensorManager(my_hmmwv.GetSystem())
manager.scene.AddPointLight(chrono.ChVectorF(0, 0, 100),
chrono.ChVectorF(2, 2, 2), 5000)
manager.SetKeyframeSizeFromTimeStep(.001, 1 / 5)
# ------------------------------------------------
# Create a camera and add it to the sensor manager
# ------------------------------------------------
fov = 1.408
lag = 0
update_rate = 5
exposure_time = 1 / update_rate
offset_pose = chrono.ChFrameD(chrono.ChVectorD(-5, 0, 2))
cam = sens.ChCameraSensor(
my_hmmwv.GetChassisBody(), # 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(0);
# cam.SetCollectionWindow(0);
# Visualizes the image
if vis:
cam.PushFilter(
sens.ChFilterVisualize(image_width, image_height, "HMMWV Camera"))
# Save the current image to a png file at the specified path
if save:
cam.PushFilter(sens.ChFilterSave(out_dir + "cam/"))
# Add a camera to a sensor manager
manager.AddSensor(cam)
# ----------------------------------------------
# Create an IMU sensor and add it to the manager
# ----------------------------------------------
offset_pose = chrono.ChFrameD(
chrono.ChVectorD(-8, 0, 1),
chrono.Q_from_AngAxis(0, chrono.ChVectorD(0, 1, 0)))
imu = sens.ChIMUSensor(
my_hmmwv.GetChassisBody(), # body imu is attached to
imu_update_rate, # update rate in Hz
offset_pose, # offset pose
imu_noise_none # noise model
)
imu.SetName("IMU Sensor")
imu.SetLag(imu_lag)
imu.SetCollectionWindow(imu_collection_time)
# Provides the host access to the imu data
imu.PushFilter(sens.ChFilterIMUAccess())
# Add the imu to the sensor manager
manager.AddSensor(imu)
# ----------------------------------------------
# Create an GPS sensor and add it to the manager
# ----------------------------------------------
offset_pose = chrono.ChFrameD(
chrono.ChVectorD(-8, 0, 1),
chrono.Q_from_AngAxis(0, chrono.ChVectorD(0, 1, 0)))
gps = sens.ChGPSSensor(
my_hmmwv.GetChassisBody(), # body imu is attached to
gps_update_rate, # update rate in Hz
offset_pose, # offset pose
gps_reference,
gps_noise_none # noise model
)
gps.SetName("GPS Sensor")
gps.SetLag(gps_lag)
gps.SetCollectionWindow(gps_collection_time)
# Provides the host access to the gps data
gps.PushFilter(sens.ChFilterGPSAccess())
# Add the gps to the sensor manager
manager.AddSensor(gps)
realtime_timer = chrono.ChRealtimeStepTimer()
while (app.GetDevice().run()):
time = my_hmmwv.GetSystem().GetChTime()
#End simulation
if (time >= t_end):
break
if (step_number % render_steps == 0):
app.BeginScene(True, True, irr.SColor(255, 140, 161, 192))
app.DrawAll()
app.EndScene()
#Debug logging
if (debug_output and step_number % debug_steps == 0):
print("\n\n============ System Information ============\n")
print("Time = " << time << "\n\n")
#my_hmmwv.DebugLog(OUT_SPRINGS | OUT_SHOCKS | OUT_CONSTRAINTS)
marker_driver = my_hmmwv.GetChassis().GetMarkers()[0].GetAbsCoord(
).pos
marker_com = my_hmmwv.GetChassis().GetMarkers()[1].GetAbsCoord(
).pos
print("Markers\n")
print(" Driver loc: ", marker_driver.x, " ", marker_driver.y,
" ", marker_driver.z)
print(" Chassis COM loc: ", marker_com.x, " ", marker_com.y, " ",
marker_com.z)
# Get driver inputs
driver_inputs = driver.GetInputs()
# Update modules (process inputs from other modules)
driver.Synchronize(time)
terrain.Synchronize(time)
my_hmmwv.Synchronize(time, driver_inputs, terrain)
app.Synchronize(driver.GetInputModeAsString(), driver_inputs)
# Advance simulation for one timestep for all modules
driver.Advance(step_size)
terrain.Advance(step_size)
my_hmmwv.Advance(step_size)
app.Advance(step_size)
# Update sensor manager
# Will render/save/filter automatically
manager.Update()
# Increment frame number
step_number += 1
# Spin in place for real time to catch up
realtime_timer.Spin(step_size)
return 0