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(chrono.ChContactMethod_SMC)
my_hmmwv.SetInitPosition(
chrono.ChCoordsysD(chrono.ChVectorD(-5, -2, 0.6),
chrono.ChQuaternionD(1, 0, 0, 0)))
my_hmmwv.SetPowertrainType(veh.PowertrainModelType_SHAFTS)
my_hmmwv.SetDriveType(veh.DrivelineType_AWD)
my_hmmwv.SetTireType(veh.TireModelType_RIGID)
my_hmmwv.Initialize()
my_hmmwv.SetChassisVisualizationType(veh.VisualizationType_NONE)
my_hmmwv.SetSuspensionVisualizationType(veh.VisualizationType_PRIMITIVES)
my_hmmwv.SetSteeringVisualizationType(veh.VisualizationType_PRIMITIVES)
my_hmmwv.SetWheelVisualizationType(veh.VisualizationType_NONE)
my_hmmwv.SetTireVisualizationType(veh.VisualizationType_MESH)
# Create the (custom) driver
driver = MyDriver(my_hmmwv.GetVehicle(), 0.5)
driver.Initialize()
# Create the SCM deformable terrain patch
terrain = veh.SCMDeformableTerrain(my_hmmwv.GetSystem())
terrain.SetSoilParameters(
2e6, # Bekker Kphi
0, # Bekker Kc
1.1, # Bekker n exponent
0, # Mohr cohesive limit (Pa)
30, # Mohr friction limit (degrees)
0.01, # Janosi shear coefficient (m)
2e8, # Elastic stiffness (Pa/m), before plastic yield
3e4 # Damping (Pa s/m), proportional to negative vertical speed (optional)
)
# Optionally, enable moving patch feature (single patch around vehicle chassis)
terrain.AddMovingPatch(my_hmmwv.GetChassisBody(),
chrono.ChVectorD(0, 0,
0), chrono.ChVectorD(5, 3, 1))
# Set plot type for SCM (false color plotting)
terrain.SetPlotType(veh.SCMDeformableTerrain.PLOT_SINKAGE, 0, 0.1)
# Initialize the SCM terrain, specifying the initial mesh grid
terrain.Initialize(terrainLength, terrainWidth, delta)
# Create the vehicle Irrlicht interface
app = veh.ChWheeledVehicleIrrApp(my_hmmwv.GetVehicle(),
'HMMWV Deformable Soil Demo',
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(chrono.ChVectorD(0.0, 0.0, 1.75), 6.0, 0.5)
app.SetTimestep(step_size)
app.AssetBindAll()
app.AssetUpdateAll()
# Simulation loop
while (app.GetDevice().run()):
time = my_hmmwv.GetSystem().GetChTime()
# End simulation
if (time >= 4):
break
# 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)
return 0
# Create motor
motor = chrono.ChLinkMotorRotationAngle()
motor.SetSpindleConstraint(chrono.ChLinkMotorRotation.SpindleConstraint_OLDHAM)
motor.SetAngleFunction(chrono.ChFunction_Ramp(0, math.pi / 4))
motor.Initialize(body, ground,
chrono.ChFrameD(tire_center, chrono.Q_from_AngY(math.pi / 2)))
mysystem.Add(motor)
# ------------------------
# Create SCM terrain patch
# ------------------------
# Note that SCMDeformableTerrain uses a default ISO reference frame (Z up). Since the mechanism is modeled here in
# a Y-up global frame, we rotate the terrain plane by -90 degrees about the X axis.
terrain = veh.SCMDeformableTerrain(mysystem)
terrain.SetPlane(
chrono.ChCoordsysD(chrono.ChVectorD(0, 0.2, 0),
chrono.Q_from_AngX(-math.pi / 2)))
terrain.Initialize(2.0, 6.0, 0.04)
my_params = MySoilParams()
if var_params:
# Location-dependent soil properties
terrain.RegisterSoilParametersCallback(my_params)
else:
# Constant soil properties
terrain.SetSoilParameters(
0.2e6, # Bekker Kphi
0, # Bekker Kc
1.1, # Bekker n exponent
def reset(self):
n = 10
b1 = 0
b2 = 0
r1 = n
r2 = n
r3 = n
r4 = n
r5 = n
t1 = 0
t2 = 0
t3 = 0
c = 0
self.assets = AssetList(b1, b2, r1, r2, r3, r4, r5, t1, t2, t3, c)
# Create systems
self.system = chrono.ChSystemNSC()
self.system.Set_G_acc(chrono.ChVectorD(0, 0, -9.81))
self.system.SetSolverType(chrono.ChSolver.Type_BARZILAIBORWEIN)
self.system.SetSolverMaxIterations(150)
self.system.SetMaxPenetrationRecoverySpeed(4.0)
# Create the terrain
rigid_terrain = False
# Ghost Terrain: rigid, vis, no contact
self.ghost_terrain = veh.RigidTerrain(self.system)
patch_mat = chrono.ChMaterialSurfaceNSC()
patch_mat.SetFriction(0.9)
patch_mat.SetRestitution(0.01)
# Real Terrain: deformable, no vis, contact
self.terrain = veh.SCMDeformableTerrain(self.system, False)
# Parameters
randpar = random.randint(1, 6)
params = SCMParameters()
params.InitializeParametersAsHard()
params.SetParameters(self.terrain)
# Bulldozing
self.terrain.EnableBulldozing(True)
self.terrain.SetBulldozingParameters(
55, # angle of friction for erosion of displaced material at the border of the rut
1, # displaced material vs downward pressed material.
5, # number of erosion refinements per timestep
10) # number of concentric vertex selections subject to erosion
self.bitmap_file = os.path.dirname(os.path.realpath(__file__)) + "/../../utils/height_map.bmp"
self.bitmap_file_backup = os.path.dirname(os.path.realpath(__file__)) + "/../../utils/height_map_backup.bmp"
shape = (252, 252)
generate_random_bitmap(shape=shape, resolutions=[(2, 2)], mappings=[(-1.5, 1.5)], file_name=self.bitmap_file)
try:
patch = self.ghost_terrain.AddPatch(patch_mat,
chrono.CSYSNORM, # position
self.bitmap_file, # heightmap file (.bmp)
"test", # mesh name
self.terrain_length * 1.5, # sizeX
self.terrain_width * 1.5, # sizeY
self.min_terrain_height, # hMin
self.max_terrain_height) # hMax
self.terrain.Initialize(self.bitmap_file, # heightmap file (.bmp)
self.terrain_length * 1.5, # sizeX
self.terrain_width * 1.5, # sizeY
self.min_terrain_height, # hMin
self.max_terrain_height, # hMax
0.05) # Delta
except Exception:
print('Corrupt Bitmap File')
patch = self.ghost_terrain.AddPatch(patch_mat,
chrono.CSYSNORM, # position
self.bitmap_file_backup, # heightmap file (.bmp)
"test", # mesh name
self.terrain_length * 1.5, # sizeX
self.terrain_width * 1.5, # sizeY
self.min_terrain_height, # hMin
self.max_terrain_height) # hMax
self.terrain.Initialize(self.bitmap_file_backup, # heightmap file (.bmp)
self.terrain_length * 1.5, # sizeX
self.terrain_width * 1.5, # sizeY
self.min_terrain_height, # hMin
self.max_terrain_height, # hMax
0.05)
patch.SetTexture(veh.GetDataFile("terrain/textures/grass.jpg"), 200, 200)
patch.SetColor(chrono.ChColor(1.0, 1.0, 1.0))
self.ghost_terrain.Initialize()
ground_body = patch.GetGroundBody()
ground_body.SetCollide(False)
ground_asset = ground_body.GetAssets()[0]
visual_asset = chrono.CastToChVisualization(ground_asset)
visual_asset.SetStatic(True)
vis_mat = chrono.ChVisualMaterial()
tex = np.random.randint(1,6)
if tex%2 == 0 and tex%5 != 0 :
vis_mat.SetKdTexture(veh.GetDataFile("terrain/textures/grass.jpg"))
elif tex%2 == 1 and tex%5 != 0 :
vis_mat.SetKdTexture(chrono.GetChronoDataFile("sensor/textures/grass_texture.jpg"))
visual_asset.material_list.append(vis_mat)
theta = random.random() * 2 * np.pi
x, y = self.terrain_length * 0.5 * np.cos(theta), self.terrain_width * 0.5 * np.sin(theta)
z = self.terrain.GetHeight(chrono.ChVectorD(x, y, 0)) + 0.25
ang = np.pi + theta
self.initLoc = chrono.ChVectorD(x, y, z)
self.initRot = chrono.Q_from_AngZ(ang)
self.vehicle = veh.Gator(self.system)
self.vehicle.SetContactMethod(chrono.ChContactMethod_NSC)
self.vehicle.SetChassisCollisionType(veh.ChassisCollisionType_NONE)
self.vehicle.SetChassisFixed(False)
self.m_inputs = veh.Inputs()
self.vehicle.SetInitPosition(chrono.ChCoordsysD(self.initLoc, self.initRot))
self.vehicle.SetTireType(veh.TireModelType_RIGID_MESH)
self.vehicle.SetTireStepSize(self.timestep)
self.vehicle.Initialize()
if self.play_mode:
self.vehicle.SetChassisVisualizationType(veh.VisualizationType_MESH)
self.vehicle.SetWheelVisualizationType(veh.VisualizationType_MESH)
self.vehicle.SetTireVisualizationType(veh.VisualizationType_MESH)
else:
self.vehicle.SetChassisVisualizationType(veh.VisualizationType_PRIMITIVES)
self.vehicle.SetWheelVisualizationType(veh.VisualizationType_PRIMITIVES)
self.vehicle.SetTireVisualizationType(veh.VisualizationType_PRIMITIVES)
self.vehicle.SetSuspensionVisualizationType(veh.VisualizationType_PRIMITIVES)
self.vehicle.SetSteeringVisualizationType(veh.VisualizationType_PRIMITIVES)
self.chassis_body = self.vehicle.GetChassisBody()
# self.chassis_body.GetCollisionModel().ClearModel()
# size = chrono.ChVectorD(3,2,0.2)
# self.chassis_body.GetCollisionModel().AddBox(0.5 * size.x, 0.5 * size.y, 0.5 * size.z)
# self.chassis_body.GetCollisionModel().BuildModel()
self.chassis_collision_box = chrono.ChVectorD(3, 2, 0.2)
# Driver
self.driver = veh.ChDriver(self.vehicle.GetVehicle())
# create goal
# pi/4 ang displ
delta_theta = (random.random() - 0.5) * 1.0 * np.pi
gx, gy = self.terrain_length * 0.5 * np.cos(theta + np.pi + delta_theta), self.terrain_width * 0.5 * np.sin(
theta + np.pi + delta_theta)
self.goal = chrono.ChVectorD(gx, gy, self.terrain.GetHeight(chrono.ChVectorD(gx, gy, 0)) + 1.0)
i = 0
while (self.goal - self.initLoc).Length() < 15:
gx = random.random() * self.terrain_length - self.terrain_length / 2
gy = random.random() * self.terrain_width - self.terrain_width / 2
self.goal = chrono.ChVectorD(gx, gy, self.max_terrain_height + 1)
if i > 100:
print('Break')
break
i += 1
# self.goal = chrono.ChVectorD(75, 0, 0)
# Origin in Madison WI
self.origin = chrono.ChVectorD(43.073268, -89.400636, 260.0)
self.goal_coord = chrono.ChVectorD(self.goal)
sens.Cartesian2GPS(self.goal_coord, self.origin)
self.goal_sphere = chrono.ChBodyEasySphere(.55, 1000, True, False)
self.goal_sphere.SetBodyFixed(True)
sphere_asset = self.goal_sphere.GetAssets()[0]
visual_asset = chrono.CastToChVisualization(sphere_asset)
vis_mat = chrono.ChVisualMaterial()
vis_mat.SetAmbientColor(chrono.ChVectorF(0, 0, 0))
vis_mat.SetDiffuseColor(chrono.ChVectorF(.2, .2, .9))
vis_mat.SetSpecularColor(chrono.ChVectorF(.9, .9, .9))
visual_asset.material_list.append(vis_mat)
visual_asset.SetStatic(True)
self.goal_sphere.SetPos(self.goal)
if self.play_mode:
self.system.Add(self.goal_sphere)
# create obstacles
# start = t.time()
self.assets.Clear()
self.assets.RandomlyPositionAssets(self.system, self.initLoc, self.goal, self.terrain,
self.terrain_length * 1.5, self.terrain_width * 1.5, should_scale=False)
# Set the time response for steering and throttle inputs.
# NOTE: this is not exact, since we do not render quite at the specified FPS.
steering_time = 0.75
# time to go from 0 to +1 (or from 0 to -1)
throttle_time = .5
# time to go from 0 to +1
braking_time = 0.3
# time to go from 0 to +1
self.SteeringDelta = (self.timestep / steering_time)
self.ThrottleDelta = (self.timestep / throttle_time)
self.BrakingDelta = (self.timestep / braking_time)
self.manager = sens.ChSensorManager(self.system)
self.manager.scene.AddPointLight(chrono.ChVectorF(100, 100, 100), chrono.ChVectorF(1, 1, 1), 5000.0)
self.manager.scene.AddPointLight(chrono.ChVectorF(-100, -100, 100), chrono.ChVectorF(1, 1, 1), 5000.0)
# Let's not, for the moment, give a different scenario during test
"""
if self.play_mode:
self.manager.scene.GetBackground().has_texture = True;
self.manager.scene.GetBackground().env_tex = "sensor/textures/qwantani_8k.hdr"
self.manager.scene.GetBackground().has_changed = True;
"""
# -----------------------------------------------------
# Create a self.camera and add it to the sensor manager
# -----------------------------------------------------
#chrono.ChFrameD(chrono.ChVectorD(1.5, 0, .875), chrono.Q_from_AngAxis(0, chrono.ChVectorD(0, 1, 0))),
self.camera = sens.ChCameraSensor(
self.chassis_body, # body camera is attached to
20, # scanning rate in Hz
chrono.ChFrameD(chrono.ChVectorD(.65, 0, .75), chrono.Q_from_AngAxis(0, chrono.ChVectorD(0, 1, 0))),
# offset pose
self.camera_width, # number of horizontal samples
self.camera_height, # number of vertical channels
chrono.CH_C_PI / 2, # horizontal field of view
6 # supersampling factor
)
self.camera.SetName("Camera Sensor")
self.camera.PushFilter(sens.ChFilterRGBA8Access())
#if self.play_mode:
# self.camera.PushFilter(sens.ChFilterVisualize(self.camera_width, self.camera_height, "RGB Camera"))
self.manager.AddSensor(self.camera)
# -----------------------------------------------------
# Create a self.gps and add it to the sensor manager
# -----------------------------------------------------
gps_noise_none = sens.ChGPSNoiseNone()
self.gps = sens.ChGPSSensor(
self.chassis_body,
15,
chrono.ChFrameD(chrono.ChVectorD(0, 0, 0), chrono.Q_from_AngAxis(0, chrono.ChVectorD(0, 1, 0))),
self.origin,
gps_noise_none
)
self.gps.SetName("GPS Sensor")
self.gps.PushFilter(sens.ChFilterGPSAccess())
self.manager.AddSensor(self.gps)
# have to reconstruct scene because sensor loads in meshes separately (ask Asher)
# start = t.time()
if self.assets.GetNum() > 0:
# self.assets.TransformAgain()
# start = t.time()
for asset in self.assets.assets:
if len(asset.frames) > 0:
self.manager.AddInstancedStaticSceneMeshes(asset.frames, asset.mesh.shape)
# self.manager.ReconstructScenes()
# self.manager.AddInstancedStaticSceneMeshes(self.assets.frames, self.assets.shapes)
# self.manager.Update()
self.old_dist = (self.goal - self.initLoc).Length()
self.step_number = 0
self.c_f = 0
self.old_ac = 0
self.isdone = False
self.render_setup = False
self.dist0 = (self.goal - self.chassis_body.GetPos()).Length()
if self.play_mode:
self.render()
return self.get_ob()