def test_get_steering(): world = BulletWorld() # Chassis shape = BulletBoxShape(Vec3(0.6, 1.4, 0.5)) body = BulletRigidBodyNode('Vehicle') body.addShape(shape) world.attach(body) # Vehicle vehicle = BulletVehicle(world, body) world.attachVehicle(vehicle) # Wheel wheel = vehicle.createWheel() wheel.setSteering(30.0) # Returns the steering angle in degrees. assert wheel.getSteering() == approx(30.0)
class Game(DirectObject): def __init__(self): base.setBackgroundColor(0.1, 0.1, 0.8, 1) base.setFrameRateMeter(True) base.cam.setPos(0, -20, 4) base.cam.lookAt(0, 0, 0) # Light alight = AmbientLight('ambientLight') alight.setColor(Vec4(0.5, 0.5, 0.5, 1)) alightNP = render.attachNewNode(alight) dlight = DirectionalLight('directionalLight') dlight.setDirection(Vec3(1, 1, -1)) dlight.setColor(Vec4(0.7, 0.7, 0.7, 1)) dlightNP = render.attachNewNode(dlight) render.clearLight() render.setLight(alightNP) render.setLight(dlightNP) # Input self.accept('escape', self.doExit) self.accept('r', self.doReset) self.accept('f1', self.toggleWireframe) self.accept('f2', self.toggleTexture) self.accept('f3', self.toggleDebug) self.accept('f5', self.doScreenshot) inputState.watchWithModifiers('forward', 'w') inputState.watchWithModifiers('left', 'a') inputState.watchWithModifiers('reverse', 's') inputState.watchWithModifiers('right', 'd') inputState.watchWithModifiers('turnLeft', 'q') inputState.watchWithModifiers('turnRight', 'e') # Task taskMgr.add(self.update, 'updateWorld') # Physics self.setup() # _____HANDLER_____ def doExit(self): self.cleanup() sys.exit(1) def doReset(self): self.cleanup() self.setup() def toggleWireframe(self): base.toggleWireframe() def toggleTexture(self): base.toggleTexture() def toggleDebug(self): if self.debugNP.isHidden(): self.debugNP.show() else: self.debugNP.hide() def doScreenshot(self): base.screenshot('Bullet') # ____TASK___ def processInput(self, dt): engineForce = 0.0 brakeForce = 0.0 if inputState.isSet('forward'): engineForce = 1000.0 brakeForce = 0.0 if inputState.isSet('reverse'): engineForce = 0.0 brakeForce = 100.0 if inputState.isSet('turnLeft'): self.steering += dt * self.steeringIncrement self.steering = min(self.steering, self.steeringClamp) if inputState.isSet('turnRight'): self.steering -= dt * self.steeringIncrement self.steering = max(self.steering, -self.steeringClamp) # Apply steering to front wheels self.vehicle.setSteeringValue(self.steering, 0) self.vehicle.setSteeringValue(self.steering, 1) # Apply engine and brake to rear wheels self.vehicle.applyEngineForce(engineForce, 2) self.vehicle.applyEngineForce(engineForce, 3) self.vehicle.setBrake(brakeForce, 2) self.vehicle.setBrake(brakeForce, 3) def update(self, task): dt = globalClock.getDt() self.processInput(dt) self.world.doPhysics(dt, 10, 0.008) #print self.vehicle.getWheel(0).getRaycastInfo().isInContact() #print self.vehicle.getWheel(0).getRaycastInfo().getContactPointWs() #print self.vehicle.getChassis().isKinematic() return task.cont def cleanup(self): self.world = None self.worldNP.removeNode() def setup(self): self.worldNP = render.attachNewNode('World') # World self.debugNP = self.worldNP.attachNewNode(BulletDebugNode('Debug')) self.debugNP.show() self.world = BulletWorld() self.world.setGravity(Vec3(0, 0, -9.81)) self.world.setDebugNode(self.debugNP.node()) # Plane shape = BulletPlaneShape(Vec3(0, 0, 1), 0) np = self.worldNP.attachNewNode(BulletRigidBodyNode('Ground')) np.node().addShape(shape) np.setPos(0, 0, -1) np.setCollideMask(BitMask32.allOn()) self.world.attachRigidBody(np.node()) # Chassis shape = BulletBoxShape(Vec3(0.6, 1.4, 0.5)) ts = TransformState.makePos(Point3(0, 0, 0.5)) np = self.worldNP.attachNewNode(BulletRigidBodyNode('Vehicle')) np.node().addShape(shape, ts) np.setPos(0, 0, 1) np.node().setMass(800.0) np.node().setDeactivationEnabled(False) self.world.attachRigidBody(np.node()) #np.node().setCcdSweptSphereRadius(1.0) #np.node().setCcdMotionThreshold(1e-7) # Vehicle self.vehicle = BulletVehicle(self.world, np.node()) self.vehicle.setCoordinateSystem(ZUp) self.world.attachVehicle(self.vehicle) self.yugoNP = loader.loadModel('car/yugo.egg') self.yugoNP.reparentTo(np) # Right front wheel np = loader.loadModel('car/yugotireR.egg') np.reparentTo(self.worldNP) self.addWheel(Point3(0.70, 1.05, 0.3), True, np) # Left front wheel np = loader.loadModel('car/yugotireL.egg') np.reparentTo(self.worldNP) self.addWheel(Point3(-0.70, 1.05, 0.3), True, np) # Right rear wheel np = loader.loadModel('car/yugotireR.egg') np.reparentTo(self.worldNP) self.addWheel(Point3(0.70, -1.05, 0.3), False, np) # Left rear wheel np = loader.loadModel('car/yugotireL.egg') np.reparentTo(self.worldNP) self.addWheel(Point3(-0.70, -1.05, 0.3), False, np) # Steering info self.steering = 0.0 # degree self.steeringClamp = 45.0 # degree self.steeringIncrement = 120.0 # degree per second def addWheel(self, pos, front, np): wheel = self.vehicle.createWheel() wheel.setNode(np.node()) wheel.setChassisConnectionPointCs(pos) wheel.setFrontWheel(front) wheel.setWheelDirectionCs(Vec3(0, 0, -1)) wheel.setWheelAxleCs(Vec3(1, 0, 0)) wheel.setWheelRadius(0.25) wheel.setMaxSuspensionTravelCm(40.0) wheel.setSuspensionStiffness(40.0) wheel.setWheelsDampingRelaxation(2.3) wheel.setWheelsDampingCompression(4.4) wheel.setFrictionSlip(100.0) wheel.setRollInfluence(0.1)
class Game(DirectObject): def __init__(self): base.setBackgroundColor(0.1, 0.1, 0.8, 1) base.setFrameRateMeter(True) # Light alight = AmbientLight('ambientLight') alight.setColor(Vec4(0.5, 0.5, 0.5, 1)) alightNP = render.attachNewNode(alight) dlight = DirectionalLight('directionalLight') dlight.setDirection(Vec3(1, 1, -1)) dlight.setColor(Vec4(0.7, 0.7, 0.7, 1)) dlightNP = render.attachNewNode(dlight) render.clearLight() render.setLight(alightNP) render.setLight(dlightNP) # Input self.accept('escape', self.doExit) self.accept('r', self.doReset) self.accept('f1', self.toggleWireframe) self.accept('f2', self.toggleTexture) self.accept('f3', self.toggleDebug) self.accept('f5', self.doScreenshot) # ROS self.crash_pub = rospy.Publisher('crash', std_msgs.msg.Empty, queue_size=100) ### subscribers (info sent to Arduino) self.cmd_steer_sub = rospy.Subscriber( 'cmd/steer', std_msgs.msg.Float32, callback=self._cmd_steer_callback) self.cmd_motor_sub = rospy.Subscriber( 'cmd/motor', std_msgs.msg.Float32, callback=self._cmd_motor_callback) self.cmd_vel_sub = rospy.Subscriber('cmd/vel', std_msgs.msg.Float32, callback=self._cmd_vel_callback) self.reset_sub = rospy.Subscriber('reset', std_msgs.msg.Empty, callback=self._reset_callback) self.cmd_steer_queue = Queue.Queue(maxsize=1) self.cmd_motor_queue = Queue.Queue(maxsize=1) # Task taskMgr.add(self.update, 'updateWorld') # Physics self.setup() print('Starting ROS thread') threading.Thread(target=self._ros_servos_thread).start() threading.Thread(target=self._ros_crash_thread).start() threading.Thread(target=self._ros_image_thread).start() # Callbacks def _cmd_steer_callback(self, msg): if msg.data >= 0 and msg.data <= 99.0: self.cmd_steer_queue.put(msg.data) def _cmd_motor_callback(self, msg): if msg.data >= 0 and msg.data <= 99.0: self.cmd_motor_queue.put(msg.data) def _cmd_vel_callback(self, msg): data = numpy.clip(msg.data * 3 + 49.5, 0, 99.0) self.cmd_motor_queue.put(data) def _reset_callback(self, msg): self.doReset() # ROS thread def _ros_servos_thread(self): """ Publishes/subscribes to Ros. """ self.steering = 0.0 # degree self.steeringClamp = rospy.get_param('~steeringClamp') self.engineForce = 0.0 self.engineClamp = rospy.get_param('~engineClamp') r = rospy.Rate(60) while not rospy.is_shutdown(): r.sleep() for var, queue in (('steer', self.cmd_steer_queue), ('motor', self.cmd_motor_queue)): if not queue.empty(): try: if var == 'steer': self.steering = self.steeringClamp * ( (queue.get() - 49.5) / 49.5) self.vehicle.setSteeringValue(self.steering, 0) self.vehicle.setSteeringValue(self.steering, 1) elif var == 'motor': self.vehicle.setBrake(100.0, 2) self.vehicle.setBrake(100.0, 3) self.engineForce = self.engineClamp * ( (queue.get() - 49.5) / 49.5) self.vehicle.applyEngineForce(self.engineForce, 2) self.vehicle.applyEngineForce(self.engineForce, 3) except Exception as e: print(e) def _ros_crash_thread(self): crash = 0 r = rospy.Rate(30) while not rospy.is_shutdown(): r.sleep() try: result = self.world.contactTest(self.vehicle_node) if result.getNumContacts() > 0: self.crash_pub.publish(std_msgs.msg.Empty()) except Exception as e: print(e) def _ros_image_thread(self): camera_pub = ImageROSPublisher("image") depth_pub = ImageROSPublisher("depth") r = rospy.Rate(30) i = 0 while not rospy.is_shutdown(): r.sleep() # sometimes fails to observe try: obs = self.camera_sensor.observe() camera_pub.publish_image(obs[0]) depth_pub.publish_image(obs[1], image_format="passthrough") except: pass # _____HANDLER_____ def doExit(self): self.cleanup() sys.exit(1) def doReset(self): self.cleanup() self.setup() def toggleWireframe(self): base.toggleWireframe() def toggleTexture(self): base.toggleTexture() def toggleDebug(self): if self.debugNP.isHidden(): self.debugNP.show() else: self.debugNP.hide() def doScreenshot(self): base.screenshot('Bullet') # ____TASK___ def update(self, task): dt = globalClock.getDt() self.world.doPhysics(dt, 10, 0.008) #print self.vehicle.getWheel(0).getRaycastInfo().isInContact() #print self.vehicle.getWheel(0).getRaycastInfo().getContactPointWs() #print self.vehicle.getChassis().isKinematic() return task.cont def cleanup(self): self.world = None self.worldNP.removeNode() def setup(self): self.worldNP = render.attachNewNode('World') # World self.debugNP = self.worldNP.attachNewNode(BulletDebugNode('Debug')) self.world = BulletWorld() self.world.setGravity(Vec3(0, 0, -9.81)) self.world.setDebugNode(self.debugNP.node()) # Plane shape = BulletPlaneShape(Vec3(0, 0, 1), 0) np = self.ground = self.worldNP.attachNewNode( BulletRigidBodyNode('Ground')) np.node().addShape(shape) np.setPos(0, 0, -1) np.setCollideMask(BitMask32.allOn()) self.world.attachRigidBody(np.node()) # collision self.maze = [] for pos in [(0.0, 72.0, 0.0), (-11.0, 60.0, 0.0), (11.0, 60.0, 0.0), (-11.0, 48.0, 0.0), (11.0, 48.0, 0.0), (-11.0, 36.0, 0.0), (11.0, 36.0, 0.0), (-11.0, 24.0, 0.0), (11.0, 24.0, 0.0), (-11.0, 12.0, 0.0), (11.0, 12.0, 0.0), (-11.0, 0.0, 0.0), (11.0, 0.0, 0.0), (0.0, -12.0, 0.0), (0.5, 12.0, 1.0), (-0.5, 12.0, 1.0)]: translate = False if (abs(pos[0]) == 0.5): translate = True visNP = loader.loadModel('../models/ball.egg') else: visNP = loader.loadModel('../models/maze.egg') visNP.clearModelNodes() visNP.reparentTo(self.ground) visNP.setPos(pos[0], pos[1], pos[2]) bodyNPs = BulletHelper.fromCollisionSolids(visNP, True) for bodyNP in bodyNPs: bodyNP.reparentTo(render) if translate: bodyNP.setPos(pos[0], pos[1], pos[2] - 1) else: bodyNP.setPos(pos[0], pos[1], pos[2]) if isinstance(bodyNP.node(), BulletRigidBodyNode): bodyNP.node().setMass(0.0) bodyNP.node().setKinematic(True) self.maze.append(bodyNP) for bodyNP in self.maze: self.world.attachRigidBody(bodyNP.node()) # Chassis mass = rospy.get_param('~mass') #chassis_shape = rospy.get_param('~chassis_shape') shape = BulletBoxShape(Vec3(0.6, 1.4, 0.5)) ts = TransformState.makePos(Point3(0, 0, 0.5)) np = self.worldNP.attachNewNode(BulletRigidBodyNode('Vehicle')) np.node().addShape(shape, ts) rand_vals = numpy.random.random(2) * 8 - 4.0 np.setPos(rand_vals[0], 0.0, -0.6) np.node().setMass(mass) np.node().setDeactivationEnabled(False) first_person = rospy.get_param('~first_person') self.camera_sensor = Panda3dCameraSensor(base, color=True, depth=True, size=(160, 90)) self.camera_node = self.camera_sensor.cam if first_person: self.camera_node.reparentTo(np) self.camera_node.setPos(0.0, 1.0, 1.0) self.camera_node.lookAt(0.0, 6.0, 0.0) else: self.camera_node.reparentTo(np) self.camera_node.setPos(0.0, -10.0, 5.0) self.camera_node.lookAt(0.0, 5.0, 0.0) base.cam.reparentTo(np) base.cam.setPos(0.0, -10.0, 5.0) base.cam.lookAt(0.0, 5.0, 0.0) self.world.attachRigidBody(np.node()) np.node().setCcdSweptSphereRadius(1.0) np.node().setCcdMotionThreshold(1e-7) # Vehicle self.vehicle_node = np.node() self.vehicle = BulletVehicle(self.world, np.node()) self.vehicle.setCoordinateSystem(ZUp) self.world.attachVehicle(self.vehicle) self.yugoNP = loader.loadModel('../models/yugo/yugo.egg') self.yugoNP.reparentTo(np) # Right front wheel np = loader.loadModel('../models/yugo/yugotireR.egg') np.reparentTo(self.worldNP) self.addWheel(Point3(0.70, 1.05, 0.3), True, np) # Left front wheel np = loader.loadModel('../models/yugo/yugotireL.egg') np.reparentTo(self.worldNP) self.addWheel(Point3(-0.70, 1.05, 0.3), True, np) # Right rear wheel np = loader.loadModel('../models/yugo/yugotireR.egg') np.reparentTo(self.worldNP) self.addWheel(Point3(0.70, -1.05, 0.3), False, np) # Left rear wheel np = loader.loadModel('../models/yugo/yugotireL.egg') np.reparentTo(self.worldNP) self.addWheel(Point3(-0.70, -1.05, 0.3), False, np) # Collision handle #base.cTrav = CollisionTraverser() #self.notifier = CollisionHandlerEvent() #self.notifier.addInPattern("%fn") #self.accept("Vehicle", self.onCollision) def onCollision(self): print("crash") def addWheel(self, pos, front, np): wheel = self.vehicle.createWheel() wheel.setNode(np.node()) wheel.setChassisConnectionPointCs(pos) wheel.setFrontWheel(front) wheel.setWheelDirectionCs(Vec3(0, 0, -1)) wheel.setWheelAxleCs(Vec3(1, 0, 0)) wheel.setWheelRadius(0.25) wheel.setMaxSuspensionTravelCm(40.0) wheel.setSuspensionStiffness(40.0) wheel.setWheelsDampingRelaxation(2.3) wheel.setWheelsDampingCompression(4.4) wheel.setFrictionSlip(1e3) wheel.setRollInfluence(0.0)
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 Game(DirectObject): def __init__(self, model): self.model = model base.setBackgroundColor(0.1, 0.1, 0.8, 1) base.setFrameRateMeter(True) base.cam.setPos(0, -20, 4) base.cam.lookAt(0, 0, 0) # Light alight = AmbientLight('ambientLight') alight.setColor(Vec4(0.5, 0.5, 0.5, 1)) alightNP = render.attachNewNode(alight) dlight = DirectionalLight('directionalLight') dlight.setDirection(Vec3(1, 1, -1)) dlight.setColor(Vec4(0.7, 0.7, 0.7, 1)) dlightNP = render.attachNewNode(dlight) render.clearLight() render.setLight(alightNP) render.setLight(dlightNP) # Input self.accept('escape', self.doExit) self.accept('r', self.doReset) self.accept('f1', self.toggleWireframe) self.accept('f2', self.toggleTexture) self.accept('f3', self.toggleDebug) self.accept('f5', self.doScreenshot) inputState.watchWithModifiers('forward', 'w') inputState.watchWithModifiers('left', 'a') inputState.watchWithModifiers('reverse', 's') inputState.watchWithModifiers('right', 'd') inputState.watchWithModifiers('turnLeft', 'q') inputState.watchWithModifiers('turnRight', 'e') # Task taskMgr.add(self.update, 'updateWorld') # Physics self.setup() # _____HANDLER_____ def doExit(self): self.cleanup() sys.exit(1) def doReset(self): self.cleanup() self.setup() def toggleWireframe(self): base.toggleWireframe() def toggleTexture(self): base.toggleTexture() def toggleDebug(self): if self.debugNP.isHidden(): self.debugNP.show() else: self.debugNP.hide() def doScreenshot(self): base.screenshot('Bullet') # ____TASK___ def calculate_moves(self): self.y = self.model.predict(self.x) self.moves = self.y > 0 # 0.5 def processInput(self, dt): engineForce = 0.0 brakeForce = 0.0 if self.moves[0]: #inputState.isSet('forward'): engineForce = 1000.0 brakeForce = 0.0 if not self.moves[0]: #inputState.isSet('reverse'): engineForce = 0.0 brakeForce = 100.0 if self.moves[1]: #inputState.isSet('turnLeft'): self.steering += dt * self.steeringIncrement self.steering = min(self.steering, self.steeringClamp) if not self.moves[1]: #inputState.isSet('turnRight'): self.steering -= dt * self.steeringIncrement self.steering = max(self.steering, -self.steeringClamp) """ if inputState.isSet('forward'): engineForce = 1000.0 brakeForce = 0.0 if inputState.isSet('reverse'): engineForce = 0.0 brakeForce = 100.0 if inputState.isSet('turnLeft'): self.steering += dt * self.steeringIncrement self.steering = min(self.steering, self.steeringClamp) if inputState.isSet('turnRight'): self.steering -= dt * self.steeringIncrement self.steering = max(self.steering, -self.steeringClamp) """ # Apply steering to front wheels self.vehicle.setSteeringValue(self.steering, 0) self.vehicle.setSteeringValue(self.steering, 1) # Apply engine and brake to rear wheels self.vehicle.applyEngineForce(engineForce, 2) self.vehicle.applyEngineForce(engineForce, 3) self.vehicle.setBrake(brakeForce, 2) self.vehicle.setBrake(brakeForce, 3) def raycast(self): """pFrom = render.getRelativePoint(self.yugoNP,Point3(0,0,0))#Point3(0,0,0) pFrom -= Point3(0,0,pFrom[2]) pRel = render.getRelativePoint(base.cam,self.yugoNP.getPos()) # FIXME THIS IS IT!! get rid of z component pRel -= Point3(0,0,pRel[2]) p45 = Point3(pRel[0] - pRel[1], pRel[1] + pRel[0],0) pn45 = Point3(pRel[0] + pRel[1], pRel[1] - pRel[0],0) #print(render.getRelativePoint(self.yugoNP,Point3(0,0,0))) #print(dir(self.yugoNP)) pTo = [pFrom + pn45, pFrom + pRel, pFrom + p45]#[pFrom + Vec3(-10,10,0)*999,pFrom + Vec3(0,10,0)*999,pFrom + Vec3(10,10,0)*999]# FIXME should be relative to front of car, getting cloe! #self.yugoNP.getPosDelta()*99999]#[Point3(-10,10,0) * 99999,Point3(0,10,0) * 99999,Point3(10,10,0) * 99999] #self.ray = CollisionRay(0,0,0,100,0,0) result = [self.world.rayTestClosest(pFrom,pt) for pt in pTo] #print(dir(self.yugoNP)) #print(result.getHitPos()) return tuple([res.getHitPos().length() for res in result]) """#queue = CollisionHandlerQueue() #traverser.addCollider(fromObject, queue) #traverser.traverse(render) #queue.sortEntries() #for entry in queue.getEntries(): #print(entry) #print(result.getHitPos()) #if result.getNode() != None: #print(self.yugoNP.getPos(result.getNode())) #print(self.cTrav) self.cTrav.traverse(render) entries = list(self.colHandler.getEntries()) entries.sort(key=lambda y: y.getSurfacePoint(render).getY()) #for entry in entries: print(entry.getFromNodePath().getName()) if entries: # and len(result) > 1: for r in entries: if r.getIntoNodePath().getName( ) == 'Box' and r.getFromNodePath().getName() in [ 'ray%d' % i for i in range(3) ]: self.ray_col_vec_dict[ r.getFromNodePath().getName()].append( numpy.linalg.norm( list(r.getSurfacePoint( r.getFromNodePath()))[:-1])) self.ray_col_vec_dict = { k: (min(self.ray_col_vec_dict[k]) if len(self.ray_col_vec_dict[k]) >= 1 else 10000) for k in self.ray_col_vec_dict } self.x = numpy.array(list(self.ray_col_vec_dict.values())) #return entries def update(self, task): dt = globalClock.getDt() self.raycast() self.calculate_moves() self.ray_col_vec_dict = {k: [] for k in self.ray_col_vec_dict} self.processInput(dt) self.world.doPhysics(dt, 10, 0.008) #print(dir(result[1])) #print(numpy.linalg.norm(list(result[1].getSurfacePoint(result[1].getFromNodePath()))[:-1])) #base.camera.setPos(0,-40,10) #print self.vehicle.getWheel(0).getRaycastInfo().isInContact() #print self.vehicle.getWheel(0).getRaycastInfo().getContactPointWs() #print self.vehicle.getChassis().isKinematic() return task.cont def cleanup(self): self.world = None self.worldNP.removeNode() def setup(self): self.worldNP = render.attachNewNode('World') # World self.debugNP = self.worldNP.attachNewNode(BulletDebugNode('Debug')) self.debugNP.show() self.world = BulletWorld() self.world.setGravity(Vec3(0, 0, -9.81)) self.world.setDebugNode(self.debugNP.node()) #terrain = GeoMipTerrain("mySimpleTerrain") #terrain.setHeightfield("./models/heightfield_2.png") #terrain.getRoot().reparentTo(self.worldNP)#render) #terrain.generate() # Plane shape = BulletPlaneShape(Vec3(0, 0, 1), 0) np = self.worldNP.attachNewNode(BulletRigidBodyNode('Ground')) np.node().addShape(shape) np.setPos(0, 0, -1) np.setCollideMask(BitMask32.allOn()) self.world.attachRigidBody(np.node()) np = self.worldNP.attachNewNode(BulletRigidBodyNode('Track')) np.node().setMass(5000.0) np.setPos(3, 0, 10) np.setCollideMask(BitMask32.allOn()) #(0x0f)) #self.track = BulletVehicle(self.world, np.node()) #self.track.setCoordinateSystem(ZUp) self.track_np = loader.loadModel('models/race_track.egg') self.track_np.setScale(100) self.track_np.reparentTo(np) self.track_np.setCollideMask(BitMask32.allOn()) self.world.attachRigidBody(np.node()) self.track_np = np #self.track_np.show() # Chassis shape = BulletBoxShape(Vec3(0.6, 1.4, 0.5)) ts = TransformState.makePos(Point3(0, 0, 0.5)) np = self.worldNP.attachNewNode(BulletRigidBodyNode('Vehicle')) np.node().addShape(shape, ts) np.setPos(0, 0, 1) np.node().setMass(800.0) np.node().setDeactivationEnabled(False) self.world.attachRigidBody(np.node()) #np.node().setCcdSweptSphereRadius(1.0) #np.node().setCcdMotionThreshold(1e-7) self.cTrav = CollisionTraverser() # Vehicle self.vehicle = BulletVehicle(self.world, np.node()) self.vehicle.setCoordinateSystem(ZUp) self.yugoNP = loader.loadModel('models/yugo/yugo.egg') self.yugoNP.reparentTo(np) self.colHandler = CollisionHandlerQueue() self.ray_col_np = {} self.ray_col_vec_dict = {} for ray_dir in range(-1, 2): # populate collision rays self.ray = CollisionRay() self.ray.setOrigin(ray_dir, 0.5, 0.5) self.ray.setDirection(ray_dir, 1, 0) self.ray_col = CollisionNode('ray%d' % (ray_dir + 1)) self.ray_col.addSolid(self.ray) self.ray_col.setFromCollideMask( BitMask32.allOn()) #(0x0f))#CollideMask.bit(0) #self.ray_col.setIntoCollideMask(CollideMask.allOff()) self.ray_col_np['ray%d' % (ray_dir + 1)] = self.yugoNP.attachNewNode( self.ray_col) self.cTrav.addCollider(self.ray_col_np['ray%d' % (ray_dir + 1)], self.colHandler) self.ray_col_np['ray%d' % (ray_dir + 1)].show() self.ray_col_vec_dict['ray%d' % (ray_dir + 1)] = [] self.world.attachVehicle(self.vehicle) self.cTrav.showCollisions(render) # FIXME base.camera.reparentTo(self.yugoNP) # Right front wheel np = loader.loadModel('models/yugo/yugotireR.egg') np.reparentTo(self.worldNP) self.addWheel(Point3(0.70, 1.05, 0.3), True, np) # Left front wheel np = loader.loadModel('models/yugo/yugotireL.egg') np.reparentTo(self.worldNP) self.addWheel(Point3(-0.70, 1.05, 0.3), True, np) # Right rear wheel np = loader.loadModel('models/yugo/yugotireR.egg') np.reparentTo(self.worldNP) self.addWheel(Point3(0.70, -1.05, 0.3), False, np) # Left rear wheel np = loader.loadModel('models/yugo/yugotireL.egg') np.reparentTo(self.worldNP) self.addWheel(Point3(-0.70, -1.05, 0.3), False, np) # Steering info self.steering = 0.0 # degree self.steeringClamp = 45.0 # degree self.steeringIncrement = 120.0 # degree per second # Box for i, j in [(0, 8), (-3, 5), (6, -5), (8, 3), (-4, -4)]: shape = BulletBoxShape(Vec3(0.5, 0.5, 0.5)) # https://discourse.panda3d.org/t/wall-collision-help/23606 np = self.worldNP.attachNewNode(BulletRigidBodyNode('Box')) np.node().setMass(1.0) np.node().addShape(shape) np.setPos(i, j, 2) np.setCollideMask(BitMask32.allOn()) #(0x0f)) self.world.attachRigidBody(np.node()) self.boxNP = np #self.colHandler2 = CollisionHandlerQueue() visualNP = loader.loadModel('models/box.egg') visualNP.reparentTo(self.boxNP) #self.cTrav.addCollider(self.boxNP,self.colHandler) """ aNode = CollisionNode("TheRay") self.ray = CollisionRay() self.ray.setOrigin( self.yugoNP.getPos() ) self.ray.setDirection( Vec3(0, 10, 0) ) #self.ray.show() aNodePath = self.yugoNP.attachNewNode( CollisionNode("TheRay") ) aNodePath.node().addSolid(self.ray) aNodePath.show() """ #aNode.addSolid(self.ray) #self.ray = CollisionRay(0,0,0,10,0,0) #self.ray.reparentTo(self.yugoNP) #self.rayColl = CollisionNode('PlayerRay') #self.rayColl.addSolid(self.ray) #self.playerRayNode = self.yugoNP.attachNewNode( self.rayColl ) #self.playerRayNode.show() #base.myTraverser.addCollider (self.playerRayNode, base.floor) #base.floor.addCollider( self.playerRayNode, self.yugoNP) """ MyEvent=CollisionHandlerFloor() MyEvent.setReach(100) MyEvent.setOffset(15.0) aNode = CollisionNode("TheRay") ray = CollisionRay() ray.setOrigin( self.boxNP.getPos() ) ray.setDirection( Vec3(10, 0, 0) ) aNode.addSolid(ray) aNodePath = MyModel.attachNewNode( aNode ) Collision = ( aNode, "TheRay" ) Collision[0].setFromCollideMask( BitMask32.bit( 1 ) ) """ def addWheel(self, pos, front, np): wheel = self.vehicle.createWheel() wheel.setNode(np.node()) wheel.setChassisConnectionPointCs(pos) wheel.setFrontWheel(front) wheel.setWheelDirectionCs(Vec3(0, 0, -1)) wheel.setWheelAxleCs(Vec3(1, 0, 0)) wheel.setWheelRadius(0.25) wheel.setMaxSuspensionTravelCm(40.0) wheel.setSuspensionStiffness(40.0) wheel.setWheelsDampingRelaxation(2.3) wheel.setWheelsDampingCompression(4.4) wheel.setFrictionSlip(100.0) wheel.setRollInfluence(0.1)
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 RonnieRacer(DirectObject): gameState = 'INIT' gameLevel = 1 distanceTravelled = 0 speed = 0 score = 0 triesLeft = 3 count = 0 rot = 0 time = 0 pause = False def __init__(self): self.imageObject = OnscreenImage(image = 'media/images/splashscreen.png', pos=(0,0,0), scale=(1.4,1,1)) self.loseScreen = OnscreenImage(image = 'media/images/gameover.png', pos=(0,0,0), scale=(1,1,0.8)) self.loseScreen.hide() self.retryScreen = OnscreenImage(image = 'media/images/retry.png', pos=(0,0,0), scale=(1,1,0.8)) self.retryScreen.hide() self.congratScreen = OnscreenImage(image = 'media/images/congratulations.png', pos=(0,0,0), scale = (1,1,0.8)) self.congratScreen.hide() self.winScreen = OnscreenImage(image = 'media/images/victory.png', pos=(0,0,0), scale = (1,1,0.8)) self.winScreen.hide() self.pauseScreen = OnscreenImage(image = 'media/images/pause.png', pos=(0,0,0), scale = (1,1,0.8)) self.pauseScreen.hide() self.instructionScreen = OnscreenImage(image = 'media/images/instructions.png', pos=(0,0,0), scale = (1,1,0.8)) self.instructionScreen.hide() preloader = Preloader() base.setBackgroundColor(0, 0, 0, 1) base.setFrameRateMeter(True) # Audio self.loseSound = base.loader.loadSfx("media/audio/sfxboo.wav") self.winSound = base.loader.loadSfx("media/audio/cheer2.aif") self.menuMusic = base.loader.loadSfx("media/audio/Scattershot.mp3") self.gameMusic = base.loader.loadSfx("media/audio/Ghostpocalypse - 7 Master.mp3") self.menuMusic.setLoop(True) self.menuMusic.setLoopCount(0) self.gameMusic.setLoop(True) self.gameMusic.setLoopCount(0) #setup buttons self.retryBtn = DirectButton(text="Retry", scale = 0.1, pos = (0,0,0), command = self.doRetry) self.retryBtn.hide() self.menuBtn = DirectButton(text="Main Menu", scale = 0.1, pos = (0,0,0), command = self.doMenu) self.menuBtn.hide() self.nextBtn = DirectButton(text='Next', scale = 0.1, pos = (0,0,0), command = self.doNext) self.nextBtn.hide() self.backBtn = DirectButton(text='back', scale = 0.1, pos = (-0.7,0,-0.7), command = self.doBack) self.backBtn.hide() #setup font self.font = loader.loadFont('media/SHOWG.TTF') self.font.setPixelsPerUnit(60) #setup text self.text = OnscreenText(text = '', pos = (0, 0), scale = 0.07, font = self.font) self.rpmText = OnscreenText(text = '', pos = (-0.9, -0.9), scale = 0.07, font = self.font) self.speedText = OnscreenText(text = '', pos = (0, -0.9), scale = 0.07, font = self.font) self.distanceText = OnscreenText(text = '', pos = (0.9, -0.9), scale = 0.07, font = self.font) self.triesLeftText = OnscreenText(text = '', pos = (1.0, 0.9), scale = 0.07, font = self.font) self.gameLevelText = OnscreenText(text = '', pos = (-1.0, 0.9), scale = 0.07, font = self.font) self.timeText = OnscreenText(text = '', pos = (0, 0.9), scale = 0.07, font = self.font) self.scoreText = OnscreenText(text = '', pos = (1.0, 0.8), scale = 0.07, font = self.font) self.finalScoreText = OnscreenText(text = '', pos = (0, 0.2), scale = 0.07, font = self.font) # Light alight = AmbientLight('ambientLight') alight.setColor(Vec4(0.5, 0.5, 0.5, 1)) alightNP = render.attachNewNode(alight) dlight = DirectionalLight('directionalLight') dlight.setDirection(Vec3(1, 1, -1)) dlight.setColor(Vec4(0.7, 0.7, 0.7, 1)) dlightNP = render.attachNewNode(dlight) render.clearLight() render.setLight(alightNP) render.setLight(dlightNP) # Input self.accept('escape', self.doExit) self.accept('r', self.doReset) self.accept('f1', self.toggleWireframe) self.accept('f2', self.toggleTexture) self.accept('f3', self.toggleDebug) self.accept('f5', self.doScreenshot) inputState.watchWithModifiers('forward', 'w') inputState.watchWithModifiers('left', 'a') inputState.watchWithModifiers('reverse', 's') inputState.watchWithModifiers('right', 'd') inputState.watchWithModifiers('turnLeft', 'a') inputState.watchWithModifiers('turnRight', 'd') # Task taskMgr.add(self.update, 'updateWorld') # _____HANDLER_____ def doExit(self): sys.exit(1) def doReset(self): self.cleanup() self.terrain.getRoot().removeNode() self.setup() def doBack(self): self.backBtn.hide() self.instructionScreen.hide() self.imageObject.show() self.helpBtn.show() self.startBtn.show() self.exitBtn.show() def toggleWireframe(self): base.toggleWireframe() def toggleTexture(self): base.toggleTexture() def toggleDebug(self): if self.debugNP.isHidden(): self.debugNP.show() else: self.debugNP.hide() def doScreenshot(self): base.screenshot('Bullet') # ____TASK___ def processInput(self, dt): # Process input engineForce = 0.0 brakeForce = 0.0 self.accept('p', self.doPause) if inputState.isSet('forward'): engineForce = 15.0 brakeForce = 0.0 if inputState.isSet('reverse'): engineForce = -25.0 brakeForce = 25.0 if inputState.isSet('turnLeft'): self.steering += dt * self.steeringIncrement self.steering = min(self.steering, self.steeringClamp) if inputState.isSet('turnRight'): self.steering -= dt * self.steeringIncrement self.steering = max(self.steering, -self.steeringClamp) # Apply steering to front wheels self.vehicle.setSteeringValue(self.steering, 0) self.vehicle.setSteeringValue(self.steering, 1) # Apply engine and brake to rear wheels self.vehicle.applyEngineForce(engineForce, 2) self.vehicle.applyEngineForce(engineForce, 3) self.vehicle.setBrake(brakeForce, 2) self.vehicle.setBrake(brakeForce, 3) def processContacts(self, dt): result = self.world.contactTestPair(self.vehicleNP.node(), self.flagNP.node()) if(result.getNumContacts() > 0): self.gameState = 'WIN' self.doContinue() def doContinue(self): if(self.gameState == 'INIT'): self.gameState = 'MENU' self.menuMusic.play() self.text.hide() self.startBtn = DirectButton(text=("Play"), scale = 0.1, pos = (0.5,0,0),command=self.playGame) self.helpBtn = DirectButton(text=("Help"), scale = 0.1, pos = (0.5,0,-0.2),command=self.help) self.exitBtn = DirectButton(text=("Exit"), scale = 0.1, pos = (0.5,0,-0.4), command = self.doExit) return if(self.gameState == 'RETRY'): self.retryScreen.show() self.retryBtn.show() self.loseSound.play() return if(self.gameState == 'LOSE'): self.loseScreen.show() self.menuBtn.show() return if(self.gameState == 'WIN'): if(self.gameLevel < 3): self.congratScreen.show() self.nextBtn.show() elif(self.gameLevel >= 3): self.winScreen.show() self.menuBtn.show() self.finalScoreText.setText('Your Score: '+str(int(self.score))) self.finalScoreText.show() self.winSound.play() def help(self): self.gameState = 'HELP' self.startBtn.hide() self.exitBtn.hide() self.helpBtn.hide() self.imageObject.hide() self.instructionScreen.show() self.backBtn.show() def doNext(self): self.nextBtn.hide() self.finalScoreText.hide() self.congratScreen.hide() self.gameLevel += 1 if(self.gameLevel == 2): self.score += 2000 elif(self.gameLevel == 3): self.score += 3000 self.doReset() self.triesLeft = 3 self.gameState = 'PLAY' def doRetry(self): self.doReset() self.gameState = 'PLAY' self.retryScreen.hide() self.retryBtn.hide() self.triesLeft -= 1 def doMenu(self): self.cleanup() self.terrain.getRoot().removeNode() self.gameState = 'MENU' self.score = 0 self.imageObject.show() self.startBtn.show() self.exitBtn.show() self.helpBtn.show() self.loseScreen.hide() self.menuBtn.hide() self.winScreen.hide() self.finalScoreText.hide() self.speedText.hide() self.distanceText.hide() self.rpmText.hide() self.scoreText.hide() self.gameLevelText.hide() self.timeText.hide() self.triesLeftText.hide() self.gameMusic.stop() self.menuMusic.play() def doPause(self): self.pause = not self.pause if(self.pause): self.pauseScreen.show() else: self.pauseScreen.hide() def playGame(self): self.gameState = 'PLAY' self.triesLeft = 3 self.gameLevel = 1 self.imageObject.hide() self.startBtn.hide() self.exitBtn.hide() self.helpBtn.hide() self.menuMusic.stop() self.gameMusic.play() self.speedText.show() self.distanceText.show() self.rpmText.show() self.scoreText.show() self.gameLevelText.show() self.triesLeftText.show() self.timeText.show() # Physics self.setup() def update(self, task): dt = globalClock.getDt() if(not self.pause): if(self.gameState == 'RETRY'): return task.cont if (self.gameState == 'INIT'): self.accept('space', self.doContinue) self.text.setText('Press Space to Continue') if(self.gameState == 'PLAY'): if (self.steering > 0): self.steering -= dt * 50 if (self.steering < 0): self.steering += dt * 50 playerOldSpeed = self.vehicle.getCurrentSpeedKmHour() self.processInput(dt) self.processContacts(dt) self.world.doPhysics(dt, 10, 0.008) #calculate speed,rpm,distance and display text self.speed = self.vehicle.getCurrentSpeedKmHour() if(self.speed<0): self.speed = -self.speed self.speedText.setText('Speed: ' + str(int(self.speed)) + 'Km/h') self.distanceTravelled += self.speed*(dt/3600) self.distanceText.setText('Distance: '+str(float(int(self.distanceTravelled * 1000))/1000) + 'Km') playerNewSpeed = self.vehicle.getCurrentSpeedKmHour() playerAcceleration = (playerNewSpeed - playerOldSpeed) / (dt/60) #playerPosText = self.vehicleNP.getPos() #self.text.setText('Player position: %s'%playerPosText) self.rpmText.setText('Engine RPM: ' + str(int(((self.vehicle.getCurrentSpeedKmHour() / 60) * 1000) / (2 * 0.4 * 3.14159265))) + ' Rpm') self.triesLeftText.setText('Tries Left: ' + str(self.triesLeft)) self.gameLevelText.setText('Level: '+ str(self.gameLevel)) #update camera #position d = self.vehicleNP.getPos() - base.cam.getPos() if(d.length() > 8): base.cam.setX(base.cam.getX() + d.getX()*dt) base.cam.setY(base.cam.getY() + d.getY()*dt) base.cam.setZ(self.vehicleNP.getZ() + 4) #lookat base.cam.lookAt(self.vehicleNP.getPos()+Vec3(0,0,1)) if(self.gameLevel == 1): if(self.vehicleNP.getZ() < -17): if(self.triesLeft > 0): self.gameState = 'RETRY' else: self.gameState = 'LOSE' self.doContinue() elif(self.gameLevel == 2): if(self.vehicleNP.getZ() < -20): if(self.triesLeft > 0): self.gameState = 'RETRY' else: self.gameState = 'LOSE' self.doContinue() elif(self.gameLevel == 3): if(self.vehicleNP.getZ() < -17): if(self.triesLeft > 0): self.gameState = 'RETRY' else: self.gameState = 'LOSE' self.doContinue() if(self.speed < 5): self.steeringIncrement = 120 elif(self.speed >= 5 and self.speed < 10): self.steeringIncrement = 100 elif(self.speed >= 10 and self.speed < 15): self.steeringIncrement = 80 elif(self.speed >=15 and self.speed < 30): self.steeringIncrement = 60 #spin the flag self.rot += 1 self.flagNP.setHpr(self.rot,0,0) #time self.time += dt self.timeText.setText('Time: ' + str(int(self.time))) if(self.score > 0): self.score -= dt self.scoreText.setText('Score: '+str(int(self.score))) return task.cont def cleanup(self): self.world = None self.worldNP.removeNode() def setup(self): # Steering info self.steering = 0.0 # degree self.steeringClamp = 30.0 # degree self.steeringIncrement = 80.0 # degree per second self.worldNP = render.attachNewNode('World') # World self.debugNP = self.worldNP.attachNewNode(BulletDebugNode('Debug')) #self.debugNP.show() self.world = BulletWorld() self.world.setGravity(Vec3(0, 0, -9.81)) self.world.setDebugNode(self.debugNP.node()) if(self.gameLevel == 1): #set score print('GameLevel') self.score = 1000 self.distanceTravelled = 0 self.time = 0 # Plane img = PNMImage(Filename('media/terrain/SIMP_Assignment_2_Terrain_1.png')) shape = BulletHeightfieldShape(img, 50.0, ZUp) np = self.worldNP.attachNewNode(BulletRigidBodyNode('Ground')) np.node().addShape(shape) np.setPos(0, 0, 0) np.setCollideMask(BitMask32.allOn()) self.world.attachRigidBody(np.node()) #skybox skybox = loader.loadModel('media/models/skybox/skybox_01.X') skybox.reparentTo(render) # Chassis shape = BulletBoxShape(Vec3(0.6, 1.4, 0.5)) ts = TransformState.makePos(Point3(0, 0, 1.0)) self.vehicleNP = self.worldNP.attachNewNode(BulletRigidBodyNode('Vehicle')) self.vehicleNP.node().addShape(shape, ts) self.vehicleNP.setPos(-93, -88, -7)#-93, -88, -7) #(-82,65.8,-8) #(55,8.38,-6)#(45, -19, -8)#(-93, -88, -7) self.vehicleNP.setHpr(-90,0,0) self.vehicleNP.node().setMass(5.0) self.vehicleNP.node().setDeactivationEnabled(False) base.cam.setPos(self.vehicleNP.getPos().getX()+2,self.vehicleNP.getPos().getY()+2,self.vehicleNP.getPos().getZ()+2) self.world.attachRigidBody(self.vehicleNP.node()) # Vehicle self.vehicle = BulletVehicle(self.world, self.vehicleNP.node()) self.vehicle.setCoordinateSystem(ZUp) self.world.attachVehicle(self.vehicle) self.hummerNP = loader.loadModel('media/models/vehicle/body.X') self.hummerNP.reparentTo(self.vehicleNP) # Right front wheel np = loader.loadModel('media/models/vehicle/front_right.X') np.reparentTo(self.worldNP) self.addWheel(Point3( 0.8, 0.9, 0.8), True, np) # Left front wheel np = loader.loadModel('media/models/vehicle/front_left.X') np.reparentTo(self.worldNP) self.addWheel(Point3(-0.8, 0.9, 0.8), True, np) # Right rear wheel np = loader.loadModel('media/models/vehicle/back_right.X') np.reparentTo(self.worldNP) self.addWheel(Point3( 0.8, -0.7, 0.8), False, np) # Left rear wheel np = loader.loadModel('media/models/vehicle/back_left.X') np.reparentTo(self.worldNP) self.addWheel(Point3(-0.8, -0.7, 0.8), False, np) #Obstacles self.setupObstacleOne(Vec3(50, -5, -4), 1.8, Vec3(60, 0, 0)) self.setupObstacleFour(Vec3(63.3, 59.2, -10), 1.5, Vec3(0,0,0)) self.setupObstacleFour(Vec3(41, 57, -10), 1.5, Vec3(0,0,0)) self.setupObstacleFour(Vec3(7.5, 53.8, -10), 1.5, Vec3(0,0,0)) self.setupObstacleFour(Vec3(-28, 81.4, -10), 1.5, Vec3(0,0,0)) self.setupObstacleSix(Vec3(-91, 81 , -6), 1, Vec3(60,0,0)) #Goal self.setupGoal(Vec3(-101,90.6,-6.5)) #self.vehicleNP.setPos(Vec3(6,52,-6)) self.setupTerrain() elif(self.gameLevel == 2): self.distanceTravelled = 0 self.time = 0 # Plane img = PNMImage(Filename('media/terrain/SIMP_Assignment_2_Terrain_2.png')) shape = BulletHeightfieldShape(img, 50.0, ZUp) np = self.worldNP.attachNewNode(BulletRigidBodyNode('Ground')) np.node().addShape(shape) np.setPos(0, 0, 0) np.setCollideMask(BitMask32.allOn()) self.world.attachRigidBody(np.node()) #skybox skybox = loader.loadModel('media/models/skybox/skybox_01.X') skybox.reparentTo(render) # Chassis shape = BulletBoxShape(Vec3(0.6, 1.4, 0.5)) ts = TransformState.makePos(Point3(0, 0, 1.0)) self.vehicleNP = self.worldNP.attachNewNode(BulletRigidBodyNode('Vehicle')) self.vehicleNP.node().addShape(shape, ts) self.vehicleNP.setPos(-99.6,105,-11.8)#(88, 21, -11)#(34.3,8.4,-11.8)#(-99.6,105,-11.8)#(86.4,41.2,-12) self.vehicleNP.setHpr(-130,0,0) self.vehicleNP.node().setMass(5.0) self.vehicleNP.node().setDeactivationEnabled(False) base.cam.setPos(self.vehicleNP.getPos().getX()+2,self.vehicleNP.getPos().getY()+2,self.vehicleNP.getPos().getZ()+2) self.world.attachRigidBody(self.vehicleNP.node()) # Vehicle self.vehicle = BulletVehicle(self.world, self.vehicleNP.node()) self.vehicle.setCoordinateSystem(ZUp) self.world.attachVehicle(self.vehicle) self.hummerNP = loader.loadModel('media/models/vehicle/body.X') self.hummerNP.reparentTo(self.vehicleNP) # Right front wheel np = loader.loadModel('media/models/vehicle/front_right.X') np.reparentTo(self.worldNP) self.addWheel(Point3( 0.8, 0.9, 0.8), True, np) # Left front wheel np = loader.loadModel('media/models/vehicle/front_left.X') np.reparentTo(self.worldNP) self.addWheel(Point3(-0.8, 0.9, 0.8), True, np) # Right rear wheel np = loader.loadModel('media/models/vehicle/back_right.X') np.reparentTo(self.worldNP) self.addWheel(Point3( 0.8, -0.7, 0.8), False, np) # Left rear wheel np = loader.loadModel('media/models/vehicle/back_left.X') np.reparentTo(self.worldNP) self.addWheel(Point3(-0.8, -0.7, 0.8), False, np) self.setupObstacleFive(Vec3(91, 3, -9),1,Vec3(90,0,0)) self.setupObstacleFive(Vec3(94,-19, -10),0.9,Vec3(90,0,0)) self.setupObstacleFive(Vec3(85,-40, -10),1,Vec3(90,0,0)) self.setupObstacleFour(Vec3(-33.5, 23.4,-14.5),1,Vec3(0,0,0)) self.setupObstacleFour(Vec3(-43.3, 24.2,-14.5),1,Vec3(0,0,0)) self.setupObstacleTwo(Vec3(34.7,20.9,-8.5),1,Vec3(90,0,0)) self.setupObstacleTwo(Vec3(26.8,20.3,-8.5),1,Vec3(90,0,0)) self.setupObstacleTwo(Vec3(42.1,22.5,-8.5),1,Vec3(90,0,0)) #self.setupObstacleFive(Vec3(91,0.2, -8),2.1,Vec3(90,0,0)) #Goal self.setupGoal(Vec3(94,-89.7,-10)) self.setupTerrain() elif(self.gameLevel == 3): self.distanceTravelled = 0 self.time = 0 # Plane img = PNMImage(Filename('media/terrain/SIMP_Assignment_2_Terrain_3.png')) shape = BulletHeightfieldShape(img, 50.0, ZUp) np = self.worldNP.attachNewNode(BulletRigidBodyNode('Ground')) np.node().addShape(shape) np.setPos(0, 0, 0) np.setCollideMask(BitMask32.allOn()) self.world.attachRigidBody(np.node()) #skybox skybox = loader.loadModel('media/models/skybox/skybox_01.X') skybox.reparentTo(render) # Chassis shape = BulletBoxShape(Vec3(0.6, 1.4, 0.5)) ts = TransformState.makePos(Point3(0, 0, 1.0)) self.vehicleNP = self.worldNP.attachNewNode(BulletRigidBodyNode('Vehicle')) self.vehicleNP.node().addShape(shape, ts) self.vehicleNP.setPos(-110, -110, 0) self.vehicleNP.setHpr(-40,0,0) self.vehicleNP.node().setMass(5.0) self.vehicleNP.node().setDeactivationEnabled(False) base.cam.setPos(self.vehicleNP.getPos().getX()+2,self.vehicleNP.getPos().getY()+2,self.vehicleNP.getPos().getZ()+2) self.world.attachRigidBody(self.vehicleNP.node()) # Vehicle self.vehicle = BulletVehicle(self.world, self.vehicleNP.node()) self.vehicle.setCoordinateSystem(ZUp) self.world.attachVehicle(self.vehicle) self.hummerNP = loader.loadModel('media/models/vehicle/body.X') self.hummerNP.reparentTo(self.vehicleNP) # Right front wheel np = loader.loadModel('media/models/vehicle/front_right.X') np.reparentTo(self.worldNP) self.addWheel(Point3( 0.8, 0.9, 0.8), True, np) # Left front wheel np = loader.loadModel('media/models/vehicle/front_left.X') np.reparentTo(self.worldNP) self.addWheel(Point3(-0.8, 0.9, 0.8), True, np) # Right rear wheel np = loader.loadModel('media/models/vehicle/back_right.X') np.reparentTo(self.worldNP) self.addWheel(Point3( 0.8, -0.7, 0.8), False, np) # Left rear wheel np = loader.loadModel('media/models/vehicle/back_left.X') np.reparentTo(self.worldNP) self.addWheel(Point3(-0.8, -0.7, 0.8), False, np) self.setupTerrain() #Goal self.setupGoal(Vec3(114,100,-13)) #Obstacles self.setupObstacleFour(Vec3(-60, -73, -9), 1, Vec3(0, 0, 0)) self.setupObstacleFour(Vec3(-63, -77, -9), 1, Vec3(0, 0, 0)) self.setupObstacleTwo(Vec3(-15, -40, -3), 1, Vec3(0, 0, 0)) self.setupObstacleFour(Vec3(-60, 12, -11), 1, Vec3(0, 0, 0)) self.setupObstacleSix(Vec3(-15, 90, -6), 1.5, Vec3(-30, 0, 0)) self.setupObstacleFour(Vec3(28, 87, -11), 1, Vec3(0, 0, 0)) self.setupObstacleFour(Vec3(32, 90, -11), 1, Vec3(0, 0, 0)) def addWheel(self, pos, front, np): wheel = self.vehicle.createWheel() wheel.setNode(np.node()) wheel.setChassisConnectionPointCs(pos) wheel.setFrontWheel(front) wheel.setWheelDirectionCs(Vec3(0, 0, -1)) wheel.setWheelAxleCs(Vec3(1, 0, 0)) wheel.setWheelRadius(0.4) wheel.setMaxSuspensionTravelCm(40.0) wheel.setSuspensionStiffness(40.0) wheel.setWheelsDampingRelaxation(2.3) wheel.setWheelsDampingCompression(4.4) wheel.setFrictionSlip(100.0); wheel.setRollInfluence(0.1) def setupTerrain(self): if(self.gameLevel == 1): #terrain setting img = PNMImage(Filename('media/terrain/SIMP_Assignment_2_Terrain_1.png')) self.terrain = GeoMipTerrain("myTerrain") self.terrain.setHeightfield(img) self.terrain.getRoot().setSz(50) self.terrain.setBlockSize(4) #self.terrain.setFactor(10) #self.terrain.setMinLevel(0) self.terrain.setNear(50) self.terrain.setFar(1000) self.terrain.setFocalPoint(base.camera) self.terrain.getRoot().reparentTo(render) offset = img.getXSize() / 2.0 - 0.5 self.terrain.getRoot().setPos(-offset, -offset, -50 / 2.0) self.terrain.generate() #load textures tex0 = loader.loadTexture("media/terrain/SIMP_Assignment_2_Terrain_1_d.png") tex0.setMinfilter(Texture.FTLinearMipmapLinear) tex1 = loader.loadTexture("media/terrain/longGrass.png") tex1.setMinfilter(Texture.FTLinearMipmapLinear) tex2 = loader.loadTexture("media/terrain/bigRockFace.png") tex2.setMinfilter(Texture.FTLinearMipmapLinear) tex3 = loader.loadTexture("media/terrain/greenrough.png") tex3.setMinfilter(Texture.FTLinearMipmapLinear) tex4 = loader.loadTexture("media/terrain/grayRock.png") tex4.setMinfilter(Texture.FTLinearMipmapLinear) tex5 = loader.loadTexture("media/terrain/SIMP_Assignment_2_Terrain_1_c.png") tex5.setMinfilter(Texture.FTLinearMipmapLinear) tex6 = loader.loadTexture("media/terrain/SIMP_Assignment_2_Terrain_1_l.png") tex6.setMinfilter(Texture.FTLinearMipmapLinear) #set mutiltextures self.terrain.getRoot().setTexture( TextureStage('tex0'),tex0 ) self.terrain.getRoot().setTexture( TextureStage('tex1'),tex1 ) self.terrain.getRoot().setTexture( TextureStage('tex2'),tex2 ) self.terrain.getRoot().setTexture( TextureStage('tex3'),tex3 ) self.terrain.getRoot().setTexture( TextureStage('tex4'),tex4 ) self.terrain.getRoot().setTexture( TextureStage('tex5'),tex5 ) self.terrain.getRoot().setTexture( TextureStage('tex6'),tex6 ) #load shader self.terrain.getRoot().setShader(loader.loadShader('terraintexture.sha')) elif(self.gameLevel == 2): #terrain setting img = PNMImage(Filename('media/terrain/SIMP_Assignment_2_Terrain_2.png')) self.terrain = GeoMipTerrain("myTerrain") self.terrain.setHeightfield(img) self.terrain.getRoot().setSz(50) self.terrain.setBlockSize(4) #self.terrain.setFactor(10) #self.terrain.setMinLevel(0) self.terrain.setNear(50) self.terrain.setFar(100) self.terrain.setFocalPoint(base.camera) self.terrain.getRoot().reparentTo(render) offset = img.getXSize() / 2.0 - 0.5 self.terrain.getRoot().setPos(-offset, -offset, -50 / 2.0) self.terrain.generate() #load textures tex0 = loader.loadTexture("media/terrain/SIMP_Assignment_2_Terrain_2_d.png") tex0.setMinfilter(Texture.FTLinearMipmapLinear) tex1 = loader.loadTexture("media/terrain/sandripple.png") tex1.setMinfilter(Texture.FTLinearMipmapLinear) tex2 = loader.loadTexture("media/terrain/orangesand.png") tex2.setMinfilter(Texture.FTLinearMipmapLinear) tex3 = loader.loadTexture("media/terrain/grayRock.png") tex3.setMinfilter(Texture.FTLinearMipmapLinear) tex4 = loader.loadTexture("media/terrain/bigRockFace.png") tex4.setMinfilter(Texture.FTLinearMipmapLinear) tex5 = loader.loadTexture("media/terrain/SIMP_Assignment_2_Terrain_2_c.png") tex5.setMinfilter(Texture.FTLinearMipmapLinear) tex6 = loader.loadTexture("media/terrain/SIMP_Assignment_2_Terrain_2_l.png") tex6.setMinfilter(Texture.FTLinearMipmapLinear) #set mutiltextures self.terrain.getRoot().setTexture( TextureStage('tex0'),tex0 ) self.terrain.getRoot().setTexture( TextureStage('tex1'),tex1 ) self.terrain.getRoot().setTexture( TextureStage('tex2'),tex2 ) self.terrain.getRoot().setTexture( TextureStage('tex3'),tex3 ) self.terrain.getRoot().setTexture( TextureStage('tex4'),tex4 ) self.terrain.getRoot().setTexture( TextureStage('tex5'),tex5 ) self.terrain.getRoot().setTexture( TextureStage('tex6'),tex6 ) #load shader self.terrain.getRoot().setShader(loader.loadShader('terraintexture.sha')) elif(self.gameLevel == 3): #terrain setting img = PNMImage(Filename('media/terrain/SIMP_Assignment_2_Terrain_3.png')) self.terrain = GeoMipTerrain("myTerrain") self.terrain.setHeightfield(img) self.terrain.getRoot().setSz(50) self.terrain.setBlockSize(4) #self.terrain.setFactor(10) #self.terrain.setMinLevel(0) self.terrain.setNear(50) self.terrain.setFar(100) self.terrain.setFocalPoint(base.camera) self.terrain.getRoot().reparentTo(render) offset = img.getXSize() / 2.0 - 0.5 self.terrain.getRoot().setPos(-offset, -offset, -50 / 2.0) self.terrain.generate() #load textures tex0 = loader.loadTexture("media/terrain/SIMP_Assignment_2_Terrain_3_d.png") tex0.setMinfilter(Texture.FTLinearMipmapLinear) tex1 = loader.loadTexture("media/terrain/hardDirt.png") tex1.setMinfilter(Texture.FTLinearMipmapLinear) tex2 = loader.loadTexture("media/terrain/littlerocks.png") tex2.setMinfilter(Texture.FTLinearMipmapLinear) tex3 = loader.loadTexture("media/terrain/greenrough.png") tex3.setMinfilter(Texture.FTLinearMipmapLinear) tex4 = loader.loadTexture("media/terrain/bigRockFace.png") tex4.setMinfilter(Texture.FTLinearMipmapLinear) tex5 = loader.loadTexture("media/terrain/SIMP_Assignment_2_Terrain_3_c.png") tex5.setMinfilter(Texture.FTLinearMipmapLinear) tex6 = loader.loadTexture("media/terrain/SIMP_Assignment_2_Terrain_3_l.png") tex6.setMinfilter(Texture.FTLinearMipmapLinear) #set mutiltextures self.terrain.getRoot().setTexture( TextureStage('tex0'),tex0 ) self.terrain.getRoot().setTexture( TextureStage('tex1'),tex1 ) self.terrain.getRoot().setTexture( TextureStage('tex2'),tex2 ) self.terrain.getRoot().setTexture( TextureStage('tex3'),tex3 ) self.terrain.getRoot().setTexture( TextureStage('tex4'),tex4 ) self.terrain.getRoot().setTexture( TextureStage('tex5'),tex5 ) self.terrain.getRoot().setTexture( TextureStage('tex6'),tex6 ) #load shader self.terrain.getRoot().setShader(loader.loadShader('terraintexture.sha')) def setupObstacleOne(self, pos, scale, turn): #box A shape = BulletBoxShape(Vec3(3, 0.1, 0.1) * scale) bodyA = BulletRigidBodyNode('Box A') bodyNP= self.worldNP.attachNewNode(bodyA) bodyNP.node().addShape(shape) bodyNP.setCollideMask(BitMask32.allOn()) bodyNP.setPos(pos) bodyNP.setHpr(turn) visNP = loader.loadModel('media/models/box.egg') visNP.setScale(Vec3(3, 0.1, 0.1)*2 * scale) visNP.clearModelNodes() visNP.reparentTo(bodyNP) self.world.attachRigidBody(bodyA) # Box C shape = BulletBoxShape(Vec3(0.1, 0.1, 0.9)*scale) bodyC = BulletRigidBodyNode('Box C') bodyNP = self.worldNP.attachNewNode(bodyC) bodyNP.node().addShape(shape) bodyNP.node().setMass(1.0) bodyNP.node().setLinearDamping(0.5) bodyNP.node().setDeactivationEnabled(False) bodyNP.setCollideMask(BitMask32.allOn()) bodyNP.setPos(pos) bodyNP.setHpr(turn) visNP = loader.loadModel('media/models/box.egg') visNP.setScale(Vec3(0.1, 0.1, 0.9)*2*scale) visNP.clearModelNodes() visNP.reparentTo(bodyNP) self.world.attachRigidBody(bodyC) pivotA = Point3(0, 0, -0.1 * scale) pivotB = Point3(0, 0, 1 * scale) axisA = Vec3(1, 0, 0) axisB = Vec3(1, 0, 0) hinge = BulletHingeConstraint(bodyA, bodyC, pivotA, pivotB, axisA, axisB, True) hinge.setDebugDrawSize(2.0) hinge.setLimit(-90,90, softness=1.0, bias=0.3, relaxation=1.0) self.world.attachConstraint(hinge) # Box B shape = BulletBoxShape(Vec3(3, 2, 0.1)*scale) bodyB = BulletRigidBodyNode('Box B') bodyNP = self.worldNP.attachNewNode(bodyB) bodyNP.node().addShape(shape) bodyNP.node().setMass(1.0) bodyNP.node().setLinearDamping(0.5) bodyNP.node().setDeactivationEnabled(False) bodyNP.setCollideMask(BitMask32.allOn()) bodyNP.setPos(pos) bodyNP.setHpr(turn); visNP = loader.loadModel('media/models/box.egg') visNP.setScale(Vec3(3, 2, 0.1)*2*scale) visNP.clearModelNodes() visNP.reparentTo(bodyNP) self.world.attachRigidBody(bodyB) # Hinge pivotA = Point3(0, 0, 0) pivotB = Point3(0, 0, -1 * scale) hinge = BulletHingeConstraint(bodyB, bodyC, pivotA, pivotB, axisA, axisB, True) hinge.setLimit(0,360, softness=1.0, bias=0.3, relaxation=1.0) self.world.attachConstraint(hinge) def setupObstacleTwo(self,pos,scale,turn): #box A shape = BulletBoxShape(Vec3(3, 0.1, 0.1)*scale) bodyA = BulletRigidBodyNode('Box A') bodyNP= self.worldNP.attachNewNode(bodyA) bodyNP.node().addShape(shape) bodyNP.setCollideMask(BitMask32.allOn()) bodyNP.setPos(pos) bodyNP.setHpr(turn) visNP = loader.loadModel('media/models/box.egg') visNP.setScale(Vec3(3, 0.1, 0.1)*2*scale) visNP.clearModelNodes() visNP.reparentTo(bodyNP) self.world.attachRigidBody(bodyA) # Box B shape = BulletBoxShape(Vec3(0.1, 1, 1)*scale) bodyB = BulletRigidBodyNode('Box B') bodyNP = self.worldNP.attachNewNode(bodyB) bodyNP.node().addShape(shape) bodyNP.node().setMass(100.0) bodyNP.node().setDeactivationEnabled(False) bodyNP.setCollideMask(BitMask32.allOn()) bodyNP.setPos(pos) bodyNP.setHpr(turn) visNP = loader.loadModel('media/models/box.egg') visNP.setScale(Vec3(0.1, 1, 1)*2*scale) visNP.clearModelNodes() visNP.reparentTo(bodyNP) self.world.attachRigidBody(bodyB) # Hinge pivotA = Point3(2, 0, 0) pivotB = Point3(0, 0, 2) axisA = Vec3(1, 0, 0) axisB = Vec3(1, 0, 0) hinge = BulletHingeConstraint(bodyA, bodyB, pivotA, pivotB, axisA, axisB, True) hinge.setDebugDrawSize(2.0) hinge.setLimit(-90,90, softness=1.0, bias=0.3, relaxation=1.0) self.world.attachConstraint(hinge) # Box C shape = BulletBoxShape(Vec3(0.1, 1, 1)*scale) bodyC = BulletRigidBodyNode('Box C') bodyNP = self.worldNP.attachNewNode(bodyC) bodyNP.node().addShape(shape) bodyNP.node().setMass(100.0) bodyNP.node().setDeactivationEnabled(False) bodyNP.setCollideMask(BitMask32.allOn()) bodyNP.setPos(pos) bodyNP.setHpr(turn) visNP = loader.loadModel('media/models/box.egg') visNP.setScale(Vec3(0.1, 1, 1)*2*scale) visNP.clearModelNodes() visNP.reparentTo(bodyNP) self.world.attachRigidBody(bodyC) pivotA = Point3(-2, 0, 0) pivotB = Point3(0, 0, 2) hinge = BulletHingeConstraint(bodyA, bodyC, pivotA, pivotB, axisA, axisB, True) self.world.attachConstraint(hinge) def setupObstacleThree(self, pos, scale, turn): # Box A shape = BulletBoxShape(Vec3(0.1, 0.1, 0.1)) bodyA = BulletRigidBodyNode('Box A') bodyA.setRestitution(1.0) bodyNP = self.worldNP.attachNewNode(bodyA) bodyNP.node().addShape(shape) bodyNP.setCollideMask(BitMask32.allOn()) bodyNP.setPos(pos) bodyNP.setHpr(turn) visNP = loader.loadModel('media/models/box.egg') visNP.setScale(Vec3(0.1, 0.1, 0.1)*2*scale) visNP.clearModelNodes() visNP.reparentTo(bodyNP) self.world.attachRigidBody(bodyA) #Box B shape = BulletBoxShape(Vec3(0.1,0.1,0.1)) bodyB = BulletRigidBodyNode('Box B') bodyB.setRestitution(1.0) bodyNP = self.worldNP.attachNewNode(bodyB) bodyNP.node().addShape(shape) bodyNP.setCollideMask(BitMask32.allOn()) bodyNP.setPos(pos) bodyNP.setHpr(turn) visNP = loader.loadModel('media/models/box.egg') visNP.setScale(Vec3(0.1,0.1,0.1)*2*scale) visNP.clearModelNodes() visNP.reparentTo(bodyNP) self.world.attachRigidBody(bodyB) # Slider frameA = TransformState.makePosHpr(Point3(0, 0, 0), Vec3(0, 0, 0)) frameB = TransformState.makePosHpr(Point3(0, 0, 0), Vec3(0, 0, 0)) slider = BulletSliderConstraint(bodyA, bodyB, frameA, frameB, True) slider.setDebugDrawSize(2.0) slider.setLowerLinearLimit(0) slider.setUpperLinearLimit(12) slider.setLowerAngularLimit(-90) slider.setUpperAngularLimit(-85) self.world.attachConstraint(slider) # Box C shape = BulletBoxShape(Vec3(1, 3, 0.1)) bodyC = BulletRigidBodyNode('Box C') bodyC.setRestitution(1.0) bodyNP = self.worldNP.attachNewNode(bodyC) bodyNP.node().addShape(shape) bodyNP.node().setMass(0.1) bodyNP.node().setDeactivationEnabled(False) bodyNP.setCollideMask(BitMask32.allOn()) bodyNP.setPos(Vec3(pos.getX() + 3, pos.getY() - 4, pos.getZ())) bodyNP.setHpr(turn) visNP = loader.loadModel('media/models/box.egg') visNP.setScale(Vec3(1, 3, 0.1)*2*scale) visNP.clearModelNodes() visNP.reparentTo(bodyNP) self.world.attachRigidBody(bodyC) bodyNP.node().setLinearVelocity(-100) # Slider frameA = TransformState.makePosHpr(Point3(0, 0, 0), Vec3(0, 0, 0)) frameB = TransformState.makePosHpr(Point3(0, 0, 0), Vec3(0, 0, 0)) slider = BulletSliderConstraint(bodyA, bodyC, frameA, frameB, True) slider.setDebugDrawSize(2.0) slider.setLowerLinearLimit(2) slider.setUpperLinearLimit(6) slider.setLowerAngularLimit(-90) slider.setUpperAngularLimit(-85) self.world.attachConstraint(slider) def setupObstacleFour(self, pos, scale, turn): #Start Here # Box A shape = BulletBoxShape(Vec3(0.01, 0.01, 0.01) * scale) bodyA = BulletRigidBodyNode('Box A') bodyNP = self.worldNP.attachNewNode(bodyA) bodyNP.node().addShape(shape) bodyNP.setCollideMask(BitMask32.allOn()) bodyNP.setPos(pos.getX(), pos.getY(), pos.getZ() + 4) #(0, 0, 4) visNP = loader.loadModel('media/models/box.egg') visNP.setScale(Vec3(0.01, 0.01, 0.01)*2*scale) visNP.clearModelNodes() visNP.reparentTo(bodyNP) self.world.attachRigidBody(bodyA) # Box B shape = BulletSphereShape(0.5*scale) bodyB = BulletRigidBodyNode('Sphere B') bodyNP = self.worldNP.attachNewNode(bodyB) bodyNP.node().addShape(shape) bodyNP.node().setMass(10.0) bodyNP.node().setDeactivationEnabled(False) bodyNP.setCollideMask(BitMask32.allOn()) bodyNP.setPos(pos.getX(), pos.getY(), pos.getZ() + 5) #(0, 0, 0.001) visNP = loader.loadModel('media/models/ball.egg') visNP.clearModelNodes() visNP.setScale(1.25*scale) visNP.reparentTo(bodyNP) bodyNP.node().setLinearVelocity(100) self.world.attachRigidBody(bodyB) # Cone frameA = TransformState.makePosHpr(Point3(0, 0, 0), Vec3(0, 0, 90)) frameB = TransformState.makePosHpr(Point3(2, 0, 0)*scale, Vec3(0, 0, 0)) cone = BulletConeTwistConstraint(bodyA, bodyB, frameA, frameB) cone.setDebugDrawSize(2.0) cone.setLimit(30, 90, 270, softness=1.0, bias=0.3, relaxation=10.0) self.world.attachConstraint(cone) # Box C shape = BulletBoxShape(Vec3(0.1, 0.1, 1)*scale) bodyC = BulletRigidBodyNode('Box C') bodyNP = self.worldNP.attachNewNode(bodyC) bodyNP.node().addShape(shape) bodyNP.node().setDeactivationEnabled(False) bodyNP.setCollideMask(BitMask32.allOn()) bodyNP.setPos(pos.getX(), pos.getY(), pos.getZ() + 3) self.world.attachRigidBody(bodyC) visNP = loader.loadModel('media/models/box.egg') visNP.setScale(Vec3(0.1, 0.1, 1)*2*scale) visNP.clearModelNodes() visNP.reparentTo(bodyNP) def setupObstacleSix(self, pos, scale, turn): #box A shape = BulletBoxShape(Vec3(0.1, 0.1, 0.1)*scale) bodyA = BulletRigidBodyNode('Box A') bodyNP= self.worldNP.attachNewNode(bodyA) bodyNP.node().addShape(shape) bodyNP.setCollideMask(BitMask32.allOff()) bodyNP.setPos(pos.getX()-2,pos.getY(),pos.getZ()+2.5)#-2,0,2.5) bodyNP.setHpr(turn) # Box B shape = BulletBoxShape(Vec3(2, 0.1, 3)*scale) bodyB = BulletRigidBodyNode('Box B') bodyNP = self.worldNP.attachNewNode(bodyB) bodyNP.node().addShape(shape) bodyNP.node().setMass(1.0) bodyNP.node().setLinearDamping(0.5) bodyNP.node().setDeactivationEnabled(False) bodyNP.setCollideMask(BitMask32.allOn()) bodyNP.setPos(pos.getX()-3,pos.getY(), pos.getZ())#, 0, 0) bodyNP.setHpr(turn) visNP = loader.loadModel('media/models/box.egg') visNP.setScale(Vec3(2, 0.1, 3)*2*scale) visNP.clearModelNodes() visNP.reparentTo(bodyNP) self.world.attachRigidBody(bodyB) # Hinge pivotA = Point3(-2, 0, -3) pivotB = Point3(-2, 0, -3) axisA = Vec3(0, 0, 1) axisB = Vec3(0, 0, 1) hinge = BulletHingeConstraint(bodyA, bodyB, pivotA, pivotB, axisA, axisB, True) hinge.setDebugDrawSize(2.0) hinge.setLimit(0,90, softness=1.0, bias=0.3, relaxation=1.0) self.world.attachConstraint(hinge) #box A shape = BulletBoxShape(Vec3(0.1, 0.1, 0.1)*scale) bodyA = BulletRigidBodyNode('Box A') bodyNP= self.worldNP.attachNewNode(bodyA) bodyNP.node().addShape(shape) bodyNP.setCollideMask(BitMask32.allOff()) bodyNP.setPos(pos.getX()+2,pos.getY(),pos.getZ()+2.5)#2,0,2.5) bodyNP.setHpr(turn) # Box B shape = BulletBoxShape(Vec3(2, 0.1, 3)*scale) bodyB = BulletRigidBodyNode('Box B') bodyNP = self.worldNP.attachNewNode(bodyB) bodyNP.node().addShape(shape) bodyNP.node().setMass(1.0) bodyNP.node().setLinearDamping(0.5) bodyNP.node().setDeactivationEnabled(False) bodyNP.setCollideMask(BitMask32.allOn()) bodyNP.setPos(pos.getX()+4, pos.getY(), pos.getZ())# 0, 0) bodyNP.setHpr(turn) visNP = loader.loadModel('media/models/box.egg') visNP.setScale(Vec3(2, 0.1, 3)*2*scale) visNP.clearModelNodes() visNP.reparentTo(bodyNP) self.world.attachRigidBody(bodyB) pivotA = Point3(2, 0, -3) pivotB = Point3(2, 0, -3) hinge = BulletHingeConstraint(bodyA, bodyB, pivotA, pivotB, axisA, axisB, True) hinge.setLimit(-90,0, softness=1.0, bias=0.3, relaxation=1.0) self.world.attachConstraint(hinge) def setupObstacleFive(self, pos, scale, turn): #box A shape = BulletBoxShape(Vec3(3, 0.1, 0.1)*scale) bodyA = BulletRigidBodyNode('Box A') bodyNP= self.worldNP.attachNewNode(bodyA) bodyNP.node().addShape(shape) bodyNP.setCollideMask(BitMask32.allOn()) bodyNP.setPos(pos) bodyNP.setHpr(turn) visNP = loader.loadModel('media/models/box.egg') visNP.setScale(Vec3(3, 0.1, 0.1)*2*scale) visNP.clearModelNodes() visNP.reparentTo(bodyNP) self.world.attachRigidBody(bodyA) # Box B shape = BulletBoxShape(Vec3(3, 2, 0.1)*scale) bodyB = BulletRigidBodyNode('Box B') bodyNP = self.worldNP.attachNewNode(bodyB) bodyNP.node().addShape(shape) bodyNP.node().setMass(1.0) bodyNP.node().setDeactivationEnabled(False) bodyNP.setCollideMask(BitMask32.allOn()) bodyNP.setPos(pos) bodyNP.setHpr(turn) visNP = loader.loadModel('media/models/box.egg') visNP.setScale(Vec3(3, 2, 0.1)*2*scale) visNP.clearModelNodes() visNP.reparentTo(bodyNP) self.world.attachRigidBody(bodyB) # Hinge pivotA = Point3(0, 0, 0) pivotB = Point3(0, 0, 5) axisA = Vec3(1, 0, 0) axisB = Vec3(1, 0, 0) hinge = BulletHingeConstraint(bodyA, bodyB, pivotA, pivotB, axisA, axisB, True) hinge.setDebugDrawSize(2.0) hinge.setLimit(-50,50, softness=0.5, bias=0.3, relaxation=0.6) self.world.attachConstraint(hinge) # Box C shape = BulletBoxShape(Vec3(0.1, 0.1, 0.9)*scale) bodyC = BulletRigidBodyNode('Box C') bodyNP = self.worldNP.attachNewNode(bodyC) bodyNP.node().addShape(shape) bodyNP.node().setMass(1.0) bodyNP.node().setDeactivationEnabled(False) bodyNP.setCollideMask(BitMask32.allOn()) bodyNP.setPos(pos) bodyNP.setHpr(turn) visNP = loader.loadModel('media/models/box.egg') visNP.setScale(Vec3(0.1, 0.1, 0.9)*2*scale) visNP.clearModelNodes() visNP.reparentTo(bodyNP) self.world.attachRigidBody(bodyC) pivotA = Point3(0, 0, -1.1) pivotB = Point3(0, 0, 1) hinge = BulletHingeConstraint(bodyA, bodyC, pivotA, pivotB, axisA, axisB, True) hinge.setLimit(-90,90, softness=1.0, bias=0.3, relaxation=1.0) self.world.attachConstraint(hinge) def setupGoal(self, pos): # Goal shape = BulletBoxShape(Vec3(1, 1, 1)) body = BulletRigidBodyNode('Flag') self.flagNP = self.worldNP.attachNewNode(body) self.flagNP.node().addShape(shape) self.flagNP.setCollideMask(BitMask32.allOn()) self.flagNP.setPos(pos) visNP = loader.loadModel('media/models/Flag.X') visNP.clearModelNodes() visNP.reparentTo(self.flagNP) self.world.attachRigidBody(body)
class Game(DirectObject): def __init__(self): base.setBackgroundColor(0.1, 0.1, 0.8, 1) base.setFrameRateMeter(True) base.cam.setPos(0, -20, 4) base.cam.lookAt(0, 0, 0) # Light alight = AmbientLight('ambientLight') alight.setColor(Vec4(0.5, 0.5, 0.5, 1)) alightNP = render.attachNewNode(alight) dlight = DirectionalLight('directionalLight') dlight.setDirection(Vec3(1, 1, -1)) dlight.setColor(Vec4(0.7, 0.7, 0.7, 1)) dlightNP = render.attachNewNode(dlight) render.clearLight() render.setLight(alightNP) render.setLight(dlightNP) # Input self.accept('escape', self.doExit) self.accept('r', self.doReset) self.accept('f1', self.toggleWireframe) self.accept('f2', self.toggleTexture) self.accept('f3', self.toggleDebug) self.accept('f5', self.doScreenshot) inputState.watchWithModifiers('forward', 'w') inputState.watchWithModifiers('left', 'a') inputState.watchWithModifiers('reverse', 's') inputState.watchWithModifiers('right', 'd') inputState.watchWithModifiers('turnLeft', 'q') inputState.watchWithModifiers('turnRight', 'e') # Task taskMgr.add(self.update, 'updateWorld') # Physics self.setup() # _____HANDLER_____ def doExit(self): self.cleanup() sys.exit(1) def doReset(self): self.cleanup() self.setup() def toggleWireframe(self): base.toggleWireframe() def toggleTexture(self): base.toggleTexture() def toggleDebug(self): if self.debugNP.isHidden(): self.debugNP.show() else: self.debugNP.hide() def doScreenshot(self): base.screenshot('Bullet') # ____TASK___ def processInput(self, dt): engineForce = 0.0 brakeForce = 0.0 if inputState.isSet('forward'): engineForce = 1000.0 brakeForce = 0.0 if inputState.isSet('reverse'): engineForce = 0.0 brakeForce = 100.0 if inputState.isSet('turnLeft'): self.steering += dt * self.steeringIncrement self.steering = min(self.steering, self.steeringClamp) if inputState.isSet('turnRight'): self.steering -= dt * self.steeringIncrement self.steering = max(self.steering, -self.steeringClamp) # Apply steering to front wheels self.vehicle.setSteeringValue(self.steering, 0); self.vehicle.setSteeringValue(self.steering, 1); # Apply engine and brake to rear wheels self.vehicle.applyEngineForce(engineForce, 2); self.vehicle.applyEngineForce(engineForce, 3); self.vehicle.setBrake(brakeForce, 2); self.vehicle.setBrake(brakeForce, 3); def update(self, task): dt = globalClock.getDt() self.processInput(dt) self.world.doPhysics(dt, 10, 0.008) #print self.vehicle.getWheel(0).getRaycastInfo().isInContact() #print self.vehicle.getWheel(0).getRaycastInfo().getContactPointWs() #print self.vehicle.getChassis().isKinematic() return task.cont def cleanup(self): self.world = None self.worldNP.removeNode() def setup(self): self.worldNP = render.attachNewNode('World') # World self.debugNP = self.worldNP.attachNewNode(BulletDebugNode('Debug')) self.debugNP.show() self.world = BulletWorld() self.world.setGravity(Vec3(0, 0, -9.81)) self.world.setDebugNode(self.debugNP.node()) # Plane shape = BulletPlaneShape(Vec3(0, 0, 1), 0) np = self.worldNP.attachNewNode(BulletRigidBodyNode('Ground')) np.node().addShape(shape) np.setPos(0, 0, -1) np.setCollideMask(BitMask32.allOn()) self.world.attachRigidBody(np.node()) # Chassis shape = BulletBoxShape(Vec3(0.6, 1.4, 0.5)) ts = TransformState.makePos(Point3(0, 0, 0.5)) np = self.worldNP.attachNewNode(BulletRigidBodyNode('Vehicle')) np.node().addShape(shape, ts) np.setPos(0, 0, 1) np.node().setMass(800.0) np.node().setDeactivationEnabled(False) self.world.attachRigidBody(np.node()) #np.node().setCcdSweptSphereRadius(1.0) #np.node().setCcdMotionThreshold(1e-7) # Vehicle self.vehicle = BulletVehicle(self.world, np.node()) self.vehicle.setCoordinateSystem(ZUp) self.world.attachVehicle(self.vehicle) self.yugoNP = loader.loadModel('models/yugo/yugo.egg') self.yugoNP.reparentTo(np) # Right front wheel np = loader.loadModel('models/yugo/yugotireR.egg') np.reparentTo(self.worldNP) self.addWheel(Point3( 0.70, 1.05, 0.3), True, np) # Left front wheel np = loader.loadModel('models/yugo/yugotireL.egg') np.reparentTo(self.worldNP) self.addWheel(Point3(-0.70, 1.05, 0.3), True, np) # Right rear wheel np = loader.loadModel('models/yugo/yugotireR.egg') np.reparentTo(self.worldNP) self.addWheel(Point3( 0.70, -1.05, 0.3), False, np) # Left rear wheel np = loader.loadModel('models/yugo/yugotireL.egg') np.reparentTo(self.worldNP) self.addWheel(Point3(-0.70, -1.05, 0.3), False, np) # Steering info self.steering = 0.0 # degree self.steeringClamp = 45.0 # degree self.steeringIncrement = 120.0 # degree per second def addWheel(self, pos, front, np): wheel = self.vehicle.createWheel() wheel.setNode(np.node()) wheel.setChassisConnectionPointCs(pos) wheel.setFrontWheel(front) wheel.setWheelDirectionCs(Vec3(0, 0, -1)) wheel.setWheelAxleCs(Vec3(1, 0, 0)) wheel.setWheelRadius(0.25) wheel.setMaxSuspensionTravelCm(40.0) wheel.setSuspensionStiffness(40.0) wheel.setWheelsDampingRelaxation(2.3) wheel.setWheelsDampingCompression(4.4) wheel.setFrictionSlip(100.0); wheel.setRollInfluence(0.1)
class Game(DirectObject): def __init__(self, model): self.model = model base.setBackgroundColor(0.1, 0.1, 0.8, 1) base.setFrameRateMeter(True) base.cam.setPos(0, -20, 4) base.cam.lookAt(0, 0, 0) w = WindowProperties() w.setFullscreen(False) w.setOrigin(5, 20) w.setSize(865, 800) base.win.requestProperties(w) # Light alight = AmbientLight('ambientLight') alight.setColor(Vec4(0.5, 0.5, 0.5, 1)) alightNP = render.attachNewNode(alight) dlight = DirectionalLight('directionalLight') dlight.setDirection(Vec3(1, 1, -1)) dlight.setColor(Vec4(0.7, 0.7, 0.7, 1)) dlightNP = render.attachNewNode(dlight) render.clearLight() render.setLight(alightNP) render.setLight(dlightNP) # Input self.accept('escape', self.doExit) self.accept('r', self.doReset) self.accept('f1', self.toggleWireframe) self.accept('f2', self.toggleTexture) self.accept('f3', self.toggleDebug) self.accept('f5', self.doScreenshot) inputState.watchWithModifiers('forward', 'w') inputState.watchWithModifiers('left', 'a') inputState.watchWithModifiers('reverse', 's') inputState.watchWithModifiers('right', 'd') inputState.watchWithModifiers('turnLeft', 'q') inputState.watchWithModifiers('turnRight', 'e') # Task taskMgr.add(self.update, 'updateWorld') # Physics self.setup() # _____HANDLER_____ def doExit(self): self.cleanup() sys.exit(1) def endLoop(self): self.penalized_distance = self.distance * (numpy.exp( -self.time_max_steering / self.total_time)) pickle.dump(self.penalized_distance, open('distance.p', 'wb')) sys.exit() #quit() #print("Distance was: ",self.distance) #os.execv(sys.executable,['python']+[__file__]) #taskMgr.running = False def doReset(self): self.cleanup() self.setup() def toggleWireframe(self): base.toggleWireframe() def toggleTexture(self): base.toggleTexture() def toggleDebug(self): if self.debugNP.isHidden(): self.debugNP.show() else: self.debugNP.hide() def doScreenshot(self): base.screenshot('Bullet') # ____TASK___ def calculate_moves(self): self.y = self.model.predict(self.x) #print(self.y) self.moves = self.y > 0 #+ self.model_offset # 0.5 #self.moves[0] = True # FIXME test def processInput(self, dt): engineForce = 0.0 brakeForce = 0.0 if self.moves[ 0]: #inputState.isSet('forward'): FIXME maybe engine and brake can be linked to self.y, rectified, continuous engineForce = 2000.0 # 1000. brakeForce = 0.0 if not self.moves[0]: #inputState.isSet('reverse'): engineForce = 200.0 #0.0 brakeForce = 100.0 self.steering = self.y[1] if self.moves[1]: self.steering = min(self.steering, self.steeringClamp) if not self.moves[1]: self.steering = max(self.steering, -self.steeringClamp) """ # FIXME option may be better if self.steering is fed into self.y[3] for 4th element if not self.moves[2]: # enabled steering lock if self.moves[1]:#inputState.isSet('turnLeft'): self.steering += dt * self.steeringIncrement self.steering = min(self.steering, self.steeringClamp) if not self.moves[1]:#inputState.isSet('turnRight'): self.steering -= dt * self.steeringIncrement self.steering = max(self.steering, -self.steeringClamp) """ """ if inputState.isSet('forward'): engineForce = 1000.0 brakeForce = 0.0 if inputState.isSet('reverse'): engineForce = 0.0 brakeForce = 100.0 if inputState.isSet('turnLeft'): self.steering += dt * self.steeringIncrement self.steering = min(self.steering, self.steeringClamp) if inputState.isSet('turnRight'): self.steering -= dt * self.steeringIncrement self.steering = max(self.steering, -self.steeringClamp) """ # Apply steering to front wheels self.vehicle.setSteeringValue(self.steering, 0) self.vehicle.setSteeringValue(self.steering, 1) # Apply engine and brake to rear wheels self.vehicle.applyEngineForce(engineForce, 2) self.vehicle.applyEngineForce(engineForce, 3) self.vehicle.setBrake(brakeForce, 2) self.vehicle.setBrake(brakeForce, 3) def check_collisions(self): """pFrom = render.getRelativePoint(self.yugoNP,Point3(0,0,0))#Point3(0,0,0) pFrom -= Point3(0,0,pFrom[2]) pRel = render.getRelativePoint(base.cam,self.yugoNP.getPos()) # FIXME THIS IS IT!! get rid of z component pRel -= Point3(0,0,pRel[2]) p45 = Point3(pRel[0] - pRel[1], pRel[1] + pRel[0],0) pn45 = Point3(pRel[0] + pRel[1], pRel[1] - pRel[0],0) #print(render.getRelativePoint(self.yugoNP,Point3(0,0,0))) #print(dir(self.yugoNP)) pTo = [pFrom + pn45, pFrom + pRel, pFrom + p45]#[pFrom + Vec3(-10,10,0)*999,pFrom + Vec3(0,10,0)*999,pFrom + Vec3(10,10,0)*999]# FIXME should be relative to front of car, getting cloe! #self.yugoNP.getPosDelta()*99999]#[Point3(-10,10,0) * 99999,Point3(0,10,0) * 99999,Point3(10,10,0) * 99999] #self.ray = CollisionRay(0,0,0,100,0,0) result = [self.world.rayTestClosest(pFrom,pt) for pt in pTo] #print(dir(self.yugoNP)) #print(result.getHitPos()) return tuple([res.getHitPos().length() for res in result]) """#queue = CollisionHandlerQueue() #traverser.addCollider(fromObject, queue) #traverser.traverse(render) #queue.sortEntries() #for entry in queue.getEntries(): #print(entry) #print(result.getHitPos()) #if result.getNode() != None: #print(self.yugoNP.getPos(result.getNode())) #print(self.cTrav) self.cTrav.traverse(render) entries = list(self.colHandler.getEntries()) #print(entries) entries.sort(key=lambda y: y.getSurfacePoint(render).getY()) #for entry in entries: print(entry.getFromNodePath().getName()) if entries: # and len(result) > 1: #print(entries) for r in entries: #print(r.getIntoNodePath().getName()) if r.getIntoNodePath().getName( ) == 'Plane' and r.getFromNodePath().getName() == 'yugo_box': self.endLoop() if r.getIntoNodePath().getName( ) == 'Plane' and r.getFromNodePath().getName() in [ 'ray%d' % i for i in range(self.n_rays) ]: #Box self.ray_col_vec_dict[ r.getFromNodePath().getName()].append( numpy.linalg.norm( list(r.getSurfacePoint( r.getFromNodePath()))[:-1])) self.ray_col_vec_dict = { k: (min(self.ray_col_vec_dict[k]) if len(self.ray_col_vec_dict[k]) >= 1 else 10000) for k in self.ray_col_vec_dict } self.x = numpy.array(list(self.ray_col_vec_dict.values())) #print(self.x) result = self.world.contactTest(self.yugoNP.node()) #print(result.getNumContacts()) #print(dir(self.yugoNP)) #return entries def check_prevPos(self): if len(self.prevPos) > 80: #print(self.prevPos) #print(numpy.linalg.norm(self.prevPos[-1] - self.prevPos[0])) if numpy.linalg.norm(self.prevPos[-1] - self.prevPos[0]) < 4.5: #print("ERROR") self.endLoop() del self.prevPos[0:len(self.prevPos) - 80] def update(self, task): self.prevPos.append(self.yugoNP.getPos(render)) dx = numpy.linalg.norm(self.prevPos[-1] - self.prevPos[-2]) self.distance += dx self.distance_text.setText('Distance=%.3f' % (self.distance)) #print(len(self.prevPos)) dt = globalClock.getDt() self.total_time += dt if abs(self.steering) == abs(self.steeringClamp): self.time_max_steering += dt self.time_text.setText('TotalTime=%.3f' % (self.total_time)) #self.time_maxsteer_text.setText('TotalTimeMaxSteer=%f'%(self.time_max_steering)) #self.penalized_distance = self.distance*(1.-numpy.exp(-self.time_max_steering/self.total_time)) if self.distance > 10000: self.endLoop() self.check_prevPos() self.speed = dx / dt self.speed_text.setText('Speed=%.3f' % (self.speed)) self.check_collisions() self.calculate_moves() self.model.plot_NN() #self.nn_image.setImage('neural_net_vis.png') self.ray_col_vec_dict = {k: [] for k in self.ray_col_vec_dict} self.processInput(dt) self.world.doPhysics(dt, 10, 0.008) # FIXME KEEP TRACK OF TOTAL DEGREES TURNED AND PENALIZE #self.doReset() #print(dir(result[1])) #print(numpy.linalg.norm(list(result[1].getSurfacePoint(result[1].getFromNodePath()))[:-1])) #base.camera.setPos(0,-40,10) #print self.vehicle.getWheel(0).getRaycastInfo().isInContact() #print self.vehicle.getWheel(0).getRaycastInfo().getContactPointWs() #print self.vehicle.getChassis().isKinematic() return task.cont def cleanup(self): self.world = None self.worldNP.removeNode() def setup(self): self.worldNP = render.attachNewNode('World') self.distance_text = OnscreenText( text='Distance=0', pos=(0.75, 0.85), scale=0.08, mayChange=1) #Directxxxxxx(distance='Distance=%d'%(0)) self.speed_text = OnscreenText( text='Speed=0', pos=(0.75, 0.78), scale=0.08, mayChange=1) #Directxxxxxx(distance='Distance=%d'%(0)) self.time_text = OnscreenText( text='TotalTime=0', pos=(0.75, 0.71), scale=0.08, mayChange=1) #Directxxxxxx(distance='Distance=%d'%(0)) #self.time_maxsteer_text = OnscreenText(text='TotalTimeMaxSteer=0', pos = (0.85,0.70), scale = 0.05, mayChange=1)#Directxxxxxx(distance='Distance=%d'%(0)) #self.nn_image = OnscreenImage(image='blank.png', pos= (0.85,0,0.15), scale=0.45) # http://dev-wiki.gestureworks.com/index.php/GestureWorksCore:Python_%26_Panda3D:_Getting_Started_II_(Hello_Multitouch)#8._Create_a_method_to_draw_touchpoint_data self.total_time = 0. self.time_max_steering = 0. # World self.debugNP = self.worldNP.attachNewNode(BulletDebugNode('Debug')) self.debugNP.show() self.world = BulletWorld() self.world.setGravity(Vec3(0, 0, -9.81)) self.world.setDebugNode(self.debugNP.node()) #terrain = GeoMipTerrain("mySimpleTerrain") #terrain.setHeightfield("./models/heightfield_2.png") #terrain.getRoot().reparentTo(self.worldNP)#render) #terrain.generate() # Plane shape = BulletPlaneShape(Vec3(0, 0, 1), 0) np = self.worldNP.attachNewNode(BulletRigidBodyNode('Ground')) np.node().addShape(shape) np.setPos(0, 0, -1) np.setCollideMask(BitMask32.allOn()) self.world.attachRigidBody(np.node()) #np = self.worldNP.attachNewNode(BulletRigidBodyNode('Track')) #np.node().setMass(5000.0) #np.setPos(3, 0, 10) #np.setCollideMask(BitMask32.allOn())#(0x0f)) #self.track = BulletVehicle(self.world, np.node()) #self.track.setCoordinateSystem(ZUp) self.track_np = loader.loadModel( 'models/race_track_2.egg' ) # https://discourse.panda3d.org/t/panda3d-and-bullet-physics/15724/10 self.track_np.setPos(-72, -7, -3.5) self.track_np.setScale(10) self.track_np.reparentTo(render) self.track_np.setCollideMask(BitMask32.allOn()) #(0))#.allOn()) self.world.attachRigidBody(np.node()) self.track_np = np #self.track_np.show() # Chassis shape = BulletBoxShape(Vec3(0.6, 1.4, 0.5)) ts = TransformState.makePos(Point3(0, 0, 0.5)) np = self.worldNP.attachNewNode(BulletRigidBodyNode('Vehicle')) np.node().addShape(shape, ts) np.setPos(0, 0, 0.05) np.node().setMass(800.0) np.node().setDeactivationEnabled(False) self.world.attachRigidBody(np.node()) #np.node().setCcdSweptSphereRadius(1.0) #np.node().setCcdMotionThreshold(1e-7) self.cTrav = CollisionTraverser() # Vehicle self.vehicle = BulletVehicle(self.world, np.node()) self.vehicle.setCoordinateSystem(ZUp) self.yugoNP = loader.loadModel('models/yugo/yugo.egg') self.yugoNP.setCollideMask(BitMask32(0)) #.allOn()) self.yugoNP.reparentTo(np) self.colHandler = CollisionHandlerQueue() # travel distance self.distance = 0. """self.sphere = CollisionSphere(0,0,0,2) self.sphere_col = CollisionNode('yugo') self.sphere_col.addSolid(self.sphere) self.sphere_col.setFromCollideMask(BitMask32.allOn()) self.sphere_col_np = self.yugoNP.attachNewNode(self.sphere_col) self.cTrav.addCollider(self.sphere_col_np,self.colHandler) self.sphere_col_np.show()""" self.yugo_col = CollisionNode('yugo_box') self.yugo_col.addSolid(CollisionBox(Point3(0, 0, 0.7), 0.9, 1.6, 0.05)) self.yugo_col.setFromCollideMask(BitMask32(1)) self.box_col_np = self.yugoNP.attachNewNode(self.yugo_col) self.cTrav.addCollider(self.box_col_np, self.colHandler) self.box_col_np.show() self.ray_col_np = {} self.ray_col_vec_dict = {} self.n_rays = self.model.shape[0] for i, ray_dir in enumerate( numpy.linspace(-numpy.pi / 4, numpy.pi / 4, self.n_rays)): # populate collision rays #print(ray_dir) self.ray = CollisionRay() y_dir, x_dir = numpy.cos(ray_dir), numpy.sin(ray_dir) self.ray.setOrigin(1.3 * x_dir, 1.3 * y_dir, 0.5) self.ray.setDirection(x_dir, y_dir, 0) self.ray_col = CollisionNode('ray%d' % (i)) self.ray_col.addSolid(self.ray) self.ray_col.setFromCollideMask( BitMask32.allOn()) #(0x0f))#CollideMask.bit(0) #self.ray_col.setIntoCollideMask(CollideMask.allOff()) self.ray_col_np['ray%d' % (i)] = self.yugoNP.attachNewNode( self.ray_col) self.cTrav.addCollider(self.ray_col_np['ray%d' % (i)], self.colHandler) self.ray_col_np['ray%d' % (i)].show() self.ray_col_vec_dict['ray%d' % (i)] = [] self.world.attachVehicle(self.vehicle) self.cTrav.showCollisions(render) # FIXME base.camera.reparentTo(self.yugoNP) # Right front wheel np = loader.loadModel('models/yugo/yugotireR.egg') np.reparentTo(self.worldNP) self.addWheel(Point3(0.70, 1.05, 0.3), True, np) # Left front wheel np = loader.loadModel('models/yugo/yugotireL.egg') np.reparentTo(self.worldNP) self.addWheel(Point3(-0.70, 1.05, 0.3), True, np) # Right rear wheel np = loader.loadModel('models/yugo/yugotireR.egg') np.reparentTo(self.worldNP) self.addWheel(Point3(0.70, -1.05, 0.3), False, np) # Left rear wheel np = loader.loadModel('models/yugo/yugotireL.egg') np.reparentTo(self.worldNP) self.addWheel(Point3(-0.70, -1.05, 0.3), False, np) # Steering info self.steering = 0.0 # degree self.steeringClamp = 38.0 #45.0 # degree self.steeringIncrement = 105.0 #120.0 # degree per second # add previous positions self.prevPos = [] self.prevPos.append(self.yugoNP.getPos(render)) self.model_offset = 0.5 if self.model.activation == 'relu' else 0. # Box """ for i,j in [(0,8),(-3,5),(6,-5),(8,3),(-4,-4),(0,0)]: shape = BulletBoxShape(Vec3(0.5, 0.5, 0.5)) # https://discourse.panda3d.org/t/wall-collision-help/23606 np = self.worldNP.attachNewNode(BulletRigidBodyNode('Box')) np.node().setMass(1.0) np.node().addShape(shape) np.setPos(i, j, 2) np.setCollideMask(BitMask32.allOn())#(0x0f)) self.world.attachRigidBody(np.node()) self.boxNP = np #self.colHandler2 = CollisionHandlerQueue() visualNP = loader.loadModel('models/box.egg') visualNP.reparentTo(self.boxNP) #self.cTrav.addCollider(self.boxNP,self.colHandler) """ """ aNode = CollisionNode("TheRay") self.ray = CollisionRay() self.ray.setOrigin( self.yugoNP.getPos() ) self.ray.setDirection( Vec3(0, 10, 0) ) #self.ray.show() aNodePath = self.yugoNP.attachNewNode( CollisionNode("TheRay") ) aNodePath.node().addSolid(self.ray) aNodePath.show() """ #aNode.addSolid(self.ray) #self.ray = CollisionRay(0,0,0,10,0,0) #self.ray.reparentTo(self.yugoNP) #self.rayColl = CollisionNode('PlayerRay') #self.rayColl.addSolid(self.ray) #self.playerRayNode = self.yugoNP.attachNewNode( self.rayColl ) #self.playerRayNode.show() #base.myTraverser.addCollider (self.playerRayNode, base.floor) #base.floor.addCollider( self.playerRayNode, self.yugoNP) """ MyEvent=CollisionHandlerFloor() MyEvent.setReach(100) MyEvent.setOffset(15.0) aNode = CollisionNode("TheRay") ray = CollisionRay() ray.setOrigin( self.boxNP.getPos() ) ray.setDirection( Vec3(10, 0, 0) ) aNode.addSolid(ray) aNodePath = MyModel.attachNewNode( aNode ) Collision = ( aNode, "TheRay" ) Collision[0].setFromCollideMask( BitMask32.bit( 1 ) ) """ def addWheel(self, pos, front, np): wheel = self.vehicle.createWheel() wheel.setNode(np.node()) wheel.setChassisConnectionPointCs(pos) wheel.setFrontWheel(front) wheel.setWheelDirectionCs(Vec3(0, 0, -1)) wheel.setWheelAxleCs(Vec3(1, 0, 0)) wheel.setWheelRadius(0.25) wheel.setMaxSuspensionTravelCm(40.0) wheel.setSuspensionStiffness(40.0) wheel.setWheelsDampingRelaxation(2.3) wheel.setWheelsDampingCompression(4.4) wheel.setFrictionSlip(100.0) wheel.setRollInfluence(0.1)
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
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")