class Drive(ShowBase): def __init__(self): ShowBase.__init__(self) #Setup scene = BulletWorld() scene.setGravity(Vec3(0, 0, -9.81)) base.setBackgroundColor(0.6,0.9,0.9) fog = Fog("The Fog") fog.setColor(0.9,0.9,1.0) fog.setExpDensity(0.003) render.setFog(fog) #Lighting #Sun light sun = DirectionalLight("The Sun") sun_np = render.attachNewNode(sun) sun_np.setHpr(0,-60,0) render.setLight(sun_np) #Ambient light amb = AmbientLight("The Ambient Light") amb.setColor(VBase4(0.39,0.39,0.39, 1)) amb_np = render.attachNewNode(amb) render.setLight(amb_np) #Variables self.gear = 0 self.start = 0 self.Pbrake = 0 self.terrain_var = 1 self.time = 0 self.headlight_var = 0 self.RPM = 0 self.clutch = 0 self.carmaxspeed = 100 #KPH self.carmaxreversespeed = -40 #KPH self.steering = 0 #Functions def V1(): camera.setPos(0.25,-1.2,0.5) camera.setHpr(0,-13,0) def V2(): camera.setPos(0,-15,3) camera.setHpr(0,-10,0) def V3(): camera.setPos(0,0,9) camera.setHpr(0,-90,0) def up(): self.gear = self.gear -1 if self.gear < -1: self.gear = -1 def down(): self.gear = self.gear +1 if self.gear > 1: self.gear = 1 def start_function(): self.start = 1 self.start_sound.play() self.engine_idle_sound.play() self.RPM = 1000 def stop_function(): self.start = 0 self.engine_idle_sound.stop() def parkingbrake(): self.Pbrake = (self.Pbrake + 1) % 2 def rotate(): Car_np.setHpr(0, 0, 0) def horn(): self.horn_sound.play() def set_time(): if self.time == -1: sun.setColor(VBase4(0.4, 0.3, 0.3, 1)) base.setBackgroundColor(0.8,0.7,0.7) if self.time == 0: sun.setColor(VBase4(0.7, 0.7, 0.7, 1)) base.setBackgroundColor(0.6,0.9,0.9) if self.time == 1: sun.setColor(VBase4(0.2, 0.2, 0.2, 1)) base.setBackgroundColor(0.55,0.5,0.5) if self.time == 2: sun.setColor(VBase4(0.02, 0.02, 0.05, 1)) base.setBackgroundColor(0.3,0.3,0.3) if self.time == -2: self.time = -1 if self.time == 3: self.time = 2 def time_forward(): self.time = self.time + 1 def time_backward(): self.time = self.time -1 def set_terrain(): if self.terrain_var == 1: self.ground_model.setTexture(self.ground_tex, 1) self.ground_model.setScale(3) if self.terrain_var == 2: self.ground_model.setTexture(self.ground_tex2, 1) self.ground_model.setScale(3) if self.terrain_var == 3: self.ground_model.setTexture(self.ground_tex3, 1) self.ground_model.setScale(4) if self.terrain_var == 4: self.terrain_var = 1 if self.terrain_var == 0: self.terrain_var = 3 def next_terrain(): self.terrain_var = self.terrain_var + 1 def previous_terrain(): self.terrain_var = self.terrain_var - 1 def show_menu(): self.menu_win.show() self.a1.show() self.a2.show() self.a3.show() self.a4.show() self.t1.show() self.t2.show() self.ok.show() self.exit_button.show() def hide_menu(): self.menu_win.hide() self.a1.hide() self.a2.hide() self.a3.hide() self.a4.hide() self.ok.hide() self.t1.hide() self.t2.hide() self.exit_button.hide() def Menu(): self.menu_win = OnscreenImage(image = "Textures/menu.png", pos = (0.9,0,0), scale = (0.5)) self.menu_win.setTransparency(TransparencyAttrib.MAlpha) #The Arrow Buttons self.a1 = DirectButton(text = "<", scale = 0.2, pos = (0.55,0,0.25), command = previous_terrain) self.a2 = DirectButton(text = ">", scale = 0.2, pos = (1.15,0,0.25), command = next_terrain) self.a3 = DirectButton(text = "<", scale = 0.2, pos = (0.55,0,0.0), command = time_backward) self.a4 = DirectButton(text = ">", scale = 0.2, pos = (1.15,0,0.0), command = time_forward) #The Text self.t1 = OnscreenText(text = "Terrain", pos = (0.85,0.25,0), scale = 0.1, fg = (0.4,0.4,0.5,1)) self.t2 = OnscreenText(text = "Time", pos = (0.85,0,0), scale = 0.1, fg = (0.4,0.4,0.5,1)) #The Buttons self.ok = DirectButton(text = "Okay", scale = 0.11, pos = (0.87,0,-0.25), command = hide_menu) self.exit_button = DirectButton(text = "Quit", scale = 0.11, pos = (0.87,0,-0.42), command = sys.exit) Menu() def take_screenshot(): base.screenshot("Screenshot") def set_headlights(): if self.headlight_var == 1: Headlight1.setColor(VBase4(9.0,8.9,8.9,1)) Headlight2.setColor(VBase4(9.0,8.9,8.9,1)) if self.headlight_var == 0: Headlight1.setColor(VBase4(0,0,0,1)) Headlight2.setColor(VBase4(0,0,0,1)) def headlights(): self.headlight_var = (self.headlight_var + 1) % 2 def update_rpm(): #Simulate RPM if self.start == 1: if self.gear == 0: self.RPM = self.RPM - self.RPM / 400 else: self.RPM = self.RPM + self.carspeed / 9 self.RPM = self.RPM - self.RPM / 200 #Reset RPM to 0 when engine is off if self.start == 0: if self.RPM > 0.0: self.RPM = self.RPM - 40 if self.RPM < 10: self.RPM = 0.0 #Idle RPM power if self.start == 1: if self.RPM < 650: self.RPM = self.RPM + 4 if self.RPM < 600: self.clutch = 1 else: self.clutch = 0 #RPM limit if self.RPM > 6000: self.RPM = 6000 #Controls inputState.watchWithModifiers("F", "arrow_up") inputState.watchWithModifiers("B", "arrow_down") inputState.watchWithModifiers("L", "arrow_left") inputState.watchWithModifiers("R", "arrow_right") do = DirectObject() do.accept("escape", show_menu) do.accept("1", V1) do.accept("2", V2) do.accept("3", V3) do.accept("page_up", up) do.accept("page_down", down) do.accept("x-repeat", start_function) do.accept("x", stop_function) do.accept("p", parkingbrake) do.accept("backspace", rotate) do.accept("enter", horn) do.accept("f12", take_screenshot) do.accept("h", headlights) #The ground self.ground = BulletPlaneShape(Vec3(0, 0, 1,), 1) self.ground_node = BulletRigidBodyNode("The ground") self.ground_node.addShape(self.ground) self.ground_np = render.attachNewNode(self.ground_node) self.ground_np.setPos(0, 0, -2) scene.attachRigidBody(self.ground_node) self.ground_model = loader.loadModel("Models/plane.egg") self.ground_model.reparentTo(render) self.ground_model.setPos(0,0,-1) self.ground_model.setScale(3) self.ground_tex = loader.loadTexture("Textures/ground.png") self.ground_tex2 = loader.loadTexture("Textures/ground2.png") self.ground_tex3 = loader.loadTexture("Textures/ground3.png") self.ground_model.setTexture(self.ground_tex, 1) #The car Car_shape = BulletBoxShape(Vec3(1, 2.0, 1.0)) Car_node = BulletRigidBodyNode("The Car") Car_node.setMass(1200.0) Car_node.addShape(Car_shape) Car_np = render.attachNewNode(Car_node) Car_np.setPos(0,0,3) Car_np.setHpr(0,0,0) Car_np.node().setDeactivationEnabled(False) scene.attachRigidBody(Car_node) Car_model = loader.loadModel("Models/Car.egg") Car_model.reparentTo(Car_np) Car_tex = loader.loadTexture("Textures/Car1.png") Car_model.setTexture(Car_tex, 1) self.Car_sim = BulletVehicle(scene, Car_np.node()) self.Car_sim.setCoordinateSystem(ZUp) scene.attachVehicle(self.Car_sim) #The inside of the car Car_int = loader.loadModel("Models/inside.egg") Car_int.reparentTo(Car_np) Car_int_tex = loader.loadTexture("Textures/inside.png") Car_int.setTexture(Car_int_tex, 1) Car_int.setTransparency(TransparencyAttrib.MAlpha) #The steering wheel Sw = loader.loadModel("Models/Steering wheel.egg") Sw.reparentTo(Car_np) Sw.setPos(0.25,0,-0.025) #The first headlight Headlight1 = Spotlight("Headlight1") lens = PerspectiveLens() lens.setFov(180) Headlight1.setLens(lens) Headlight1np = render.attachNewNode(Headlight1) Headlight1np.reparentTo(Car_np) Headlight1np.setPos(-0.8,2.5,-0.5) Headlight1np.setP(-15) render.setLight(Headlight1np) #The second headlight Headlight2 = Spotlight("Headlight2") Headlight2.setLens(lens) Headlight2np = render.attachNewNode(Headlight2) Headlight2np.reparentTo(Car_np) Headlight2np.setPos(0.8,2.5,-0.5) Headlight2np.setP(-15) render.setLight(Headlight2np) #Sounds self.horn_sound = loader.loadSfx("Sounds/horn.ogg") self.start_sound = loader.loadSfx("Sounds/enginestart.ogg") self.engine_idle_sound = loader.loadSfx("Sounds/engineidle.ogg") self.engine_idle_sound.setLoop(True) self.accelerate_sound = loader.loadSfx("Sounds/enginethrottle.ogg") #Camera base.disableMouse() camera.reparentTo(Car_np) camera.setPos(0,-15,3) camera.setHpr(0,-10,0) #Wheel function def Wheel(pos, np, r, f): w = self.Car_sim.createWheel() w.setNode(np.node()) w.setChassisConnectionPointCs(pos) w.setFrontWheel(f) w.setWheelDirectionCs(Vec3(0, 0, -1)) w.setWheelAxleCs(Vec3(1, 0, 0)) w.setWheelRadius(r) w.setMaxSuspensionTravelCm(40) w.setSuspensionStiffness(120) w.setWheelsDampingRelaxation(2.3) w.setWheelsDampingCompression(4.4) w.setFrictionSlip(50) w.setRollInfluence(0.1) #Wheels w1_np = loader.loadModel("Models/Lwheel") w1_np.reparentTo(render) w1_np.setColorScale(0,6) Wheel(Point3(-1,1,-0.6), w1_np, 0.4, False) w2_np = loader.loadModel("Models/Rwheel") w2_np.reparentTo(render) w2_np.setColorScale(0,6) Wheel(Point3(-1.1,-1.2,-0.6), w2_np, 0.4, True) w3_np = loader.loadModel("Models/Lwheel") w3_np.reparentTo(render) w3_np.setColorScale(0,6) Wheel(Point3(1.1,-1,-0.6), w3_np, 0.4, True) w4_np = loader.loadModel("Models/Rwheel") w4_np.reparentTo(render) w4_np.setColorScale(0,6) Wheel(Point3(1,1,-0.6), w4_np, 0.4, False) #The engine and steering def processInput(dt): #Vehicle properties self.steeringClamp = 35.0 self.steeringIncrement = 70 engineForce = 0.0 brakeForce = 0.0 #Get the vehicle's current speed self.carspeed = self.Car_sim.getCurrentSpeedKmHour() #Engage clutch when in gear 0 if self.gear == 0: self.clutch = 1 #Slow the steering when at higher speeds self.steeringIncrement = self.steeringIncrement - self.carspeed / 1.5 #Reset the steering if not inputState.isSet("L") and not inputState.isSet("R"): if self.steering < 0.00: self.steering = self.steering + 0.6 if self.steering > 0.00: self.steering = self.steering - 0.6 if self.steering < 1.0 and self.steering > -1.0: self.steering = 0 #Slow the car down while it's moving if self.clutch == 0: brakeForce = brakeForce + self.carspeed / 5 else: brakeForce = brakeForce + self.carspeed / 15 #Forward if self.start == 1: if inputState.isSet("F"): self.RPM = self.RPM + 35 self.accelerate_sound.play() if self.clutch == 0: if self.gear == -1: if self.carspeed > self.carmaxreversespeed: engineForce = -self.RPM / 3 if self.gear == 1: if self.carspeed < self.carmaxspeed: engineForce = self.RPM / 1 #Brake if inputState.isSet("B"): engineForce = 0.0 brakeForce = 12.0 if self.gear != 0 and self.clutch == 0: self.RPM = self.RPM - 20 #Left if inputState.isSet("L"): if self.steering < 0.0: #This makes the steering reset at the correct speed when turning from right to left self.steering += dt * self.steeringIncrement + 0.6 self.steering = min(self.steering, self.steeringClamp) else: #Normal steering self.steering += dt * self.steeringIncrement self.steering = min(self.steering, self.steeringClamp) #Right if inputState.isSet("R"): if self.steering > 0.0: #This makes the steering reset at the correct speed when turning from left to right self.steering -= dt * self.steeringIncrement + 0.6 self.steering = max(self.steering, -self.steeringClamp) else: #Normal steering self.steering -= dt * self.steeringIncrement self.steering = max(self.steering, -self.steeringClamp) #Park if self.Pbrake == 1: brakeForce = 10.0 if self.gear != 0 and self. clutch == 0: self.RPM = self.RPM - 20 #Apply forces to wheels self.Car_sim.applyEngineForce(engineForce, 0); self.Car_sim.applyEngineForce(engineForce, 3); self.Car_sim.setBrake(brakeForce, 1); self.Car_sim.setBrake(brakeForce, 2); self.Car_sim.setSteeringValue(self.steering, 0); self.Car_sim.setSteeringValue(self.steering, 3); #Steering wheel Sw.setHpr(0,0,-self.steering*10) #The HUD self.gear_hud = OnscreenImage(image = "Textures/gear_hud.png", pos = (-1,0,-0.85), scale = (0.2)) self.gear_hud.setTransparency(TransparencyAttrib.MAlpha) self.gear2_hud = OnscreenImage(image = "Textures/gear2_hud.png", pos = (-1,0,-0.85), scale = (0.2)) self.gear2_hud.setTransparency(TransparencyAttrib.MAlpha) self.starter = OnscreenImage(image = "Textures/starter.png", pos = (-1.2,0,-0.85), scale = (0.15)) self.starter.setTransparency(TransparencyAttrib.MAlpha) self.park = OnscreenImage(image = "Textures/pbrake.png", pos = (-0.8,0,-0.85), scale = (0.1)) self.park.setTransparency(TransparencyAttrib.MAlpha) self.rev_counter = OnscreenImage(image = "Textures/dial.png", pos = (-1.6, 0.0, -0.70), scale = (0.6,0.6,0.4)) self.rev_counter.setTransparency(TransparencyAttrib.MAlpha) self.rev_needle = OnscreenImage(image = "Textures/needle.png", pos = (-1.6, 0.0, -0.70), scale = (0.5)) self.rev_needle.setTransparency(TransparencyAttrib.MAlpha) self.rev_text = OnscreenText(text = " ", pos = (-1.6, -0.90, 0), scale = 0.05) self.speedometer = OnscreenImage(image = "Textures/dial.png", pos = (-1.68, 0.0, -0.10), scale = (0.7,0.7,0.5)) self.speedometer.setTransparency(TransparencyAttrib.MAlpha) self.speedometer_needle = OnscreenImage(image = "Textures/needle.png", pos = (-1.68, 0.0, -0.10), scale = (0.5)) self.speedometer_needle.setTransparency(TransparencyAttrib.MAlpha) self.speedometer_text = OnscreenText(text = " ", pos = (-1.68, -0.35, 0), scale = 0.05) #Update the HUD def Update_HUD(): #Move gear selector if self.gear == -1: self.gear2_hud.setPos(-1,0,-0.785) if self.gear == 0: self.gear2_hud.setPos(-1,0,-0.85) if self.gear == 1: self.gear2_hud.setPos(-1,0,-0.91) #Rotate starter if self.start == 0: self.starter.setHpr(0,0,0) else: self.starter.setHpr(0,0,45) #Update the parking brake light if self.Pbrake == 1: self.park.setImage("Textures/pbrake2.png") self.park.setTransparency(TransparencyAttrib.MAlpha) else: self.park.setImage("Textures/pbrake.png") self.park.setTransparency(TransparencyAttrib.MAlpha) #Update the rev counter self.rev_needle.setR(self.RPM/22) rev_string = str(self.RPM)[:4] self.rev_text.setText(rev_string+" RPM") #Update the speedometer if self.carspeed > 0.0: self.speedometer_needle.setR(self.carspeed*2.5) if self.carspeed < 0.0: self.speedometer_needle.setR(-self.carspeed*2.5) speed_string = str(self.carspeed)[:3] self.speedometer_text.setText(speed_string+" KPH") #Update the program def update(task): dt = globalClock.getDt() processInput(dt) Update_HUD() set_time() set_terrain() set_headlights() update_rpm() scene.doPhysics(dt, 5, 1.0/180.0) return task.cont taskMgr.add(update, "Update")
class CarEnv(DirectObject): def __init__(self, params={}): self._params = params if 'random_seed' in self._params: np.random.seed(self._params['random_seed']) self._use_vel = self._params.get('use_vel', True) self._run_as_task = self._params.get('run_as_task', False) self._do_back_up = self._params.get('do_back_up', False) self._use_depth = self._params.get('use_depth', False) self._use_back_cam = self._params.get('use_back_cam', False) self._collision_reward = self._params.get('collision_reward', 0.) if not self._params.get('visualize', False): loadPrcFileData('', 'window-type offscreen') # Defines base, render, loader try: ShowBase() except: pass base.setBackgroundColor(0.0, 0.0, 0.0, 1) # World self._worldNP = render.attachNewNode('World') self._world = BulletWorld() self._world.setGravity(Vec3(0, 0, -9.81)) self._dt = params.get('dt', 0.25) self._step = 0.05 # Vehicle shape = BulletBoxShape(Vec3(0.6, 1.0, 0.25)) ts = TransformState.makePos(Point3(0., 0., 0.25)) self._vehicle_node = BulletRigidBodyNode('Vehicle') self._vehicle_node.addShape(shape, ts) self._mass = self._params.get('mass', 10.) self._vehicle_node.setMass(self._mass) self._vehicle_node.setDeactivationEnabled(False) self._vehicle_node.setCcdSweptSphereRadius(1.0) self._vehicle_node.setCcdMotionThreshold(1e-7) self._vehicle_pointer = self._worldNP.attachNewNode(self._vehicle_node) self._world.attachRigidBody(self._vehicle_node) self._vehicle = BulletVehicle(self._world, self._vehicle_node) self._vehicle.setCoordinateSystem(ZUp) self._world.attachVehicle(self._vehicle) self._addWheel(Point3(0.3, 0.5, 0.07), True, 0.07) self._addWheel(Point3(-0.3, 0.5, 0.07), True, 0.07) self._addWheel(Point3(0.3, -0.5, 0.07), False, 0.07) self._addWheel(Point3(-0.3, -0.5, 0.07), False, 0.07) # Camera size = self._params.get('size', [160, 90]) hfov = self._params.get('hfov', 60) near_far = self._params.get('near_far', [0.1, 100.]) self._camera_sensor = Panda3dCameraSensor(base, color=not self._use_depth, depth=self._use_depth, size=size, hfov=hfov, near_far=near_far, title='front cam') self._camera_node = self._camera_sensor.cam self._camera_node.setPos(0.0, 0.5, 0.375) self._camera_node.lookAt(0.0, 6.0, 0.0) self._camera_node.reparentTo(self._vehicle_pointer) if self._use_back_cam: self._back_camera_sensor = Panda3dCameraSensor( base, color=not self._use_depth, depth=self._use_depth, size=size, hfov=hfov, near_far=near_far, title='back cam') self._back_camera_node = self._back_camera_sensor.cam self._back_camera_node.setPos(0.0, -0.5, 0.375) self._back_camera_node.lookAt(0.0, -6.0, 0.0) self._back_camera_node.reparentTo(self._vehicle_pointer) # Car Simulator self._des_vel = None self._setup() # Input self.accept('escape', self._doExit) self.accept('r', self.reset) self.accept('f1', self._toggleWireframe) self.accept('f2', self._toggleTexture) self.accept('f3', self._view_image) self.accept('f5', self._doScreenshot) self.accept('q', self._forward_0) self.accept('w', self._forward_1) self.accept('e', self._forward_2) self.accept('a', self._left) self.accept('s', self._stop) self.accept('x', self._backward) self.accept('d', self._right) self.accept('m', self._mark) self._steering = 0.0 # degree self._engineForce = 0.0 self._brakeForce = 0.0 self._p = self._params.get('p', 1.25) self._d = self._params.get('d', 0.0) self._last_err = 0.0 self._curr_time = 0.0 self._accelClamp = self._params.get('accelClamp', 2.0) self._engineClamp = self._accelClamp * self._mass self._collision = False if self._run_as_task: self._mark_d = 0.0 taskMgr.add(self._update_task, 'updateWorld') base.run() # _____HANDLER_____ def _doExit(self): sys.exit(1) def _toggleWireframe(self): base.toggleWireframe() def _toggleTexture(self): base.toggleTexture() def _doScreenshot(self): base.screenshot('Bullet') def _forward_0(self): self._des_vel = 1 self._brakeForce = 0.0 def _forward_1(self): self._des_vel = 2 self._brakeForce = 0.0 def _forward_2(self): self._des_vel = 4 self._brakeForce = 0.0 def _stop(self): self._des_vel = 0.0 self._brakeForce = 0.0 def _backward(self): self._des_vel = -4 self._brakeForce = 0.0 def _right(self): self._steering = np.min([np.max([-30, self._steering - 5]), 0.0]) def _left(self): self._steering = np.max([np.min([30, self._steering + 5]), 0.0]) def _view_image(self): from matplotlib import pyplot as plt image = self._camera_sensor.observe()[0] if self._use_depth: plt.imshow(image[:, :, 0], cmap='gray') else: import cv2 def rgb2gray(rgb): return np.dot(rgb[..., :3], [0.299, 0.587, 0.114]) image = rgb2gray(image) im = cv2.resize(image, (64, 36), interpolation=cv2.INTER_AREA ) # TODO how does this deal with aspect ratio plt.imshow(im.astype(np.uint8), cmap='Greys_r') plt.show() def _mark(self): self._mark_d = 0.0 # Setup def _setup(self): if hasattr(self, '_model_path'): # Collidable objects visNP = loader.loadModel(self._model_path) visNP.clearModelNodes() visNP.reparentTo(render) pos = (0., 0., 0.) visNP.setPos(pos[0], pos[1], pos[2]) bodyNPs = BulletHelper.fromCollisionSolids(visNP, True) for bodyNP in bodyNPs: bodyNP.reparentTo(render) bodyNP.setPos(pos[0], pos[1], pos[2]) if isinstance(bodyNP.node(), BulletRigidBodyNode): bodyNP.node().setMass(0.0) bodyNP.node().setKinematic(True) bodyNP.setCollideMask(BitMask32.allOn()) self._world.attachRigidBody(bodyNP.node()) else: ground = self._worldNP.attachNewNode(BulletRigidBodyNode('Ground')) shape = BulletPlaneShape(Vec3(0, 0, 1), 0) ground.node().addShape(shape) ground.setCollideMask(BitMask32.allOn()) self._world.attachRigidBody(ground.node()) self._place_vehicle() self._setup_light() self._setup_restart_pos() def _setup_restart_pos(self): self._restart_pos = [] self._restart_index = 0 if self._params.get('position_ranges', None) is not None: ranges = self._params['position_ranges'] num_pos = self._params['num_pos'] if self._params.get('range_type', 'random') == 'random': for _ in range(num_pos): ran = ranges[np.random.randint(len(ranges))] self._restart_pos.append(np.random.uniform(ran[0], ran[1])) elif self._params['range_type'] == 'fix_spacing': num_ran = len(ranges) num_per_ran = num_pos // num_ran for i in range(num_ran): ran = ranges[i] low = np.array(ran[0]) diff = np.array(ran[1]) - np.array(ran[0]) for j in range(num_per_ran): val = diff * ((j + 0.0) / num_per_ran) + low self._restart_pos.append(val) elif self._params.get('positions', None) is not None: self._restart_pos = self._params['positions'] else: self._restart_pos = self._default_restart_pos() def _next_restart_pos_hpr(self): num = len(self._restart_pos) if num == 0: return None, None else: pos_hpr = self._restart_pos[self._restart_index] self._restart_index = (self._restart_index + 1) % num return pos_hpr[:3], pos_hpr[3:] def _next_random_restart_pos_hpr(self): num = len(self._restart_pos) if num == 0: return None, None else: index = np.random.randint(num) pos_hpr = self._restart_pos[index] self._restart_index = (self._restart_index + 1) % num return pos_hpr[:3], pos_hpr[3:] def _setup_light(self): alight = AmbientLight('ambientLight') alight.setColor(Vec4(0.5, 0.5, 0.5, 1)) alightNP = render.attachNewNode(alight) render.clearLight() render.setLight(alightNP) # Vehicle def _default_pos(self): return (0.0, 0.0, 0.3) def _default_hpr(self): return (0.0, 0.0, 3.14) def _default_restart_pos(): return [self._default_pos() + self._default_hpr()] def _get_speed(self): vel = self._vehicle.getCurrentSpeedKmHour() / 3.6 return vel def _update(self, dt=1.0, coll_check=True): self._vehicle.setSteeringValue(self._steering, 0) self._vehicle.setSteeringValue(self._steering, 1) self._vehicle.setBrake(self._brakeForce, 0) self._vehicle.setBrake(self._brakeForce, 1) self._vehicle.setBrake(self._brakeForce, 2) self._vehicle.setBrake(self._brakeForce, 3) if dt >= self._step: # TODO maybe change number of timesteps for i in range(int(dt / self._step)): if self._des_vel is not None: vel = self._get_speed() err = self._des_vel - vel d_err = (err - self._last_err) / self._step self._last_err = err self._engineForce = np.clip( self._p * err + self._d * d_err, -self._accelClamp, self._accelClamp) * self._mass self._vehicle.applyEngineForce(self._engineForce, 0) self._vehicle.applyEngineForce(self._engineForce, 1) self._vehicle.applyEngineForce(self._engineForce, 2) self._vehicle.applyEngineForce(self._engineForce, 3) self._world.doPhysics(self._step, 1, self._step) self._collision = self._is_contact() elif self._run_as_task: self._curr_time += dt if self._curr_time > 0.05: if self._des_vel is not None: vel = self._get_speed() self._mark_d += vel * self._curr_time print(vel, self._mark_d, self._is_contact()) err = self._des_vel - vel d_err = (err - self._last_err) / 0.05 self._last_err = err self._engineForce = np.clip( self._p * err + self._d * d_err, -self._accelClamp, self._accelClamp) * self._mass self._curr_time = 0.0 self._vehicle.applyEngineForce(self._engineForce, 0) self._vehicle.applyEngineForce(self._engineForce, 1) self._vehicle.applyEngineForce(self._engineForce, 2) self._vehicle.applyEngineForce(self._engineForce, 3) self._world.doPhysics(dt, 1, dt) self._collision = self._is_contact() else: raise ValueError( "dt {0} s is too small for velocity control".format(dt)) def _stop_car(self): self._steering = 0.0 self._engineForce = 0.0 self._vehicle.setSteeringValue(0.0, 0) self._vehicle.setSteeringValue(0.0, 1) self._vehicle.applyEngineForce(0.0, 0) self._vehicle.applyEngineForce(0.0, 1) self._vehicle.applyEngineForce(0.0, 2) self._vehicle.applyEngineForce(0.0, 3) if self._des_vel is not None: self._des_vel = 0 self._vehicle_node.setLinearVelocity(Vec3(0.0, 0.0, 0.0)) self._vehicle_node.setAngularVelocity(Vec3(0.0, 0.0, 0.0)) for i in range(self._vehicle.getNumWheels()): wheel = self._vehicle.getWheel(i) wheel.setRotation(0.0) self._vehicle_node.clearForces() def _place_vehicle(self, pos=None, hpr=None): if pos is None: pos = self._default_pos() if hpr is None: hpr = self._default_hpr() self._vehicle_pointer.setPos(pos[0], pos[1], pos[2]) self._vehicle_pointer.setHpr(hpr[0], hpr[1], hpr[2]) self._stop_car() def _addWheel(self, pos, front, radius=0.25): wheel = self._vehicle.createWheel() wheel.setChassisConnectionPointCs(pos) wheel.setFrontWheel(front) wheel.setWheelDirectionCs(Vec3(0, 0, -1)) wheel.setWheelAxleCs(Vec3(1, 0, 0)) wheel.setWheelRadius(radius) wheel.setMaxSuspensionTravelCm(40.0) wheel.setSuspensionStiffness(40.0) wheel.setWheelsDampingRelaxation(2.3) wheel.setWheelsDampingCompression(4.4) wheel.setFrictionSlip(1e2) wheel.setRollInfluence(0.1) # Task def _update_task(self, task): dt = globalClock.getDt() self._update(dt=dt) self._get_observation() return task.cont # Helper functions def _get_observation(self): self._obs = self._camera_sensor.observe() observation = [] observation.append(self._obs[0]) if self._use_back_cam: self._back_obs = self._back_camera_sensor.observe() observation.append(self._back_obs[0]) observation = np.concatenate(observation, axis=2) return observation def _get_reward(self): reward = self._collision_reward if self._collision else self._get_speed( ) return reward def _get_done(self): return self._collision def _get_info(self): info = {} info['pos'] = np.array(self._vehicle_pointer.getPos()) info['hpr'] = np.array(self._vehicle_pointer.getHpr()) info['vel'] = self._get_speed() info['coll'] = self._collision return info def _back_up(self): assert (self._use_vel) back_up_vel = self._params['back_up'].get('vel', -2.0) self._des_vel = back_up_vel back_up_steer = self._params['back_up'].get('steer', (-5.0, 5.0)) # TODO self._steering = np.random.uniform(*back_up_steer) self._brakeForce = 0. duration = self._params['back_up'].get('duration', 1.0) self._update(dt=duration) self._des_vel = 0.0 self._steering = 0.0 self._update(dt=duration) self._brakeForce = 0. def _is_contact(self): result = self._world.contactTest(self._vehicle_node) num_contacts = result.getNumContacts() return result.getNumContacts() > 0 # Environment functions def reset(self, pos=None, hpr=None, hard_reset=False, random_reset=False): if self._do_back_up and not hard_reset and \ pos is None and hpr is None: if self._collision: self._back_up() else: if pos is None and hpr is None: if random_reset: pos, hpr = self._next_random_restart_pos_hpr() else: pos, hpr = self._next_restart_pos_hpr() self._place_vehicle(pos=pos, hpr=hpr) self._collision = False return self._get_observation() def step(self, action): self._steering = action[0] if action[1] == 0.0: self._brakeForce = 1000. else: self._brakeForce = 0. if self._use_vel: # Convert from m/s to km/h self._des_vel = action[1] else: self._engineForce = self._engineClamp * \ ((action[1] - 49.5) / 49.5) self._update(dt=self._dt) observation = self._get_observation() reward = self._get_reward() done = self._get_done() info = self._get_info() return observation, reward, done, info
class CarPhys(Phys): def __init__(self, mdt, carphys_props): Phys.__init__(self, mdt) self.pnode = None self.vehicle = None self.curr_speed_factor = 1.0 self.__prev_speed = 0 self.__finds = {} # cache for find's results self.props = carphys_props self._load_phys() self.__set_collision_mesh() self.__set_phys_node() self.__set_vehicle() self.__set_wheels() eng.attach_obs(self.on_end_frame) def _load_phys(self): fpath = self.props.phys_file % self.mdt.name with open(fpath) as phys_file: # pass phys props as a class self.cfg = load(phys_file) self.cfg['max_speed'] = self.get_speed() self.cfg['friction_slip'] = self.get_friction() self.cfg['roll_influence'] = self.get_roll_influence() s_a = (self.mdt.name, round(self.cfg['max_speed'], 2), self.props.driver_engine) LogMgr().log('speed %s: %s (%s)' % s_a) fr_slip = round(self.cfg['friction_slip'], 2) f_a = (self.mdt.name, fr_slip, self.props.driver_tires) LogMgr().log('friction %s: %s (%s)' % f_a) r_a = (self.mdt.name, round(self.cfg['roll_influence'], 2), self.props.driver_suspensions) LogMgr().log('roll %s: %s (%s)' % r_a) s_a = lambda field: setattr(self, field, self.cfg[field]) map(s_a, self.cfg.keys()) def __set_collision_mesh(self): fpath = self.props.coll_path % self.mdt.name self.coll_mesh = loader.loadModel(fpath) chassis_shape = BulletConvexHullShape() for geom in PhysMgr().find_geoms(self.coll_mesh, self.props.coll_name): chassis_shape.addGeom(geom.node().getGeom(0), geom.getTransform()) self.mdt.gfx.nodepath.node().addShape(chassis_shape) self.mdt.gfx.nodepath.setCollideMask( BitMask32.bit(1) | BitMask32.bit(2 + self.props.cars.index(self.mdt.name))) #nodepath = self.mdt.gfx.nodepath.attachNewNode(BulletGhostNode('car ghost')) #nodepath.node().addShape(BulletCapsuleShape(4, 5, ZUp)) #eng.attach_ghost(nodepath.node()) #nodepath.node().notifyCollisions(False) def __set_phys_node(self): self.pnode = self.mdt.gfx.nodepath.node() self.pnode.setMass(self.mass) self.pnode.setDeactivationEnabled(False) PhysMgr().attach_rigid_body(self.pnode) PhysMgr().add_collision_obj(self.pnode) def __set_vehicle(self): self.vehicle = BulletVehicle(PhysMgr().root, self.pnode) self.vehicle.setCoordinateSystem(ZUp) self.vehicle.setPitchControl(self.pitch_control) tuning = self.vehicle.getTuning() tuning.setSuspensionCompression(self.suspension_compression) tuning.setSuspensionDamping(self.suspension_damping) PhysMgr().attach_vehicle(self.vehicle) def __set_wheels(self): fwheel_bounds = self.mdt.gfx.wheels['fr'].get_tight_bounds() f_radius = (fwheel_bounds[1][2] - fwheel_bounds[0][2]) / 2.0 + .01 rwheel_bounds = self.mdt.gfx.wheels['rr'].get_tight_bounds() r_radius = (rwheel_bounds[1][2] - rwheel_bounds[0][2]) / 2.0 + .01 ffr = self.coll_mesh.find('**/' + self.props.wheel_names[0][0]) ffl = self.coll_mesh.find('**/' + self.props.wheel_names[0][1]) rrr = self.coll_mesh.find('**/' + self.props.wheel_names[0][2]) rrl = self.coll_mesh.find('**/' + self.props.wheel_names[0][3]) meth = self.coll_mesh.find fr_node = ffr if ffr else meth('**/' + self.props.wheel_names[1][0]) fl_node = ffl if ffl else meth('**/' + self.props.wheel_names[1][1]) rr_node = rrr if rrr else meth('**/' + self.props.wheel_names[1][2]) rl_node = rrl if rrl else meth('**/' + self.props.wheel_names[1][3]) wheel_fr_pos = fr_node.get_pos() + (0, 0, f_radius) wheel_fl_pos = fl_node.get_pos() + (0, 0, f_radius) wheel_rr_pos = rr_node.get_pos() + (0, 0, r_radius) wheel_rl_pos = rl_node.get_pos() + (0, 0, r_radius) frw = self.mdt.gfx.wheels['fr'] flw = self.mdt.gfx.wheels['fl'] rrw = self.mdt.gfx.wheels['rr'] rlw = self.mdt.gfx.wheels['rl'] wheels_info = [(wheel_fr_pos, True, frw, f_radius), (wheel_fl_pos, True, flw, f_radius), (wheel_rr_pos, False, rrw, r_radius), (wheel_rl_pos, False, rlw, r_radius)] for (pos, front, nodepath, radius) in wheels_info: self.__add_wheel(pos, front, nodepath.node(), radius) def __add_wheel(self, pos, front, node, radius): whl = self.vehicle.createWheel() whl.setNode(node) whl.setChassisConnectionPointCs(LPoint3f(*pos)) whl.setFrontWheel(front) whl.setWheelDirectionCs((0, 0, -1)) whl.setWheelAxleCs((1, 0, 0)) whl.setWheelRadius(radius) whl.setSuspensionStiffness(self.suspension_stiffness) whl.setWheelsDampingRelaxation(self.wheels_damping_relaxation) whl.setWheelsDampingCompression(self.wheels_damping_compression) whl.setFrictionSlip(self.friction_slip) # high -> more adherence whl.setRollInfluence(self.roll_influence) # low -> more stability whl.setMaxSuspensionForce(self.max_suspension_force) whl.setMaxSuspensionTravelCm(self.max_suspension_travel_cm) whl.setSkidInfo(self.skid_info) @property def lateral_force(self): vel = self.vehicle.get_chassis().get_linear_velocity() vel.normalize() dir = self.mdt.logic.car_vec lat = dir.dot(vel) lat_force = 0 if lat > .5: lat_force = min(1, (lat - 1.0) / -.2) return lat_force @property def is_flying(self): # no need to be cached rays = [whl.getRaycastInfo() for whl in self.vehicle.get_wheels()] return not any(ray.isInContact() for ray in rays) @property def prev_speed(self): return self.__prev_speed @property def prev_speed_ratio(self): return max(0, min(1.0, self.prev_speed / self.max_speed)) def on_end_frame(self): self.__prev_speed = self.speed @property def speed(self): if self.mdt.fsm.getCurrentOrNextState() == 'Countdown': return 0 # getCurrentSpeedKmHour returns odd values otherwise return self.vehicle.getCurrentSpeedKmHour() @property def speed_ratio(self): return max(0, min(1.0, self.speed / self.max_speed)) def set_forces(self, eng_frc, brake_frc, steering): self.vehicle.setSteeringValue(steering, 0) self.vehicle.setSteeringValue(steering, 1) self.vehicle.applyEngineForce(eng_frc, 2) self.vehicle.applyEngineForce(eng_frc, 3) self.vehicle.setBrake(brake_frc, 2) self.vehicle.setBrake(brake_frc, 3) def update_car_props(self): speeds = [] for whl in self.vehicle.get_wheels(): contact_pt = whl.get_raycast_info().getContactPointWs() gnd_name = self.gnd_name(contact_pt) if not gnd_name: continue if gnd_name not in self.__finds: gnd = self.props.track_phys.find('**/' + gnd_name) self.__finds[gnd_name] = gnd gfx_node = self.__finds[gnd_name] if gfx_node.has_tag('speed'): speeds += [float(gfx_node.get_tag('speed'))] if gfx_node.has_tag('friction'): fric = float(gfx_node.get_tag('friction')) whl.setFrictionSlip(self.friction_slip * fric) self.curr_speed_factor = (sum(speeds) / len(speeds)) if speeds else 1.0 @property def gnd_names(self): # no need to be cached whls = self.vehicle.get_wheels() pos = map(lambda whl: whl.get_raycast_info().getContactPointWs(), whls) return map(self.gnd_name, pos) @staticmethod def gnd_name(pos): top = pos + (0, 0, 20) bottom = pos + (0, 0, -20) result = PhysMgr().ray_test_closest(bottom, top) ground = result.get_node() return ground.get_name() if ground else '' def apply_damage(self, reset=False): if reset: self.max_speed = self.get_speed() self.friction_slip = self.get_friction() self.roll_influence = self.get_roll_influence() else: self.max_speed *= .95 self.friction_slip *= .95 self.roll_influence *= 1.05 fric = lambda whl: whl.setFrictionSlip(self.friction_slip) map(fric, self.vehicle.get_wheels()) roll = lambda whl: whl.setRollInfluence(self.roll_influence) map(roll, self.vehicle.get_wheels()) s_a = (str(round(self.max_speed, 2)), self.props.driver_engine) LogMgr().log('speed: %s (%s)' % s_a) f_a = (str(round(self.friction_slip, 2)), self.props.driver_tires) LogMgr().log('friction: %s (%s)' % f_a) r_a = (str(round(self.roll_influence, 2)), self.props.driver_suspensions) LogMgr().log('roll: %s (%s)' % r_a) def get_speed(self): return self.cfg['max_speed'] * (1 + .01 * self.props.driver_engine) def get_friction(self): return self.cfg['friction_slip'] * (1 + .01 * self.props.driver_tires) def get_roll_influence(self): return self.cfg['roll_influence'] * ( 1 + .01 * self.props.driver_suspensions) def destroy(self): eng.detach_obs(self.on_end_frame) eng.remove_vehicle(self.vehicle) self.pnode = self.vehicle = self.__finds = self.__track_phys = \ self.coll_mesh = None Phys.destroy(self)
class CarEnv(DirectObject): def __init__(self, params={}): self._params = params if 'random_seed' in self._params: np.random.seed(self._params['random_seed']) self._use_vel = self._params.get('use_vel', True) self._run_as_task = self._params.get('run_as_task', False) self._do_back_up = self._params.get('do_back_up', False) self._use_depth = self._params.get('use_depth', False) self._use_back_cam = self._params.get('use_back_cam', False) self._collision_reward_only = self._params.get('collision_reward_only', False) self._collision_reward = self._params.get('collision_reward', -10.0) self._obs_shape = self._params.get('obs_shape', (64, 36)) self._steer_limits = params.get('steer_limits', (-30., 30.)) self._speed_limits = params.get('speed_limits', (-4.0, 4.0)) self._motor_limits = params.get('motor_limits', (-0.5, 0.5)) self._fixed_speed = (self._speed_limits[0] == self._speed_limits[1] and self._use_vel) if not self._params.get('visualize', False): loadPrcFileData('', 'window-type offscreen') # Defines base, render, loader try: ShowBase() except: pass base.setBackgroundColor(0.0, 0.0, 0.0, 1) # World self._worldNP = render.attachNewNode('World') self._world = BulletWorld() self._world.setGravity(Vec3(0, 0, -9.81)) self._dt = params.get('dt', 0.25) self._step = 0.05 # Vehicle shape = BulletBoxShape(Vec3(0.6, 1.0, 0.25)) ts = TransformState.makePos(Point3(0., 0., 0.25)) self._vehicle_node = BulletRigidBodyNode('Vehicle') self._vehicle_node.addShape(shape, ts) self._mass = self._params.get('mass', 10.) self._vehicle_node.setMass(self._mass) self._vehicle_node.setDeactivationEnabled(False) self._vehicle_node.setCcdSweptSphereRadius(1.0) self._vehicle_node.setCcdMotionThreshold(1e-7) self._vehicle_pointer = self._worldNP.attachNewNode(self._vehicle_node) self._world.attachRigidBody(self._vehicle_node) self._vehicle = BulletVehicle(self._world, self._vehicle_node) self._vehicle.setCoordinateSystem(ZUp) self._world.attachVehicle(self._vehicle) self._addWheel(Point3(0.3, 0.5, 0.07), True, 0.07) self._addWheel(Point3(-0.3, 0.5, 0.07), True, 0.07) self._addWheel(Point3(0.3, -0.5, 0.07), False, 0.07) self._addWheel(Point3(-0.3, -0.5, 0.07), False, 0.07) # Camera size = self._params.get('size', [160, 90]) hfov = self._params.get('hfov', 120) near_far = self._params.get('near_far', [0.1, 100.]) self._camera_sensor = Panda3dCameraSensor(base, color=not self._use_depth, depth=self._use_depth, size=size, hfov=hfov, near_far=near_far, title='front cam') self._camera_node = self._camera_sensor.cam self._camera_node.setPos(0.0, 0.5, 0.375) self._camera_node.lookAt(0.0, 6.0, 0.0) self._camera_node.reparentTo(self._vehicle_pointer) if self._use_back_cam: self._back_camera_sensor = Panda3dCameraSensor( base, color=not self._use_depth, depth=self._use_depth, size=size, hfov=hfov, near_far=near_far, title='back cam') self._back_camera_node = self._back_camera_sensor.cam self._back_camera_node.setPos(0.0, -0.5, 0.375) self._back_camera_node.lookAt(0.0, -6.0, 0.0) self._back_camera_node.reparentTo(self._vehicle_pointer) # Car Simulator self._des_vel = None self._setup() # Input self.accept('escape', self._doExit) self.accept('r', self.reset) self.accept('f1', self._toggleWireframe) self.accept('f2', self._toggleTexture) self.accept('f3', self._view_image) self.accept('f5', self._doScreenshot) self.accept('q', self._forward_0) self.accept('w', self._forward_1) self.accept('e', self._forward_2) self.accept('a', self._left) self.accept('s', self._stop) self.accept('x', self._backward) self.accept('d', self._right) self.accept('m', self._mark) self._steering = 0.0 # degree self._engineForce = 0.0 self._brakeForce = 0.0 self._env_time_step = 0 self._p = self._params.get('p', 1.25) self._d = self._params.get('d', 0.0) self._last_err = 0.0 self._curr_time = 0.0 self._accelClamp = self._params.get('accelClamp', 0.5) self._engineClamp = self._accelClamp * self._mass self._collision = False self._setup_spec() self.spec = EnvSpec(observation_im_space=self.observation_im_space, action_space=self.action_space, action_selection_space=self.action_selection_space, observation_vec_spec=self.observation_vec_spec, action_spec=self.action_spec, action_selection_spec=self.action_selection_spec, goal_spec=self.goal_spec) if self._run_as_task: self._mark_d = 0.0 taskMgr.add(self._update_task, 'updateWorld') base.run() def _setup_spec(self): self.action_spec = OrderedDict() self.action_selection_spec = OrderedDict() self.observation_vec_spec = OrderedDict() self.goal_spec = OrderedDict() self.action_spec['steer'] = Box(low=-45., high=45.) self.action_selection_spec['steer'] = Box(low=self._steer_limits[0], high=self._steer_limits[1]) if self._use_vel: self.action_spec['speed'] = Box(low=-4., high=4.) self.action_space = Box(low=np.array([ self.action_spec['steer'].low[0], self.action_spec['speed'].low[0] ]), high=np.array([ self.action_spec['steer'].high[0], self.action_spec['speed'].high[0] ])) self.action_selection_spec['speed'] = Box( low=self._speed_limits[0], high=self._speed_limits[1]) self.action_selection_space = Box( low=np.array([ self.action_selection_spec['steer'].low[0], self.action_selection_spec['speed'].low[0] ]), high=np.array([ self.action_selection_spec['steer'].high[0], self.action_selection_spec['speed'].high[0] ])) else: self.action_spec['motor'] = Box(low=-self._accelClamp, high=self._accelClamp) self.action_space = Box(low=np.array([ self.action_spec['steer'].low[0], self.action_spec['motor'].low[0] ]), high=np.array([ self.action_spec['steer'].high[0], self.action_spec['motor'].high[0] ])) self.action_selection_spec['motor'] = Box( low=self._motor_limits[0], high=self._motor_limits[1]) self.action_selection_space = Box( low=np.array([ self.action_selection_spec['steer'].low[0], self.action_selection_spec['motor'].low[0] ]), high=np.array([ self.action_selection_spec['steer'].high[0], self.action_selection_spec['motor'].high[0] ])) assert (np.logical_and( self.action_selection_space.low >= self.action_space.low - 1e-4, self.action_selection_space.high <= self.action_space.high + 1e-4).all()) self.observation_im_space = Box(low=0, high=255, shape=tuple( self._get_observation()[0].shape)) self.observation_vec_spec['coll'] = Discrete(1) self.observation_vec_spec['heading'] = Box(low=0, high=2 * 3.14) self.observation_vec_spec['speed'] = Box(low=-4.0, high=4.0) @property def _base_dir(self): return os.path.join(os.path.dirname(os.path.abspath(__file__)), 'models') @property def horizon(self): return np.inf @property def max_reward(self): return np.inf # _____HANDLER_____ def _doExit(self): sys.exit(1) def _toggleWireframe(self): base.toggleWireframe() def _toggleTexture(self): base.toggleTexture() def _doScreenshot(self): base.screenshot('Bullet') def _forward_0(self): self._des_vel = 1 self._brakeForce = 0.0 def _forward_1(self): self._des_vel = 2 self._brakeForce = 0.0 def _forward_2(self): self._des_vel = 4 self._brakeForce = 0.0 def _stop(self): self._des_vel = 0.0 self._brakeForce = 0.0 def _backward(self): self._des_vel = -4 self._brakeForce = 0.0 def _right(self): self._steering = np.min([np.max([-30, self._steering - 5]), 0.0]) def _left(self): self._steering = np.max([np.min([30, self._steering + 5]), 0.0]) def _view_image(self): from matplotlib import pyplot as plt image = self._camera_sensor.observe()[0] if self._use_depth: plt.imshow(image[:, :, 0], cmap='gray') else: def rgb2gray(rgb): return np.dot(rgb[..., :3], [0.299, 0.587, 0.114]) image = rgb2gray(image) im = cv2.resize(image, (64, 36), interpolation=cv2.INTER_AREA ) # TODO how does this deal with aspect ratio plt.imshow(im.astype(np.uint8), cmap='Greys_r') plt.show() def _mark(self): self._mark_d = 0.0 # Setup def _setup(self): self._setup_map() self._place_vehicle() self._setup_light() self._setup_restart_pos() def _setup_map(self): if hasattr(self, '_model_path'): # Collidable objects self._setup_collision_object(self._model_path) else: ground = self._worldNP.attachNewNode(BulletRigidBodyNode('Ground')) shape = BulletPlaneShape(Vec3(0, 0, 1), 0) ground.node().addShape(shape) ground.setCollideMask(BitMask32.allOn()) self._world.attachRigidBody(ground.node()) def _setup_collision_object(self, path, pos=(0.0, 0.0, 0.0), hpr=(0.0, 0.0, 0.0), scale=1): visNP = loader.loadModel(path) visNP.clearModelNodes() visNP.reparentTo(render) visNP.setPos(pos[0], pos[1], pos[2]) visNP.setHpr(hpr[0], hpr[1], hpr[2]) visNP.set_scale(scale, scale, scale) bodyNPs = BulletHelper.fromCollisionSolids(visNP, True) for bodyNP in bodyNPs: bodyNP.reparentTo(render) bodyNP.setPos(pos[0], pos[1], pos[2]) bodyNP.setHpr(hpr[0], hpr[1], hpr[2]) bodyNP.set_scale(scale, scale, scale) if isinstance(bodyNP.node(), BulletRigidBodyNode): bodyNP.node().setMass(0.0) bodyNP.node().setKinematic(True) bodyNP.setCollideMask(BitMask32.allOn()) self._world.attachRigidBody(bodyNP.node()) else: print("Issue") def _setup_restart_pos(self): self._restart_index = 0 self._restart_pos = self._default_restart_pos() def _next_restart_pos_hpr(self): num = len(self._restart_pos) if num == 0: return None, None else: pos_hpr = self._restart_pos[self._restart_index] self._restart_index = (self._restart_index + 1) % num return pos_hpr[:3], pos_hpr[3:] def _setup_light(self): # alight = AmbientLight('ambientLight') # alight.setColor(Vec4(0.5, 0.5, 0.5, 1)) # alightNP = render.attachNewNode(alight) # render.clearLight() # render.setLight(alightNP) pass # Vehicle def _default_pos(self): return (0.0, 0.0, 0.3) def _default_hpr(self): return (0.0, 0.0, 0.0) def _default_restart_pos(self): return [self._default_pos() + self._default_hpr()] def _get_speed(self): vel = self._vehicle.getCurrentSpeedKmHour() / 3.6 return vel def _get_heading(self): h = np.array(self._vehicle_pointer.getHpr())[0] ori = h * (pi / 180.) while (ori > 2 * pi): ori -= 2 * pi while (ori < 0): ori += 2 * pi return ori def _update(self, dt=1.0, coll_check=True): self._vehicle.setSteeringValue(self._steering, 0) self._vehicle.setSteeringValue(self._steering, 1) self._vehicle.setBrake(self._brakeForce, 0) self._vehicle.setBrake(self._brakeForce, 1) self._vehicle.setBrake(self._brakeForce, 2) self._vehicle.setBrake(self._brakeForce, 3) if dt >= self._step: # TODO maybe change number of timesteps # for i in range(int(dt/self._step)): if self._des_vel is not None: vel = self._get_speed() err = self._des_vel - vel d_err = (err - self._last_err) / self._step self._last_err = err self._engineForce = np.clip(self._p * err + self._d * d_err, -self._accelClamp, self._accelClamp) * self._mass self._vehicle.applyEngineForce(self._engineForce, 0) self._vehicle.applyEngineForce(self._engineForce, 1) self._vehicle.applyEngineForce(self._engineForce, 2) self._vehicle.applyEngineForce(self._engineForce, 3) for _ in range(int(dt / self._step)): self._world.doPhysics(self._step, 1, self._step) self._collision = self._is_contact() elif self._run_as_task: self._curr_time += dt if self._curr_time > 0.05: if self._des_vel is not None: vel = self._get_speed() self._mark_d += vel * self._curr_time print(vel, self._mark_d, self._is_contact()) err = self._des_vel - vel d_err = (err - self._last_err) / 0.05 self._last_err = err self._engineForce = np.clip( self._p * err + self._d * d_err, -self._accelClamp, self._accelClamp) * self._mass self._curr_time = 0.0 self._vehicle.applyEngineForce(self._engineForce, 0) self._vehicle.applyEngineForce(self._engineForce, 1) self._vehicle.applyEngineForce(self._engineForce, 2) self._vehicle.applyEngineForce(self._engineForce, 3) self._world.doPhysics(dt, 1, dt) self._collision = self._is_contact() else: raise ValueError( "dt {0} s is too small for velocity control".format(dt)) def _stop_car(self): self._steering = 0.0 self._engineForce = 0.0 self._vehicle.setSteeringValue(0.0, 0) self._vehicle.setSteeringValue(0.0, 1) self._vehicle.applyEngineForce(0.0, 0) self._vehicle.applyEngineForce(0.0, 1) self._vehicle.applyEngineForce(0.0, 2) self._vehicle.applyEngineForce(0.0, 3) if self._des_vel is not None: self._des_vel = 0 self._vehicle_node.setLinearVelocity(Vec3(0.0, 0.0, 0.0)) self._vehicle_node.setAngularVelocity(Vec3(0.0, 0.0, 0.0)) for i in range(self._vehicle.getNumWheels()): wheel = self._vehicle.getWheel(i) wheel.setRotation(0.0) self._vehicle_node.clearForces() def _place_vehicle(self, pos=None, hpr=None): if pos is None: pos = self._default_pos() if hpr is None: hpr = self._default_hpr() self._vehicle_pointer.setPos(pos[0], pos[1], pos[2]) self._vehicle_pointer.setHpr(hpr[0], hpr[1], hpr[2]) self._stop_car() def _addWheel(self, pos, front, radius=0.25): wheel = self._vehicle.createWheel() wheel.setChassisConnectionPointCs(pos) wheel.setFrontWheel(front) wheel.setWheelDirectionCs(Vec3(0, 0, -1)) wheel.setWheelAxleCs(Vec3(1, 0, 0)) wheel.setWheelRadius(radius) wheel.setMaxSuspensionTravelCm(40.0) wheel.setSuspensionStiffness(40.0) wheel.setWheelsDampingRelaxation(2.3) wheel.setWheelsDampingCompression(4.4) wheel.setFrictionSlip(1e2) wheel.setRollInfluence(0.1) # Task def _update_task(self, task): dt = globalClock.getDt() self._update(dt=dt) self._get_observation() return task.cont # Helper functions def _get_observation(self): self._obs = self._camera_sensor.observe() observation = [] observation.append(self.process(self._obs[0], self._obs_shape)) if self._use_back_cam: self._back_obs = self._back_camera_sensor.observe() observation.append(self.process(self._back_obs[0], self._obs_shape)) observation_im = np.concatenate(observation, axis=2) coll = self._collision heading = self._get_heading() speed = self._get_speed() observation_vec = np.array([coll, heading, speed]) return observation_im, observation_vec def _get_goal(self): return np.array([]) def process(self, image, obs_shape): if self._use_depth: im = np.reshape(image, (image.shape[0], image.shape[1])) if im.shape != obs_shape: im = cv2.resize(im, obs_shape, interpolation=cv2.INTER_AREA) return im.astype(np.uint8) else: image = np.dot(image[..., :3], [0.299, 0.587, 0.114]) im = cv2.resize(image, obs_shape, interpolation=cv2.INTER_AREA ) #TODO how does this deal with aspect ratio # TODO might not be necessary im = np.expand_dims(im, 2) return im.astype(np.uint8) def _get_reward(self): if self._collision_reward_only: if self._collision: reward = self._collision_reward else: reward = 0.0 else: if self._collision: reward = self._collision_reward else: reward = self._get_speed() assert (reward <= self.max_reward) return reward def _get_done(self): return self._collision def _get_info(self): info = {} info['pos'] = np.array(self._vehicle_pointer.getPos()) info['hpr'] = np.array(self._vehicle_pointer.getHpr()) info['vel'] = self._get_speed() info['coll'] = self._collision info['env_time_step'] = self._env_time_step return info def _back_up(self): assert (self._use_vel) # logger.debug('Backing up!') self._params['back_up'] = self._params.get('back_up', {}) back_up_vel = self._params['back_up'].get('vel', -1.0) self._des_vel = back_up_vel back_up_steer = self._params['back_up'].get('steer', (-5.0, 5.0)) self._steering = np.random.uniform(*back_up_steer) self._brakeForce = 0. duration = self._params['back_up'].get('duration', 3.0) self._update(dt=duration) self._des_vel = 0. self._steering = 0. self._update(dt=duration) self._brakeForce = 0. def _is_contact(self): result = self._world.contactTest(self._vehicle_node) return result.getNumContacts() > 0 # Environment functions def reset(self, pos=None, hpr=None, hard_reset=False): if self._do_back_up and not hard_reset and \ pos is None and hpr is None: if self._collision: self._back_up() else: if hard_reset: logger.debug('Hard resetting!') if pos is None and hpr is None: pos, hpr = self._next_restart_pos_hpr() self._place_vehicle(pos=pos, hpr=hpr) self._collision = False self._env_time_step = 0 return self._get_observation(), self._get_goal() def step(self, action): self._steering = action[0] if action[1] == 0.0: self._brakeForce = 1000. else: self._brakeForce = 0. if self._use_vel: # Convert from m/s to km/h self._des_vel = action[1] else: self._engineForce = self._mass * action[1] self._update(dt=self._dt) observation = self._get_observation() goal = self._get_goal() reward = self._get_reward() done = self._get_done() info = self._get_info() self._env_time_step += 1 return observation, goal, reward, done, info
class RoadRally(ShowBase): def __init__(self): ShowBase.__init__(self) scene = BulletWorld() scene.setGravity(Vec3(0, 0, -9.81)) base.setBackgroundColor(0.6, 0.9, 0.9) #Variables self.steering = 0 #Controls inputState.watchWithModifiers("F", "arrow_up") inputState.watchWithModifiers("B", "arrow_down") inputState.watchWithModifiers("L", "arrow_left") inputState.watchWithModifiers("R", "arrow_right") #The ground self.ground = BulletPlaneShape(Vec3( 0, 0, 1, ), 1) self.ground_node = BulletRigidBodyNode("The ground") self.ground_node.addShape(self.ground) self.ground_np = render.attachNewNode(self.ground_node) self.ground_np.setPos(0, 0, -2) scene.attachRigidBody(self.ground_node) self.track_model = loader.loadModel("Models/Track.egg") self.track_model.reparentTo(self.render) self.track_model.setPos(0, 0, -7) self.track_tex = loader.loadTexture("Textures/Road.jpg") self.track_model.setTexture(self.track_tex, 1) #The car Car_shape = BulletBoxShape(Vec3(1, 2.0, 1.0)) Car_node = BulletRigidBodyNode("The Car") Car_node.setMass(1200.0) Car_node.addShape(Car_shape) Car_np = render.attachNewNode(Car_node) Car_np.setPos(0, 0, 0) Car_np.setHpr(0, 0, 0) Car_np.node().setDeactivationEnabled(False) scene.attachRigidBody(Car_node) #Load and transform the Car Actor self.car_model = loader.loadModel("Models/Car.egg") self.car_model.setPos(0, 20, -3) self.car_model.setHpr(180, 0, 0) self.car_model.reparentTo(Car_np) self.Car_sim = BulletVehicle(scene, Car_np.node()) self.Car_sim.setCoordinateSystem(ZUp) scene.attachVehicle(self.Car_sim) #Camera #base.disableMouse() camera.reparentTo(Car_np) camera.setPos(0, 0, 0) camera.setHpr(0, 0, 0) def Wheel(pos, r, f): w = self.Car_sim.createWheel() w.setChassisConnectionPointCs(pos) w.setFrontWheel(f) w.setWheelDirectionCs(Vec3(0, 0, -1)) w.setWheelAxleCs(Vec3(1, 0, 0)) w.setWheelRadius(r) w.setMaxSuspensionTravelCm(40) w.setSuspensionStiffness(120) w.setWheelsDampingRelaxation(2.3) w.setWheelsDampingCompression(4.4) w.setFrictionSlip(50) w.setRollInfluence(0.1) #Wheels Wheel(Point3(-1, 1, -0.6), 0.4, False) Wheel(Point3(-1.1, -1.2, -0.6), 0.4, True) Wheel(Point3(1.1, -1, -0.6), 0.4, True) Wheel(Point3(1, 1, -0.6), 0.4, False) def ProcessInput(dt): engineForce = 0.0 self.steeringClamp = 35.0 self.steeringIncrement = 70 #Get the vehicle's current speed self.carspeed = self.Car_sim.getCurrentSpeedKmHour() #Reset the steering if not inputState.isSet("L") and not inputState.isSet("R"): if self.steering < 0.00: self.steering = self.steering + 0.6 if self.steering > 0.00: self.steering = self.steering - 0.6 if self.steering < 1.0 and self.steering > -1.0: self.steering = 0 if inputState.isSet("F"): engineForce = 35 if inputState.isSet("B"): engineForce = -35 #Left if inputState.isSet("L"): if self.steering < 0.0: #This makes the steering reset at the correct speed when turning from right to left self.steering += dt * self.steeringIncrement + 0.6 self.steering = min(self.steering, self.steeringClamp) else: #Normal steering self.steering += dt * self.steeringIncrement self.steering = min(self.steering, self.steeringClamp) #Right if inputState.isSet("R"): if self.steering > 0.0: #This makes the steering reset at the correct speed when turning from left to right self.steering -= dt * self.steeringIncrement + 0.6 self.steering = max(self.steering, -self.steeringClamp) else: #Normal steering self.steering -= dt * self.steeringIncrement self.steering = max(self.steering, -self.steeringClamp) #Apply forces to wheels self.Car_sim.applyEngineForce(engineForce, 0) self.Car_sim.applyEngineForce(engineForce, 3) def Update(task): dt = globalClock.getDt() ProcessInput(dt) scene.doPhysics(dt, 5, 1.0 / 180.0) return task.cont taskMgr.add(Update, "Update")
class Car(DirectObject): def __init__(self, base, world, track): super().__init__() self.world = world self.track = track self.steering = 0 self.accelerator = 0 self.brake = 0 self.gear_ratios = [-4, 0, 3.9, 2.9, 2.3, 1.87, 1.68, 1.54, 1.46] self.gear = 1 self.differential_ratio = 4.5 self.transmission_efficiency = 0.95 self.wheel_radius = 0.4 self.drag_coefficient = 0.48 self.engine_torque_curve = [ (0, 466), (564, 469), (1612, 469), (2822, 518), (3870, 517), (4516, 597), (5000, 613), (5564, 600), (6048, 655), (6693, 681), (7177, 716), (7822, 696), (8306, 696), (11048, 569), (13951, 391), (15000, 339), (15483, 301), (16612, 247), (17177, 65), (18306, 55) ] self.fw_wingspan = 0.64 self.fw_cord = 1.01 self.fw_clift = 0.7 self.rw_wingspan = 0.64 self.rw_cord = 0.51 self.rw_clift = 0.2 self.car_node = None self.car = None dial_scale = 0.2 speed_dial = OnscreenImage(image='tex/speed360.rgb', pos=(-dial_scale, 0, dial_scale), scale=(dial_scale, 1, dial_scale), parent=base.a2dBottomCenter) speed_dial.setTransparency(TransparencyAttrib.MAlpha) self.speed_dial = OnscreenImage(image='tex/dial.rgb', parent=speed_dial) rpm_dial = OnscreenImage(image='tex/rpm20000.rgb', pos=(dial_scale, 0, dial_scale), scale=(dial_scale, 1, dial_scale), parent=base.a2dBottomCenter) rpm_dial.setTransparency(TransparencyAttrib.MAlpha) self.rpm_dial = OnscreenImage(image='tex/dial.rgb', parent=rpm_dial) self.gear_text = OnscreenText(text='N', pos=(-0.02, -0.67), scale=0.4, parent=rpm_dial, fg=(255, 0, 0, 1)) self.reset() taskMgr.add(self.update, 'update') def make_wheel(self, pos, front, np): wheel = self.car.createWheel(0.02) wheel.setNode(np.node()) wheel.setChassisConnectionPointCs(pos) wheel.setFrontWheel(front) wheel.setWheelDirectionCs(Vec3(0, 0, -1)) wheel.setWheelAxleCs(Vec3(1, 0, 0)) wheel.setWheelRadius(self.wheel_radius) wheel.setSuspensionStiffness(200) wheel.setWheelsDampingRelaxation(23) wheel.setWheelsDampingCompression(44) wheel.setRollInfluence(0.1) wheel.setMaxSuspensionTravelCm(10) wheel.setFrictionSlip(1.2) # wheel.setMaxSuspensionForce(1000) def reset(self): car = loader.loadModel("models/car.egg") car.flattenLight() car_bounds = car.getTightBounds() car_shape = BulletBoxShape(Vec3((car_bounds[1].x - car_bounds[0].x) / 2, (car_bounds[1].y - car_bounds[0].y) / 2, (car_bounds[1].z - car_bounds[0].z) / 2)) car_ts = TransformState.makePos(Point3(0, 0, 0.5)) if self.car_node is not None: self.car_node.removeNode() self.car_node = render.attachNewNode(BulletRigidBodyNode('Car')) self.car_node.node().setDeactivationEnabled(False) self.car_node.node().setMass(600) self.car_node.node().addShape(car_shape, car_ts) self.world.attachRigidBody(self.car_node.node()) car.reparentTo(self.car_node) self.car_node.setPos(0, 6, 1) self.car = BulletVehicle(self.world, self.car_node.node()) self.car.setCoordinateSystem(ZUp) self.world.attachVehicle(self.car) self.car_node.setPos(0, 6, 1) self.car_node.setHpr(0, 0, 0) self.car.resetSuspension() self.car_node.node().clearForces() wheel_fl = loader.loadModel("models/wheelL") wheel_fl.reparentTo(render) self.make_wheel(Point3(-0.4, 1.28, 0), True, wheel_fl) wheel_fr = loader.loadModel("models/wheelR") wheel_fr.reparentTo(render) self.make_wheel(Point3(0.4, 1.28, 0), True, wheel_fr) wheel_rl = loader.loadModel("models/wheelL") wheel_rl.reparentTo(render) self.make_wheel(Point3(-0.4, -1.35, 0), False, wheel_rl) wheel_rr = loader.loadModel("models/wheelR") wheel_rr.reparentTo(render) self.make_wheel(Point3(0.4, -1.35, 0), False, wheel_rr) def get_engine_torque(self, rpm): min_rpm = min(self.engine_torque_curve, key=lambda p: p[0]) max_rpm = max(self.engine_torque_curve, key=lambda p: p[0]) if rpm <= min_rpm[0]: return min_rpm[1] elif rpm >= max_rpm[0]: return 0 less_rpm = filter(lambda p: p[0] <= rpm, self.engine_torque_curve) more_rpm = filter(lambda p: p[0] >= rpm, self.engine_torque_curve) max_less_rpm = max(less_rpm, key=lambda p: p[0]) min_more_rpm = min(more_rpm, key=lambda p: p[0]) rpm_diff = min_more_rpm[0] - max_less_rpm[0] torque_diff = min_more_rpm[1] - max_less_rpm[1] slope = torque_diff / rpm_diff diff = rpm - max_less_rpm[0] return max_less_rpm[1] + (slope * diff) @property def pos(self): return self.car_node.getPos() @property def forward_vector(self): return self.car.getForwardVector() def update(self, task): car_pos = self.pos car_vec = self.forward_vector track_bounds = self.track.tight_bounds if car_pos.x < track_bounds[0].x or \ car_pos.x > track_bounds[1].x or \ car_pos.y < track_bounds[0].y or \ car_pos.y > track_bounds[1].y or \ car_pos.z < track_bounds[0].z: self.reset() return task.cont car_speed = self.car.getCurrentSpeedKmHour() car_speed_ms = car_speed * 1000 / 3600 car_speed_abs = abs(car_speed) self.car_node.node().clearForces() self.apply_wing_force(car_speed_ms) self.apply_drag_force(car_speed_ms, car_vec) rpm = self.apply_engine_force(car_speed_ms) self.apply_brake_force() self.apply_steering_moment(car_speed_abs) self.update_dials(rpm, car_speed_abs) return task.cont def apply_wing_force(self, car_speed_ms): fw_downforce = 0.5 * self.fw_cord * self.fw_wingspan * self.fw_clift * 1.29 * car_speed_ms ** 2 rw_downforce = 0.5 * self.rw_cord * self.rw_wingspan * self.rw_clift * 1.29 * car_speed_ms ** 2 self.car_node.node().applyForce(Vec3(0, 0, -fw_downforce), Point3(-0.61, 2.32, 0.23)) self.car_node.node().applyForce(Vec3(0, 0, -fw_downforce), Point3(0.61, 2.32, 0.23)) self.car_node.node().applyForce(Vec3(0, 0, -rw_downforce), Point3(-0.61, -2.47, 1.2)) self.car_node.node().applyForce(Vec3(0, 0, -rw_downforce), Point3(0.61, -2.47, 1.2)) def apply_drag_force(self, car_speed_ms, car_vec): drag_coefficient = 0.5 * self.drag_coefficient * 1.29 * 5 drag_force = drag_coefficient * car_speed_ms ** 2 rr_force = drag_coefficient * 30 * car_speed_ms if car_speed_ms < 0: drag_force = -drag_force self.car_node.node().applyCentralForce(car_vec * -drag_force) self.car_node.node().applyCentralForce(car_vec * -rr_force) def apply_engine_force(self, car_speed_ms): angular_velocity = car_speed_ms / self.wheel_radius rpm = angular_velocity * self.gear_ratios[self.gear] * self.differential_ratio * 60 / (2 * math.pi) rpm = max(0, rpm) engine_torque = self.get_engine_torque(rpm) max_engine_force = engine_torque * self.gear_ratios[self.gear] * self.differential_ratio * \ self.transmission_efficiency / self.wheel_radius engine_force = max_engine_force * (self.accelerator / 128) self.car.applyEngineForce(engine_force, 2) self.car.applyEngineForce(engine_force, 3) return rpm def apply_brake_force(self): brake_adj = 5 self.car.setBrake(self.brake * brake_adj, 0) self.car.setBrake(self.brake * brake_adj, 1) self.car.setBrake(self.brake * brake_adj, 2) self.car.setBrake(self.brake * brake_adj, 3) def apply_steering_moment(self, car_speed_abs): steering_ratio = max((car_speed_abs / 3), 30) self.car.setSteeringValue(self.steering / steering_ratio, 0) self.car.setSteeringValue(self.steering / steering_ratio, 1) def update_dials(self, rpm, car_speed_abs): min_rpm = min(self.engine_torque_curve, key=lambda p: p[0])[0] if self.gear == 1: min_rpm = max(self.engine_torque_curve, key=lambda p: p[0])[0] self.speed_dial.setHpr(0, 0, car_speed_abs * (270 / 360)) self.rpm_dial.setHpr(0, 0, max(rpm, min_rpm) * (270 / 20000) * (self.accelerator / 128)) def update_gear_text(self): if self.gear >= 2: text = str(self.gear - 1) elif self.gear == 0: text = "R" elif self.gear == 1: text = "N" else: text = "" self.gear_text.setText(text) def down_gear(self): self.gear -= 1 if self.gear < 0: self.gear = 0 self.update_gear_text() def up_gear(self): self.gear += 1 if self.gear >= len(self.gear_ratios): self.gear = len(self.gear_ratios) - 1 self.update_gear_text()