def read_vehicle_model_file(filename: str) -> tuple: """Read a json specification file to get additional file names to be loaded into ChVehicle classes Will find the vehicle, powertrain and tire input files. The input files are other json files that are readable by the Chrono simulator to be used to create bodies attached to the vehicle. Args: filename (str): the json specification file with Vehicle, Powertrain and Tire input models Returns: tuple: returns each json specification file for the vehicle, powertrain and tire """ j = _load_json(filename) # Validate json file _check_field(j, "Vehicle", field_type=dict) _check_field(j, "Powertrain", field_type=dict) _check_field(j, "Tire", field_type=dict) _check_field(j["Vehicle"], "Input File", field_type=str) _check_field(j["Powertrain"], "Input File", field_type=str) _check_field(j["Tire"], "Input File", field_type=str) # Extract the actual files vehicle_filename = veh.GetDataFile(j["Vehicle"]["Input File"]) powertrain_filename = veh.GetDataFile(j["Powertrain"]["Input File"]) tire_filename = veh.GetDataFile(j["Tire"]["Input File"]) return vehicle_filename, powertrain_filename, tire_filename
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()) patch = terrain.AddPatch(chrono.ChCoordsysD(chrono.ChVectorD(0, 0, terrainHeight - 5), chrono.QUNIT), chrono.ChVectorD(terrainLength, 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)) terrain.Initialize() # Create the vehicle Irrlicht interface # please note that wchar_t conversion requres some workaround app = veh.ChWheeledVehicleIrrApp(my_hmmwv.GetVehicle()) app.SetSkyBox() app.AddTypicalLights(chronoirr.vector3df(30, -30, 100), chronoirr.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(chronoirr.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, chronoirr.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 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 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
# Spin in place for real time to catch up realtime_timer.Spin(step_size) # ============================================================================= # The path to the Chrono data directory containing various assets (meshes, textures, data files) # is automatically set, relative to the default location of this demo. # If running from a different directory, you must change the path to the data directory with: #chrono.SetChronoDataPath('path/to/data') veh.SetDataPath(chrono.GetChronoDataPath() + 'vehicle/') # JSON files for vehicle model vehicle_file = veh.GetDataFile('sedan/vehicle/Sedan_Vehicle.json') vehicle_tire_file = veh.GetDataFile('sedan/tire/Sedan_TMeasyTire.json') vehicle_powertrain_file = veh.GetDataFile( 'sedan/powertrain/Sedan_SimpleMapPowertrain.json') # JSON files for trailer model trailer_file = veh.GetDataFile('ultra_tow/UT_Trailer.json') trailer_tire_file = veh.GetDataFile('ultra_tow/UT_TMeasyTire.json') # JSON files for terrain rigidterrain_file = veh.GetDataFile('terrain/RigidPlane.json') # Initial vehicle position initLoc = chrono.ChVectorD(0, 0, 0.5) # Initial vehicle orientation
# Create the terrain terrain = veh.RigidTerrain(my_bus.GetSystem()) if (contact_method == chrono.ChContactMethod_NSC): patch_mat = chrono.ChMaterialSurfaceNSC() patch_mat.SetFriction(0.9) patch_mat.SetRestitution(0.01) elif (contact_method == chrono.ChContactMethod_SMC): patch_mat = chrono.ChMaterialSurfaceSMC() patch_mat.SetFriction(0.9) patch_mat.SetRestitution(0.01) patch_mat.SetYoungModulus(2e7) patch = terrain.AddPatch(patch_mat, chrono.ChVectorD(0, 0, 0), chrono.ChVectorD(0, 0, 1), terrainLength, terrainWidth) patch.SetTexture(veh.GetDataFile("terrain/textures/tile4.jpg"), 200, 200) patch.SetColor(chrono.ChColor(0.8, 0.8, 0.5)) terrain.Initialize() # Create the vehicle Irrlicht interface app = veh.ChWheeledVehicleIrrApp(my_bus.GetVehicle(), 'Citybus', irr.dimension2du(1000, 800)) app.SetSkyBox() app.AddTypicalLights(irr.vector3df(30, -30, 100), irr.vector3df(30, 50, 100), 250, 130) app.AddTypicalLogo(chrono.GetChronoDataFile('logo_pychrono_alpha.png')) app.SetChaseCamera(trackPoint, 15.0, 0.5) app.SetTimestep(step_size) app.AssetBindAll() app.AssetUpdateAll()
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 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
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 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
def main(): #print("Copyright (c) 2017 projectchrono.org\nChrono version: ", CHRONO_VERSION , "\n\n") step_size = 0.005 sys = chrono.ChSystemNSC() sys.Set_G_acc(chrono.ChVectorD(0, 0, -9.81)) sys.SetSolverType(chrono.ChSolver.Type_BARZILAIBORWEIN) sys.SetSolverMaxIterations(150) sys.SetMaxPenetrationRecoverySpeed(4.0) # Create the terrain terrain = veh.RigidTerrain(sys) patch_mat = chrono.ChMaterialSurfaceNSC() patch_mat.SetFriction(0.9) patch_mat.SetRestitution(0.01) patch = terrain.AddPatch(patch_mat, chrono.ChVectorD(0, 0, 0), chrono.ChVectorD(0, 0, 1), 200, 100) patch.SetColor(chrono.ChColor(0.8, 0.8, 0.5)) patch.SetTexture(veh.GetDataFile("terrain/textures/tile4.jpg"), 200, 200) terrain.Initialize() # Create and initialize the first vehicle hmmwv_1 = veh.HMMWV_Reduced(sys) hmmwv_1.SetInitPosition( chrono.ChCoordsysD(chrono.ChVectorD(0, -1.5, 1.0), chrono.ChQuaternionD(1, 0, 0, 0))) hmmwv_1.SetPowertrainType(veh.PowertrainModelType_SIMPLE) hmmwv_1.SetDriveType(veh.DrivelineType_RWD) hmmwv_1.SetTireType(veh.TireModelType_RIGID) hmmwv_1.Initialize() hmmwv_1.SetChassisVisualizationType(veh.VisualizationType_PRIMITIVES) hmmwv_1.SetSuspensionVisualizationType(veh.VisualizationType_PRIMITIVES) hmmwv_1.SetSteeringVisualizationType(veh.VisualizationType_PRIMITIVES) hmmwv_1.SetWheelVisualizationType(veh.VisualizationType_NONE) hmmwv_1.SetTireVisualizationType(veh.VisualizationType_PRIMITIVES) driver_data_1 = veh.vector_Entry([ veh.DataDriverEntry(0.0, 0.0, 0.0, 0.0), veh.DataDriverEntry(0.5, 0.0, 0.0, 0.0), veh.DataDriverEntry(0.7, 0.3, 0.7, 0.0), veh.DataDriverEntry(1.0, 0.3, 0.7, 0.0), veh.DataDriverEntry(3.0, 0.5, 0.1, 0.0) ]) driver_1 = veh.ChDataDriver(hmmwv_1.GetVehicle(), driver_data_1) driver_1.Initialize() # Create and initialize the second vehicle hmmwv_2 = veh.HMMWV_Reduced(sys) hmmwv_2.SetInitPosition( chrono.ChCoordsysD(chrono.ChVectorD(7, 1.5, 1.0), chrono.ChQuaternionD(1, 0, 0, 0))) hmmwv_2.SetPowertrainType(veh.PowertrainModelType_SIMPLE) hmmwv_2.SetDriveType(veh.DrivelineType_RWD) hmmwv_2.SetTireType(veh.TireModelType_RIGID) hmmwv_2.Initialize() hmmwv_2.SetChassisVisualizationType(veh.VisualizationType_PRIMITIVES) hmmwv_2.SetSuspensionVisualizationType(veh.VisualizationType_PRIMITIVES) hmmwv_2.SetSteeringVisualizationType(veh.VisualizationType_PRIMITIVES) hmmwv_2.SetWheelVisualizationType(veh.VisualizationType_NONE) hmmwv_2.SetTireVisualizationType(veh.VisualizationType_PRIMITIVES) driver_data_2 = veh.vector_Entry([ veh.DataDriverEntry(0.0, 0.0, 0.0, 0.0), veh.DataDriverEntry(0.5, 0.0, 0.0, 0.0), veh.DataDriverEntry(0.7, -0.3, 0.7, 0.0), veh.DataDriverEntry(1.0, -0.3, 0.7, 0.0), veh.DataDriverEntry(3.0, -0.5, 0.1, 0.0) ]) driver_2 = veh.ChDataDriver(hmmwv_2.GetVehicle(), driver_data_2) driver_2.Initialize() # Create the vehicle Irrlicht interface app = veh.ChWheeledVehicleIrrApp(hmmwv_1.GetVehicle(), 'Two Car 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, 0.75), 6.0, 0.5) app.SetChaseCameraState(veh.ChChaseCamera.Track) app.SetChaseCameraPosition(chrono.ChVectorD(-15, 0, 2.0)) app.SetTimestep(step_size) app.AssetBindAll() app.AssetUpdateAll() # Simulation loop realtime_timer = chrono.ChRealtimeStepTimer() while (app.GetDevice().run()): time = hmmwv_1.GetSystem().GetChTime() app.BeginScene(True, True, irr.SColor(255, 140, 161, 192)) app.DrawAll() app.EndScene() # Get driver inputs driver_inputs_1 = driver_1.GetInputs() driver_inputs_2 = driver_2.GetInputs() # Update modules (process inputs from other modules) driver_1.Synchronize(time) driver_2.Synchronize(time) hmmwv_1.Synchronize(time, driver_inputs_1, terrain) hmmwv_2.Synchronize(time, driver_inputs_2, terrain) terrain.Synchronize(time) app.Synchronize("", driver_inputs_1) # Advance simulation for one timestep for all modules driver_1.Advance(step_size) driver_2.Advance(step_size) hmmwv_1.Advance(step_size) hmmwv_2.Advance(step_size) terrain.Advance(step_size) app.Advance(step_size) # Advance state of entire system (containing both vehicles) sys.DoStepDynamics(step_size) # Spin in place for real time to catch up realtime_timer.Spin(step_size) return 0
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()
def main(): #print("Copyright (c) 2017 projectchrono.org\nChrono version: ", CHRONO_VERSION , "\n\n") # -------------------------- # Create the various modules # -------------------------- # Create the vehicle system vehicle = veh.WheeledVehicle(vehicle_file, chrono.ChMaterialSurface.NSC) vehicle.Initialize(chrono.ChCoordsysD(initLoc, initRot)) #vehicle.GetChassis().SetFixed(True) vehicle.SetStepsize(step_size) vehicle.SetChassisVisualizationType(veh.VisualizationType_PRIMITIVES) vehicle.SetSuspensionVisualizationType(veh.VisualizationType_PRIMITIVES) vehicle.SetSteeringVisualizationType(veh.VisualizationType_PRIMITIVES) vehicle.SetWheelVisualizationType(veh.VisualizationType_NONE) # Create the ground terrain = veh.RigidTerrain(vehicle.GetSystem(), rigidterrain_file) # Create and initialize the powertrain system powertrain = veh.SimplePowertrain(simplepowertrain_file) vehicle.InitializePowertrain(powertrain) # Create and initialize the tires for axle in vehicle.GetAxles(): tireL = veh.RigidTire(rigidtire_file) vehicle.InitializeTire(tireL, axle.m_wheels[0], veh.VisualizationType_MESH) tireR = veh.RigidTire(rigidtire_file) vehicle.InitializeTire(tireR, axle.m_wheels[1], veh.VisualizationType_MESH) # ------------- # Create driver # ------------- driver = Driver(vehicle) # 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 = 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) # ------------------ # Load and draw path # ------------------ # path_file = 'paths/straight.txt' # path_file = 'paths/curve.txt' path_file = 'paths/NATO_double_lane_change.txt' # path_file = 'paths/ISO_double_lane_change.txt' path = chrono.ChBezierCurve.read(veh.GetDataFile(path_file)) if irrlicht: road = vehicle.GetSystem().NewBody() road.SetBodyFixed(True) vehicle.GetSystem().AddBody(road) num_points = path.getNumPoints() path_asset = chrono.ChLineShape() path_asset.SetLineGeometry(chrono.ChLineBezier(path)) path_asset.SetColor(chrono.ChColor(0.0, 0.8, 0.0)) path_asset.SetNumRenderPoints(max(2 * num_points, 400)) road.AddAsset(path_asset) # -------------------- # Create controller(s) # -------------------- steering_controller = PIDSteeringController(vehicle, driver, path) steering_controller.SetGains(Kp=0.4, Ki=0, Kd=0.25) steering_controller.SetLookAheadDistance(dist=5.0) throttle_controller = PIDThrottleController(vehicle, driver) throttle_controller.SetGains(Kp=0.4, Ki=0, Kd=0) throttle_controller.SetTargetSpeed(target_speed) # ----------------------- # Initialize irrlicht app # ----------------------- if irrlicht: app = veh.ChVehicleIrrApp(vehicle) app.SetHUDLocation(500, 20) app.SetSkyBox() app.AddTypicalLogo() app.AddTypicalLights(chronoirr.vector3df(-150., -150., 200.), chronoirr.vector3df(-150., 150., 200.), 100, 100) app.AddTypicalLights(chronoirr.vector3df(150., -150., 200.), chronoirr.vector3df(150., 150., 200.), 100, 100) app.EnableGrid(False) app.SetChaseCamera(trackPoint, 6.0, 0.5) app.SetTimestep(step_size) app.AssetBindAll() app.AssetUpdateAll() # ----------------- # Initialize output # ----------------- if output: try: os.mkdir(out_dir) except: print("Error creating directory ") # Generate JSON information with available output channels out_json = vehicle.ExportComponentList() print(out_json) vehicle.ExportComponentList(out_dir + "/component_list.json") # --------------- # Simulation loop # --------------- # Number of simulation steps between miscellaneous events render_steps = int(math.ceil(render_step_size / step_size)) # Initialize simulation frame counter and simulation time step_number = 0 time = 0 if irrlicht: while (app.GetDevice().run()): # Render scene if step_number % render_steps == 0: app.BeginScene(True, True, chronoirr.SColor(255, 140, 161, 192)) app.DrawAll() app.EndScene() # Collect output data from modules (for inter-module communication) driver_inputs = driver.GetInputs() # Update modules (process inputs from other modules) time = vehicle.GetSystem().GetChTime() driver.Synchronize(time) vehicle.Synchronize(time, driver_inputs, terrain) terrain.Synchronize(time) app.Synchronize("", driver_inputs) # Advance simulation for one timestep for all modules step = step_size # Update controllers steering_controller.Advance(step) throttle_controller.Advance(step) driver.Advance(step) vehicle.Advance(step) terrain.Advance(step) app.Advance(step) # Increment frame number step_number += 1 else: while (True): vehicle.GetSystem().DoStepDynamics(step_size) # Collect output data from modules (for inter-module communication) driver_inputs = driver.GetInputs() # Update modules (process inputs from other modules) time = vehicle.GetSystem().GetChTime() driver.Synchronize(time) vehicle.Synchronize(time, driver_inputs, terrain) terrain.Synchronize(time) # Advance simulation for one timestep for all modules step = step_size # Update controllers steering_controller.Advance(step) throttle_controller.Advance(step) driver.Advance(step) vehicle.Advance(step) terrain.Advance(step) # Increment frame number step_number += 1
def main(): # ---------------------- # Set path to data files # ---------------------- # Path to Chrono data files (textures, etc.) chrono.SetChronoDataPath(CHRONO_DATA_DIR) # Path to the data files for this vehicle (JSON specification files) veh.SetDataPath("./data/") # -------------------------- # Create the various modules # -------------------------- # Create and initialize the vehicle system vehicle = veh.WheeledVehicle(veh.GetDataFile(vehicle_file), NSC_SMC) vehicle.Initialize(chrono.ChCoordsysD(initLoc, initRot)) vehicle.SetSuspensionVisualizationType(veh.VisualizationType_PRIMITIVES) vehicle.SetSteeringVisualizationType(veh.VisualizationType_PRIMITIVES) vehicle.SetWheelVisualizationType(veh.VisualizationType_NONE) # Create the terrain terrain = veh.RigidTerrain(vehicle.GetSystem(), veh.GetDataFile(rigidterrain_file)) AddFixedObstacles(vehicle.GetSystem()) AddMovingObstacles(vehicle.GetSystem()) # Create and initialize the powertrain system powertrain = veh.SimplePowertrain(veh.GetDataFile(simplepowertrain_file)) vehicle.InitializePowertrain(powertrain) # Create and initialize the tires for axle in vehicle.GetAxles(): tireL = veh.RigidTire(veh.GetDataFile(rigidtire_file)) tireR = veh.RigidTire(veh.GetDataFile(rigidtire_file)) vehicle.InitializeTire(tireL, axle.m_wheels[0], veh.VisualizationType_MESH) vehicle.InitializeTire(tireR, axle.m_wheels[1], veh.VisualizationType_MESH) # Create the Irrlicht vehicle application app = veh.ChVehicleIrrApp(vehicle, "Vehicle Demo") app.SetSkyBox() app.AddTypicalLights(chronoirr.vector3df(30, -30, 100), chronoirr.vector3df(30, 50, 100), 250, 130) app.SetChaseCamera(trackPoint, 6.0, 0.5) app.SetTimestep(step_size) app.AssetBindAll() app.AssetUpdateAll() # Create the driver system (interactive) driver = veh.ChIrrGuiDriver(app) # Set the time response for steering and throttle keyboard 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 = 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) # ----------------- # Initialize output # ----------------- try: os.mkdir(out_dir) except: print("Error creating directory ") if (povray_output): try: os.mkdir(pov_dir) except: print("Error creating POV directory ") terrain.ExportMeshPovray(out_dir) # --------------- # Simulation loop # --------------- """driveshaft_speed powertrain_torque throttle_input steering_input braking_input""" # Number of simulation steps between two 3D view render frames render_steps = m.ceil(render_step_size / step_size) #Initialize simulation frame counter and simulation time step_number = 0 frame_number = 0 time = 0 realtime_timer = chrono.ChRealtimeStepTimer() while (app.GetDevice().run()): # Render scene if (step_number % render_steps == 0): app.BeginScene(True, True, chronoirr.SColor(255, 140, 161, 192)) app.DrawAll() app.EndScene() #char filename[100]; #sprintf(filename, "%s/data_%03d.dat", pov_dir.c_str(), frame_number + 1); #utils::WriteShapesPovray(vehicle.GetSystem(), filename); frame_number += 1 # Get driver inputs driver_inputs = driver.GetInputs() # Update modules (process inputs from other modules) time = vehicle.GetSystem().GetChTime() driver.Synchronize(time) vehicle.Synchronize(time, driver_inputs, terrain) terrain.Synchronize(time) app.Synchronize(driver.GetInputModeAsString(), driver_inputs) # Advance simulation for one timestep for all modules driver.Advance(step_size) vehicle.Advance(step_size) terrain.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
# Spin in place for real time to catch up realtime_timer.Spin(step_size) # ============================================================================= # The path to the Chrono data directory containing various assets (meshes, textures, data files) # is automatically set, relative to the default location of this demo. # If running from a different directory, you must change the path to the data directory with: #chrono.SetChronoDataPath('path/to/data') veh.SetDataPath(chrono.GetChronoDataPath() + 'vehicle/') # JSON file for vehicle model vehicle_file = veh.GetDataFile('hmmwv/vehicle/HMMWV_Vehicle.json') # JSON files for terrain rigidterrain_file = veh.GetDataFile('terrain/RigidPlane.json') # JSON file for powertrain (simple) simplepowertrain_file = veh.GetDataFile( 'generic/powertrain/SimplePowertrain.json') # JSON files tire models (rigid) rigidtire_file = veh.GetDataFile('hmmwv/tire/HMMWV_RigidTire.json') # Initial vehicle position initLoc = chrono.ChVectorD(0, 0, 1.6) # Initial vehicle orientation
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