def add_asset(self, asset: 'Any'):
if isinstance(asset, chrono.ChBody):
self._system._system.AddBody(asset)
return
if isinstance(asset, WABody):
if not hasattr(asset, 'size') or not hasattr(asset, 'position'):
raise AttributeError(
"Body must have 'size', and 'position' fields")
position = asset.position
yaw = 0 if not hasattr(asset, 'yaw') else asset.yaw
size = asset.size
body_type = 'box'
if hasattr(asset, 'body_type'):
body_type = asset.body_type
if body_type == 'sphere':
body = chrono.ChBodyEasySphere(size.length, 1000, True, False)
body.SetBodyFixed(True)
elif body_type == 'box':
body = chrono.ChBodyEasyBox(
size.x, size.y, size.z, 1000, True, False)
body.SetBodyFixed(True)
else:
raise ValueError(
f"'{asset.body_type}' not a supported body type.")
body.SetPos(WAVector_to_ChVector(position))
body.SetRot(chrono.Q_from_AngZ(-yaw + WA_PI / 2))
if hasattr(asset, 'color'):
color = asset.color
body.AddAsset(chrono.ChColorAsset(
chrono.ChColor(color.x, color.y, color.z)))
texture = chrono.ChVisualMaterial()
texture.SetDiffuseColor(
chrono.ChVectorF(color.x, color.y, color.z))
chrono.CastToChVisualization(
body.GetAssets()[0]).material_list.append(texture)
if hasattr(asset, 'texture'):
texture = chrono.ChVisualMaterial()
texture.SetKdTexture(get_wa_data_file(asset.texture))
chrono.CastToChVisualization(
body.GetAssets()[0]).material_list.append(texture)
self._system._system.AddBody(body)
asset.chrono_body = body
super().add_asset(asset)
def DrawBarriers(self, points, n=5, height=1, width=1):
points = points[::n]
if points[-1] != points[0]:
points.append(points[-1])
for i in range(len(points) - 1):
p1 = points[i]
p2 = points[i + 1]
box = chrono.ChBodyEasyBox((p2 - p1).Length(), height, width, 1000,
True, True)
box.SetPos(p1)
q = chrono.ChQuaternionD()
v1 = p2 - p1
v2 = chrono.ChVectorD(1, 0, 0)
ang = math.atan2((v1 % v2).Length(), v1 ^ v2)
if chrono.ChVectorD(0, 0, 1) ^ (v1 % v2) > 0.0:
ang *= -1
q.Q_from_AngZ(ang)
box.SetRot(q)
box.SetBodyFixed(True)
box_asset = box.GetAssets()[0]
visual_asset = chrono.CastToChVisualization(box_asset)
vis_mat = chrono.ChVisualMaterial()
vis_mat.SetAmbientColor(chrono.ChVectorF(0, 0, 0))
if i % 2 == 0:
vis_mat.SetDiffuseColor(chrono.ChVectorF(1.0, 0, 0))
else:
vis_mat.SetDiffuseColor(chrono.ChVectorF(1.0, 1.0, 1.0))
vis_mat.SetSpecularColor(chrono.ChVectorF(0.9, 0.9, 0.9))
vis_mat.SetFresnelMin(0)
vis_mat.SetFresnelMax(0.1)
visual_asset.material_list.append(vis_mat)
color = chrono.ChColorAsset()
if i % 2 == 0:
color.SetColor(chrono.ChColor(1, 0, 0))
else:
color.SetColor(chrono.ChColor(1, 1, 1))
box.AddAsset(color)
self.system.Add(box)
self.barriers.append(box)
def DrawObstacles(self, obstacles, z=0.0):
self.boxes = []
for i, o in obstacles.items():
p1 = o.p1
p2 = o.p2
box = chrono.ChBodyEasyBox(o.length, o.width, 1, 1000, True, True)
box.SetPos(p1)
q = chrono.ChQuaternionD()
v1 = p2 - p1
v2 = chrono.ChVectorD(1, 0, 0)
ang = math.atan2((v1 % v2).Length(), v1 ^ v2)
if chrono.ChVectorD(0, 0, 1) ^ (v1 % v2) > 0.0:
ang *= -1
q.Q_from_AngZ(ang)
box.SetRot(q)
box.SetBodyFixed(True)
box_asset = box.GetAssets()[0]
visual_asset = chrono.CastToChVisualization(box_asset)
self.system.Add(box)
self.boxes.append(box)
def create_sphere_in_chrono(
system: 'WAChronoSystem', rgb=(1, 0, 0)) -> chrono.ChBodyEasySphere:
"""Create and add a sphere to the chrono world
Args:
system (WASystem): the system that manages the simulation
rgb (tuple, optional): (red, green, blue). Defaults to (1, 0, 0) or red.
Returns:
chrono.ChBodyEasySphere: the sphere that was created
"""
sphere = chrono.ChBodyEasySphere(0.25, 1000, True, False)
sphere.SetBodyFixed(True)
sphere.AddAsset(chrono.ChColorAsset(*rgb))
texture = chrono.ChVisualMaterial()
texture.SetDiffuseColor(chrono.ChVectorF(*rgb))
chrono.CastToChVisualization(
sphere.GetAssets()[0]).material_list.append(texture)
system._system.Add(sphere)
return sphere
def load_chrono_terrain_from_json(system: 'WAChronoSystem', filename: str):
"""Load a ChTerrain from a json specification file
Args:
filename (str): the relative path to a terrain json file
system (WAChronoSystem): the chrono system used to handle the terrain
Returns:
ChTerrain: The loaded terrain
"""
j = _load_json(filename)
# Validate the json file
_check_field(j, 'Terrain', field_type=dict)
t = j['Terrain']
_check_field(t, 'Input File', field_type=str)
_check_field(t, 'Texture', field_type=str, optional=True)
terrain = veh.RigidTerrain(system._system, chrono.GetChronoDataFile(t['Input File'])) # noqa
# Add texture to the terrain, if desired
if 'Texture' in t:
texture_filename = chrono.GetChronoDataFile(t['Texture'])
vis_mat = chrono.ChVisualMaterial()
vis_mat.SetKdTexture(texture_filename)
vis_mat.SetSpecularColor(chrono.ChVectorF(0.0, 0.0, 0.0))
vis_mat.SetFresnelMin(0)
vis_mat.SetFresnelMax(0.1)
patch_asset = terrain.GetPatches()[0].GetGroundBody().GetAssets()[0]
patch_visual_asset = chrono.CastToChVisualization(patch_asset)
patch_visual_asset.material_list.append(vis_mat)
return terrain
def reset(self):
n = 2 * np.random.randint(0, 4)
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 = True
self.terrain = veh.RigidTerrain(self.system)
if rigid_terrain:
patch_mat = chrono.ChMaterialSurfaceNSC()
patch_mat.SetFriction(0.9)
patch_mat.SetRestitution(0.01)
patch = self.terrain.AddPatch(patch_mat, chrono.ChVectorD(0, 0, 0),
chrono.ChVectorD(0, 0, 1),
self.terrain_length * 1.5,
self.terrain_width * 1.5)
else:
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.terrain.AddPatch(
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
except Exception:
print('Corrupt Bitmap File')
patch = self.terrain.AddPatch(
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
patch.SetTexture(veh.GetDataFile("terrain/textures/grass.jpg"), 200,
200)
patch.SetColor(chrono.ChColor(0.8, 0.8, 0.5))
self.terrain.Initialize()
ground_body = patch.GetGroundBody()
ground_asset = ground_body.GetAssets()[0]
visual_asset = chrono.CastToChVisualization(ground_asset)
visual_asset.SetStatic(True)
vis_mat = chrono.ChVisualMaterial()
vis_mat.SetKdTexture(veh.GetDataFile("terrain/textures/grass.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_TMEASY)
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()
# print('Reconstruction :: ', t.time() - start)
self.old_dist = (self.goal - self.initLoc).Length()
self.step_number = 0
self.c_f = 0
self.isdone = False
self.render_setup = False
self.dist0 = (self.goal - self.chassis_body.GetPos()).Length()
if self.play_mode:
self.render()
# print(self.get_ob()[1])
return self.get_ob()
def reset(self):
x_half_length = 90
y_half_length = 40
self.path = BezierPath(x_half_length, y_half_length, 0.5,
self.interval)
pos, rot = self.path.getPosRot(self.path.current_t - self.interval)
self.initLoc = chrono.ChVectorD(pos)
self.initRot = chrono.ChQuaternionD(rot)
self.vehicle = veh.HMMWV_Reduced()
self.vehicle.SetContactMethod(chrono.ChContactMethod_NSC)
self.surf_material = chrono.ChMaterialSurfaceNSC()
self.vehicle.SetChassisCollisionType(
veh.ChassisCollisionType_PRIMITIVES)
self.vehicle.SetChassisFixed(False)
self.vehicle.SetInitPosition(
chrono.ChCoordsysD(self.initLoc, self.initRot))
self.vehicle.SetPowertrainType(veh.PowertrainModelType_SHAFTS)
self.vehicle.SetDriveType(veh.DrivelineType_AWD)
# self.vehicle.SetSteeringType(veh.SteeringType_PITMAN_ARM)
self.vehicle.SetTireType(veh.TireModelType_TMEASY)
self.vehicle.SetTireStepSize(self.timestep)
self.vehicle.Initialize()
if self.play_mode == True:
# self.vehicle.SetChassisVisualizationType(veh.VisualizationType_MESH)
self.vehicle.SetChassisVisualizationType(
veh.VisualizationType_PRIMITIVES)
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.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(self.surf_material,
0.5 * size.x,
0.5 * size.y,
0.5 * size.z)
self.chassis_body.GetCollisionModel().BuildModel()
self.m_inputs = veh.Inputs()
self.system = self.vehicle.GetVehicle().GetSystem()
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)
# Driver
#self.driver = veh.ChDriver(self.vehicle.GetVehicle())
self.terrain = veh.RigidTerrain(self.system)
patch_mat = chrono.ChMaterialSurfaceNSC()
patch_mat.SetFriction(0.9)
patch_mat.SetRestitution(0.01)
patch = self.terrain.AddPatch(patch_mat, chrono.ChVectorD(0, 0, 0),
chrono.ChVectorD(0, 0, 1),
self.terrainLength * 1.5,
self.terrainWidth * 1.5)
patch.SetTexture(veh.GetDataFile("terrain/textures/grass.jpg"), 200,
200)
patch.SetColor(chrono.ChColor(0.8, 0.8, 0.5))
self.terrain.Initialize()
self.groundBody = patch.GetGroundBody()
ground_asset = self.groundBody.GetAssets()[0]
visual_asset = chrono.CastToChVisualization(ground_asset)
vis_mat = chrono.ChVisualMaterial()
vis_mat.SetKdTexture(veh.GetDataFile("terrain/textures/grass.jpg"))
visual_asset.material_list.append(vis_mat)
self.leaders.addLeaders(self.system, self.path)
self.leader_box = self.leaders.getBBox()
self.lead_pos = (self.chassis_body.GetPos() -
self.leaders[0].GetPos()).Length()
# Add obstacles:
self.obstacles = []
self.placeObstacle(8)
# for leader in self.leaders:
# ------------------------------------------------
# Create a self.camera and add it to the sensor manager
# ------------------------------------------------
self.camera = sens.ChCameraSensor(
self.chassis_body, # body camera is attached to
10, # scanning rate in Hz
chrono.ChFrameD(chrono.ChVectorD(1.5, 0, .875)),
# 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)
self.camera.SetName("Camera Sensor")
self.camera.PushFilter(sens.ChFilterRGBA8Access())
self.manager.AddSensor(self.camera)
# -----------------------------------------------------
# Create a self.gps and add it to the sensor manager
# -----------------------------------------------------
gps_noise_none = sens.ChGPSNoiseNone()
self.AgentGPS = sens.ChGPSSensor(
self.chassis_body, 10,
chrono.ChFrameD(
chrono.ChVectorD(0, 0, 0),
chrono.Q_from_AngAxis(0, chrono.ChVectorD(0, 1, 0))),
self.origin, gps_noise_none)
self.AgentGPS.SetName("AgentGPS Sensor")
self.AgentGPS.PushFilter(sens.ChFilterGPSAccess())
self.manager.AddSensor(self.AgentGPS)
### Target GPS
self.TargetGPS = sens.ChGPSSensor(
self.leaders[0], 10,
chrono.ChFrameD(
chrono.ChVectorD(0, 0, 0),
chrono.Q_from_AngAxis(0, chrono.ChVectorD(0, 1, 0))),
self.origin, gps_noise_none)
self.TargetGPS.SetName("TargetGPS Sensor")
self.TargetGPS.PushFilter(sens.ChFilterGPSAccess())
self.manager.AddSensor(self.TargetGPS)
self.step_number = 0
self.num_frames = 0
self.num_updates = 0
self.c_f = 0
self.old_ac = [0, 0]
self.isdone = False
self.render_setup = False
if self.play_mode:
self.render()
return self.get_ob()
def reset(self):
self.generate_track()
self.vehicle = veh.Sedan()
self.vehicle.SetContactMethod(chrono.ChMaterialSurface.NSC)
self.vehicle.SetChassisCollisionType(veh.ChassisCollisionType_NONE)
self.vehicle.SetChassisFixed(False)
self.vehicle.SetInitPosition(
chrono.ChCoordsysD(self.initLoc, self.initRot))
self.vehicle.SetTireType(veh.TireModelType_RIGID)
self.vehicle.SetTireStepSize(self.timestep)
self.vehicle.Initialize()
self.vehicle.SetChassisVisualizationType(
veh.VisualizationType_PRIMITIVES)
self.vehicle.SetWheelVisualizationType(
veh.VisualizationType_PRIMITIVES)
self.vehicle.SetSuspensionVisualizationType(
veh.VisualizationType_PRIMITIVES)
self.vehicle.SetSteeringVisualizationType(
veh.VisualizationType_PRIMITIVES)
self.vehicle.SetTireVisualizationType(veh.VisualizationType_PRIMITIVES)
self.system = self.vehicle.GetSystem()
self.chassis_body = self.vehicle.GetChassisBody()
# Create contact model
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()
# Driver
self.driver = Driver(self.vehicle.GetVehicle())
# Throttle controller
#self.throttle_controller = PIDThrottleController()
#self.throttle_controller.SetGains(Kp=0.4, Ki=0, Kd=0)
#self.throttle_controller.SetTargetSpeed(speed=self.target_speed)
# Rigid terrain
self.terrain = veh.RigidTerrain(self.system)
patch = self.terrain.AddPatch(
chrono.ChCoordsysD(chrono.ChVectorD(0, 0, self.terrainHeight - 5),
chrono.QUNIT),
chrono.ChVectorD(self.terrainLength, self.terrainWidth, 10))
patch.SetContactFrictionCoefficient(0.9)
patch.SetContactRestitutionCoefficient(0.01)
patch.SetContactMaterialProperties(2e7, 0.3)
patch.SetTexture(veh.GetDataFile("terrain/textures/tile4.jpg"), 200,
200)
patch.SetColor(chrono.ChColor(0.8, 0.8, 0.5))
self.terrain.Initialize()
ground_body = patch.GetGroundBody()
ground_asset = ground_body.GetAssets()[0]
visual_asset = chrono.CastToChVisualization(ground_asset)
vis_mat = chrono.ChVisualMaterial()
vis_mat.SetKdTexture(chrono.GetChronoDataFile("concrete.jpg"))
visual_asset.material_list.append(vis_mat)
# create barriers
self.barriers = []
self.DrawBarriers(self.track.left.points)
self.DrawBarriers(self.track.right.points)
# 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.5
# time to go from 0 to +1 (or from 0 to -1)
throttle_time = 0.5
# time to go from 0 to +1
braking_time = 0.3
# time to go from 0 to +1
self.driver.SetSteeringDelta(self.timestep / steering_time)
self.driver.SetThrottleDelta(self.timestep / throttle_time)
self.driver.SetBrakingDelta(self.timestep / braking_time)
self.manager = sens.ChSensorManager(self.system)
self.manager.scene.AddPointLight(chrono.ChVectorF(100, 100, 100),
chrono.ChVectorF(1, 1, 1), 500.0)
self.manager.scene.AddPointLight(chrono.ChVectorF(-100, -100, 100),
chrono.ChVectorF(1, 1, 1), 500.0)
# -----------------------------------------------------
# Create a self.camera and add it to the sensor manager
# -----------------------------------------------------
self.camera = sens.ChCameraSensor(
self.chassis_body, # body camera is attached to
50, # scanning rate in Hz
chrono.ChFrameD(
chrono.ChVectorD(1.5, 0, .875),
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
(self.camera_height / self.camera_width) * chrono.CH_C_PI /
3. # vertical field of view
)
self.camera.SetName("Camera Sensor")
self.camera.FilterList().append(sens.ChFilterRGBA8Access())
self.manager.AddSensor(self.camera)
self.old_dist = self.track.center.calcDistance(
self.chassis_body.GetPos())
self.step_number = 0
self.c_f = 0
self.isdone = False
self.render_setup = False
if self.play_mode:
self.render()
return self.get_ob()
def __init__(self, step_size, sys, irrlicht=False, terrain_type='json', height=-0.5, width=300, length=300):
# Chrono parameters
self.step_size = step_size
self.irrlicht = irrlicht
self.step_number = 0
# Rigid terrain dimensions
self.height = height
self.length = length # size in X direction
self.width = width # size in Y direction
self.sys = sys
if terrain_type == 'json':
import os
# JSON files for terrain
self.rigidterrain_file = veh.GetDataPath() + os.path.join('terrain', 'RigidPlane.json')
checkFile(self.rigidterrain_file)
# Create the ground
self.terrain = veh.RigidTerrain(self.sys, self.rigidterrain_file)
elif terrain_type == 'concrete':
# Create the terrain
self.terrain = veh.RigidTerrain(self.sys)
patch = self.terrain.AddPatch(chrono.ChCoordsysD(chrono.ChVectorD(0, 0, self.height - 5), chrono.QUNIT),
chrono.ChVectorD(self.width, self.length, 10))
patch.SetContactFrictionCoefficient(0.9)
patch.SetContactRestitutionCoefficient(0.01)
patch.SetContactMaterialProperties(2e7, 0.3)
patch.SetTexture(chrono.GetChronoDataFile("concrete.jpg"), self.length, self.width)
patch.SetColor(chrono.ChColor(0.8, 0.8, 0.5))
self.terrain.Initialize()
try:
ground_body = patch.GetGroundBody()
ground_asset = ground_body.GetAssets()[0]
visual_asset = chrono.CastToChVisualization(ground_asset)
vis_mat = chrono.ChVisualMaterial()
vis_mat.SetKdTexture(chrono.GetChronoDataFile("concrete.jpg"))
vis_mat.SetFresnelMax(0);
visual_asset.material_list.append(vis_mat)
except:
print("Not Visual Material")
elif terrain_type == 'hallway':
y_max = 5.65
x_max = 23
offset = chrono.ChVectorD(-x_max/2, -y_max/2, .21)
offsetF = chrono.ChVectorF(offset.x, offset.y, offset.z)
self.terrain = veh.RigidTerrain(self.sys)
coord_sys = chrono.ChCoordsysD(offset, chrono.ChQuaternionD(1,0,0,0))
patch = self.terrain.AddPatch(coord_sys, chrono.GetChronoDataFile("sensor/textures/hallway.obj"), "mesh", 0.01, False)
vis_mesh = chrono.ChTriangleMeshConnected()
vis_mesh.LoadWavefrontMesh(chrono.GetChronoDataFile("sensor/textures/hallway.obj"), True, True)
trimesh_shape = chrono.ChTriangleMeshShape()
trimesh_shape.SetMesh(vis_mesh)
trimesh_shape.SetName("mesh_name")
trimesh_shape.SetStatic(True)
patch.GetGroundBody().AddAsset(trimesh_shape)
patch.SetContactFrictionCoefficient(0.9)
patch.SetContactRestitutionCoefficient(0.01)
patch.SetContactMaterialProperties(2e7, 0.3)
self.terrain.Initialize()
def reset(self):
del self.manager
material = chrono.ChMaterialSurfaceNSC()
self.vehicle = veh.HMMWV_Reduced()
self.vehicle.SetContactMethod(chrono.ChContactMethod_NSC)
self.vehicle.SetChassisCollisionType(
veh.ChassisCollisionType_PRIMITIVES)
self.vehicle.SetChassisFixed(False)
self.vehicle.SetInitPosition(
chrono.ChCoordsysD(self.initLoc, self.initRot))
self.vehicle.SetPowertrainType(veh.PowertrainModelType_SHAFTS)
self.vehicle.SetDriveType(veh.DrivelineType_AWD)
#self.vehicle.SetSteeringType(veh.SteeringType_PITMAN_ARM)
self.vehicle.SetTireType(veh.TireModelType_TMEASY)
self.vehicle.SetTireStepSize(self.timestep)
self.vehicle.Initialize()
#self.vehicle.SetStepsize(self.timestep)
if self.play_mode == True:
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.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(material, 0.5 * size.x,
0.5 * size.y,
0.5 * size.z)
self.chassis_body.GetCollisionModel().BuildModel()
# Driver
self.driver = veh.ChDriver(self.vehicle.GetVehicle())
# Rigid terrain
self.system = self.vehicle.GetSystem()
self.terrain = veh.RigidTerrain(self.system)
patch = self.terrain.AddPatch(
material, chrono.ChVectorD(0, 0, self.terrainHeight - .5),
chrono.VECT_Z, self.terrainLength, self.terrainWidth, 10)
material.SetFriction(0.9)
material.SetRestitution(0.01)
#patch.SetContactMaterialProperties(2e7, 0.3)
patch.SetTexture(veh.GetDataFile("terrain/textures/tile4.jpg"), 200,
200)
patch.SetColor(chrono.ChColor(0.8, 0.8, 0.5))
self.terrain.Initialize()
ground_body = patch.GetGroundBody()
ground_asset = ground_body.GetAssets()[0]
visual_asset = chrono.CastToChVisualization(ground_asset)
vis_mat = chrono.ChVisualMaterial()
vis_mat.SetKdTexture(chrono.GetChronoDataFile("concrete.jpg"))
visual_asset.material_list.append(vis_mat)
# create obstacles
self.boxes = []
for i in range(3):
box = chrono.ChBodyEasyBox(2, 2, 10, 1000, True, True)
box.SetPos(
chrono.ChVectorD(25 + 25 * i,
(np.random.rand(1)[0] - 0.5) * 10, .05))
box.SetBodyFixed(True)
box_asset = box.GetAssets()[0]
visual_asset = chrono.CastToChVisualization(box_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.boxes.append(box)
self.system.Add(box)
# 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 = 1.0
# 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), 4000.0)
self.manager.scene.AddPointLight(chrono.ChVectorF(-100, -100, 100),
chrono.ChVectorF(1, 1, 1), 4000.0)
# ------------------------------------------------
# Create a self.camera and add it to the sensor manager
# ------------------------------------------------
self.camera = sens.ChCameraSensor(
self.chassis_body, # body camera is attached to
50, # scanning rate in Hz
chrono.ChFrameD(chrono.ChVectorD(1.5, 0, .875)),
# offset pose
self.camera_width, # number of horizontal samples
self.camera_height, # number of vertical channels
chrono.CH_C_PI / 3, # horizontal field of view
#(self.camera_height / self.camera_width) * chrono.CH_C_PI / 3. # vertical field of view
)
self.camera.SetName("Camera Sensor")
self.camera.PushFilter(sens.ChFilterRGBA8Access())
self.manager.AddSensor(self.camera)
# -----------------------------------------------------------------
# Create a filter graph for post-processing the data from the lidar
# -----------------------------------------------------------------
self.d_old = np.linalg.norm(self.Xtarg + self.Ytarg)
self.step_number = 0
self.c_f = 0
self.isdone = False
self.render_setup = False
if self.play_mode:
self.render()
return self.get_ob()