def __init__(self, system: 'WAChronoSystem', vehicle_inputs: 'WAVehicleInputs', env: 'WAEnvironment', filename: str, init_loc: WAVector = WAVector([0, 0, 0.5]), init_rot: WAQuaternion = WAQuaternion([1, 0, 0, 0]), init_speed: float = 0.0): super().__init__( system, vehicle_inputs, get_wa_data_file("vehicles/GoKart/GoKart_KinematicBicycle.json")) # Get the filenames vehicle_file, powertrain_file, tire_file = read_vehicle_model_file( filename) # Create the vehicle vehicle = veh.WheeledVehicle(system._system, vehicle_file) # Initialize the vehicle init_loc = WAVector_to_ChVector(init_loc) init_rot = WAQuaternion_to_ChQuaternion(init_rot) vehicle.Initialize(chrono.ChCoordsysD(init_loc, init_rot), init_speed) # Set the visualization components for the vehicle vehicle.SetChassisVisualizationType(veh.VisualizationType_MESH) vehicle.SetSuspensionVisualizationType(veh.VisualizationType_NONE) vehicle.SetSteeringVisualizationType(veh.VisualizationType_NONE) vehicle.SetWheelVisualizationType(veh.VisualizationType_MESH) # Create the powertrain # Assumes a SimplePowertrain powertrain = veh.SimplePowertrain(powertrain_file) vehicle.InitializePowertrain(powertrain) # Create and initialize the tires for axle in vehicle.GetAxles(): tireL = create_tire_from_json(tire_file) vehicle.InitializeTire(tireL, axle.m_wheels[0], veh.VisualizationType_MESH) tireR = create_tire_from_json(tire_file) vehicle.InitializeTire(tireR, axle.m_wheels[1], veh.VisualizationType_MESH) self._vehicle = vehicle self._terrain = env._terrain
def __init__(self, step_size, sys, controller, irrlicht=False, vehicle_type='json', initLoc=chrono.ChVectorD(0,0,0), initRot=chrono.ChQuaternionD(1,0,0,0), vis_balls=False, render_step_size=1.0/60): # Chrono parameters self.step_size = step_size self.irrlicht = irrlicht self.step_number = 0 # Vehicle controller self.controller = controller # Initial vehicle position self.initLoc = initLoc # Initial vehicle orientation self.initRot = initRot # Point on chassis tracked by the camera (Irrlicht only) self.trackPoint = chrono.ChVectorD(0.0, 0.0, 1.75) if vehicle_type == 'json': # JSON file for vehicle model self.vehicle_file = veh.GetDataPath() + os.path.join('hmmwv', 'vehicle', 'HMMWV_Vehicle.json') checkFile(self.vehicle_file) # JSON file for powertrain (simple) self.simplepowertrain_file = veh.GetDataPath() + os.path.join('generic', 'powertrain', 'SimplePowertrain.json') checkFile(self.simplepowertrain_file) # JSON files tire models (rigid) self.rigidtire_file = veh.GetDataPath() + os.path.join('hmmwv', 'tire', 'HMMWV_RigidTire.json') checkFile(self.rigidtire_file) # -------------------------- # Create the various modules # -------------------------- if sys == None: self.wheeled_vehicle = veh.WheeledVehicle(self.vehicle_file) else: self.wheeled_vehicle = veh.WheeledVehicle(sys, self.vehicle_file) self.wheeled_vehicle.Initialize(chrono.ChCoordsysD(self.initLoc, self.initRot)) self.wheeled_vehicle.SetChassisVisualizationType(veh.VisualizationType_PRIMITIVES) self.wheeled_vehicle.SetSuspensionVisualizationType(veh.VisualizationType_PRIMITIVES) self.wheeled_vehicle.SetSteeringVisualizationType(veh.VisualizationType_PRIMITIVES) self.wheeled_vehicle.SetWheelVisualizationType(veh.VisualizationType_NONE) # Create and initialize the powertrain system self.powertrain = veh.SimplePowertrain(self.simplepowertrain_file) self.wheeled_vehicle.InitializePowertrain(self.powertrain) # Create and initialize the tires for axle in self.wheeled_vehicle.GetAxles(): tireL = veh.RigidTire(self.rigidtire_file) self.wheeled_vehicle.InitializeTire(tireL, axle.m_wheels[0], veh.VisualizationType_MESH) tireR = veh.RigidTire(self.rigidtire_file) self.wheeled_vehicle.InitializeTire(tireR, axle.m_wheels[1], veh.VisualizationType_MESH) self.vehicle = self.wheeled_vehicle self.sys = self.wheeled_vehicle.GetSystem() elif vehicle_type == 'rccar': if sys == None: self.rc_vehicle = veh.RCCar() self.rc_vehicle.SetContactMethod(chrono.ChMaterialSurface.SMC) self.rc_vehicle.SetChassisCollisionType(veh.ChassisCollisionType_NONE) else: self.rc_vehicle = veh.RCCar(sys) self.rc_vehicle.SetChassisFixed(False) self.rc_vehicle.SetInitPosition(chrono.ChCoordsysD(initLoc, initRot)) self.rc_vehicle.SetTireType(veh.TireModelType_RIGID) self.rc_vehicle.SetTireStepSize(step_size) self.rc_vehicle.Initialize() self.rc_vehicle.SetChassisVisualizationType(veh.VisualizationType_PRIMITIVES) self.rc_vehicle.SetSuspensionVisualizationType(veh.VisualizationType_PRIMITIVES) self.rc_vehicle.SetSteeringVisualizationType(veh.VisualizationType_PRIMITIVES) self.rc_vehicle.SetWheelVisualizationType(veh.VisualizationType_PRIMITIVES) self.rc_vehicle.SetTireVisualizationType(veh.VisualizationType_PRIMITIVES) self.vehicle = self.rc_vehicle.GetVehicle() self.sys = self.vehicle.GetSystem() self.trackPoint = chrono.ChVectorD(4, 0.0, .15) elif vehicle_type == 'sedan': if sys == None: self.sedan = veh.Sedan() self.sedan.SetContactMethod(chrono.ChMaterialSurface.NSC) self.sedan.SetChassisCollisionType(veh.ChassisCollisionType_NONE) else: self.sedan = veh.Sedan(sys) self.sedan.SetChassisFixed(False) self.sedan.SetInitPosition(chrono.ChCoordsysD(initLoc, initRot)) self.sedan.SetTireType(veh.TireModelType_RIGID) self.sedan.SetTireStepSize(step_size) self.sedan.Initialize() self.sedan.SetChassisVisualizationType(veh.VisualizationType_PRIMITIVES) self.sedan.SetSuspensionVisualizationType(veh.VisualizationType_PRIMITIVES) self.sedan.SetSteeringVisualizationType(veh.VisualizationType_PRIMITIVES) self.sedan.SetWheelVisualizationType(veh.VisualizationType_PRIMITIVES) self.sedan.SetTireVisualizationType(veh.VisualizationType_PRIMITIVES) self.vehicle = self.sedan.GetVehicle() self.sys = self.vehicle.GetVehicle().GetSystem() # ------------- # Create driver # ------------- self.driver = Driver(self.vehicle) self.driver.SetStepSize(step_size) # 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 self.driver.SetSteeringDelta(render_step_size / steering_time) self.driver.SetThrottleDelta(render_step_size / throttle_time) self.driver.SetBrakingDelta(render_step_size / braking_time) self.vis_balls = vis_balls if self.vis_balls: self.sentinel_sphere = chrono.ChBodyEasySphere(.25, 1000, False, True) self.sentinel_sphere.SetBodyFixed(True) self.sentinel_sphere.AddAsset(chrono.ChColorAsset(1,0,0)) self.sys.Add(self.sentinel_sphere) self.sentinel_target = chrono.ChBodyEasySphere(.25, 1000, False, True) self.sentinel_target.SetBodyFixed(True) self.sentinel_target.AddAsset(chrono.ChColorAsset(0,1,0)); self.sys.Add(self.sentinel_target) # Vehicle parameters for matplotlib self.length = self.vehicle.GetWheelbase() + 2.0 # [m] self.width = self.vehicle.GetWheeltrack(0) # [m] self.backtowheel = 1.0 # [m] self.wheel_len = self.vehicle.GetWheel(0, 1).GetWidth() * 2 # [m] self.wheel_width = self.vehicle.GetWheel(0, 1).GetWidth() # [m] self.tread = self.vehicle.GetWheeltrack(0) / 2 # [m] self.wb = self.vehicle.GetWheelbase() # [m] self.offset = [-4.0,0] # [m]
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.ChContactMethod_NSC) vehicle.Initialize(chrono.ChCoordsysD(initLoc, initRot)) #vehicle.GetChassis().SetFixed(True) vehicle.SetChassisVisualizationType(veh.VisualizationType_MESH) vehicle.SetSuspensionVisualizationType(veh.VisualizationType_PRIMITIVES) vehicle.SetSteeringVisualizationType(veh.VisualizationType_PRIMITIVES) vehicle.SetWheelVisualizationType(veh.VisualizationType_MESH) # Create and initialize the vehicle tires for axle in vehicle.GetAxles(): tireL = veh.TMeasyTire(vehicle_tire_file) vehicle.InitializeTire(tireL, axle.m_wheels[0], veh.VisualizationType_MESH) tireR = veh.TMeasyTire(vehicle_tire_file) vehicle.InitializeTire(tireR, axle.m_wheels[1], veh.VisualizationType_MESH) # Create and initialize the powertrain system powertrain = veh.SimpleMapPowertrain(vehicle_powertrain_file) vehicle.InitializePowertrain(powertrain) # Create and initialize the trailer trailer = veh.WheeledTrailer(vehicle.GetSystem(), trailer_file) trailer.Initialize(vehicle.GetChassis()) trailer.SetChassisVisualizationType(veh.VisualizationType_PRIMITIVES) trailer.SetSuspensionVisualizationType(veh.VisualizationType_PRIMITIVES) trailer.SetWheelVisualizationType(veh.VisualizationType_NONE) # Create abd initialize the trailer tires for axle in trailer.GetAxles(): tireL = veh.TMeasyTire(trailer_tire_file) trailer.InitializeTire(tireL, axle.m_wheels[0], veh.VisualizationType_PRIMITIVES) tireR = veh.TMeasyTire(trailer_tire_file) trailer.InitializeTire(tireR, axle.m_wheels[1], veh.VisualizationType_PRIMITIVES) # Create the ground terrain = veh.RigidTerrain(vehicle.GetSystem(), rigidterrain_file) app = veh.ChVehicleIrrApp(vehicle, 'Sedan+Trailer (JSON specification)', 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() 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) driver.Initialize() # --------------- # Simulation loop # --------------- realtime_timer = chrono.ChRealtimeStepTimer() while (app.GetDevice().run()): # Render scene app.BeginScene(True, True, irr.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) trailer.Synchronize(time, driver_inputs.m_braking, 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) trailer.Advance(step_size) terrain.Advance(step_size) app.Advance(step_size) # Spin in place for real time to catch up realtime_timer.Spin(step_size)
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.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) app = veh.ChVehicleIrrApp(vehicle) app.SetSkyBox() app.AddTypicalLights(chronoirr.vector3df(30, -30, 100), chronoirr.vector3df(30, 50, 100), 250, 130) app.AddTypicalLogo(chrono.GetChronoDataPath() + 'logo_pychrono_alpha.png') app.SetChaseCamera(trackPoint, 6.0, 0.5) app.SetTimestep(step_size) app.AssetBindAll() app.AssetUpdateAll() 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) driver.Initialize() # ----------------- # Initialize 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 # --------------- realtime_timer = chrono.ChRealtimeStepTimer() while (app.GetDevice().run()) : # Render scene 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.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) # Spin in place for real time to catch up realtime_timer.Spin(step_size)
def main(): # print("Copyright (c) 2017 projectchrono.org\nChrono version: ", CHRONO_VERSION , "\n\n") # --------- # Load path # --------- path_file = "../data/paths/loop2.txt" path = np.genfromtxt(path_file, delimiter=",") ds = 5 # [m] distance of each intepolated points sp = Spline2D(path[:, 0], path[:, 1]) s = np.arange(0, sp.s[-1], ds) px, py = [], [] for i_s in s: ix, iy = sp.calc_position(i_s) px.append(ix) py.append(iy) px.append(px[0]) py.append(py[0]) initLoc = chrono.ChVectorD(px[0], py[0], 0.5) # -------------------------- # 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) # ------------------ # Draw path # ------------------ if irrlicht: road = vehicle.GetSystem().NewBody() road.SetBodyFixed(True) vehicle.GetSystem().AddBody(road) num_points = len(px) lines = chrono.ChLinePath() for i in range(num_points - 1): lines.AddSubLine( chrono.ChLineSegment( chrono.ChVectorD(px[i], py[i], 0.1), chrono.ChVectorD(px[i + 1], py[i + 1], 0.1))) path_asset = chrono.ChLineShape() path_asset.SetLineGeometry(lines) 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) # -------------------- controller = MPCController(vehicle, driver, path) # ----------------------- # Initialize irrlicht app # ----------------------- if irrlicht: app = veh.ChVehicleIrrApp(vehicle) app.SetHUDLocation(500, 20) app.SetSkyBox() app.AddTypicalLogo() app.AddTypicalLights( chronoirr.vector3df(-150.0, -150.0, 200.0), chronoirr.vector3df(-150.0, 150.0, 200.0), 100, 100, ) app.AddTypicalLights( chronoirr.vector3df(150.0, -150.0, 200.0), chronoirr.vector3df(150.0, 150.0, 200.0), 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 controller.Advance(step) driver.Advance(step) vehicle.Advance(step) terrain.Advance(step) app.Advance(step) # Increment frame number step_number += 1 else: while True: # 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) vis.Advance(step) # Increment frame number step_number += 1
def reset(self): print("reset") self.generator.generatePath(difficulty=50, seed=randint(1, 1000)) self.path = Path(self.generator) self.path_tracker = PathTracker(self.path) self.initLoc = self.path_tracker.GetInitLoc() self.initRot = self.path_tracker.GetInitRot() self.vehicle = veh.WheeledVehicle(self.vehicle_file, chrono.ChMaterialSurface.NSC) self.vehicle.Initialize(chrono.ChCoordsysD(self.initLoc, self.initRot)) self.vehicle.SetStepsize(self.timestep) self.vehicle.SetChassisVisualizationType( veh.VisualizationType_PRIMITIVES) self.vehicle.SetSuspensionVisualizationType( veh.VisualizationType_PRIMITIVES) self.vehicle.SetSteeringVisualizationType( veh.VisualizationType_PRIMITIVES) self.vehicle.SetWheelVisualizationType(veh.VisualizationType_NONE) # Create and initialize the powertrain system self.powertrain = veh.SimplePowertrain(self.simplepowertrain_file) self.vehicle.InitializePowertrain(self.powertrain) # Create the ground self.terrain = veh.RigidTerrain(self.vehicle.GetSystem(), self.rigidterrain_file) for axle in self.vehicle.GetAxles(): tireL = veh.RigidTire(self.rigidtire_file) self.vehicle.InitializeTire(tireL, axle.m_wheels[0], veh.VisualizationType_MESH) tireR = veh.RigidTire(self.rigidtire_file) self.vehicle.InitializeTire(tireR, axle.m_wheels[1], veh.VisualizationType_MESH) # ------------- # Create driver # ------------- self.driver = Driver(self.vehicle) # Time interval between two render frames render_step_size = 1.0 / 60 # FPS = 60 # 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 self.driver.SetSteeringDelta(render_step_size / steering_time) self.driver.SetThrottleDelta(render_step_size / throttle_time) self.driver.SetBrakingDelta(render_step_size / braking_time) vec = chrono.ChVectorD(0, 0, 0) self.path_tracker.calcClosestPoint(self.vehicle.GetVehiclePos(), vec) self.last_dist = vec.Length() self.last_throttle, self.last_braking, self.last_steering = 0, 0, 0 if self.render: road = self.vehicle.GetSystem().NewBody() road.SetBodyFixed(True) self.vehicle.GetSystem().AddBody(road) num_points = self.path.getNumPoints() path_asset = chrono.ChLineShape() path_asset.SetLineGeometry( chrono.ChLineBezier(self.path_tracker.path)) path_asset.SetColor(chrono.ChColor(0.0, 0.8, 0.0)) path_asset.SetNumRenderPoints(max(2 * num_points, 400)) road.AddAsset(path_asset) if self.render: self.app = veh.ChVehicleIrrApp(self.vehicle) self.app.SetHUDLocation(500, 20) self.app.SetSkyBox() self.app.AddTypicalLogo() self.app.AddTypicalLights(chronoirr.vector3df(-150., -150., 200.), chronoirr.vector3df(-150., 150., 200.), 100, 100) self.app.AddTypicalLights(chronoirr.vector3df(150., -150., 200.), chronoirr.vector3df(150., 150., 200.), 100, 100) self.app.EnableGrid(False) self.app.SetChaseCamera(self.trackPoint, 6.0, 0.5) self.app.SetTimestep(self.timestep) # --------------------------------------------------------------------- # # Create an Irrlicht application to visualize the system # # ==IMPORTANT!== Use this function for adding a ChIrrNodeAsset to all items # in the system. These ChIrrNodeAsset assets are 'proxies' to the Irrlicht meshes. # If you need a finer control on which item really needs a visualization proxy # Irrlicht, just use application.AssetBind(myitem); on a per-item basis. self.app.AssetBindAll() # ==IMPORTANT!== Use this function for 'converting' into Irrlicht meshes the assets # that you added to the bodies into 3D shapes, they can be visualized by Irrlicht! self.app.AssetUpdateAll() self.isdone = False self.steps = 0 self.step(np.zeros(3)) self.tracknum = self.tracknum = 0 if self.tracknum >= 3 else self.tracknum + 1 # self.tracknum = self.tracknum + 1 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
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) powertrain.Initialize(vehicle.GetChassisBody(), vehicle.GetDriveshaft()) #// Create and initialize the tires num_axles = vehicle.GetNumberAxles() num_wheels = 2 * num_axles tires = [ veh.RigidTire(rigidtire_file) for i in range(num_wheels)] for i, t in enumerate(tires): #t = std::make_shared<vehicle::RigidTire>(vehicle::GetDataFile(rigidtire_file)); s = [veh.LEFT, veh.RIGHT] t.Initialize(vehicle.GetWheelBody(veh.WheelID(i)), s[i % 2]) t.SetVisualizationType(veh.VisualizationType_MESH) app = veh.ChVehicleIrrApp (vehicle, powertrain) 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() """ bool do_shadows = false; // shadow map is experimental irr::scene::ILightSceneNode* mlight = 0; if (do_shadows) { mlight = application.AddLightWithShadow( irr::core::vector3df(10.f, 30.f, 60.f), irr::core::vector3df(0.f, 0.f, 0.f), 150, 60, 80, 15, 512, irr::video::SColorf(1, 1, 1), false, false); } else { application.AddTypicalLights( irr::core::vector3df(30.f, -30.f, 100.f), irr::core::vector3df(30.f, 50.f, 100.f), 250, 130); } if (do_shadows) application.AddShadowAll(); """ 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) # Set file with driver input time series driver.SetInputDataFile(driver_file) """ #else ChDataDriver driver(vehicle, vehicle::GetDataFile(driver_file)); #endif """ driver.Initialize() # ----------------- # 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)""" # Generate JSON information with available output channels out_json = vehicle.ExportComponentList() print(out_json) vehicle.ExportComponentList(out_dir + "/component_list.json") # --------------- # Simulation loop # --------------- # Inter-module communication data tire_forces = veh.TerrainForces(num_wheels) wheel_states = veh.WheelStates(num_wheels) # Initialize simulation frame counter and simulation time step_number = 0 time = 0 #ifdef USE_IRRLICHT realtime_timer = chrono.ChRealtimeStepTimer() while (app.GetDevice().run()) : # Render scene app.BeginScene(True, True, chronoirr.SColor(255, 140, 161, 192)) app.DrawAll() # Collect output data from modules (for inter-module communication) throttle_input = driver.GetThrottle() steering_input = driver.GetSteering() braking_input = driver.GetBraking() powertrain_torque = powertrain.GetOutputTorque() driveshaft_speed = vehicle.GetDriveshaftSpeed() for i in range (num_wheels) : tire_forces[i] = tires[i].GetTireForce() wheel_states[i] = vehicle.GetWheelState(veh.WheelID(i)) # Update modules (process inputs from other modules) time = vehicle.GetSystem().GetChTime() driver.Synchronize(time) powertrain.Synchronize(time, throttle_input, driveshaft_speed) vehicle.Synchronize(time, steering_input, braking_input, powertrain_torque, tire_forces) terrain.Synchronize(time) for i in range (num_wheels): tires[i].Synchronize(time, wheel_states[i], terrain) app.Synchronize(driver.GetInputModeAsString(), steering_input, throttle_input, braking_input) # Advance simulation for one timestep for all modules step = realtime_timer.SuggestSimulationStep(step_size) driver.Advance(step) powertrain.Advance(step) vehicle.Advance(step) terrain.Advance(step) for i in range(num_wheels) : tires[i].Advance(step) app.Advance(step) # Increment frame number step_number += 1 app.EndScene()