def rayHit(pfrom, pto, geom): """ NOTE: this function is quite slow find the nearest collision point between vec(pto-pfrom) and the mesh of nodepath :param pfrom: starting point of the ray, Point3 :param pto: ending point of the ray, Point3 :param geom: meshmodel, a panda3d datatype :return: None or Point3 author: weiwei date: 20161201 """ bulletworld = BulletWorld() facetmesh = BulletTriangleMesh() facetmesh.addGeom(geom) facetmeshnode = BulletRigidBodyNode('facet') bullettmshape = BulletTriangleMeshShape(facetmesh, dynamic=True) bullettmshape.setMargin(0) facetmeshnode.addShape(bullettmshape) bulletworld.attachRigidBody(facetmeshnode) result = bulletworld.rayTestClosest(pfrom, pto) if result.hasHit(): return result.getHitPos() else: return None
def rayHit(pfrom, pto, geom): """ NOTE: this function is quite slow find the nearest BulletCD point between vec(pto-pfrom) and the mesh of nodepath :param pfrom: starting point of the ray, Point3 :param pto: ending point of the ray, Point3 :param geom: meshmodel, a panda3d datatype :return: None or Point3 author: weiwei date: 20161201 """ bulletworld = BulletWorld() facetmesh = BulletTriangleMesh() facetmesh.addGeom(geom) facetmeshnode = BulletRigidBodyNode('facet') bullettmshape = BulletTriangleMeshShape(facetmesh, dynamic=True) bullettmshape.setMargin(0) facetmeshnode.addShape(bullettmshape) bulletworld.attachRigidBody(facetmeshnode) result = bulletworld.rayTestClosest(pfrom, pto) if result.hasHit(): return result.getHitPos() else: return None
class Cory2SrvNode(CarSrv): 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) # collision visNP = loader.loadModel('../models/coryf2.egg') visNP.clearModelNodes() visNP.reparentTo(render) pos = (7., 60.0, 3.8) 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())
class TestApplication: def __init__(self): self.setupPhysics() self.clock_ = ClockObject() def sim(self): while True: try: time.sleep(LOOP_DELAY) self.clock_.tick() self.physics_world_.doPhysics(self.clock_.getDt(), 5, 1.0/180.0) # printing location of first box print "Box 0: %s"%(str(self.boxes_[0].getPos())) except KeyboardInterrupt: print "Simulation finished" sys.exit() def setupPhysics(self): # setting up physics world and parent node path self.physics_world_ = BulletWorld() self.world_node_ = NodePath() self.physics_world_.setGravity(Vec3(0, 0, -9.81)) # setting up ground self.ground_ = self.world_node_.attachNewNode(BulletRigidBodyNode('Ground')) self.ground_.node().addShape(BulletPlaneShape(Vec3(0, 0, 1), 0)) self.ground_.setPos(0,0,0) self.ground_.setCollideMask(BitMask32.allOn()) self.physics_world_.attachRigidBody(self.ground_.node()) self.boxes_ = [] num_boxes = 20 side_length = 0.2 size = Vec3(side_length,side_length,side_length) start_pos = Vec3(-num_boxes*side_length,0,10) for i in range(0,20): self.addBox("name %i"%(i),size,start_pos + Vec3(i*2*side_length,0,0)) def addBox(self,name,size,pos): # Box (dynamic) box = self.world_node_.attachNewNode(BulletRigidBodyNode(name)) box.node().setMass(1.0) box.node().addShape(BulletBoxShape(size)) box.setPos(pos) box.setCollideMask(BitMask32.allOn()) self.physics_world_.attachRigidBody(box.node()) self.boxes_.append(box)
def task_1_Bullet_Hello_World(): strOf_FuncName = "task_1_Bullet_Hello_World" '''################### step : 1 opening, vars ###################''' print() print ("[%s:%d] starting : %s (time=%s)" % ( os.path.basename(os.path.basename(libs.thisfile())) , libs.linenum() , strOf_FuncName , libs.get_TimeLabel_Now() ) ) '''################### step : 2 ###################''' base.cam.setPos(0, -10, 0) base.cam.lookAt(0, 0, 0) # World world = BulletWorld() world.setGravity(Vec3(0, 0, -9.81)) # Plane shape = BulletPlaneShape(Vec3(0, 0, 1), 1) node = BulletRigidBodyNode('Ground') node.addShape(shape) np = render.attachNewNode(node) np.setPos(0, 0, -2) world.attachRigidBody(node) # Box shape = BulletBoxShape(Vec3(0.5, 0.5, 0.5)) node = BulletRigidBodyNode('Box') node.setMass(1.0) node.addShape(shape) np = render.attachNewNode(node) np.setPos(0, 0, 2) world.attachRigidBody(node) model = loader.loadModel('models/box.egg') model.flattenLight() model.reparentTo(np) # Update def update(task): dt = globalClock.getDt() world.doPhysics(dt) return task.cont taskMgr.add(update, 'update') base.run() '''###################
def genAvailableFAGsSgl(base, basepath, baseMat4, objpath, objMat4, gdb, handpkg): """ find the collision freeairgrips of objpath without considering rotation :param base: panda base :param basepath: the path of base object :param objpath: the path of obj object, the object to be assembled :param baseMat4, objMat4: all in world coordinates, not relative :param gdb: grasp db :param handpkg: hand package :return: [assgripcontacts, assgripnormals, assgriprotmat4s, assgripjawwidth, assgripidfreeair] author: weiwei date: 20170307 """ bulletworld = BulletWorld() robothand = handpkg.newHandNM(hndcolor=[1, 0, 0, .1]) basetrimesh = trimesh.load_mesh(basepath) basenp = pg.packpandanp(basetrimesh.vertices, basetrimesh.face_normals, basetrimesh.faces) basenp.setMat(baseMat4) basebullnode = cd.genCollisionMeshNp(basenp, base.render) bulletworld.attachRigidBody(basebullnode) dbobjname = os.path.splitext(os.path.basename(objpath))[0] objfag = F*g(gdb, dbobjname, handpkg) assgripcontacts = [] assgripnormals = [] assgriprotmat4s = [] assgripjawwidth = [] assgripidfreeair = [] for i, rotmat in enumerate(objfag.freegriprotmats): assgrotmat = rotmat*objMat4 robothand.setMat(assgrotmat) # detect the collisions when hand is open! robothand.setJawwidth(robothand.jawwidthopen) hndbullnode = cd.genCollisionMeshMultiNp(robothand.handnp) result0 = bulletworld.contactTest(hndbullnode) robothand.setJawwidth(objfag.freegripjawwidth[i]) hndbullnode = cd.genCollisionMeshMultiNp(robothand.handnp) result1 = bulletworld.contactTest(hndbullnode) if (not result0.getNumContacts()) and (not result1.getNumContacts()): cct0 = objMat4.xformPoint(objfag.freegripcontacts[i][0]) cct1 = objMat4.xformPoint(objfag.freegripcontacts[i][1]) cctn0 = objMat4.xformPoint(objfag.freegripnormals[i][0]) cctn1 = objMat4.xformPoint(objfag.freegripnormals[i][1]) assgripcontacts.append([cct0, cct1]) assgripnormals.append([cctn0, cctn1]) assgriprotmat4s.append(assgrotmat) assgripjawwidth.append(objfag.freegripjawwidth[i]) assgripidfreeair.append(objfag.freegripids[i]) return [assgripcontacts, assgripnormals, assgriprotmat4s, assgripjawwidth, assgripidfreeair]
def genAvailableFAGsSgl(base, basepath, baseMat4, objpath, objMat4, gdb, handpkg): """ find the collision freeairgrips of objpath without considering rotation :param base: panda base :param basepath: the path of base object :param objpath: the path of obj object, the object to be assembled :param baseMat4, objMat4: all in world coordinates, not relative :param gdb: grasp db :param handpkg: hand package :return: [assgripcontacts, assgripnormals, assgriprotmat4s, assgripjawwidth, assgripidfreeair] author: weiwei date: 20170307 """ bulletworld = BulletWorld() robothand = handpkg.newHandNM(hndcolor=[1, 0, 0, .1]) basetrimesh = trimesh.load_mesh(basepath) basenp = pg.packpandanp(basetrimesh.vertices, basetrimesh.face_normals, basetrimesh.faces) basenp.setMat(baseMat4) basebullnode = cd.genCollisionMeshNp(basenp, base.render) bulletworld.attachRigidBody(basebullnode) dbobjname = os.path.splitext(os.path.basename(objpath))[0] objfag = F*g(gdb, dbobjname, handpkg) assgripcontacts = [] assgripnormals = [] assgriprotmat4s = [] assgripjawwidth = [] assgripidfreeair = [] for i, rotmat in enumerate(objfag.freegriprotmats): assgrotmat = rotmat*objMat4 robothand.setMat(assgrotmat) # detect the collisions when hand is open! robothand.setJawwidth(robothand.jawwidthopen) hndbullnode = cd.genCollisionMeshMultiNp(robothand.handnp) result0 = bulletworld.contactTest(hndbullnode) robothand.setJawwidth(objfag.freegripjawwidth[i]) hndbullnode = cd.genCollisionMeshMultiNp(robothand.handnp) result1 = bulletworld.contactTest(hndbullnode) if (not result0.getNumContacts()) and (not result1.getNumContacts()): cct0 = objMat4.xformPoint(objfag.freegripcontacts[i][0]) cct1 = objMat4.xformPoint(objfag.freegripcontacts[i][1]) cctn0 = objMat4.xformPoint(objfag.freegripnormals[i][0]) cctn1 = objMat4.xformPoint(objfag.freegripnormals[i][1]) assgripcontacts.append([cct0, cct1]) assgripnormals.append([cctn0, cctn1]) assgriprotmat4s.append(assgrotmat) assgripjawwidth.append(objfag.freegripjawwidth[i]) assgripidfreeair.append(objfag.freegripids[i]) return [assgripcontacts, assgripnormals, assgriprotmat4s, assgripjawwidth, assgripidfreeair]
class TestWorld: def __init__(self): load_prc_file_data("", """ stm-max-chunk-count 1024 stm-max-views 20 """) self.world = None self.worldNP = None # Setup everything self.setupWorld() # Get the player CreatePlayer.CreatePlayer(self.world, self.worldNP) def setupWorld(self): # Set the background to be black base.win.setClearColor((0, 0, 0 ,1)) # Load Chris's Room self.chrisRoom = loader.loadModel("Resources/models/ROOMS/ChrisRoom.egg") self.chrisRoom.reparentTo(render) self.chrisRoom.setPos(0, 0, -0.1) self.chrisRoom.setScale(0.4) # Load his laptop onto the desk self.chrisLaptop = loader.loadModel("Resources/models/OBJ/ChrisLaptop.egg") self.chrisLaptop.reparentTo(render) self.chrisLaptop.setPosHpr(-5.5, 1.5, 1.0, 90, 0, 0) self.chrisLaptop.setScale(0.8) # TODO: Set the screen texture #screenTexture = loader.loadTexture("Resources/maps/skypeChat.png") #self.chrisLaptopScreen = self.chrisLaptop.find("**/screen") #self.chrisLaptopScreen.setTexture(screenTexture, 1) self.world = BulletWorld() self.world.setGravity(Vec3(0, 0, -9.81)) self.worldNP = render.attachNewNode('World') self.terrain_np = render.attach_new_node("terrain") # Ground ground = BulletPlaneShape(Vec3(0, 0, 1), 0) #img = PNMImage(Filename('models/elevation2.png')) #shape = BulletHeightfieldShape(img, 1.0, ZUp) np = self.worldNP.attachNewNode(BulletRigidBodyNode('Ground')) np.node().addShape(ground) np.setPos(0, 0, 0) np.setCollideMask(BitMask32.allOn()) self.world.attachRigidBody(np.node())
class gameMechanics(DirectObject): def __init__(self, **kwargs): super(gameMechanics, self).__init__() self.joy = joystick() self.world = BulletWorld() self.world.setGravity(Vec3(0, 0, -9.81)) self.arena = kwargs['environment'] self.worldNp = render.attachNewNode('world') self.worldNp.setScale(3) self.arena.setupPhysics(world=self.worldNp, mask=BitMask32.allOn()) self.world.setDebugNode(self.arena.debugNP.node()) self.world.setDebugNode(self.arena.debugNP.node()) self.listOfPlayers = kwargs['playerslist'] self.setupCollisions(playerslist=self.listOfPlayers) self.world.attachRigidBody(self.arena.mainNp.node()) for model in self.arena.modelList: self.world.attachRigidBody(model.nodePath.node()) self.numofjoy = 0 taskMgr.add(self.mech_update, 'mechupdate') self.listOfPlayers[1].characterNP.setY(self.listOfPlayers[0], -5) # base.disableMouse() def setupCollisions(self, **kwargs): for player in self.listOfPlayers: player.setupPhysics(worldnp=self.worldNp, world=self.world) player.getModel() def mech_update(self, task): for player in self.listOfPlayers: player.setup_collision dt = globalClock.getDt() self.world.doPhysics(dt, 80, 1 / 180) self.cameraTask(task) self.joy.get_input() return task.cont def cameraTask(self, task): pass
def _set_physics(self): """Set the world physics. Returns: panda3d.bullet.BulletWorld: Physical world. """ world = BulletWorld() world.setGravity(Vec3(0, 0, -0.5)) shape = BulletPlaneShape(Vec3(0, 0, 1), 0.02) node = BulletRigidBodyNode("Ground") node.addShape(shape) render.attachNewNode(node) # noqa: F821 world.attachRigidBody(node) base.train.set_physics(world) # noqa: F821 return world
class GameState(ShowBase): def __init__(self): loadPrcFile("server-config.prc") ShowBase.__init__(self) self.__rotations = {} # Panda pollutes the global namespace. Some of the extra globals can be referred to in nicer ways # (for example self.render instead of render). The globalClock object, though, is only a global! We # create a reference to it here, in a way that won't upset PyFlakes. self.globalClock = __builtins__["globalClock"] # Set up physics: the ground plane and the capsule which represents the player. self.world = BulletWorld() # The ground first: shape = BulletPlaneShape(Vec3(0, 0, 1), 0) node = BulletRigidBodyNode("Ground") node.addShape(shape) np = self.render.attachNewNode(node) np.setPos(0, 0, 0) self.world.attachRigidBody(node) # Create a task to update the scene regularly. self.taskMgr.add(self.update, "UpdateTask") # Update the scene by turning objects if necessary, and processing physics. def update(self, task): asyncore.loop(timeout=0.1, use_poll=True, count=1) Player.update_all() for node, angular_velocity in self.__rotations.iteritems(): node.setAngularMovement(angular_velocity) dt = self.globalClock.getDt() self.world.doPhysics(dt) return task.cont def set_angular_velocity(self, node, angular_velocity): if angular_velocity != 0: self.__rotations[node] = angular_velocity elif node in self.__rotations: del self.__rotations[node]
def __init__(self, world: BulletWorld, entity: Entity, shape, name, rotation, mass): super().__init__(name) self.setMass(mass) self.addShape(shape) self.np = application.base.render.attachNewNode(self) world.attachRigidBody(self) if entity.parent: self.np.reparent_to(entity.parent) if None in rotation: hpr = entity.getHpr() for x in range(len(hpr)): rotation[x] = hpr[x] self.np.setHpr(Vec3(*rotation)) self.np.setPos(entity.x, entity.y, entity.z) entity.reparent_to(self.np)
class Game(ShowBase): def __init__(self): ShowBase.__init__(self) #Camera pos self.cam.setPos(0, 0, 20) self.world_create(1000, 1000) def world_create(self, sizex, sizey): self.worldNP = self.render.attachNewNode('World') self.world = BulletWorld() self.world.setGravity(Vec3(0, 0, -9.81)) self.shape = BulletPlaneShape(Vec3(0, 0, 1), 1) self.groundNP = self.worldNP.attachNewNode(BulletRigidBodyNode('Floor')) self.groundNP.node().addShape(self.shape) self.world.attachRigidBody(self.groundNP.node()) # Load model self.ground = self.loader.loadModel("Models/floor_basic.egg") self.ground.reparentTo(self.groundNP) # Scale and position model self.ground.setScale(sizex, sizey, 0) self.ground.setPos(0, 0, 0) self.taskMgr.add(self.update, 'update') def update(self, task): dt = globalClock.getDt() self.world.doPhysics(dt, 10, 1.0/180.0) return Task.cont
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)
class GameScene(ShowBase): def __init__(self): ShowBase.__init__(self) self.level = 1 # self.setupScene() def getPhycisWorld(self): return self.world def setupScene(self): # Set Scene Background Color base.setBackgroundColor(0.1, 0.1, 0.8, 1) base.setFrameRateMeter(True) # TODO: remove this Add Camera self.cameraHeight = DEFAULT_CAMERA_HEIGHT # Add Physics to GameScene self.addPhysicsWorld() # Add lightings to GameScene self.addLights() # Load Game Map self.loadMap() # Add Character self.addCharacters() # Add Sound Effects self.addSoundEffects() def addCharacters(self): self.addPlayer() self.addShield() self.addGuard() def addSoundEffects(self): self.completeLevelSound = base.loader.loadSfx( "sounds/completeLevel.mp3") self.deadthSound = base.loader.loadSfx("sounds/dead.wav") # self.completeLevelSound.setVolume(5) # self.completeLevelSound.play() self.hitSound = base.loader.loadSfx("sounds/hit.mp3") self.pushSound = base.loader.loadSfx("sounds/push.wav") self.jumpSound = base.loader.loadSfx("sounds/Jump.wav") self.pickupSpringSound = base.loader.loadSfx( "sounds/Pickup_Spring.wav") self.backgroundSound = base.loader.loadSfx("sounds/background.mp3") self.backgroundSound.setVolume(0.5) self.backgroundSound.setLoop(True) self.backgroundSound.play() # ========== Functions setting up GameScene def loadMap(self): self.addSky() self.addGround() self.addWalls() self.addStages() self.addSpings() def addPhysicsWorld(self): # Create Physics World self.world = BulletWorld() # Add Gravity towards ground self.world.setGravity(Vec3(0, 0, -9.81)) def addLights(self): # Add Lights 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) self.render.clearLight() self.render.setLight(alightNP) self.render.setLight(dlightNP) def addSky(self): self.blue_sky_sphere = self.loader.loadModel( "models/blue_sky_sphere/blue_sky_sphere.egg") self.blue_sky_sphere.reparentTo(self.render) self.blue_sky_sphere.setScale(0.04, 0.04, 0.04) self.blue_sky_sphere.setPos(0, 0, 0) def addGround(self): self.garden = self.loader.loadModel( "models/garden/garden.egg") self.garden.reparentTo(self.render) self.garden.setScale(2, 2, 2) self.garden.setPos(0, 0, 0) shape = BulletPlaneShape(Vec3(0, 0, 1), 0) floorNP = self.render.attachNewNode(BulletRigidBodyNode('Ground')) floorNP.node().addShape(shape) floorNP.setPos(0, 0, 0) floorNP.setCollideMask(BitMask32.allOn()) self.world.attachRigidBody(floorNP.node()) def addWalls(self): def addWall(size, posX, posY): shape = BulletBoxShape(size) wallNP = self.render.attachNewNode(BulletRigidBodyNode('wall')) wallNP.node().addShape(shape) wallNP.setPos(posX, posY, size.getZ()) wallNP.setCollideMask(BitMask32.allOn()) if (posY is 0): left = -(size.getY()) right = -left pos = left - 5 while pos <= right: wallModel = loader.loadModel('models/fence.egg') wallModel.reparentTo(wallNP) wallModel.setPos(0, pos, -(size.getZ())) wallModel.setScale(1, 1, 1) wallModel.setH(90) pos += 13.5 else: left = -(size.getX()) right = -left pos = left - 5 while pos <= right: wallModel = loader.loadModel('models/fence.egg') wallModel.reparentTo(wallNP) wallModel.setPos(pos, 0, -(size.getZ())) wallModel.setScale(1, 1, 1) pos += 13.5 self.world.attachRigidBody(wallNP.node()) addWall(Vec3(1, 90, 5), 80, 0) addWall(Vec3(1, 90, 5), -80, 0) addWall(Vec3(100, 1, 5), 0, 100) addWall(Vec3(100, 1, 5), 0, -100) def addStages(self): for stage in STAGE_POS_LIST: boxSize = stage[0] pos = stage[1] name = stage[2] # create a BodyNode and attach to render become a NodePath objNP = self.render.attachNewNode(BulletRigidBodyNode(name)) # attach the BodyNode inside the NodePath to physics world self.world.attachRigidBody(objNP.node()) # set the properties of the NodePath # - shape shape = BulletBoxShape(boxSize) objNP.node().addShape(shape) # - position objNP.setPos(pos.getX(), pos.getY(), pos.getZ()) # - collideMask objNP.setCollideMask(BitMask32.allOn()) # - model objModel = self.loader.loadModel( "models/brick-cube/brick.egg") objModel.setScale(boxSize.getX() * 2, boxSize.getY() * 2, boxSize.getZ() * 2) objModel.setPos(0, 0, boxSize.getZ() / -1) objModel.reparentTo(objNP) # - texture ts = TextureStage.getDefault() texture = objModel.getTexture() objModel.setTexOffset(ts, -0.5, -0.5) objModel.setTexScale(ts, boxSize.getX() / 2.0, boxSize.getY() / 2.0, boxSize.getZ() / 2.0) def addSpings(self): self.springs = [] for pos in SPRING_LIST: print "add spring #{} at: {}".format(len(self.springs), pos) shape = BulletBoxShape(Vec3(0.3, 0.3, 0.8)) node = BulletGhostNode('Spring' + str(len(self.springs))) node.addShape(shape) springNP = self.render.attachNewNode(node) springNP.setCollideMask(BitMask32.allOff()) springNP.setPos(pos.getX(), pos.getY(), pos.getZ() + 3.4) modelNP = loader.loadModel('models/spring/spring.egg') modelNP.reparentTo(springNP) modelNP.setScale(1, 1, 1) modelNP.setPos(0, 0, -1) self.world.attachGhost(node) self.springs.append(springNP) # ========== Functions add Characters into GameScene def addPlayer(self): self.player = Player(self.world, self.render, "Bricker", PLAYER_POSITIONS[self.level]) def addShield(self): counter = 0 self.shields = [] for position in SHIELD_POSITIONS: shield = Shield(self.world, self.render, "Shield_{}".format(counter), position) counter+=1 self.shields.append(shield) def addGuard(self): counter = 0 self.guards = [] for position in GUARD_POSITIONS: guard = Guard(self.world, self.render, "Guard_{}".format(counter), position) counter+=1 self.guards.append(guard)
class BulletSim(object): def __init__(self, base, objpath, bdebug=False): self.base = base self.smiley = loader.loadModel(objpath) self.smileyCount = 0 self.setupBullet() self.addGround() # self.addWalls() self.partlist = [] taskMgr.doMethodLater(.1, self.addSmiley, "AddSmiley") taskMgr.add(self.updateBlt, "UpdateBlt") if bdebug: debugNode = BulletDebugNode('Debug') debugNode.showWireframe(True) debugNode.showConstraints(True) debugNode.showBoundingBoxes(False) debugNode.showNormals(False) bullcldrnp = self.base.render.attachNewNode("bulletcollider") debugNP = bullcldrnp.attachNewNode(debugNode) debugNP.show() self.bltWorld.setDebugNode(debugNP.node()) def setupBullet(self): self.bltWorld = BulletWorld() self.bltWorld.setGravity(Vec3(0, 0, -9.81)) def addGround(self): boxx = 500.0 boxy = 500.0 boxz = 1.0 shape = BulletBoxShape(Vec3(boxx, boxy, boxz)) # the parameters are half extends node = BulletRigidBodyNode('Ground') node.addShape(shape) self.bltWorld.attachRigidBody(node) pg.plotBox(self.base.render, pos=[0, 0, -1.0], x=boxx * 2.0, y=boxy * 2.0, z=boxz * 2.0, rgba=None) # shape = BulletPlaneShape(Vec3(0, 0, 1), 0.0) # node = BulletRigidBodyNode('Ground') # node.addShape(shape) # np = self.base.render.attachNewNode(node) # np.setPos(0, 0, 0) # self.bltWorld.attachRigidBody(node) def addWalls(self): """ add walls :return: """ # x+ boxx = 2.0 boxy = 50.0 boxz = 100.0 boxpx = 50.0 boxpy = 0.0 boxpz = 50.0 shape = BulletBoxShape(Vec3(boxx, boxy, boxz)) # the parameters are half extends node = BulletRigidBodyNode('Wallx+') node.addShape(shape) node.setTransform(TransformState.makePos(VBase3(boxpx, boxpy, boxpz))) self.bltWorld.attachRigidBody(node) pg.plotBox(self.base.render, pos=[boxpx, boxpy, boxpz], x=boxx * 2.0, y=boxy * 2.0, z=boxz * 2.0, rgba=None) # x- boxx = 2.0 boxy = 50.0 boxz = 100.0 boxpx = -50.0 boxpy = 0.0 boxpz = 50.0 shape = BulletBoxShape(Vec3(boxx, boxy, boxz)) # the parameters are half extends node = BulletRigidBodyNode('Wallx-') node.addShape(shape) node.setTransform(TransformState.makePos(VBase3(boxpx, boxpy, boxpz))) self.bltWorld.attachRigidBody(node) pg.plotBox(self.base.render, pos=[boxpx, boxpy, boxpz], x=boxx * 2.0, y=boxy * 2.0, z=boxz * 2.0, rgba=None) # y+ boxx = 50.0 boxy = 2.0 boxz = 100.0 boxpx = 0.0 boxpy = 50.0 boxpz = 50.0 shape = BulletBoxShape(Vec3(boxx, boxy, boxz)) # the parameters are half extends node = BulletRigidBodyNode('Wally+') node.addShape(shape) node.setTransform(TransformState.makePos(VBase3(boxpx, boxpy, boxpz))) self.bltWorld.attachRigidBody(node) pg.plotBox(self.base.render, pos=[boxpx, boxpy, boxpz], x=boxx * 2.0, y=boxy * 2.0, z=boxz * 2.0, rgba=None) # y- boxx = 50.0 boxy = 2.0 boxz = 100.0 boxpx = 0.0 boxpy = -50.0 boxpz = 50.0 shape = BulletBoxShape(Vec3(boxx, boxy, boxz)) # the parameters are half extends node = BulletRigidBodyNode('Wally-') node.addShape(shape) node.setTransform(TransformState.makePos(VBase3(boxpx, boxpy, boxpz))) self.bltWorld.attachRigidBody(node) pg.plotBox(self.base.render, pos=[boxpx, boxpy, boxpz], x=boxx * 2.0, y=boxy * 2.0, z=boxz * 2.0, rgba=None) # shape = BulletPlaneShape(Vec3(0, 0, 1), 0.0) # node = BulletRigidBodyNode('Ground') # node.addShape(shape) # np = self.base.render.attachNewNode(node) # np.setPos(0, 0, 0) # self.bltWorld.attachRigidBody(node) self.handz = 200.0 def addSmiley(self, task): node = cd.genCollisionMeshNp(self.smiley) node.setMass(1.0) node.setName("part" + str(self.smileyCount)) np = base.render.attachNewNode(node) np.setPos(random.uniform(-2, 2), random.uniform(-2, 2), 15.0 + self.smileyCount * 25.0) sm = np.attachNewNode("partrender" + str(self.smileyCount)) self.smiley.instanceTo(sm) self.bltWorld.attachRigidBody(node) self.smileyCount += 1 if self.smileyCount == 20: return task.done return task.again def updateBlt(self, task): self.bltWorld.doPhysics(globalClock.getDt()) # nodep = self.base.render.find("**/part1") # if nodep: # print nodep.getPos(), nodep.getHpr() # self.handz = self.handz-1 # self.genHand(self.handz) return task.cont def genHand(self, handz=100.0): # fgr0 boxx = 5.0 boxy = 2.0 boxz = 20.0 boxpx = 0.0 boxpy = 10.0 boxpz = handz shape = BulletBoxShape(Vec3(boxx, boxy, boxz)) # the parameters are half extends node = BulletRigidBodyNode('fgr0') node.addShape(shape) node.setTransform(TransformState.makePos(VBase3(boxpx, boxpy, boxpz))) self.bltWorld.attachRigidBody(node) pg.plotBox(self.base.render, pos=[boxpx, boxpy, boxpz], x=boxx * 2.0, y=boxy * 2.0, z=boxz * 2.0, rgba=None) # fgr1 boxx = 5.0 boxy = 2.0 boxz = 20.0 boxpx = 0.0 boxpy = -10.0 boxpz = handz shape = BulletBoxShape(Vec3(boxx, boxy, boxz)) # the parameters are half extends node = BulletRigidBodyNode('fgr1') node.addShape(shape) node.setTransform(TransformState.makePos(VBase3(boxpx, boxpy, boxpz))) self.bltWorld.attachRigidBody(node) pg.plotBox(self.base.render, pos=[boxpx, boxpy, boxpz], x=boxx * 2.0, y=boxy * 2.0, z=boxz * 2.0, rgba=None)
class FreeTabletopPlacement(object): """ manipulation.freetabletopplacement doesn't take into account the position and orientation of the object it is "free" in position and rotation around z axis in contrast, each item in regrasp.tabletopplacements has different position and orientation it is at a specific pose in the workspace To clearly indicate the difference, "free" is attached to the front of "freetabletopplacement" "s" is attached to the end of "tabletopplacements" """ def __init__(self, objpath, handpkg, gdb): self.objtrimesh = trimesh.load_mesh(objpath) self.objcom = self.objtrimesh.center_mass self.objtrimeshconv = self.objtrimesh.convex_hull # oc means object convex self.ocfacets, self.ocfacetnormals = self.objtrimeshconv.facets_over( .9999) # for dbaccess self.dbobjname = os.path.splitext(os.path.basename(objpath))[0] # use two bulletworld, one for the ray, the other for the tabletop self.bulletworldray = BulletWorld() self.bulletworldhp = BulletWorld() # plane to remove hand self.planebullnode = cd.genCollisionPlane(offset=0) self.bulletworldhp.attachRigidBody(self.planebullnode) self.handpkg = handpkg self.handname = handpkg.getHandName() self.hand = handpkg.newHandNM(hndcolor=[0, 1, 0, .1]) # self.rtq85hnd = rtq85nm.Rtq85NM(hndcolor=[1, 0, 0, .1]) # for dbsave # each tpsmat4 corresponds to a set of tpsgripcontacts/tpsgripnormals/tpsgripjawwidth list self.tpsmat4s = None self.tpsgripcontacts = None self.tpsgripnormals = None self.tpsgripjawwidth = None # for ocFacetShow self.counter = 0 self.gdb = gdb self.loadFreeAirGrip() def loadFreeAirGrip(self): """ load self.freegripid, etc. from mysqldatabase :param gdb: an object of the database.GraspDB class :return: author: weiwei date: 20170110 """ freeairgripdata = self.gdb.loadFreeAirGrip(self.dbobjname, handname=self.handname) if freeairgripdata is None: raise ValueError("Plan the freeairgrip first!") self.freegripid = freeairgripdata[0] self.freegripcontacts = freeairgripdata[1] self.freegripnormals = freeairgripdata[2] self.freegriprotmats = freeairgripdata[3] self.freegripjawwidth = freeairgripdata[4] def loadFreeTabletopPlacement(self): """ load free tabletopplacements :return: """ tpsmat4s = self.gdb.loadFreeTabletopPlacement(self.dbobjname) if tpsmat4s is not None: self.tpsmat4s = tpsmat4s return True else: self.tpsmat4s = [] return False def removebadfacets(self, base, doverh=.1): """ remove the facets that cannot support stable placements :param: doverh: d is the distance of mproj to supportfacet boundary, h is the height of com when fh>dmg, the object tends to fall over. setting doverh to 0.033 means when f>0.1mg, the object is judged to be unstable :return: author: weiwei date: 20161213 """ self.tpsmat4s = [] for i in range(len(self.ocfacets)): geom = pg.packpandageom( self.objtrimeshconv.vertices, self.objtrimeshconv.face_normals[self.ocfacets[i]], self.objtrimeshconv.faces[self.ocfacets[i]]) geombullnode = cd.genCollisionMeshGeom(geom) self.bulletworldray.attachRigidBody(geombullnode) pFrom = Point3(self.objcom[0], self.objcom[1], self.objcom[2]) pTo = self.objcom + self.ocfacetnormals[i] * 99999 pTo = Point3(pTo[0], pTo[1], pTo[2]) result = self.bulletworldray.rayTestClosest(pFrom, pTo) self.bulletworldray.removeRigidBody(geombullnode) if result.hasHit(): hitpos = result.getHitPos() facetinterpnt = np.array([hitpos[0], hitpos[1], hitpos[2]]) facetnormal = np.array(self.ocfacetnormals[i]) bdverts3d, bdverts2d, facetmat4 = pg.facetboundary( self.objtrimeshconv, self.ocfacets[i], facetinterpnt, facetnormal) facetp = Polygon(bdverts2d) facetinterpnt2d = rm.transformmat4(facetmat4, facetinterpnt)[:2] apntpnt = Point(facetinterpnt2d[0], facetinterpnt2d[1]) dist2p = apntpnt.distance(facetp.exterior) dist2c = np.linalg.norm( np.array([hitpos[0], hitpos[1], hitpos[2]]) - np.array([pFrom[0], pFrom[1], pFrom[2]])) if dist2p / dist2c >= doverh: # hit and stable self.tpsmat4s.append(pg.cvtMat4np4(facetmat4)) def gentpsgrip(self, base): """ Originally the code of this function is embedded in the removebadfacet function It is separated on 20170608 to enable common usage of placements for different hands :return: author: weiwei date: 20170608 """ self.tpsgripcontacts = [] self.tpsgripnormals = [] self.tpsgriprotmats = [] self.tpsgripjawwidth = [] # the id of the grip in freeair self.tpsgripidfreeair = [] for i in range(len(self.tpsmat4s)): self.tpsgripcontacts.append([]) self.tpsgripnormals.append([]) self.tpsgriprotmats.append([]) self.tpsgripjawwidth.append([]) self.tpsgripidfreeair.append([]) for j, rotmat in enumerate(self.freegriprotmats): tpsgriprotmat = rotmat * self.tpsmat4s[i] # check if the hand collide with tabletop # tmprtq85 = self.rtq85hnd tmphnd = self.hand # tmprtq85 = rtq85nm.Rtq85NM(hndcolor=[1, 0, 0, 1]) initmat = tmphnd.getMat() initjawwidth = tmphnd.jawwidth # open the hand to ensure it doesnt collide with surrounding obstacles # tmprtq85.setJawwidth(self.freegripjawwidth[j]) tmphnd.setJawwidth(80) tmphnd.setMat(tpsgriprotmat) # add hand model to bulletworld hndbullnode = cd.genCollisionMeshMultiNp(tmphnd.handnp) result = self.bulletworldhp.contactTest(hndbullnode) # print result.getNumContacts() if not result.getNumContacts(): self.tpsgriprotmats[-1].append(tpsgriprotmat) cct0 = self.tpsmat4s[i].xformPoint( self.freegripcontacts[j][0]) cct1 = self.tpsmat4s[i].xformPoint( self.freegripcontacts[j][1]) self.tpsgripcontacts[-1].append([cct0, cct1]) cctn0 = self.tpsmat4s[i].xformVec( self.freegripnormals[j][0]) cctn1 = self.tpsmat4s[i].xformVec( self.freegripnormals[j][1]) self.tpsgripnormals[-1].append([cctn0, cctn1]) self.tpsgripjawwidth[-1].append(self.freegripjawwidth[j]) self.tpsgripidfreeair[-1].append(self.freegripid[j]) tmphnd.setMat(initmat) tmphnd.setJawwidth(initjawwidth) def saveToDB(self): """ save freetabletopplacement manipulation.freetabletopplacement doesn't take into account the position and orientation of the object it is "free" in position and rotation around z axis in contrast, each item in regrasp.tabletopplacements has different position and orientation it is at a specific pose in the workspace To clearly indicate the difference, "free" is attached to the front of "freetabletopplacement" "s" is attached to the end of "tabletopplacements" :param discretesize: :param gdb: :return: author: weiwei date: 20170111 """ # save freetabletopplacement sql = "SELECT * FROM freetabletopplacement,object WHERE freetabletopplacement.idobject = object.idobject \ AND object.name LIKE '%s'" % self.dbobjname result = self.gdb.execute(sql) if len(result) == 0: # the fretabletopplacements for the self.dbobjname is not saved sql = "INSERT INTO freetabletopplacement(rotmat, idobject) VALUES " for i in range(len(self.tpsmat4s)): sql += "('%s', (SELECT idobject FROM object WHERE name LIKE '%s')), " % \ (dc.mat4ToStr(self.tpsmat4s[i]), self.dbobjname) sql = sql[:-2] + ";" self.gdb.execute(sql) else: print "Freetabletopplacement already exist!" # save freetabletopgrip idhand = gdb.loadIdHand(self.handname) sql = "SELECT * FROM freetabletopgrip,freetabletopplacement,freeairgrip,object WHERE \ freetabletopgrip.idfreetabletopplacement = freetabletopplacement.idfreetabletopplacement AND \ freetabletopgrip.idfreeairgrip = freeairgrip.idfreeairgrip AND \ freetabletopplacement.idobject = object.idobject AND \ object.name LIKE '%s' AND freeairgrip.idhand = %d" % ( self.dbobjname, idhand) result = self.gdb.execute(sql) if len(result) == 0: for i in range(len(self.tpsmat4s)): sql = "SELECT freetabletopplacement.idfreetabletopplacement FROM freetabletopplacement,object WHERE \ freetabletopplacement.rotmat LIKE '%s' AND \ object.name LIKE '%s'" % (dc.mat4ToStr( self.tpsmat4s[i]), self.dbobjname) result = self.gdb.execute(sql)[0] print result if len(result) != 0: idfreetabletopplacement = result[0] # note self.tpsgriprotmats[i] might be empty (no cd-free grasps) if len(self.tpsgriprotmats[i]) != 0: sql = "INSERT INTO freetabletopgrip(contactpoint0, contactpoint1, contactnormal0, contactnormal1, \ rotmat, jawwidth, idfreetabletopplacement, idfreeairgrip) VALUES " for j in range(len(self.tpsgriprotmats[i])): cct0 = self.tpsgripcontacts[i][j][0] cct1 = self.tpsgripcontacts[i][j][1] cctn0 = self.tpsgripnormals[i][j][0] cctn1 = self.tpsgripnormals[i][j][1] sql += "('%s', '%s', '%s', '%s', '%s', '%s', %d, %d), " % \ (dc.v3ToStr(cct0), dc.v3ToStr(cct1), dc.v3ToStr(cctn0), dc.v3ToStr(cctn1), \ dc.mat4ToStr(self.tpsgriprotmats[i][j]), str(self.tpsgripjawwidth[i][j]), \ idfreetabletopplacement, self.tpsgripidfreeair[i][j]) sql = sql[:-2] + ";" self.gdb.execute(sql) else: print "Freetabletopgrip already exist!" def removebadfacetsshow(self, base, doverh=.1): """ remove the facets that cannot support stable placements :param: doverh: d is the distance of mproj to supportfacet boundary, h is the height of com when fh>dmg, the object tends to fall over. setting doverh to 0.033 means when f>0.1mg, the object is judged to be unstable :return: author: weiwei date: 20161213 """ plotoffsetfp = 10 # print self.counter if self.counter < len(self.ocfacets): i = self.counter # for i in range(len(self.ocfacets)): geom = pg.packpandageom( self.objtrimeshconv.vertices, self.objtrimeshconv.face_normals[self.ocfacets[i]], self.objtrimeshconv.faces[self.ocfacets[i]]) geombullnode = cd.genCollisionMeshGeom(geom) self.bulletworldray.attachRigidBody(geombullnode) pFrom = Point3(self.objcom[0], self.objcom[1], self.objcom[2]) pTo = self.objcom + self.ocfacetnormals[i] * 99999 pTo = Point3(pTo[0], pTo[1], pTo[2]) result = self.bulletworldray.rayTestClosest(pFrom, pTo) self.bulletworldray.removeRigidBody(geombullnode) if result.hasHit(): hitpos = result.getHitPos() pg.plotArrow(base.render, spos=self.objcom, epos=self.objcom + self.ocfacetnormals[i], length=100) facetinterpnt = np.array([hitpos[0], hitpos[1], hitpos[2]]) facetnormal = np.array(self.ocfacetnormals[i]) bdverts3d, bdverts2d, facetmat4 = pg.facetboundary( self.objtrimeshconv, self.ocfacets[i], facetinterpnt, facetnormal) for j in range(len(bdverts3d) - 1): spos = bdverts3d[j] epos = bdverts3d[j + 1] pg.plotStick(base.render, spos, epos, thickness=1, rgba=[.5, .5, .5, 1]) facetp = Polygon(bdverts2d) facetinterpnt2d = rm.transformmat4(facetmat4, facetinterpnt)[:2] apntpnt = Point(facetinterpnt2d[0], facetinterpnt2d[1]) dist2p = apntpnt.distance(facetp.exterior) dist2c = np.linalg.norm( np.array([hitpos[0], hitpos[1], hitpos[2]]) - np.array([pFrom[0], pFrom[1], pFrom[2]])) if dist2p / dist2c < doverh: print "not stable" # return else: pol_ext = LinearRing(bdverts2d) d = pol_ext.project(apntpnt) p = pol_ext.interpolate(d) closest_point_coords = list(p.coords)[0] closep = np.array( [closest_point_coords[0], closest_point_coords[1], 0]) closep3d = rm.transformmat4(rm.homoinverse(facetmat4), closep)[:3] pg.plotDumbbell(base.render, spos=facetinterpnt, epos=closep3d, thickness=1.5, rgba=[0, 0, 1, 1]) for j in range(len(bdverts3d) - 1): spos = bdverts3d[j] epos = bdverts3d[j + 1] pg.plotStick(base.render, spos, epos, thickness=1.5, rgba=[0, 1, 0, 1]) # geomoff = pg.packpandageom(self.objtrimeshconv.vertices + # np.tile(plotoffsetfp * self.ocfacetnormals[i], # [self.objtrimeshconv.vertices.shape[0], 1]), # self.objtrimeshconv.face_normals[self.ocfacets[i]], # self.objtrimeshconv.faces[self.ocfacets[i]]) # # nodeoff = GeomNode('supportfacet') # nodeoff.addGeom(geomoff) # staroff = NodePath('supportfacet') # staroff.attachNewNode(nodeoff) # staroff.setColor(Vec4(1,0,1,1)) # staroff.setTransparency(TransparencyAttrib.MAlpha) # staroff.setTwoSided(True) # staroff.reparentTo(base.render) self.counter += 1 else: self.counter = 0 def grpshow(self, base): sql = "SELECT freetabletopplacement.idfreetabletopplacement, freetabletopplacement.rotmat \ FROM freetabletopplacement,object WHERE \ freetabletopplacement.idobject = object.idobject AND object.name LIKE '%s'" % self.dbobjname result = self.gdb.execute(sql) if len(result) != 0: idfreetabletopplacement = int(result[3][0]) objrotmat = dc.strToMat4(result[3][1]) # show object geom = pg.packpandageom(self.objtrimesh.vertices, self.objtrimesh.face_normals, self.objtrimesh.faces) node = GeomNode('obj') node.addGeom(geom) star = NodePath('obj') star.attachNewNode(node) star.setColor(Vec4(.77, 0.67, 0, 1)) star.setTransparency(TransparencyAttrib.MAlpha) star.setMat(objrotmat) star.reparentTo(base.render) sql = "SELECT freetabletopgrip.rotmat, freetabletopgrip.jawwidth FROM freetabletopgrip WHERE \ freetabletopgrip.idfreetabletopplacement=%d" % idfreetabletopplacement result = self.gdb.execute(sql) for resultrow in result: hndrotmat = dc.strToMat4(resultrow[0]) hndjawwidth = float(resultrow[1]) # show grasps tmprtq85 = rtq85nm.Rtq85NM(hndcolor=[0, 1, 0, .1]) tmprtq85.setMat(hndrotmat) tmprtq85.setJawwidth(hndjawwidth) # tmprtq85.setJawwidth(80) tmprtq85.reparentTo(base.render) def showOnePlacementAndAssociatedGrips(self, base): """ show one placement and its associated grasps :param base: :return: """ for i in range(len(self.tpsmat4s)): if i == 2: objrotmat = self.tpsmat4s[i] # objrotmat.setRow(0, -objrotmat.getRow3(0)) rotzmat = Mat4.rotateMat(180, Vec3(0, 0, 1)) objrotmat = objrotmat * rotzmat # show object geom = pg.packpandageom(self.objtrimesh.vertices, self.objtrimesh.face_normals, self.objtrimesh.faces) node = GeomNode('obj') node.addGeom(geom) star = NodePath('obj') star.attachNewNode(node) star.setColor(Vec4(.7, 0.3, 0, 1)) star.setTransparency(TransparencyAttrib.MAlpha) star.setMat(objrotmat) star.reparentTo(base.render) for j in range(len(self.tpsgriprotmats[i])): # for j in range(13,14): hndrotmat = self.tpsgriprotmats[i][j] hndjawwidth = self.tpsgripjawwidth[i][j] # show grasps tmphnd = self.handpkg.newHandNM(hndcolor=[0, 1, 0, .5]) tmphnd.setMat(hndrotmat) tmphnd.setJawwidth(hndjawwidth) # tmprtq85.setJawwidth(80) tmphnd.reparentTo(base.render) def ocfacetshow(self, base): print self.objcom npf = base.render.find("**/supportfacet") if npf: npf.removeNode() plotoffsetfp = 10 print self.counter print len(self.ocfacets) if self.counter < len(self.ocfacets): geom = pandageom.packpandageom( self.objtrimeshconv.vertices + np.tile(plotoffsetfp * self.ocfacetnormals[self.counter], [self.objtrimeshconv.vertices.shape[0], 1]), self.objtrimeshconv.face_normals[self.ocfacets[self.counter]], self.objtrimeshconv.faces[self.ocfacets[self.counter]]) # geom = pandageom.packpandageom(self.objtrimeshconv.vertices, # self.objtrimeshconv.face_normals, # self.objtrimeshconv.faces) node = GeomNode('supportfacet') node.addGeom(geom) star = NodePath('supportfacet') star.attachNewNode(node) star.setColor(Vec4(1, 0, 1, 1)) star.setTransparency(TransparencyAttrib.MAlpha) star.setTwoSided(True) star.reparentTo(base.render) self.counter += 1 else: self.counter = 0
class Game(DirectObject): def __init__(self): base.setBackgroundColor(0.1, 0.1, 0.5, 1) base.setFrameRateMeter(True) base.cam.setPos(0, -20, 5) base.cam.lookAt(0, 0, 0) base.disableMouse() self.zoom = 50; self.viewPoint = "FRONT" # 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) #bot self.ankleJont = 0 self.foot = 0 self.kneeJoint = 0 self.hipKneeJoint = 0 # 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) self.accept('enter', self.doShoot) self.accept('a',self.setAngleMax) self.accept('o',self.setAngleMin) self.accept('1', self.setViewPointTOP) self.accept('2', self.setViewPointFRONT) self.accept('3', self.setViewPointLEFT) self.accept('4', self.setViewPointDIAG) self.accept('b', self.setZoomInc) self.accept('m', self.setZoomDec) # Task taskMgr.add(self.update, 'updateWorld') # Physics self.setup() # _____HANDLER_____ def setViewPoint(self,view): self.viewPoint = view if(view == "TOP"): base.cam.setPos(0,0,self.zoom) elif(view == "FRONT"): base.cam.setPos(0,-self.zoom,0) elif(view == "LEFT"): base.cam.setPos(-self.zoom,0,0) elif(view == "DIAG"): base.cam.setPos(self.zoom,self.zoom,self.zoom) base.cam.lookAt(0, 0, 0) def setViewPointTOP(self): self.setViewPoint("TOP") def setViewPointFRONT(self): self.setViewPoint("FRONT") def setViewPointLEFT(self): self.setViewPoint("LEFT") def setViewPointDIAG(self): self.setViewPoint("DIAG") def setZoomLevel(self,zoom): self.zoom = zoom self.setViewPoint(self.viewPoint) def setZoomInc(self): self.setZoomLevel(self.zoom+10) def setZoomDec(self): self.setZoomLevel(self.zoom-10) 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') def setAngleMax(self): # self.kneeJoint.setMotorTarget(180,2) self.foot.applyTorqueImpulse(Vec3(1,1,1)) self.foot.applyForce(Vec3(1,1,1),Vec3(1,1,1)) def setAngleMin(self): self.foot.applyTorqueImpulse(Vec3(-1,-1,-1)) # self.kneeJoint.setMotorTarget(0,2) def doShoot(self): # Get from/to points from mouse click pMouse = base.mouseWatcherNode.getMouse() pFrom = Point3() pTo = Point3() base.camLens.extrude(pMouse, pFrom, pTo) pFrom = render.getRelativePoint(base.cam, pFrom) pTo = render.getRelativePoint(base.cam, pTo) # Calculate initial velocity v = pTo - pFrom v.normalize() v *= 100.0 # Create bullet shape = BulletSphereShape(0.3) body = BulletRigidBodyNode('Bullet') bodyNP = self.worldNP.attachNewNode(body) bodyNP.node().addShape(shape) bodyNP.node().setMass(1.0) bodyNP.node().setLinearVelocity(v) bodyNP.node().setCcdMotionThreshold(1e-7); bodyNP.node().setCcdSweptSphereRadius(0.50); bodyNP.setCollideMask(BitMask32.allOn()) bodyNP.setPos(pFrom) visNP = loader.loadModel('models/ball.egg') visNP.setScale(0.8) visNP.reparentTo(bodyNP) self.world.attachRigidBody(bodyNP.node()) # Remove the bullet again after 2 seconds taskMgr.doMethodLater(2, self.doRemove, 'doRemove', extraArgs=[bodyNP], appendTask=True) def doRemove(self, bodyNP, task): self.world.removeRigidBody(bodyNP.node()) bodyNP.removeNode() return task.done # ____TASK___ def update(self, task): angle = self.kneeJoint.getHingeAngle() impulse = self.kneeJoint.getAppliedImpulse() self.kneeJointText.setText("kneeJoint:\n Angle: %0.2f\n Impulse: %0.2f" % (angle, impulse)) angle = self.ankleJoint.getHingeAngle() impulse = self.ankleJoint.getAppliedImpulse() self.ankleJointText.setText("ankleJoint:\n Angle: %0.2f\n Impulse: %0.2f" % (angle, impulse)) force = self.hLeg.getTotalForce() t = self.hLeg.getTotalTorque() self.hLegText.setText("High Leg:\nForce: %0.2f,%0.2f,%0.2f\nTorque: %0.2f,%0.2f,%0.2f" % (force.x, force.y, force.z, t.x,t.y,t.z)) force = self.lLeg.getTotalForce() t = self.lLeg.getTotalTorque() self.lLegText.setText("Low Leg:\nForce: %0.2f,%0.2f,%0.2f\nTorque: %0.2f,%0.2f,%0.2f" % (force.x, force.y, force.z, t.x,t.y,t.z)) f = self.foot.getTotalForce() t = self.foot.getTotalTorque() self.footText.setText("Foot:\nForce: %0.2f,%0.2f,%0.2f\nTorque: %0.2f,%0.2f,%0.2f" % (f.x,f.y,f.z, t.x,t.y,t.z)) dt = globalClock.getDt() self.world.doPhysics(dt, 20, 1.0/180.0) return task.cont def cleanup(self): self.worldNP.removeNode() self.worldNP = None self.world = None def setup(self): self.worldNP = render.attachNewNode('World') # World self.debugNP = self.worldNP.attachNewNode(BulletDebugNode('Debug')) self.debugNP.show() self.debugNP.node().showWireframe(True) self.debugNP.node().showConstraints(True) self.debugNP.node().showBoundingBoxes(False) self.debugNP.node().showNormals(True) self.world = BulletWorld() self.world.setGravity(Vec3(0, 0, -9.81)) self.world.setDebugNode(self.debugNP.node()) # Box 0 shape = BulletBoxShape(Vec3(1, 1, 1)) body0 = BulletRigidBodyNode('Box 0') bodyNP = self.worldNP.attachNewNode(body0) bodyNP.node().addShape(shape) bodyNP.setCollideMask(BitMask32.allOn()) bodyNP.setPos(0, 0, 12.2) visNP = loader.loadModel('models/box.egg') visNP.clearModelNodes() visNP.reparentTo(bodyNP) self.world.attachRigidBody(body0) # Box A shape = BulletBoxShape(Vec3(0.1, 0.1, 5)) self.hLeg = BulletRigidBodyNode('Box A') bodyNP = self.worldNP.attachNewNode(self.hLeg) bodyNP.node().addShape(shape) bodyNP.node().setMass(1.0) bodyNP.node().setDeactivationEnabled(False) bodyNP.setCollideMask(BitMask32.allOn()) bodyNP.setPos(0, 0, 5.1) visNP = loader.loadModel('models/box.egg') visNP.clearModelNodes() visNP.setScale(Vec3(0.2, 0.2, 10)) visNP.reparentTo(bodyNP) self.world.attachRigidBody(self.hLeg) # Box B shape = BulletBoxShape(Vec3(0.1, 0.1, 5)) self.lLeg = BulletRigidBodyNode('Box B') bodyNP = self.worldNP.attachNewNode(self.lLeg) bodyNP.node().addShape(shape) bodyNP.node().setMass(1.0) bodyNP.node().setDeactivationEnabled(False) force = bodyNP.node().getTotalForce() bodyNP.setCollideMask(BitMask32.allOn()) bodyNP.setPos(0, 0, -5.1) visNP = loader.loadModel('models/box.egg') visNP.clearModelNodes() visNP.setScale(Vec3(0.2, 0.2, 10)) visNP.reparentTo(bodyNP) self.world.attachRigidBody(self.lLeg) # Box C shape = BulletBoxShape(Vec3(0.5, 1, 0.1)) self.foot = BulletRigidBodyNode('Box C') bodyNP = self.worldNP.attachNewNode(self.foot) bodyNP.node().addShape(shape) bodyNP.node().setMass(1.0) bodyNP.node().setDeactivationEnabled(False) bodyNP.setCollideMask(BitMask32.allOn()) bodyNP.setPos(0, 0.5, -5.2) visNP = loader.loadModel('models/box.egg') visNP.clearModelNodes() visNP.setScale(Vec3(1, 2, 0.2)) visNP.reparentTo(bodyNP) self.world.attachRigidBody(self.foot) # Text data self.kneeJointText = OnscreenText(text = 'kneeJoint', pos = (-0.4, 0.6), scale = 0.05, fg = (1.0,1.0,1.0,1.0)) self.ankleJointText = OnscreenText(text = 'ankleJoint', pos = (-0.4, 0.4), scale = 0.05, fg = (1.0,1.0,1.0,1.0)) self.hLegText = OnscreenText(text = 'highLeg', pos = (-1, 0.8), scale = 0.05, fg = (1.0,1.0,1.0,1.0)) self.lLegText = OnscreenText(text = 'lowLeg', pos = (-1, 0.6), scale = 0.05, fg = (1.0,1.0,1.0,1.0)) self.footText = OnscreenText(text = 'foot', pos = (-1, 0.4), scale = 0.05, fg = (1.0,1.0,1.0,1.0)) # attachment to hip # Hinge01 pivotA = Point3(0, 0, -2.0) pivotB = Point3(0, 0, 5.1) axisA = Vec3(1, 0, 0) axisB = Vec3(1, 0, 0) hinge = BulletHingeConstraint(body0, self.hLeg, pivotA, pivotB, axisA, axisB, True) hinge.setDebugDrawSize(2.0) hinge.setLimit(-90, 180, softness=0.1, bias=0.3, relaxation=0.1) self.world.attachConstraint(hinge) # Hinge1 pivotA = Point3(0, 0, -5.3) pivotB = Point3(0, 0, 5.3) axisA = Vec3(1, 0, 0) axisB = Vec3(1, 0, 0) self.kneeJoint = BulletHingeConstraint(self.hLeg, self.lLeg, pivotA, pivotB, axisA, axisB, True) #self.kneeJoint.enableAngularMotor(True,1,1) self.kneeJoint.setDebugDrawSize(2.0) self.kneeJoint.setLimit(-10, 180, softness=0.1, bias=0.3, relaxation=0.1) self.world.attachConstraint(self.kneeJoint) # Hinge2 pivotA = Point3(0, 0.2, -5.2) pivotB = Point3(0, 0.2, 0.1) axisA = Vec3(1, 0, 0) axisB = Vec3(1, 0, 0) self.ankleJoint = BulletHingeConstraint(self.lLeg, self.foot, pivotA, pivotB, axisA, axisB, True) self.ankleJoint.setDebugDrawSize(2.0) self.ankleJoint.setLimit(-60, 80, softness=0.1, bias=0.3, relaxation=0.1) self.world.attachConstraint(self.ankleJoint)
class Game(DirectObject): def __init__(self): base.setBackgroundColor(0.1, 0.1, 0.8, 1) base.setFrameRateMeter(True) base.cam.setPos(0, -10, 5) base.cam.lookAt(0, 0, 0.2) # 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('up', 'w') inputState.watchWithModifiers('left', 'a') inputState.watchWithModifiers('down', 's') inputState.watchWithModifiers('right', 'd') # 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): force = Vec3(0, 0, 0) if inputState.isSet('up'): force.setY(1.0) if inputState.isSet('down'): force.setY(-1.0) if inputState.isSet('left'): force.setX(-1.0) if inputState.isSet('right'): force.setX(1.0) force *= 300.0 self.bowlNP.node().setActive(True) self.bowlNP.node().applyCentralForce(force) def update(self, task): dt = globalClock.getDt() self.processInput(dt) self.world.doPhysics(dt) 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.debugNP.node().showWireframe(True) self.debugNP.node().showConstraints(True) self.debugNP.node().showBoundingBoxes(False) self.debugNP.node().showNormals(False) self.world = BulletWorld() self.world.setGravity(Vec3(0, 0, -9.81)) self.world.setDebugNode(self.debugNP.node()) # Ground shape = BulletPlaneShape(Vec3(0, 0, 1), 0) body = BulletRigidBodyNode('Ground') bodyNP = self.worldNP.attachNewNode(body) bodyNP.node().addShape(shape) bodyNP.setPos(0, 0, 0) bodyNP.setCollideMask(BitMask32.allOn()) self.world.attachRigidBody(bodyNP.node()) # Bowl visNP = loader.loadModel('models/bowl.egg') geom = visNP.findAllMatches('**/+GeomNode').getPath(0).node().getGeom( 0) mesh = BulletTriangleMesh() mesh.addGeom(geom) shape = BulletTriangleMeshShape(mesh, dynamic=True) body = BulletRigidBodyNode('Bowl') bodyNP = self.worldNP.attachNewNode(body) bodyNP.node().addShape(shape) bodyNP.node().setMass(10.0) bodyNP.setPos(0, 0, 0) bodyNP.setCollideMask(BitMask32.allOn()) self.world.attachRigidBody(bodyNP.node()) visNP.reparentTo(bodyNP) self.bowlNP = bodyNP self.bowlNP.setScale(2) # Eggs self.eggNPs = [] for i in range(5): x = random.gauss(0, 0.1) y = random.gauss(0, 0.1) z = random.gauss(0, 0.1) + 1 h = random.random() * 360 p = random.random() * 360 r = random.random() * 360 visNP = loader.loadModel('models/egg.egg') geom = visNP.findAllMatches('**/+GeomNode').getPath( 0).node().getGeom(0) shape = BulletConvexHullShape() shape.addGeom(geom) body = BulletRigidBodyNode('Egg-%i' % i) bodyNP = self.worldNP.attachNewNode(body) bodyNP.node().setMass(1.0) bodyNP.node().addShape(shape) bodyNP.node().setDeactivationEnabled(False) bodyNP.setCollideMask(BitMask32.allOn()) bodyNP.setPosHpr(x, y, z, h, p, r) #bodyNP.setScale(1.5) self.world.attachRigidBody(bodyNP.node()) visNP.reparentTo(bodyNP) self.eggNPs.append(bodyNP)
class PhysicsManager(object): num_rigidBodies = 0 num_ghosts = 0 def __init__(self, gravity=(0,0,-9.81) ): self.world = BulletWorld() self.world.setGravity(Vec3(gravity) ) self.addShape = {} self.addShape['plane'] = self.__addPlane self.addShape['sphere'] = self.__addSphere self.addShape['box'] = self.__addBox self.addShape['cylinder'] = self.__addCylinder self.addShape['capsule'] = self.__addCapsule self.addShape['cone'] = self.__addCone self.addShape['hull'] = self.__addConvexHull self.addShape['trimesh'] = self.__addTriangleMesh #self.composer = Composer() self.addShape['compound'] = self.__addCompound # self.shapesList = self.addShape.keys() def getRigidBodyDefaultProps(self): props = {} props['mass'] = .0 props['friction'] = .5 props['restitution'] = .0 props['adamping'] = .0 props['ldamping'] = .0 props['asleep'] = .08 props['lsleep'] = 1. props['deactivation'] = True props['kinematic'] = False return props def getRigidBody(self, name=None): PhysicsManager.num_rigidBodies+=1 return BulletRigidBodyNode(name or 'rigidbody'+str(PhysicsManager.num_rigidBodies) ) def createRigidBody(self, shapetype, model, props={}, name=None): body = self.getRigidBody(name) self.addShape[shapetype](body, model) props = dict(self.getRigidBodyDefaultProps().items() + props.items() ) self.setBodyProps(body, props) self.world.attachRigidBody( body ) return body def getGhost(self, name=None): PhysicsManager.num_ghosts+=1 return BulletGhostNode(name or 'ghost'+str(PhysicsManager.num_ghosts) ) def createGhost(self, shapetype, size, name=None): ghost = self.getGhost(name) self.addShape[shapetype](ghost, size) self.world.attachGhost(ghost) return ghost def attachRigidBody(self, body, props): self.setBodyProps(body, props) self.world.attachRigidBody(body) def __addCompound(self, body, model): self.createCompound(model,body) def __addSphere(self, body, model, pos=(0,0,0), hpr=(0,0,0), scale=(1,1,1) ): size = self.getSize(model) shape = BulletSphereShape( max(size)/2 ) body.addShape(shape, TransformState.makePosHprScale(pos,hpr,scale) ) def __addBox(self, body, model, pos=(0,0,0), hpr=(0,0,0), scale=(1,1,1) ): size = self.getSize(model) shape = BulletBoxShape( size/2 ) body.addShape(shape, TransformState.makePosHprScale(pos,hpr,scale) ) def __addCylinder(self, body, model, pos=(0,0,0), hpr=(0,0,0), scale=(1,1,1) ): size = self.getSize(model) shape = BulletCylinderShape(max(size.x,size.y)/2, size.z, ZUp) body.addShape( shape, TransformState.makePosHprScale(pos,hpr,scale) ) def __addCapsule(self, body, model, pos=(0,0,0), hpr=(0,0,0), scale=(1,1,1) ): size = self.getSize(model) diam = max(size.x,size.y) shape = BulletCapsuleShape(diam/2, size.z-diam, ZUp) body.addShape( shape, TransformState.makePosHprScale(pos,hpr,scale) ) def __addCone(self, body, model, pos=(0,0,0), hpr=(0,0,0), scale=(1,1,1) ): size = self.getSize(model) shape = BulletConeShape(max(size.x,size.y)/2, size.z, ZUp) body.addShape( shape, TransformState.makePosHprScale(pos,hpr,scale) ) def __addConvexHull(self, body, model, pos=(0,0,0), hpr=(0,0,0), scale=(1,1,1) ): def one(): geom = model.node().getGeom(0) shape = BulletConvexHullShape() shape.addGeom(geom) body.addShape( shape, TransformState.makePosHprScale(pos,hpr,scale) ) return [] children = model.findAllMatches('**/+GeomNode') or one() model.flattenLight() for piece in children: shape = BulletConvexHullShape() geom = piece.node().getGeom(0) shape.addGeom(geom) body.addShape( shape, TransformState.makePosHprScale(pos,hpr,scale) ) def __addTriangleMesh(self, body, model, pos=(0,0,0), hpr=(0,0,0), scale=(1,1,1), dynamic=True): mesh = BulletTriangleMesh() def one(): geom = model.node().getGeom(0) mesh.addGeom(geom) return [] children = model.findAllMatches('**/+GeomNode') or one() model.flattenLight() for piece in children: geom = piece.node().getGeom(0) mesh.addGeom(geom) shape = BulletTriangleMeshShape(mesh, dynamic=dynamic ) body.addShape( shape, TransformState.makePosHprScale(pos,hpr,scale) ) def __addPlane(self, body, size=(0,0,1), pos=(0,0,0), hpr=(0,0,0), scale=(1,1,1) ): shape = BulletPlaneShape(Vec3(size), 0) body.addShape( shape, TransformState.makePosHprScale(pos,hpr,scale) ) def setBodyProps(self, body, props): body.setMass( props['mass'] ) body.setFriction( props['friction'] ) body.setRestitution( props['restitution'] ) body.setAngularDamping( props['adamping'] ) body.setLinearDamping( props['ldamping'] ) body.setAngularSleepThreshold( props['asleep'] ) body.setLinearSleepThreshold( props['lsleep'] ) if not props['deactivation']: body.setDeactivationEnabled( False ) body.setKinematic( props['kinematic'] ) def characterController(self, name, height, mass, radius, step_height): shape = BulletCapsuleShape(radius, height - 2*radius, ZUp) body = BulletRigidBodyNode(name) body.setMass(mass) body.addShape(shape) return BulletCharacterControllerNode(shape, step_height, name) def debug(self): from panda3d.bullet import BulletDebugNode debugNP = render.attachNewNode(BulletDebugNode('Debug') ) #debugNP.show() debugNP.node().showWireframe(True) debugNP.node().showConstraints(True) debugNP.node().showBoundingBoxes(False) debugNP.node().showNormals(False) self.world.setDebugNode(debugNP.node()) return debugNP def getSize(self,model): hpr = model.getHpr() model.setHpr(0,0,0) minLimit, maxLimit = model.getTightBounds() model.setHpr(hpr) return maxLimit - minLimit def setShape(self, model, body): #To Re-Do name = model.getName() pos,hpr,scale = model.getPos(), model.getHpr(), model.getScale() shapetype = re.findall("[-a-z]+",name.lower())[0].split('-') shapetype = filter(None,shapetype) if shapetype[0] in self.addShape.keys(): self.addShape[shapetype[0]](body,model,pos,hpr,scale) not '-' in name or model.remove() def createCompound(self, model, body): children = model.find('**/*').getChildren() or model.getChildren() [self.setShape(child,body) for child in children]
class GameStatePlaying(GState): VIDAS = 3 #LEVEL_TIME = 100 LEVEL_TIME = 50 def start(self): self._playing_node = render.attachNewNode("playing-node") self._playing_node2d = aspect2d.attachNewNode("playing2d-node") self.keyMap = {"left":0, "right":0, "up":0, "down":0} base.accept( "escape" , sys.exit) props = WindowProperties() props.setCursorHidden(True) base.disableMouse() props.setMouseMode(WindowProperties.MRelative) base.win.requestProperties(props) base.win.movePointer(0, base.win.getXSize() / 2, base.win.getYSize() / 2) self._modulos = None self._paneles = None self._num_lvl = 1 self._num_lvls = 2 self.loadLevel() self.loadGUI() self.loadBkg() self._last_t = None self._last_t_space = 0 def stop(self): self._playing_node.removeNode() self._playing_node2d.removeNode() def loadLevel(self): self._level_time = self.LEVEL_TIME self.initBullet() self._player = Player(self.world) self.loadMap() self.setAI() def initBullet(self): self.world = BulletWorld() #self.world.setGravity(Vec3(0, 0, -9.81)) self.world.setGravity(Vec3(0, 0, -1.62)) groundShape = BulletPlaneShape(Vec3(0, 0, 1), 0) groundNode = BulletRigidBodyNode('Ground') groundNode.addShape(groundShape) self.world.attachRigidBody(groundNode) def loadBkg(self): self.environ1 = loader.loadModel("../data/models/skydome") self.environ1.reparentTo(self._playing_node) self.environ1.setPos(0,0,0) self.environ1.setScale(1) self.environ2 = loader.loadModel("../data/models/skydome") self.environ2.reparentTo(self._playing_node) self.environ2.setP(180) self.environ2.setH(270) self.environ2.setScale(1) self.dirnlight1 = DirectionalLight("dirn_light1") self.dirnlight1.setColor(Vec4(1.0,1.0,1.0,1.0)) self.dirnlightnode1 = self._playing_node.attachNewNode(self.dirnlight1) self.dirnlightnode1.setHpr(0,317,0) self._playing_node.setLight(self.dirnlightnode1) self.alight = AmbientLight('alight') self.alight.setColor(VBase4(0.05, 0.05, 0.05, 1)) self.alight_node = self._playing_node.attachNewNode(self.alight) self._playing_node.setLight(self.alight_node) self.environ1 = loader.loadModel("../data/models/ground") self.environ1.reparentTo(self._playing_node) self.environ1.setPos(0,0,0) self.environ1.setScale(1) def loadGUI(self): self.vidas_imgs = list() w = 0.24 for i in range(self.VIDAS): image_warning = OnscreenImage(image = '../data/Texture/signal_warning.png', pos=(-1 + i*w, 0, 0.85), parent=self._playing_node2d) image_warning.setScale(0.1) image_warning.setTransparency(TransparencyAttrib.MAlpha) image_warning.hide() image_ok = OnscreenImage(image = '../data/Texture/signal_ok.png', pos=(-1 + i*w, 0, 0.85), parent=self._playing_node2d) image_ok.setScale(0.1) image_ok.setTransparency(TransparencyAttrib.MAlpha) image_ok.show() self.vidas_imgs.append((image_ok, image_warning)) self._level_time_O = OnscreenText(text = '', pos = (0, 0.85), scale = 0.14, fg=(1.0, 1.0, 1.0, 1.0), bg=(0.0, 0.0, 0.0, 1.0), parent=self._playing_node2d) def loadMap(self): if (self._modulos is not None): for m in self._modulos: m.remove() if (self._paneles is not None): for p in self._paneles: p.remove() self._tp = TiledParser("map"+str(self._num_lvl)) self._modulos, self._paneles = self._tp.load_models(self.world, self._playing_node) def setAI(self): taskMgr.add(self.update, 'Update') def update(self, task): if (task.frame > 1): self.world.doPhysics(globalClock.getDt()) self._level_time -= globalClock.getDt() self._level_time_O.setText(str(int(self._level_time))) for panel in self._paneles: contact = self.world.contactTestPair(self._player.getRBNode(), panel.getRBNode()) if contact.getNumContacts() > 0: panel.manipulate() brokens = 0 for panel in self._paneles: if panel.isBroken(): brokens += 1 #print brokens for i, vida_imgs in enumerate(self.vidas_imgs): if i < len(self.vidas_imgs) - brokens: vida_imgs[0].show() vida_imgs[1].hide() else: vida_imgs[0].hide() vida_imgs[1].show() if brokens > self.VIDAS: self.gameOver() return task.done if self._level_time <= 0: self._num_lvl += 1 if self._num_lvl <= self._num_lvls: self.nextLevel() else: self.theEnd() return task.done return task.cont def gameOver(self): taskMgr.remove('player-task') taskMgr.remove('panel-task') taskMgr.remove('panel-sound-task') self._mission_text_O = OnscreenText(text = 'Game Over', pos = (0, 0), scale = 0.5, fg=(1.0, 1.0, 1.0, 1.0), bg=(0.0, 0.0, 0.0, 1.0)) taskMgr.add(self.gameOverTransition, 'game-over-transition') #self.loadLevel() print "Game Over" def gameOverTransition(self, task): base.win.movePointer(0, base.win.getXSize() / 2, base.win.getYSize() / 2) if task.time > 3.0: self._mission_text_O.hide() props = WindowProperties() props.setCursorHidden(False) base.win.requestProperties(props) self._state_context.changeState(gameStateMenu.GameStateMenu(self._state_context)) print "MENU" return task.done return task.cont def nextLevel(self): taskMgr.remove('player-task') taskMgr.remove('panel-task') taskMgr.remove('panel-sound-task') self._mission_text_O = OnscreenText(text = 'Mission\nComplete', pos = (0, 0), scale = 0.5, fg=(1.0, 1.0, 1.0, 1.0), bg=(0.0, 0.0, 0.0, 1.0)) taskMgr.add(self.nextLevelTransition, 'next-Level-transition') print "Mission Complete" def nextLevelTransition(self, task): base.win.movePointer(0, base.win.getXSize() / 2, base.win.getYSize() / 2) if task.time > 3.0: print "Next Level" self._mission_text_O.hide() self.loadLevel() return task.done return task.cont def theEnd(self): taskMgr.remove('player-task') taskMgr.remove('panel-task') taskMgr.remove('panel-sound-task') self._mission_text_O = OnscreenText(text = '. The End .', pos = (0, 0), scale = 0.5, fg=(1.0, 1.0, 1.0, 1.0), bg=(0.0, 0.0, 0.0, 1.0)) taskMgr.add(self.theEndTransition, 'theEnd-transition') #self.loadLevel() print "Mission Complete" def theEndTransition(self, task): base.win.movePointer(0, base.win.getXSize() / 2, base.win.getYSize() / 2) if task.time > 3.0: self._mission_text_O.hide() props = WindowProperties() props.setCursorHidden(False) base.win.requestProperties(props) self._state_context.changeState(gameStateMenu.GameStateMenu(self._state_context)) print "The End" return task.done return task.cont
class Game(DirectObject): def __init__(self): base.setBackgroundColor(0.1, 0.1, 0.8, 1) base.setFrameRateMeter(True) base.cam.setPos(0, -40, 10) 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(5, 0, -2)) 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) # 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 update(self, task): dt = globalClock.getDt() #dt *= 0.01 self.world.doPhysics(dt, 10, 0.008) 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()) # Ground p0 = Point3(-20, -20, 0) p1 = Point3(-20, 20, 0) p2 = Point3(20, -20, 0) p3 = Point3(20, 20, 0) mesh = BulletTriangleMesh() mesh.addTriangle(p0, p1, p2) mesh.addTriangle(p1, p2, p3) shape = BulletTriangleMeshShape(mesh, dynamic=False) np = self.worldNP.attachNewNode(BulletRigidBodyNode('Mesh')) np.node().addShape(shape) np.setPos(0, 0, -2) np.setCollideMask(BitMask32.allOn()) self.world.attachRigidBody(np.node()) # Soft body world information info = self.world.getWorldInfo() info.setAirDensity(1.2) info.setWaterDensity(0) info.setWaterOffset(0) info.setWaterNormal(Vec3(0, 0, 0)) # Softbody def makeSB(pos, hpr): import torus geom = torus.makeGeom() #geom = loader.loadModel('models/torus.egg') \ # .findAllMatches('**/+GeomNode').getPath(0).node() \ # .modifyGeom(0) geomNode = GeomNode('') geomNode.addGeom(geom) node = BulletSoftBodyNode.makeTriMesh(info, geom) node.linkGeom(geomNode.modifyGeom(0)) node.generateBendingConstraints(2) node.getCfg().setPositionsSolverIterations(2) node.getCfg().setCollisionFlag(BulletSoftBodyConfig.CFVertexFaceSoftSoft, True) node.randomizeConstraints() node.setTotalMass(50, True) softNP = self.worldNP.attachNewNode(node) softNP.setPos(pos) softNP.setHpr(hpr) self.world.attachSoftBody(node) geomNP = softNP.attachNewNode(geomNode) makeSB(Point3(-3, 0, 4), (0, 0, 0)) makeSB(Point3(0, 0, 4), (0, 90, 90)) makeSB(Point3(3, 0, 4), (0, 0, 0))
class TestApplication(ShowBase): def __init__(self): ShowBase.__init__(self) self.setupRendering() self.setupControls() self.setupPhysics() self.clock_ = ClockObject() self.controlled_obj_index_ = 0 self.kinematic_mode_ = False # Task taskMgr.add(self.update, 'updateWorld') def setupRendering(self): self.setBackgroundColor(0.1, 0.1, 0.8, 1) self.setFrameRateMeter(True) # Light alight = AmbientLight('ambientLight') alight.setColor(Vec4(0.5, 0.5, 0.5, 1)) alightNP = self.render.attachNewNode(alight) dlight = DirectionalLight('directionalLight') dlight.setDirection(Vec3(1, 1, -1)) dlight.setColor(Vec4(0.7, 0.7, 0.7, 1)) dlightNP = self.render.attachNewNode(dlight) self.render.clearLight() self.render.setLight(alightNP) self.render.setLight(dlightNP) def setupControls(self): # Input (Events) 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) self.accept('n', self.selectNextControlledObject) self.accept('k',self.toggleKinematicMode) self.accept('p',self.printInfo) self.accept('q',self.zoomIn) self.accept('a',self.zoomOut) self.accept('s',self.toggleRightLeft) # Inputs (Polling) self.input_state_ = InputState() self.input_state_.watchWithModifiers("right","arrow_right") self.input_state_.watchWithModifiers('left', 'arrow_left') self.input_state_.watchWithModifiers('jump', 'arrow_up') self.input_state_.watchWithModifiers('stop', 'arrow_down') self.input_state_.watchWithModifiers('roll_right', 'c') self.input_state_.watchWithModifiers('roll_left', 'z') self.input_state_.watchWithModifiers('roll_stop', 'x') self.title = addTitle("Panda3D: Sprite Animation") self.inst1 = addInstructions(0.06, "ESC: Quit") self.inst2 = addInstructions(0.12, "Up/Down: Jump/Stop") self.inst3 = addInstructions(0.18, "Left/Right: Move Left / Move Rigth") self.inst4 = addInstructions(0.24, "z/x/c : Rotate Left/ Rotate Stop / Rotate Right") self.inst5 = addInstructions(0.30, "n: Select Next Character") self.inst6 = addInstructions(0.36, "k: Toggle Kinematic Mode") self.inst7 = addInstructions(0.42, "q/a: Zoom in / Zoom out") self.inst7 = addInstructions(0.48, "s: Toggle Rigth|Left ") self.inst7 = addInstructions(0.54, "p: Print Info") def printInfo(self): self.sprt_animator_.printInfo() def setupPhysics(self): # setting up physics world and parent node path self.physics_world_ = BulletWorld() self.world_node_ = self.render.attachNewNode('world') self.cam.reparentTo(self.world_node_) self.physics_world_.setGravity(Vec3(0, 0, -9.81)) self.debug_node_ = self.world_node_.attachNewNode(BulletDebugNode('Debug')) self.debug_node_.show() self.debug_node_.node().showWireframe(True) self.debug_node_.node().showConstraints(True) self.debug_node_.node().showBoundingBoxes(False) self.debug_node_.node().showNormals(True) # setting up ground self.ground_ = self.world_node_.attachNewNode(BulletRigidBodyNode('Ground')) self.ground_.node().addShape(BulletPlaneShape(Vec3(0, 0, 1), 0)) self.ground_.setPos(0,0,0) self.ground_.setCollideMask(BitMask32.allOn()) self.physics_world_.attachRigidBody(self.ground_.node()) self.object_nodes_ = [] self.controlled_objects_=[] num_boxes = 20 side_length = 0.2 size = Vec3(0.5*side_length,0.5*side_length,0.5*side_length) start_pos = Vec3(-num_boxes*side_length,0,6) """ # creating boxes box_visual = loader.loadModel('models/box.egg') box_visual.clearModelNodes() box_visual.setTexture(loader.loadTexture('models/wood.png')) bounds = box_visual.getTightBounds() # start of box model scaling extents = Vec3(bounds[1] - bounds[0]) scale_factor = side_length/max([extents.getX(),extents.getY(),extents.getZ()]) box_visual.setScale((scale_factor,scale_factor ,scale_factor)) # end of box model scaling for i in range(0,20): self.addBox("name %i"%(i),size,start_pos + Vec3(i*side_length,0,0),box_visual) start_pos = Vec3(-num_boxes*side_length,0,8) for i in range(0,20): self.addBox("name %i"%(i),size,start_pos + Vec3(i*2*side_length,0,0),box_visual) """ # creating sprite animator sprt_animator = SpriteAnimator('hiei_run') if not sprt_animator.loadImage(SPRITE_IMAGE_DETAILS[0], # file path SPRITE_IMAGE_DETAILS[1], # columns SPRITE_IMAGE_DETAILS[2], # rows SPRITE_IMAGE_DETAILS[3], # scale x SPRITE_IMAGE_DETAILS[4], # scale y SPRITE_IMAGE_DETAILS[5]): # frame rate print "Error loading image %s"%(SPRITE_IMAGE_DETAILS[0]) sys.exit(1) self.sprt_animator_ = sprt_animator # creating Mobile Character Box size = Vec3(0.5,0.2,1) mbox_visual = loader.loadModel('models/box.egg') bounds = mbox_visual.getTightBounds() extents = Vec3(bounds[1] - bounds[0]) scale_factor = 1/max([extents.getX(),extents.getY(),extents.getZ()]) mbox_visual.setScale(size.getX()*scale_factor,size.getY()*scale_factor,size.getZ()*scale_factor) mbox = self.world_node_.attachNewNode(BulletRigidBodyNode('CharacterBox')) mbox.node().addShape(BulletBoxShape(size/2)) mbox.node().setMass(1.0) mbox.node().setLinearFactor((1,0,1)) mbox.node().setAngularFactor((0,1,0)) mbox.setCollideMask(BitMask32.allOn()) mbox_visual.instanceTo(mbox) mbox_visual.hide() mbox.setPos(Vec3(1,0,size.getZ()+1)) bounds = sprt_animator.getTightBounds() extents = Vec3(bounds[1] - bounds[0]) scale_factor = size.getZ()/extents.getZ() sprt_animator.setScale(scale_factor,1,scale_factor) sprt_animator.reparentTo(mbox) bounds = sprt_animator.getTightBounds() print "Sprite Animator bounds %s , %s"%(str(bounds[1]),str(bounds[0])) self.physics_world_.attachRigidBody(mbox.node()) self.object_nodes_.append(mbox) self.controlled_objects_.append(mbox) # creating sphere diameter = 0.4 sphere_visual = loader.loadModel('models/ball.egg') bounds = sphere_visual.getTightBounds() # start of model scaling extents = Vec3(bounds[1] - bounds[0]) scale_factor = diameter/max([extents.getX(),extents.getY(),extents.getZ()]) sphere_visual.clearModelNodes() sphere_visual.setScale(scale_factor,scale_factor,scale_factor) # end of model scaling sphere_visual.setTexture(loader.loadTexture('models/bowl.jpg')) sphere = self.world_node_.attachNewNode(BulletRigidBodyNode('Sphere')) sphere.node().addShape(BulletSphereShape(0.5*diameter)) sphere.node().setMass(1.0) sphere.node().setLinearFactor((1,0,1)) sphere.node().setAngularFactor((0,1,0)) sphere.setCollideMask(BitMask32.allOn()) sphere_visual.instanceTo(sphere) sphere.setPos(Vec3(0,0,size.getZ()+1)) self.physics_world_.attachRigidBody(sphere.node()) self.object_nodes_.append(sphere) self.controlled_objects_.append(sphere) self.setupLevel() def addBox(self,name,size,pos,visual): # Box (dynamic) box = self.world_node_.attachNewNode(BulletRigidBodyNode(name)) box.node().setMass(1.0) box.node().addShape(BulletBoxShape(size)) box.setPos(pos) box.setCollideMask(BitMask32.allOn()) box.node().setLinearFactor((1,0,1)) box.node().setAngularFactor((0,1,0)) visual.instanceTo(box) self.physics_world_.attachRigidBody(box.node()) self.object_nodes_.append(box) def setupLevel(self): # (left, top, length ,depth(y) ,height) platform_details =[ (-20, 4, 20, 4, 1 ), (-2, 5, 10, 4, 1 ), ( 4 , 2 , 16, 4, 1.4), (-4 , 1, 10, 4, 1), ( 16, 6, 30, 4, 1) ] for i in range(0,len(platform_details)): p = platform_details[i] topleft = (p[0],p[1]) size = Vec3(p[2],p[3],p[4]) self.addPlatform(topleft, size,'Platform%i'%(i)) def addPlatform(self,topleft,size,name): # Visual visual = loader.loadModel('models/box.egg') visual.setTexture(loader.loadTexture('models/iron.jpg')) bounds = visual.getTightBounds() extents = Vec3(bounds[1] - bounds[0]) scale_factor = 1/max([extents.getX(),extents.getY(),extents.getZ()]) visual.setScale(size.getX()*scale_factor,size.getY()*scale_factor,size.getZ()*scale_factor) # Box (static) platform = self.world_node_.attachNewNode(BulletRigidBodyNode(name)) platform.node().setMass(0) platform.node().addShape(BulletBoxShape(size/2)) platform.setPos(Vec3(topleft[0] + 0.5*size.getX(),0,topleft[1]-0.5*size.getZ())) platform.setCollideMask(BitMask32.allOn()) visual.instanceTo(platform) self.physics_world_.attachRigidBody(platform.node()) self.object_nodes_.append(platform) def update(self, task): self.clock_.tick() dt = self.clock_.getDt() self.processInput(dt) self.physics_world_.doPhysics(dt, 5, 1.0/180.0) self.updateCamera() return task.cont def processInput(self,dt): activate = False obj = self.controlled_objects_[self.controlled_obj_index_] if self.kinematic_mode_: obj.node().setKinematic(True) return else: obj.node().setKinematic(False) vel = obj.node().getLinearVelocity() w = obj.node().getAngularVelocity() if self.input_state_.isSet('right'): vel.setX(2) activate = True if self.input_state_.isSet('left'): vel.setX(-2) activate = True if self.input_state_.isSet('jump'): vel.setZ(4) activate = True if self.input_state_.isSet('stop'): vel.setX(0) if self.input_state_.isSet('roll_right'): w.setY(ROTATIONAl_SPEED) activate = True if self.input_state_.isSet('roll_left'): w.setY(-ROTATIONAl_SPEED) activate = True if self.input_state_.isSet('roll_stop'): w.setY(0) if activate : obj.node().setActive(True,True) obj.node().setLinearVelocity(vel) obj.node().setAngularVelocity(w) def updateCamera(self): if len(self.controlled_objects_) > 0: obj = self.controlled_objects_[self.controlled_obj_index_] self.cam.setPos(obj,0, -CAM_DISTANCE, 0) self.cam.lookAt(obj.getPos()) def cleanup(self): for i in range(0,len(self.object_nodes_)): rb = self.object_nodes_[i] self.physics_world_.removeRigidBody(rb.node()) self.object_nodes_ = [] self.controlled_objects_ = [] self.physics_world_.removeRigidBody(self.ground_.node()) self.ground_ = None self.physics_world_ = None self.debug_node_ = None self.sprt_animator_ = None self.cam.reparentTo(self.render) self.world_node_.removeNode() self.world_node_ = None # _____HANDLER_____ def zoomIn(self): global CAM_DISTANCE CAM_DISTANCE-=4 def zoomOut(self): global CAM_DISTANCE CAM_DISTANCE+=4 def toggleKinematicMode(self): self.kinematic_mode_ = not self.kinematic_mode_ print "Kinematic Mode %s"%('ON' if self.kinematic_mode_ else 'OFF') def toggleRightLeft(self): self.sprt_animator_.faceRight(not self.sprt_animator_.facing_right_) def selectNextControlledObject(self): self.controlled_obj_index_+=1 if self.controlled_obj_index_ >= len(self.controlled_objects_): self.controlled_obj_index_ = 0 # reset def doExit(self): self.cleanup() sys.exit(1) def doReset(self): self.cleanup() self.setupPhysics() def toggleDebug(self): if self.debug_node_.isHidden(): self.debug_node_.show() else: self.debug_node_.hide() def doScreenshot(self): self.screenshot('Bullet')
class Game(DirectObject): def __init__(self): base.setBackgroundColor(0.1, 0.1, 0.8, 1) base.setFrameRateMeter(True) base.cam.setPos(0, -40, 10) 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(5, 0, -2)) 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) # 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 update(self, task): dt = globalClock.getDt() self.world.doPhysics(dt, 10, 0.004) 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()) # Soft body world information info = self.world.getWorldInfo() info.setAirDensity(1.2) info.setWaterDensity(0) info.setWaterOffset(0) info.setWaterNormal(Vec3(0, 0, 0)) # Softbody def make(p1): n = 8 p2 = p1 + Vec3(10, 0, 0) bodyNode = BulletSoftBodyNode.makeRope(info, p1, p2, n, 1) bodyNode.setTotalMass(50.0) bodyNP = self.worldNP.attachNewNode(bodyNode) self.world.attachSoftBody(bodyNode) # Render option 1: Line geom #geom = BulletSoftBodyNode.makeGeomFromLinks(bodyNode) #bodyNode.linkGeom(geom) #visNode = GeomNode('') #visNode.addGeom(geom) #visNP = bodyNP.attachNewNode(visNode) # Render option 2: NURBS curve curve = NurbsCurveEvaluator() curve.reset(n + 2) bodyNode.linkCurve(curve) visNode = RopeNode('') visNode.setCurve(curve) visNode.setRenderMode(RopeNode.RMTube) visNode.setUvMode(RopeNode.UVParametric) visNode.setNumSubdiv(4) visNode.setNumSlices(8) visNode.setThickness(0.4) visNP = self.worldNP.attachNewNode(visNode) #visNP = bodyNP.attachNewNode(visNode) # --> renders with offset!!! visNP.setTexture(loader.loadTexture('models/iron.jpg')) #bodyNP.showBounds() #visNP.showBounds() return bodyNP np1 = make(Point3(-2, -1, 8)) np2 = make(Point3(-2, 1, 8)) # Box shape = BulletBoxShape(Vec3(2, 2, 6)) boxNP = self.worldNP.attachNewNode(BulletRigidBodyNode('Box')) boxNP.node().setMass(50.0) boxNP.node().addShape(shape) boxNP.setPos(10, 0, 8) boxNP.setCollideMask(BitMask32.allOn()) self.world.attachRigidBody(boxNP.node()) np1.node().appendAnchor(np1.node().getNumNodes() - 1, boxNP.node()) np2.node().appendAnchor(np1.node().getNumNodes() - 1, boxNP.node()) visNP = loader.loadModel('models/box.egg') visNP.clearModelNodes() visNP.setScale(4, 4, 12) visNP.reparentTo(boxNP)
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) self.accept('space', self.doJump) self.accept('c', self.doCrouch) 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') def doJump(self): self.character.setMaxJumpHeight(5.0) self.character.setJumpSpeed(8.0) self.character.doJump() def doCrouch(self): self.crouching = not self.crouching sz = self.crouching and 0.6 or 1.0 self.characterNP.setScale(Vec3(1, 1, sz)) #self.character.getShape().setLocalScale(Vec3(1, 1, sz)) #self.characterNP.setScale(Vec3(1, 1, sz) * 0.3048) #self.characterNP.setPos(0, 0, -1 * sz) # ____TASK___ def processInput(self, dt): speed = Vec3(0, 0, 0) omega = 0.0 if inputState.isSet('forward'): speed.setY( 2.0) if inputState.isSet('reverse'): speed.setY(-2.0) if inputState.isSet('left'): speed.setX(-2.0) if inputState.isSet('right'): speed.setX( 2.0) if inputState.isSet('turnLeft'): omega = 120.0 if inputState.isSet('turnRight'): omega = -120.0 self.character.setAngularMovement(omega) self.character.setLinearMovement(speed, True) def update(self, task): dt = globalClock.getDt() self.processInput(dt) self.world.doPhysics(dt, 4, 1./240.) 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()) # Ground shape = BulletPlaneShape(Vec3(0, 0, 1), 0) #img = PNMImage(Filename('models/elevation2.png')) #shape = BulletHeightfieldShape(img, 1.0, ZUp) np = self.worldNP.attachNewNode(BulletRigidBodyNode('Ground')) np.node().addShape(shape) np.setPos(0, 0, -1) np.setCollideMask(BitMask32.allOn()) self.world.attachRigidBody(np.node()) # Box shape = BulletBoxShape(Vec3(1.0, 3.0, 0.3)) np = self.worldNP.attachNewNode(BulletRigidBodyNode('Box')) np.node().setMass(10.0) np.node().addShape(shape) np.setPos(3, 0, 4) np.setH(20.0) np.setCollideMask(BitMask32.allOn()) self.world.attachRigidBody(np.node()) # Character self.crouching = False h = 1.75 w = 0.4 shape = BulletCapsuleShape(w, h - 2 * w, ZUp) self.character = BulletCharacterControllerNode(shape, 0.4, 'Player') # self.character.setMass(1.0) self.characterNP = self.worldNP.attachNewNode(self.character) self.characterNP.setPos(-2, 0, 14) self.characterNP.setH(45) self.characterNP.setCollideMask(BitMask32.allOn()) self.world.attachCharacter(self.character) self.actorNP = Actor('models/ralph/ralph.egg', { 'run' : 'models/ralph/ralph-run.egg', 'walk' : 'models/ralph/ralph-walk.egg', 'jump' : 'models/ralph/ralph-jump.egg'}) self.actorNP.reparentTo(self.characterNP) self.actorNP.setScale(0.3048) # 1ft = 0.3048m self.actorNP.setH(180) self.actorNP.setPos(0, 0, -1)
class Freesuc(object): def __init__(self, ompath, handpkg, ser=False, torqueresist = 50): self.objtrimesh = None # the sampled points and their normals self.objsamplepnts = None self.objsamplenrmls = None # the sampled points (bad samples removed) self.objsamplepnts_ref = None self.objsamplenrmls_ref = None # the sampled points (bad samples removed + clustered) self.objsamplepnts_refcls = None self.objsamplenrmls_refcls = None # facets is used to avoid repeated computation self.facets = None # facetnormals is used to plot overlapped facets with different heights self.facetnormals = None # facet2dbdries saves the 2d boundaries of each facet self.facet2dbdries = None # for plot self.facetcolorarray = None self.counter = 0 # for collision detection self.bulletworld = BulletWorld() self.hand = handpkg.newHandNM(hndcolor = [.2,0.7,.2,.3]) # collision free grasps self.sucrotmats = [] self.succontacts = [] self.succontactnormals = [] # collided grasps self.sucrotmatscld = [] self.succontactscld = [] self.succontactnormalscld = [] self.objcenter = [0,0,0] self.torqueresist = torqueresist if ser is False: self.loadObjModel(ompath) self.saveSerialized("tmpfsc.pickle") else: self.loadSerialized("tmpfsc.pickle", ompath) def loadObjModel(self, ompath): self.objtrimesh=trimesh.load_mesh(ompath) # oversegmentation self.facets, self.facetnormals = self.objtrimesh.facets_over(faceangle=.95) self.facetcolorarray = pandageom.randomColorArray(self.facets.shape[0]) self.sampleObjModel() # prepare the model for collision detection self.objgeom = pandageom.packpandageom(self.objtrimesh.vertices, self.objtrimesh.face_normals, self.objtrimesh.faces) self.objmeshbullnode = cd.genCollisionMeshGeom(self.objgeom) self.bulletworld.attachRigidBody(self.objmeshbullnode) # object center self.objcenter = [0,0,0] for pnt in self.objtrimesh.vertices: self.objcenter[0]+=pnt[0] self.objcenter[1]+=pnt[1] self.objcenter[2]+=pnt[2] self.objcenter[0] = self.objcenter[0]/self.objtrimesh.vertices.shape[0] self.objcenter[1] = self.objcenter[1]/self.objtrimesh.vertices.shape[0] self.objcenter[2] = self.objcenter[2]/self.objtrimesh.vertices.shape[0] def loadSerialized(self, filename, ompath): self.objtrimesh=trimesh.load_mesh(ompath) try: self.facets, self.facetnormals, self.facetcolorarray, self.objsamplepnts, \ self.objsamplenrmls, self.objsamplepnts_ref, self.objsamplenrmls_ref, \ self.objsamplepnts_refcls, self.objsamplenrmls_refcls = \ pickle.load(open(filename, mode="rb")) except: print str(sys.exc_info()[0])+" cannot load tmpcp.pickle" raise def saveSerialized(self, filename): pickle.dump([self.facets, self.facetnormals, self.facetcolorarray, self.objsamplepnts, \ self.objsamplenrmls, self.objsamplepnts_ref, self.objsamplenrmls_ref, \ self.objsamplepnts_refcls, self.objsamplenrmls_refcls], open(filename, mode="wb")) def sampleObjModel(self, numpointsoververts=5): """ sample the object model self.objsamplepnts and self.objsamplenrmls are filled in this function :param: numpointsoververts: the number of sampled points = numpointsoververts*mesh.vertices.shape[0] :return: nverts: the number of verts sampled author: weiwei date: 20160623 flight to tokyo """ nverts = self.objtrimesh.vertices.shape[0] samples, face_idx = manipulation.suction.sample.sample_surface_even(self.objtrimesh, count=(1000 if nverts*numpointsoververts > 1000 \ else nverts*numpointsoververts)) # print nverts self.objsamplepnts = np.ndarray(shape=(self.facets.shape[0],), dtype=np.object) self.objsamplenrmls = np.ndarray(shape=(self.facets.shape[0],), dtype=np.object) for i, faces in enumerate(self.facets): for face in faces: sample_idx = np.where(face_idx==face)[0] if len(sample_idx) > 0: if self.objsamplepnts[i] is not None: self.objsamplepnts[i] = np.vstack((self.objsamplepnts[i], samples[sample_idx])) self.objsamplenrmls[i] = np.vstack((self.objsamplenrmls[i], [self.objtrimesh.face_normals[face]]*samples[sample_idx].shape[0])) else: self.objsamplepnts[i] = np.array(samples[sample_idx]) self.objsamplenrmls[i] = np.array([self.objtrimesh.face_normals[face]]*samples[sample_idx].shape[0]) if self.objsamplepnts[i] is None: self.objsamplepnts[i] = np.empty(shape=[0,0]) self.objsamplenrmls[i] = np.empty(shape=[0,0]) return nverts def removeBadSamples(self, mindist=7, maxdist=9999): ''' Do the following refinement: (1) remove the samples who's minimum distance to facet boundary is smaller than mindist (2) remove the samples who's maximum distance to facet boundary is larger than mindist ## input mindist, maxdist as explained in the begining author: weiwei date: 20160623 flight to tokyo ''' # ref = refine self.objsamplepnts_ref = np.ndarray(shape=(self.facets.shape[0],), dtype=np.object) self.objsamplenrmls_ref = np.ndarray(shape=(self.facets.shape[0],), dtype=np.object) self.facet2dbdries = [] for i, faces in enumerate(self.facets): # print "removebadsample" # print i,len(self.facets) facetp = None face0verts = self.objtrimesh.vertices[self.objtrimesh.faces[faces[0]]] facetmat = robotmath.rotmatfacet(self.facetnormals[i], face0verts[0], face0verts[1]) # face samples samplepntsp =[] for j, apnt in enumerate(self.objsamplepnts[i]): apntp = np.dot(facetmat, apnt)[:2] samplepntsp.append(apntp) # face boundaries for j, faceidx in enumerate(faces): vert0 = self.objtrimesh.vertices[self.objtrimesh.faces[faceidx][0]] vert1 = self.objtrimesh.vertices[self.objtrimesh.faces[faceidx][1]] vert2 = self.objtrimesh.vertices[self.objtrimesh.faces[faceidx][2]] vert0p = np.dot(facetmat, vert0)[:2] vert1p = np.dot(facetmat, vert1)[:2] vert2p = np.dot(facetmat, vert2)[:2] facep = Polygon([vert0p, vert1p, vert2p]) if facetp is None: facetp = facep else: try: facetp = facetp.union(facep) except: continue self.facet2dbdries.append(facetp) selectedele = [] for j, apntp in enumerate(samplepntsp): try: apntpnt = Point(apntp[0], apntp[1]) dbnds = [] dbnds.append(apntpnt.distance(facetp.exterior)) for fpinter in facetp.interiors: dbnds.append(apntpnt.distance(fpinter)) dbnd = min(dbnds) if dbnd < mindist or dbnd > maxdist: pass else: selectedele.append(j) except: pass self.objsamplepnts_ref[i] = np.asarray([self.objsamplepnts[i][j] for j in selectedele]) self.objsamplenrmls_ref[i] = np.asarray([self.objsamplenrmls[i][j] for j in selectedele]) self.facet2dbdries = np.array(self.facet2dbdries) # if i is 3: # for j, apntp in enumerate(samplepntsp): # apntpnt = Point(apntp[0], apntp[1]) # plt.plot(apntpnt.x, apntpnt.y, 'bo') # for j, apnt in enumerate([samplepntsp[j] for j in selectedele]): # plt.plot(apnt[0], apnt[1], 'ro') # ftpx, ftpy = facetp.exterior.xy # plt.plot(ftpx, ftpy) # for fpinters in facetp.interiors: # ftpxi, ftpyi = fpinters.xy # plt.plot(ftpxi, ftpyi) # plt.axis('equal') # plt.show() # pass # old code for concatenating in 3d space # boundaryedges = [] # for faceid in faces: # faceverts = self.objtrimesh.faces[faceid] # try: # boundaryedges.remove([faceverts[1], faceverts[0]]) # except: # boundaryedges.append([faceverts[0], faceverts[1]]) # try: # boundaryedges.remove([faceverts[2], faceverts[1]]) # except: # boundaryedges.append([faceverts[1], faceverts[2]]) # try: # boundaryedges.remove([faceverts[0], faceverts[2]]) # except: # boundaryedges.append([faceverts[2], faceverts[0]]) # print boundaryedges # print len(boundaryedges) # TODO: compute boundary polygons, both outsider and inner should be considered # assort boundaryedges # boundarypolygonlist = [] # boundarypolygon = [boundaryedges[0]] # boundaryedgesfirstcolumn = [row[0] for row in boundaryedges] # for i in range(len(boundaryedges)-1): # vertpivot = boundarypolygon[i][1] # boundarypolygon.append(boundaryedges[boundaryedgesfirstcolumn.index(vertpivot)]) # print boundarypolygon # print len(boundarypolygon) # return boundaryedges, boundarypolygon def clusterFacetSamplesKNN(self, reduceRatio=3, maxNPnts=5): """ cluster the samples of each facet using k nearest neighbors the cluster center and their correspondent normals will be saved in self.objsamplepnts_refcls and self.objsamplenrmals_refcls :param: reduceRatio: the ratio of points to reduce :param: maxNPnts: the maximum number of points on a facet :return: None author: weiwei date: 20161129, tsukuba """ self.objsamplepnts_refcls = np.ndarray(shape=(self.facets.shape[0],), dtype=np.object) self.objsamplenrmls_refcls = np.ndarray(shape=(self.facets.shape[0],), dtype=np.object) for i, facet in enumerate(self.facets): self.objsamplepnts_refcls[i] = np.empty(shape=(0,0)) self.objsamplenrmls_refcls[i] = np.empty(shape=(0,0)) X = self.objsamplepnts_ref[i] nX = X.shape[0] if nX > reduceRatio: kmeans = KMeans(n_clusters=maxNPnts if nX/reduceRatio>maxNPnts else nX/reduceRatio, random_state=0).fit(X) self.objsamplepnts_refcls[i] = kmeans.cluster_centers_ self.objsamplenrmls_refcls[i] = np.tile(self.facetnormals[i], [self.objsamplepnts_refcls.shape[0],1]) def clusterFacetSamplesRNN(self, reduceRadius=3): """ cluster the samples of each facet using radius nearest neighbours the cluster center and their correspondent normals will be saved in self.objsamplepnts_refcls and self.objsamplenrmals_refcls :param: reduceRadius: the neighbors that fall inside the reduceradius will be removed :return: None author: weiwei date: 20161130, osaka """ self.objsamplepnts_refcls = np.ndarray(shape=(self.facets.shape[0],), dtype=np.object) self.objsamplenrmls_refcls = np.ndarray(shape=(self.facets.shape[0],), dtype=np.object) for i, facet in enumerate(self.facets): # print "cluster" # print i,len(self.facets) self.objsamplepnts_refcls[i] = [] self.objsamplenrmls_refcls[i] = [] X = self.objsamplepnts_ref[i] nX = X.shape[0] if nX > 0: neigh = RadiusNeighborsClassifier(radius=1.0) neigh.fit(X, range(nX)) neigharrays = neigh.radius_neighbors(X, radius=reduceRadius, return_distance=False) delset = set([]) for j in range(nX): if j not in delset: self.objsamplepnts_refcls[i].append(np.array(X[j])) self.objsamplenrmls_refcls[i].append(np.array(self.objsamplenrmls_ref[i][j])) # if self.objsamplepnts_refcls[i].size: # self.objsamplepnts_refcls[i] = np.vstack((self.objsamplepnts_refcls[i], X[j])) # self.objsamplenrmls_refcls[i] = np.vstack((self.objsamplenrmls_refcls[i], # self.objsamplenrmls_ref[i][j])) # else: # self.objsamplepnts_refcls[i] = np.array([]) # self.objsamplenrmls_refcls[i] = np.array([]) # self.objsamplepnts_refcls[i] = np.hstack((self.objsamplepnts_refcls[i], X[j])) # self.objsamplenrmls_refcls[i] = np.hstack((self.objsamplenrmls_refcls[i], # self.objsamplenrmls_ref[i][j])) delset.update(neigharrays[j].tolist()) if self.objsamplepnts_refcls[i]: self.objsamplepnts_refcls[i] = np.vstack(self.objsamplepnts_refcls[i]) self.objsamplenrmls_refcls[i] = np.vstack(self.objsamplenrmls_refcls[i]) else: self.objsamplepnts_refcls[i] = np.empty(shape=(0,0)) self.objsamplenrmls_refcls[i] = np.empty(shape=(0,0)) def removeHndcc(self, base, discretesize=8): """ Handcc means hand collision detection :param discretesize: the number of hand orientations :return: author: weiwei date: 20161212, tsukuba """ # isplotted = 0 # if self.rtq85plotlist: # for rtq85plotnode in self.rtq85plotlist: # rtq85plotnode.removeNode() # self.rtq85plotlist = [] self.sucrotmats = [] self.succontacts = [] self.succontactnormals = [] # collided self.sucrotmatscld = [] self.succontactscld = [] self.succontactnormalscld = [] plotoffsetfp = 3 self.counter = 0 while self.counter < self.facets.shape[0]: # print str(self.counter) + "/" + str(self.facetpairs.shape[0]-1) # print self.gripcontactpairs_precc for i in range(self.objsamplepnts_refcls[self.counter].shape[0]): for angleid in range(discretesize): cctpnt = self.objsamplepnts_refcls[self.counter][i] + plotoffsetfp * self.objsamplenrmls_refcls[self.counter][i] # check torque resistance print Vec3(cctpnt[0],cctpnt[1],cctpnt[2]).length() if Vec3(cctpnt[0],cctpnt[1],cctpnt[2]).length() < self.torqueresist: cctnrml = self.objsamplenrmls_refcls[self.counter][i] rotangle = 360.0 / discretesize * angleid tmphand = self.hand tmphand.attachTo(cctpnt[0], cctpnt[1], cctpnt[2], cctnrml[0], cctnrml[1], cctnrml[2], rotangle) hndbullnode = cd.genCollisionMeshMultiNp(tmphand.handnp, base.render) result = self.bulletworld.contactTest(hndbullnode) if not result.getNumContacts(): self.succontacts.append(self.objsamplepnts_refcls[self.counter][i]) self.succontactnormals.append(self.objsamplenrmls_refcls[self.counter][i]) self.sucrotmats.append(tmphand.getMat()) else: self.succontactscld.append(self.objsamplepnts_refcls[self.counter][i]) self.succontactnormalscld.append(self.objsamplenrmls_refcls[self.counter][i]) self.sucrotmatscld.append(tmphand.getMat()) self.counter+=1 self.counter = 0 def segShow(self, base, togglesamples=False, togglenormals=False, togglesamples_ref=False, togglenormals_ref=False, togglesamples_refcls=False, togglenormals_refcls=False, alpha = .1): """ :param base: :param togglesamples: :param togglenormals: :param togglesamples_ref: toggles the sampled points that fulfills the dist requirements :param togglenormals_ref: :return: """ nfacets = self.facets.shape[0] facetcolorarray = self.facetcolorarray # offsetf = facet # plot the segments plotoffsetf = .0 for i, facet in enumerate(self.facets): geom = pandageom.packpandageom(self.objtrimesh.vertices+np.tile(plotoffsetf*i*self.facetnormals[i], [self.objtrimesh.vertices.shape[0],1]), \ # -np.tile(self.objcenter,[self.objtrimesh.vertices.shape[0],1]), self.objtrimesh.face_normals[facet], self.objtrimesh.faces[facet]) node = GeomNode('piece') node.addGeom(geom) star = NodePath('piece') star.attachNewNode(node) star.setColor(Vec4(facetcolorarray[i][0], facetcolorarray[i][1], facetcolorarray[i][2], alpha)) star.setTransparency(TransparencyAttrib.MAlpha) star.setTwoSided(True) star.reparentTo(base.render) # sampledpnts = samples[sample_idxes[i]] # for apnt in sampledpnts: # pandageom.plotSphere(base, star, pos=apnt, radius=1, rgba=rgba) rgbapnts0 = [1,1,1,1] rgbapnts1 = [.5,.5,0,1] rgbapnts2 = [1,0,0,1] if togglesamples: for j, apnt in enumerate(self.objsamplepnts[i]): pandageom.plotSphere(star, pos=apnt+plotoffsetf*i*self.facetnormals[i], radius=3, rgba=rgbapnts0) if togglenormals: for j, apnt in enumerate(self.objsamplepnts[i]): pandageom.plotArrow(star, spos=apnt+plotoffsetf*i*self.facetnormals[i], epos=apnt + plotoffsetf*i*self.facetnormals[i] + self.objsamplenrmls[i][j], rgba=rgbapnts0, length=10) if togglesamples_ref: for j, apnt in enumerate(self.objsamplepnts_ref[i]): pandageom.plotSphere(star, pos=apnt+plotoffsetf*i*self.facetnormals[i], radius=3, rgba=rgbapnts1) if togglenormals_ref: for j, apnt in enumerate(self.objsamplepnts_ref[i]): pandageom.plotArrow(star, spos=apnt+plotoffsetf*i*self.facetnormals[i], epos=apnt + plotoffsetf*i*self.facetnormals[i] + self.objsamplenrmls_ref[i][j], rgba=rgbapnts1, length=10) if togglesamples_refcls: for j, apnt in enumerate(self.objsamplepnts_refcls[i]): pandageom.plotSphere(star, pos=apnt+plotoffsetf*i*self.facetnormals[i], radius=3, rgba=rgbapnts2) if togglenormals_refcls: for j, apnt in enumerate(self.objsamplepnts_refcls[i]): pandageom.plotArrow(star, spos=apnt+plotoffsetf*i*self.facetnormals[i], epos=apnt + plotoffsetf*i*self.facetnormals[i] + self.objsamplenrmls_refcls[i][j], rgba=rgbapnts2, length=10) def segShow2(self, base, togglesamples=False, togglenormals=False, togglesamples_ref=False, togglenormals_ref=False, togglesamples_refcls=False, togglenormals_refcls=False, specificface = True): """ :param base: :param togglesamples: :param togglenormals: :param togglesamples_ref: toggles the sampled points that fulfills the dist requirements :param togglenormals_ref: :return: """ nfacets = self.facets.shape[0] facetcolorarray = self.facetcolorarray rgbapnts0 = [1, 1, 1, 1] rgbapnts1 = [0.2, 0.7, 1, 1] rgbapnts2 = [1, 0.7, 0.2, 1] # offsetf = facet plotoffsetf = .0 faceplotted = False # plot the segments for i, facet in enumerate(self.facets): if not specificface: geom = pandageom.packpandageom(self.objtrimesh.vertices+np.tile(plotoffsetf*i*self.facetnormals[i], [self.objtrimesh.vertices.shape[0],1]), self.objtrimesh.face_normals[facet], self.objtrimesh.faces[facet]) node = GeomNode('piece') node.addGeom(geom) star = NodePath('piece') star.attachNewNode(node) star.setColor(Vec4(.77, .17, 0, 1)) star.setTransparency(TransparencyAttrib.MAlpha) star.setTwoSided(True) star.reparentTo(base.render) # sampledpnts = samples[sample_idxes[i]] # for apnt in sampledpnts: # pandageom.plotSphere(base, star, pos=apnt, radius=1, rgba=rgba) if togglesamples: for j, apnt in enumerate(self.objsamplepnts[i]): pandageom.plotSphere(star, pos=apnt+plotoffsetf*i*self.facetnormals[i], radius=2.8, rgba=rgbapnts0) if togglenormals: for j, apnt in enumerate(self.objsamplepnts[i]): pandageom.plotArrow(star, spos=apnt+plotoffsetf*i*self.facetnormals[i], epos=apnt + plotoffsetf*i*self.facetnormals[i] + self.objsamplenrmls[i][j], rgba=rgbapnts0, length=10) if togglesamples_ref: for j, apnt in enumerate(self.objsamplepnts_ref[i]): pandageom.plotSphere(star, pos=apnt+plotoffsetf*i*self.facetnormals[i], radius=2.9, rgba=rgbapnts1) if togglenormals_ref: for j, apnt in enumerate(self.objsamplepnts_ref[i]): pandageom.plotArrow(star, spos=apnt+plotoffsetf*i*self.facetnormals[i], epos=apnt + plotoffsetf*i*self.facetnormals[i] + self.objsamplenrmls_ref[i][j], rgba=rgbapnts1, length=10) if togglesamples_refcls: for j, apnt in enumerate(self.objsamplepnts_refcls[i]): pandageom.plotSphere(star, pos=apnt+plotoffsetf*i*self.facetnormals[i], radius=3, rgba=rgbapnts2) if togglenormals_refcls: for j, apnt in enumerate(self.objsamplepnts_refcls[i]): pandageom.plotArrow(star, spos=apnt+plotoffsetf*i*self.facetnormals[i], epos=apnt + plotoffsetf*i*self.facetnormals[i] + self.objsamplenrmls_refcls[i][j], rgba=rgbapnts2, length=10) if specificface: plotoffsetf = .1 if faceplotted: continue else: if len(self.objsamplepnts[i])>25: faceplotted = True geom = pandageom.packpandageom(self.objtrimesh.vertices+np.tile(plotoffsetf*i*self.facetnormals[i], [self.objtrimesh.vertices.shape[0],1]), self.objtrimesh.face_normals[facet], self.objtrimesh.faces[facet]) node = GeomNode('piece') node.addGeom(geom) star = NodePath('piece') star.attachNewNode(node) star.setColor(Vec4(facetcolorarray[i][0], facetcolorarray[i][1], facetcolorarray[i][2], 1)) star.setTransparency(TransparencyAttrib.MAlpha) star.setTwoSided(True) star.reparentTo(base.render) # sampledpnts = samples[sample_idxes[i]] # for apnt in sampledpnts: # pandageom.plotSphere(base, star, pos=apnt, radius=1, rgba=rgba) if togglesamples: for j, apnt in enumerate(self.objsamplepnts[i]): pandageom.plotSphere(star, pos=apnt+plotoffsetf*i*self.facetnormals[i], radius=2.8, rgba=rgbapnts0) if togglenormals: for j, apnt in enumerate(self.objsamplepnts[i]): pandageom.plotArrow(star, spos=apnt+plotoffsetf*i*self.facetnormals[i], epos=apnt + plotoffsetf*i*self.facetnormals[i] + self.objsamplenrmls[i][j], rgba=rgbapnts0, length=10) if togglesamples_ref: for j, apnt in enumerate(self.objsamplepnts_ref[i]): pandageom.plotSphere(star, pos=apnt+plotoffsetf*i*self.facetnormals[i], radius=3, rgba=rgbapnts1) if togglenormals_ref: for j, apnt in enumerate(self.objsamplepnts_ref[i]): pandageom.plotArrow(star, spos=apnt+plotoffsetf*i*self.facetnormals[i], epos=apnt + plotoffsetf*i*self.facetnormals[i] + self.objsamplenrmls_ref[i][j], rgba=rgbapnts1, length=10) if togglesamples_refcls: for j, apnt in enumerate(self.objsamplepnts_refcls[i]): pandageom.plotSphere(star, pos=apnt+plotoffsetf*i*self.facetnormals[i], radius=3.5, rgba=rgbapnts2) if togglenormals_refcls: for j, apnt in enumerate(self.objsamplepnts_refcls[i]): pandageom.plotArrow(star, spos=apnt+plotoffsetf*i*self.facetnormals[i], epos=apnt + plotoffsetf*i*self.facetnormals[i] + self.objsamplenrmls_refcls[i][j], rgba=rgbapnts2, length=10)
class Game(DirectObject): def __init__(self): base.setBackgroundColor(0.1, 0.1, 0.5, 1) base.setFrameRateMeter(True) base.cam.setPos(0, -20, 5) base.cam.lookAt(0, 0, 0) base.disableMouse() self.model = None self.zoom = 100; self.viewPoint = "FRONT" # 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) #bot self.kneeJoint = 0 self.hipKneeJoint = 0 # 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) self.accept('1', self.setViewPointTOP) self.accept('2', self.setViewPointFRONT) self.accept('3', self.setViewPointLEFT) self.accept('4', self.setViewPointDIAG) self.accept('b', self.setZoomInc) self.accept('m', self.setZoomDec) # Task taskMgr.add(self.update, 'updateWorld') # Physics self.setup() # _____HANDLER_____ def setViewPoint(self,view): self.viewPoint = view if(view == "TOP"): base.cam.setPos(0,0,self.zoom) elif(view == "FRONT"): base.cam.setPos(0,-self.zoom,0) elif(view == "LEFT"): base.cam.setPos(-self.zoom,0,0) elif(view == "DIAG"): base.cam.setPos(self.zoom,self.zoom,self.zoom) base.cam.lookAt(0, 0, 0) def setViewPointTOP(self): self.setViewPoint("TOP") def setViewPointFRONT(self): self.setViewPoint("FRONT") def setViewPointLEFT(self): self.setViewPoint("LEFT") def setViewPointDIAG(self): self.setViewPoint("DIAG") def setZoomLevel(self,zoom): self.zoom = zoom self.setViewPoint(self.viewPoint) def setZoomInc(self): self.setZoomLevel(self.zoom+10) def setZoomDec(self): self.setZoomLevel(self.zoom-10) 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') def doRemove(self, bodyNP, task): self.world.removeRigidBody(bodyNP.node()) bodyNP.removeNode() return task.done # ____TASK___ def update(self, task): #angle = self.kneeJoint.getHingeAngle() #impulse = self.kneeJoint.getAppliedImpulse() #self.kneeJointText.setText("kneeJoint:\n Angle: %0.2f\n Impulse: %0.2f" % (angle, impulse)) dt = globalClock.getDt() self.model.update(dt); self.world.doPhysics(dt, 60, 1.0/180.0) return task.cont def cleanup(self): self.worldNP.removeNode() self.worldNP = None self.world = None def setup(self): self.worldNP = render.attachNewNode('World') # World self.debugNP = self.worldNP.attachNewNode(BulletDebugNode('Debug')) self.debugNP.show() self.debugNP.node().showWireframe(True) self.debugNP.node().showConstraints(True) self.debugNP.node().showBoundingBoxes(False) self.debugNP.node().showNormals(True) self.world = BulletWorld() self.world.setGravity(Vec3(0, 0, -9.81)) self.world.setDebugNode(self.debugNP.node()) self.model = TinyDancer(self.world,self.worldNP) #floor shape = BulletBoxShape(Vec3(100, 100, 1)) floor = BulletRigidBodyNode('Floor') bodyNP = self.worldNP.attachNewNode(floor) bodyNP.node().addShape(shape) bodyNP.setCollideMask(BitMask32.allOn()) bodyNP.setPos(0, 0, -12) visNP = loader.loadModel('models/black.egg') visNP.setScale(Vec3(200, 200, 2)) visNP.clearModelNodes() visNP.reparentTo(bodyNP) self.world.attachRigidBody(floor)
class Application(ShowBase): def __init__(self): ShowBase.__init__(self) self.world = BulletWorld() self.world.setGravity(Vec3(0, 0, -9.81)) self.height = 85.0 self.img = PNMImage(Filename('height1.png')) self.shape = BulletHeightfieldShape(self.img, self.height, ZUp) self.offset = self.img.getXSize() / 2.0 - 0.5 self.terrain = GeoMipTerrain('terrain') self.terrain.setHeightfield(self.img) self.terrain.setColorMap("grass.png") self.terrainNP = self.terrain.getRoot() self.terrainNP.setSz(self.height) self.terrainNP.setPos(-self.offset, -self.offset, -self.height / 2.0) self.terrain.getRoot().reparentTo(render) self.terrain.generate() self.node = BulletRigidBodyNode('Ground') self.node.addShape(self.shape) self.np = render.attachNewNode(self.node) self.np.setPos(0, 0, 0) self.world.attachRigidBody(self.node) self.info = self.world.getWorldInfo() self.info.setAirDensity(1.2) self.info.setWaterDensity(0) self.info.setWaterOffset(0) self.info.setWaterNormal(Vec3(0, 0, 0)) self.cam.setPos(20, 20, 20) self.cam.setHpr(0, 0, 0) self.model = loader.loadModel('out6.egg') #self.model.setPos(0.0, 0.0, 0.0) self.model.flattenLight() min, max = self.model.getTightBounds() size = max mmax = size[0] if size[1] > mmax: mmax = size[1] if size[2] > mmax: mmax = size[2] self.rocketScale = 20.0/mmax / 2 shape = BulletBoxShape(Vec3(size[0]*self.rocketScale, size[1]*self.rocketScale, size[2]*self.rocketScale)) self.ghostshape = BulletBoxShape(Vec3(size[0]*self.rocketScale, size[1]*self.rocketScale, size[2]*self.rocketScale)) self.ghost = BulletRigidBodyNode('Ghost') self.ghost.addShape(self.ghostshape) self.ghostNP = render.attachNewNode(self.ghost) self.ghostNP.setPos(19.2220401046, 17.5158313723, 35.2665607047 ) #self.ghostNP.setCollideMask(BitMask32(0x0f)) self.model.setScale(self.rocketScale) self.world.attachRigidBody(self.ghost) self.model.copyTo(self.ghostNP) self.rocketnode = BulletRigidBodyNode('rocketBox') self.rocketnode.setMass(1.0) self.rocketnode.addShape(shape) self.rocketnp = render.attachNewNode(self.rocketnode) self.rocketnp.setPos(0.0, 0.0, 40.0) tex = loader.loadTexture('crate.jpg') self.model.find('**/Line001').setTexture(tex, 1) #self.rocketnp.setTexture(tex) self.model.setScale(self.rocketScale) self.world.attachRigidBody(self.rocketnode) self.model.copyTo(self.rocketnp) self.hh = 35.0 print self.ghostNP.getPos() self.accept('w', self.eventKeyW) self.accept('r', self.eventKeyR) self.taskMgr.add(self.update, 'update') self.massive = self.getdata('data.txt') self.massiveResampled = self.resample(self.massive) self.posInArray = 0 self.x = 0.0 self.y = 0.0 self.z = 0.0 self.timerNow = 0.0 self.taskMgr.add(self.timerTask, 'timerTask') def checkGhost(self): ''' ghost = self.ghostNP.node() if ghost.getNumOverlappingNodes() > 1: print ghost.getNumOverlappingNodes() for node in ghost.getOverlappingNodes(): print node ''' result = self.world.contactTest(self.ghost) for contact in result.getContacts(): print contact.getNode0() print contact.getNode1() def eventKeyW(self): self.hh -= 1.0 def eventKeyR(self): self.cam.setPos(self.x, self.y - 15.0, self.z + 2.50) def resample(self, array): len1 = len(array[0]) - 1 res = [] counter = 0 resamplerArray = [] bufArray = [] for i in xrange(len1): resamplerArray.append(resampler(0.02, 0.125)) bufArray.append([]) for i in xrange(len(array)): buf = [] for j in xrange(len1): bufArray[j] = resamplerArray[j].feed(array[i][j+1]) for j in xrange(len(bufArray[0])): buf.append(0.02*(counter)) for k in xrange(len1): buf.append(bufArray[k][j]) res.append(buf) counter += 1 buf = [] return res def getdata(self, name): array = [] with open(name) as f: for line in f: numbers_float = map(float, line.split()) array.append([numbers_float[0], numbers_float[1], numbers_float[2], numbers_float[3]]) return array def update(self, task): dt = globalClock.getDt() #self.world.doPhysics(dt) self.getMyPos(self.massiveResampled) self.rocketnp.setPos(self.x, self.y, self.z) self.cam.setPos(self.x, self.y - 15.0, self.z + 2.50) return task.cont def getMyPos(self, array): flag = False len1 = len(array) if self.posInArray >= len1: flag = True while not flag: if self.timerNow < array[self.posInArray][0]: flag = True self.x = array[self.posInArray][1] self.y = array[self.posInArray][2] self.z = array[self.posInArray][3] break else: if self.posInArray >= len1: flag = True break else: self.posInArray += 1 self.checkGhost() result = self.world.contactTest(self.rocketnode) for contact in result.getContacts(): print contact.getNode0() print contact.getNode1() def timerTask(self, task): self.timerNow = task.time return Task.cont
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) # self.accept('space', self.doJump) # self.accept('c', self.doCrouch) 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") # def doJump(self): # self.player.setMaxJumpHeight(5.0) # self.player.setJumpSpeed(8.0) # self.player.doJump() # def doCrouch(self): # self.crouching = not self.crouching # sz = self.crouching and 0.6 or 1.0 # self.playerNP2.setScale(Vec3(1, 1, sz)) # ____TASK___ def processInput(self, dt): speed = Vec3(0, 0, 0) omega = 0.0 if inputState.isSet("forward"): speed.setY(2.0) if inputState.isSet("reverse"): speed.setY(-2.0) if inputState.isSet("left"): speed.setX(-2.0) if inputState.isSet("right"): speed.setX(2.0) if inputState.isSet("turnLeft"): omega = 120.0 if inputState.isSet("turnRight"): omega = -120.0 self.player.setAngularMovement(omega) self.player.setLinearMovement(speed, True) def update(self, task): dt = globalClock.getDt() self.processInput(dt) self.world.doPhysics(dt, 4, 1.0 / 240.0) 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()) # Ground shape = BulletPlaneShape(Vec3(0, 0, 1), 0) # img = PNMImage(Filename('models/elevation2.png')) # shape = BulletHeightfieldShape(img, 1.0, ZUp) np = self.worldNP.attachNewNode(BulletRigidBodyNode("Ground")) np.node().addShape(shape) np.setPos(0, 0, -1) np.setCollideMask(BitMask32.allOn()) self.world.attachRigidBody(np.node()) # Box shape = BulletBoxShape(Vec3(1.0, 3.0, 0.3)) np = self.worldNP.attachNewNode(BulletRigidBodyNode("Box")) np.node().setMass(50.0) np.node().addShape(shape) np.setPos(3, 0, 4) np.setH(0) np.setCollideMask(BitMask32.allOn()) self.world.attachRigidBody(np.node()) # Character h = 1.75 w = 0.4 shape = BulletCapsuleShape(w, h - 2 * w, ZUp) self.player = BulletCharacterNode(shape, 0.4, "Player") self.player.setMass(20.0) self.player.setMaxSlope(45.0) self.player.setGravity(9.81) self.playerNP = self.worldNP.attachNewNode(self.player) self.playerNP.setPos(-2, 0, 10) self.playerNP.setH(-90) self.playerNP.setCollideMask(BitMask32.allOn()) self.world.attachCharacter(self.player)
from panda3d.bullet import BulletBoxShape base.cam.setPos(0, -10, 0) base.cam.lookAt(0, 0, 0) # World world = BulletWorld() world.setGravity(Vec3(0, 0, -9.81)) # Plane shape = BulletPlaneShape(Vec3(0, 0, 1), 1) node = BulletRigidBodyNode('Ground') node.addShape(shape) np = render.attachNewNode(node) np.setPos(0, 0, -2) world.attachRigidBody(node) # Box shape = BulletBoxShape(Vec3(0.5, 0.5, 0.5)) node = BulletRigidBodyNode('Box') node.setMass(1.0) node.addShape(shape) np = render.attachNewNode('Box') np.setPos(0, 0, 2) world.attachRigidBody(node) model = loader.loadModel('models/box.egg') model.flattenLight() model.reparentTo(np) # Update
class level_1(ShowBase): def __init__(self): ShowBase.__init__(self) # Input self.accept('escape', self.doExit) self.accept('f3', self.toggleDebug) inputState.watchWithModifiers('forward', 'w') inputState.watchWithModifiers('reverse', 's') inputState.watchWithModifiers('turnLeft', 'a') inputState.watchWithModifiers('turnRight', 'd') # Post the instructions self.title = addTitle("Infinite Loop: A Robot's Nightmare") self.inst1 = addInstructions(0.95, "[ESC]: Quit") self.inst2 = addInstructions(0.90, "[W]: Run Forward") self.inst3 = addInstructions(0.85, "[A]: Turn Left") self.inst4 = addInstructions(0.80, "[S]: Walk Backwards") self.inst5 = addInstructions(0.75, "[D]: Turn Right") self.inst6 = addInstructions(0.70, "[SPACE]: Jump") # Game state variables self.lettersRemaining = 5 self.letters = [] self.collectedLetters = [] self.health = 100 self.enemies = [] self.movingPlatforms = [] self.isTakingDamage = False self.start = True self.worldCondition = False self.onLevelTwo = False self.menuOn = True # Number of collectibles self.numObjects = addNumObj( "Find letters B R E A K to escape\nLetters Remaining: " + str(self.lettersRemaining)) # Health Bar self.bar = DirectWaitBar(text="H E A L T H", value=100, # start with full health pos=(0, .4, 0.93), # position healthbar to top center scale=(1.3, 2.5, 2.5), barColor=(0.97, 0, 0, 1), frameSize=(-0.3, 0.3, 0, 0.025), text_mayChange=1, text_shadow=(0, 0, 0, 0), text_fg=(0.9, 0.9, 0.9, 1), text_scale=0.030, text_pos=(0, 0.005, 0)) self.bar.setBin("fixed", 0) # health bar gets drawn in last scene self.bar.setDepthWrite(False) # turns of depth writing so it doesn't interfere with itself self.bar.setLightOff() # fixes the color on the bar itself base.disableMouse() # Go through gamesetup sequence self.setup() # Add update task to task manager taskMgr.add(self.update, 'updateWorld') taskMgr.add(self.updateWinLose, 'winLose') taskMgr.add(self.startMenu, 'startMenu') # Create a floater object self.floater = NodePath(PandaNode("floater")) self.floater.reparentTo(render) def toggleDebug(self): if self.debugNP.isHidden(): self.debugNP.show() else: self.debugNP.hide() def doExit(self): render.getChildren().detach() sys.exit(1) def doRestart(self): self.worldCondition = True self.menuOn = False self.onLevelTwo = False # Hide menu self.mainMenuBackground.hide() for b in self.buttons: b.hide() # Set music to level 1 music if self.backgroundMusic.play: self.backgroundMusic.stop() self.backgroundMusic = loader.loadSfx('../sounds/elfman-piano-solo.ogg') self.backgroundMusic.setLoop(True) self.backgroundMusic.play() # Set player back to starting state for level 1 self.player.startPosLevel1() self.bar["value"] = 100 self.health = 100 # Set enemies back to starting state for enemy in self.enemies: enemy.backToStartPos() # Set collectibles back to starting state for l in self.letters: l.removeAllChildren() self.world.remove(l) self.letters[:] = [] self.collectedLetters[:] = [] self.createSetOfLetters() self.numObjects.setText("Find letters B R E A K to escape\nLetters Remaining: " + str(len(self.letters))) def doRestartLevel2(self): self.worldCondition = True self.menuOn = False self.onLevelTwo = True # Hide menu self.mainMenuBackground.hide() for b in self.buttons: b.hide() # Set skybox to level 2 skybox self.skybox.removeNode() self.skybox = loader.loadModel('../models/skybox.egg') self.skybox.setScale(1100) # make big enough to cover whole terrain self.skybox.setBin('background', 1) self.skybox.setDepthWrite(0) self.skybox.setLightOff() self.skybox.reparentTo(render) # b = OnscreenImage(parent=render2d, image="../models/textures/storm.jpg") # base.cam.node().getDisplayRegion(0).setSort(30) # Set music to level 2 music if self.backgroundMusic.play: self.backgroundMusic.stop() self.backgroundMusic = loader.loadSfx('../sounds/irving-montage.m4a') self.backgroundMusic.setLoop(True) self.backgroundMusic.play() # Set player back to starting state for level 2 self.player.startPosLevel2() self.bar["value"] = 100 self.health = 100 # Set enemies back to starting state for enemy in self.enemies: enemy.backToStartPos() # Set collectibles back to starting state for l in self.letters: l.removeAllChildren() self.world.remove(l) self.letters[:] = [] self.collectedLetters[:] = [] self.createSetOfLetters() self.numObjects.setText("Find letters B R E A K to escape\nLetters Remaining: " + str(len(self.letters))) def createPlatform(self, x, y, z): self.platform = loader.loadModel('../models/disk/disk.egg') geomnodes = self.platform.findAllMatches('**/+GeomNode') gn = geomnodes.getPath(0).node() geom = gn.getGeom(0) mesh = BulletTriangleMesh() mesh.addGeom(geom) shape = BulletTriangleMeshShape(mesh, dynamic=False) node = BulletRigidBodyNode('Platform') node.setMass(0) node.addShape(shape) platformnn = render.attachNewNode(node) platformnn.setPos(x, y, z) platformnn.setScale(3) self.world.attachRigidBody(node) self.platform.reparentTo(platformnn) def createWall(self, x, y, z, h): self.wall = loader.loadModel('../models/wall/wall.egg') geomnodes = self.wall.findAllMatches('**/+GeomNode') gn = geomnodes.getPath(0).node() geom = gn.getGeom(0) mesh = BulletTriangleMesh() mesh.addGeom(geom) shape = BulletTriangleMeshShape(mesh, dynamic=False) wallNode = BulletRigidBodyNode('Wall') wallNode.setMass(0) wallNode.addShape(shape) wallnn = render.attachNewNode(wallNode) wallnn.setPos(x, y, z) wallnn.setH(h) wallnn.setScale(0.5, 50.5, 4.5) self.world.attachRigidBody(wallNode) self.wall.reparentTo(wallnn) def createLetter(self, loadFile, name, x, y, z): self.name = name self.letter = loader.loadModel(loadFile) geomnodes = self.letter.findAllMatches('**/+GeomNode') gn = geomnodes.getPath(0).node() geom = gn.getGeom(0) mesh = BulletTriangleMesh() mesh.addGeom(geom) shape = BulletTriangleMeshShape(mesh, dynamic=False) self.letterNode = BulletRigidBodyNode('Letter') self.letterNode.setMass(0) self.letterNode.addShape(shape) self.letters.append(self.letterNode) letternn = render.attachNewNode(self.letterNode) letternn.setPos(x, y, z) letternn.setScale(1) letternn.setP(90) # orients the mesh for the letters to be upright self.world.attachRigidBody(self.letterNode) self.letter.reparentTo(letternn) self.letter.setP(-90) # orients the actual letter objects to be upright # Collect the letters def collectLetters(self): for letter in self.letters: contactResult = self.world.contactTestPair(self.player.character, letter) if len(contactResult.getContacts()) > 0: self.collect.play() letter.removeAllChildren() self.world.remove(letter) self.letters.remove(letter) self.collectedLetters.append(letter) self.numObjects.setText("Find letters B R E A K to escape\nLetters Remaining: " + str(len(self.letters))) def clearRemainingLetters(self): letter.removeAllChildren() self.world.remove(letter) self.letters.remove(letter) def enemyAttackDecision(self): for enemy in self.enemies: enemyProximity = enemy.badCharacterNP.getDistance(self.player.characterNP) # Manually set enemy's z so it doesn't fly up to match player's z characterPos = self.player.characterNP.getPos() characterPos.setZ(enemy.badCharacterNP.getZ()) # Create direct path from enemy to player enemyPos = enemy.badCharacterNP.getPos() vec = characterPos - enemyPos vec.normalize() enemymovement = vec * 0.15 + enemyPos # Make a list of all the enemies in the list except for current enemy self.otherEnemies = self.enemies[:] self.otherEnemies.remove(enemy) # If 2 enemies get too close to each other, send one back to their starting position for otherEnemy in self.otherEnemies: enemy2enemyProx = enemy.badCharacterNP.getDistance(otherEnemy.badCharacterNP) if enemy2enemyProx < 1: otherEnemy.backToStartPos() if enemyProximity < 20 and enemyProximity > 2: enemy.badCharacterNP.lookAt(self.player.characterNP) enemy.badCharacterNP.setPos(enemymovement) if enemyProximity < 20 and enemyProximity > 2 and not enemy.badActorNP.getAnimControl("walk").isPlaying(): enemy.badActorNP.loop("walk") if enemyProximity < 2 and not enemy.badActorNP.getAnimControl("attack").isPlaying(): enemy.badActorNP.stop() enemy.badActorNP.loop("attack") if enemyProximity < 2 and not self.player.actorNP.getAnimControl("damage").isPlaying() and self.menuOn == False: self.player.actorNP.play("damage") self.damage.play() self.isTakingDamage = True if self.player.character.isOnGround() and self.isTakingDamage: if self.player.isNotWalking and not self.player.actorNP.getAnimControl("walk").isPlaying(): # self.player.actorNP.stop("damage") self.player.actorNP.loop("walk") self.player.isTakingDamage = False if enemyProximity < 2: self.reduceHealth() # When robot comes in contact with enemy, health is reduced def reduceHealth(self): self.bar["value"] -= 0.3 # Build menu with background and buttons def buildMenu(self): self.menuOn = True self.mainMenuBackground = OnscreenImage(image='../models/main-menu-background.png', pos=(0, 0, 0), scale=(1.4, 1, 1)) button_level1 = DirectButton(text="LEVEL 1", scale=.1, pos=(0.23, -0.2, -0.65), command=self.doRestart) button_level2 = DirectButton(text="LEVEL 2", scale=.1, pos=(0.65, -0.2, -0.65), command=self.doRestartLevel2) button_quit = DirectButton(text="QUIT", scale=.1, pos=(1, -0.2, -0.65), command=self.doExit) # Redetermine size or else buttons may not be clickable self.buttons = [button_level1, button_level2, button_quit] for b in self.buttons: b.setTransparency(1) b.resetFrameSize() # Menus for losing conditions def updateWinLose(self, task): if self.bar["value"] < 1 and self.worldCondition: # Stop music and show menu self.worldCondition = False self.backgroundMusic.stop() self.dead.play() self.buildMenu() if len(self.letters) == 0 and len(self.collectedLetters) > 0 and self.worldCondition: # Stop music and show menu self.worldCondition = False self.backgroundMusic.stop() self.winner.play() self.buildMenu() return task.cont # Load main menu def startMenu(self, task): if self.start: self.buildMenu() return task.done return task.cont def createEnemies(self): # Level 2 Enemies (Mixed bag) self.enemies.append(Enemy(render, self.world, -220.549, 427.425, -1, "Scientist")) self.enemies.append(Enemy(render, self.world, -244.055, 304.031, -1, "Cinder")) self.enemies.append(Enemy(render, self.world, -250.473, 294.736, -1, "Shield")) self.enemies.append(Enemy(render, self.world, -214.313, 77.9221, -1, "Brawler")) self.enemies.append(Enemy(render, self.world, -210.797, 63.8862, -1, "Voltage")) self.enemies.append(Enemy(render, self.world, -212.291, 19.0834, -1, "Bricker")) self.enemies.append(Enemy(render, self.world, -226.271, -170.715, -1, "Scientist")) self.enemies.append(Enemy(render, self.world, -231.582, -171.925, -1, "Brawler")) self.enemies.append(Enemy(render, self.world, -236.969, -178.02, -1, "Shield")) # Level 1 Security guards self.enemies.append(Enemy(render, self.world, 65, 68, -1, "SecurityGuard")) self.enemies.append(Enemy(render, self.world, 204.968, 212.61, -1, "SecurityGuard")) self.enemies.append(Enemy(render, self.world, 209.655, 203.636, -1, "SecurityGuard")) self.enemies.append(Enemy(render, self.world, 332.143, 455.849, -1, "SecurityGuard")) self.enemies.append(Enemy(render, self.world, 323.669, 460.24, -1, "SecurityGuard")) self.enemies.append(Enemy(render, self.world, 329.634, 468.654, -1, "SecurityGuard")) self.enemies.append(Enemy(render, self.world, 200.354, 710.706, -1, "SecurityGuard")) self.enemies.append(Enemy(render, self.world, 212.038, 724.852, -1, "SecurityGuard")) self.enemies.append(Enemy(render, self.world, 190.676, 735.513, -1, "SecurityGuard")) self.enemies.append(Enemy(render, self.world, 177.801, 714.21, -1, "SecurityGuard")) self.enemies.append(Enemy(render, self.world, 180.247, 706.548, -1, "SecurityGuard")) def createSetOfLetters(self): self.letterB = '../models/letters/letter_b.egg' self.letterR = '../models/letters/letter_r.egg' self.letterE = '../models/letters/letter_e.egg' self.letterA = '../models/letters/letter_a.egg' self.letterK = '../models/letters/letter_k.egg' if self.onLevelTwo: self.createLetter(self.letterB, "B", -220, 417, 16) self.createLetter(self.letterR, "R", -251, 288, 20) self.createLetter(self.letterE, "E", -201.7, 53, 33) self.createLetter(self.letterA, "A", -206, 41, 25) self.createLetter(self.letterK, "K", -232, -174, 16) else: self.createLetter(self.letterB, "B", 72, 70.2927, 0) self.createLetter(self.letterR, "R", 225, 223, 4) self.createLetter(self.letterE, "E", 340, 471, 3.1) self.createLetter(self.letterA, "A", 335, 483, 6) self.createLetter(self.letterK, "K", 197, 721, 0) def createMovingPlatforms(self): # Platforms to collect B self.movingPlatforms.append(MovingPlatform(render, self.world, -220, 417, -2.2)) # Platforms to collect R self.movingPlatforms.append(MovingPlatform(render, self.world, -240, 288, -2.2)) self.movingPlatforms.append(MovingPlatform(render, self.world, -251, 288, 3.2)) # Platforms to collect E and A self.movingPlatforms.append(MovingPlatform(render, self.world, -214, 72, -2.2)) self.movingPlatforms.append(MovingPlatform(render, self.world, -203, 72, 4.2)) self.movingPlatforms.append(MovingPlatform(render, self.world, -202, 53, 16.2)) self.movingPlatforms.append(MovingPlatform(render, self.world, -207, 40.5, 7.2)) # Platform to collect K self.movingPlatforms.append(MovingPlatform(render, self.world, -232, -174, -2.2)) def update(self, task): dt = globalClock.getDt() self.player.processInput(dt) self.world.doPhysics(dt, 4, 1./240.) # Call camera function in player class self.player.cameraFollow(self.floater) # Identifying player collecting items self.collectLetters() # Start from beginning position if player falls off track if self.player.characterNP.getZ() < -10.0: if self.onLevelTwo: self.player.startPosLevel2() else: self.player.startPosLevel1() # If player gets too close to enemy, enemy attacks self.enemyAttackDecision() return task.cont def setup(self): # Debug (useful to turn on for physics) self.debugNP = self.render.attachNewNode(BulletDebugNode('Debug')) self.debugNP.hide() # Physics World self.world = BulletWorld() self.world.setGravity(Vec3(0, 0, -9.81)) self.world.setDebugNode(self.debugNP.node()) # Main Character self.player = Player() self.player.createPlayer(render, self.world) # Enemies self.createEnemies() # Music self.backgroundMusic = loader.loadSfx('../sounds/elfman-piano-solo.ogg') self.backgroundMusic.setLoop(True) self.backgroundMusic.stop() self.backgroundMusic.setVolume(0.9) # lower this when I add sound effects # Sound Effects self.collect = base.loader.loadSfx("../sounds/collect-sound.wav") self.collect.setVolume(1) self.damage = base.loader.loadSfx("../sounds/damage.wav") self.damage.setVolume(0.5) self.winner = base.loader.loadSfx("../sounds/win-yay.wav") self.winner.setVolume(1) self.dead = base.loader.loadSfx("../sounds/severe-damage.wav") self.dead.setVolume(1) # Level 1 Skybox self.skybox = loader.loadModel('../models/skybox_galaxy.egg') self.skybox.setScale(1000) # make big enough to cover whole terrain self.skybox.setBin('background', 1) self.skybox.setDepthWrite(0) self.skybox.setLightOff() self.skybox.reparentTo(render) # Lighting dLight = DirectionalLight("dLight") dLight.setColor(Vec4(0.8, 0.8, 0.5, 1)) dLight.setDirection(Vec3(-5, -5, -5)) dlnp = render.attachNewNode(dLight) dlnp.setHpr(0, 60, 0) render.setLight(dlnp) aLight = AmbientLight("aLight") aLight.setColor(Vec4(0.5, 0.5, 0.5, 1)) alnp = render.attachNewNode(aLight) render.setLight(alnp) # Fog colour = (0.2, 0.2, 0.3) genfog = Fog("general fog") genfog.setColor(*colour) genfog.setExpDensity(0.003) render.setFog(genfog) base.setBackgroundColor(*colour) # Create wall (x, y, z, h) self.createWall(-30.2215, -6.2, -2, 45) self.createWall(-203, 555.8, -2, 70) #-----Level 1 Platforms----- # Platform to collect B self.createPlatform(72, 70.2927, -1) # Platforms to collect R self.createPlatform(211, 210, -1) self.createPlatform(225, 223, 2.7) # Platforms to collect E and A self.createPlatform(330, 462, -0.4) self.createPlatform(340, 471, 2.1) self.createPlatform(350, 480, 4) self.createPlatform(335, 483, 5) # Platforms to collect K self.createPlatform(184, 712, -1) self.createPlatform(208, 730, -1) self.createPlatform(207, 711, -1) self.createPlatform(186, 731, -1) #-----Level 2 Platforms----- # Moving platforms if self.movingPlatforms > 0: del self.movingPlatforms[:] self.createMovingPlatforms() # Create complex mesh for Track using BulletTriangleMeshShape mesh = BulletTriangleMesh() self.track = loader.loadModel("../models/mountain_valley_track.egg") self.track.flattenStrong() for geomNP in self.track.findAllMatches('**/+GeomNode'): geomNode = geomNP.node() ts = geomNP.getTransform(self.track) for geom in geomNode.getGeoms(): mesh.addGeom(geom, ts) shape = BulletTriangleMeshShape(mesh, dynamic=False) node = BulletRigidBodyNode('Track') node.setMass(0) node.addShape(shape) tracknn = render.attachNewNode(node) self.world.attachRigidBody(tracknn.node()) tracknn.setPos(27, -5, -2) self.track.reparentTo(tracknn)
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): force = Vec3(0, 0, 0) torque = Vec3(0, 0, 0) if inputState.isSet('forward'): force.setY(1.0) if inputState.isSet('reverse'): force.setY(-1.0) if inputState.isSet('left'): force.setX(-1.0) if inputState.isSet('right'): force.setX(1.0) if inputState.isSet('turnLeft'): torque.setZ(1.0) if inputState.isSet('turnRight'): torque.setZ(-1.0) force *= 30.0 torque *= 10.0 force = render.getRelativeVector(self.boxNP, force) torque = render.getRelativeVector(self.boxNP, torque) self.boxNP.node().setActive(True) self.boxNP.node().applyCentralForce(force) self.boxNP.node().applyTorque(torque) def update(self, task): dt = globalClock.getDt() self.processInput(dt) #self.world.doPhysics(dt) self.world.doPhysics(dt, 5, 1.0 / 180.0) return task.cont def cleanup(self): self.world.removeRigidBody(self.groundNP.node()) self.world.removeRigidBody(self.boxNP.node()) self.world = None self.debugNP = None self.groundNP = None self.boxNP = None self.worldNP.removeNode() def setup(self): self.worldNP = render.attachNewNode('World') # World self.debugNP = self.worldNP.attachNewNode(BulletDebugNode('Debug')) self.debugNP.show() self.debugNP.node().showWireframe(True) self.debugNP.node().showConstraints(True) self.debugNP.node().showBoundingBoxes(False) self.debugNP.node().showNormals(True) #self.debugNP.showTightBounds() #self.debugNP.showBounds() self.world = BulletWorld() self.world.setGravity(Vec3(0, 0, -9.81)) self.world.setDebugNode(self.debugNP.node()) # Ground (static) shape = BulletPlaneShape(Vec3(0, 0, 1), 1) self.groundNP = self.worldNP.attachNewNode( BulletRigidBodyNode('Ground')) self.groundNP.node().addShape(shape) self.groundNP.setPos(0, 0, -2) self.groundNP.setCollideMask(BitMask32.allOn()) self.world.attachRigidBody(self.groundNP.node()) # Box (dynamic) shape = BulletBoxShape(Vec3(0.5, 0.5, 0.5)) self.boxNP = self.worldNP.attachNewNode(BulletRigidBodyNode('Box')) self.boxNP.node().setMass(1.0) self.boxNP.node().addShape(shape) self.boxNP.setPos(0, 0, 2) #self.boxNP.setScale(2, 1, 0.5) self.boxNP.setCollideMask(BitMask32.allOn()) #self.boxNP.node().setDeactivationEnabled(False) self.world.attachRigidBody(self.boxNP.node()) visualNP = loader.loadModel('models/box.egg') visualNP.clearModelNodes() visualNP.reparentTo(self.boxNP)
class Game(DirectObject): def __init__(self): base.setBackgroundColor(0.1, 0.1, 0.8, 1) base.setFrameRateMeter(True) base.cam.setPos(0, -20, 5) 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) self.accept('enter', self.doShoot) # 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') def doShoot(self): # Get from/to points from mouse click pMouse = base.mouseWatcherNode.getMouse() pFrom = Point3() pTo = Point3() base.camLens.extrude(pMouse, pFrom, pTo) pFrom = render.getRelativePoint(base.cam, pFrom) pTo = render.getRelativePoint(base.cam, pTo) # Calculate initial velocity v = pTo - pFrom v.normalize() v *= 100.0 # Create bullet shape = BulletSphereShape(0.3) body = BulletRigidBodyNode('Bullet') bodyNP = self.worldNP.attachNewNode(body) bodyNP.node().addShape(shape) bodyNP.node().setMass(1.0) bodyNP.node().setLinearVelocity(v) bodyNP.node().setCcdMotionThreshold(1e-7); bodyNP.node().setCcdSweptSphereRadius(0.50); bodyNP.setCollideMask(BitMask32.allOn()) bodyNP.setPos(pFrom) visNP = loader.loadModel('models/ball.egg') visNP.setScale(0.8) visNP.reparentTo(bodyNP) self.world.attachRigidBody(bodyNP.node()) # Remove the bullet again after 2 seconds taskMgr.doMethodLater(2, self.doRemove, 'doRemove', extraArgs=[bodyNP], appendTask=True) def doRemove(self, bodyNP, task): self.world.removeRigidBody(bodyNP.node()) bodyNP.removeNode() return task.done # ____TASK___ def update(self, task): dt = globalClock.getDt() self.world.doPhysics(dt, 20, 1.0/180.0) return task.cont def cleanup(self): self.worldNP.removeNode() self.worldNP = None self.world = None def setup(self): self.worldNP = render.attachNewNode('World') # World self.debugNP = self.worldNP.attachNewNode(BulletDebugNode('Debug')) self.debugNP.show() self.debugNP.node().showWireframe(True) self.debugNP.node().showConstraints(True) self.debugNP.node().showBoundingBoxes(False) self.debugNP.node().showNormals(False) self.world = BulletWorld() self.world.setGravity(Vec3(0, 0, -9.81)) self.world.setDebugNode(self.debugNP.node()) # Box A shape = BulletBoxShape(Vec3(0.5, 0.5, 0.5)) bodyA = BulletRigidBodyNode('Box A') bodyNP = self.worldNP.attachNewNode(bodyA) bodyNP.node().addShape(shape) bodyNP.setCollideMask(BitMask32.allOn()) bodyNP.setPos(-2, 0, 1) visNP = loader.loadModel('models/box.egg') visNP.clearModelNodes() visNP.reparentTo(bodyNP) self.world.attachRigidBody(bodyA) # Box B shape = BulletBoxShape(Vec3(0.5, 0.5, 0.5)) 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(2, 0, 1) visNP = loader.loadModel('models/box.egg') visNP.clearModelNodes() visNP.reparentTo(bodyNP) self.world.attachRigidBody(bodyB) # Hinge pivotA = Point3(2, 0, 0) pivotB = Point3(-4, 0, 0) 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(-90, 120, softness=0.9, bias=0.3, relaxation=1.0) self.world.attachConstraint(hinge)
class MyApp(ShowBase): def __init__(self, screen_size=84, DEBUGGING=False, human_playable=False): ShowBase.__init__(self) self.forward_button = KeyboardButton.ascii_key(b'w') self.backward_button = KeyboardButton.ascii_key(b's') self.fps = 20 self.human_playable = human_playable self.actions = 3 self.last_frame_start_time = time.time() self.action_buffer = [1, 1, 1] self.last_teleport_time = 0.0 self.time_to_teleport = False if self.human_playable is False: winprops = WindowProperties.size(screen_size, screen_size) fbprops = FrameBufferProperties() fbprops.set_rgba_bits(8, 8, 8, 0) fbprops.set_depth_bits(24) self.pipe = GraphicsPipeSelection.get_global_ptr().make_module_pipe('pandagl') self.imageBuffer = self.graphicsEngine.makeOutput( self.pipe, "image buffer", 1, fbprops, winprops, GraphicsPipe.BFRefuseWindow) self.camera = Camera('cam') self.cam = NodePath(self.camera) self.cam.reparentTo(self.render) self.dr = self.imageBuffer.makeDisplayRegion() self.dr.setCamera(self.cam) self.render.setShaderAuto() self.cam.setPos(0.5, 0, 6) self.cam.lookAt(0.5, 0, 0) # Create Ambient Light self.ambientLight = AmbientLight('ambientLight') self.ambientLight.setColor((0.2, 0.2, 0.2, 1)) self.ambientLightNP = self.render.attachNewNode(self.ambientLight) self.render.setLight(self.ambientLightNP) # Spotlight self.light = Spotlight('light') self.light.setColor((0.9, 0.9, 0.9, 1)) self.lightNP = self.render.attachNewNode(self.light) self.lightNP.setPos(0, 10, 10) self.lightNP.lookAt(0, 0, 0) self.lightNP.node().getLens().setFov(40) self.lightNP.node().getLens().setNearFar(10, 100) self.lightNP.node().setShadowCaster(True, 1024, 1024) self.render.setLight(self.lightNP) self.world = BulletWorld() self.world.setGravity(Vec3(0, 0, -9.81)) if DEBUGGING is True: debugNode = BulletDebugNode('Debug') debugNode.showWireframe(True) debugNode.showConstraints(True) debugNode.showBoundingBoxes(False) debugNode.showNormals(False) debugNP = render.attachNewNode(debugNode) debugNP.show() self.world.setDebugNode(debugNP.node()) self.finger_speed_mps = 0.0 self.penalty_applied = False self.teleport_cooled_down = True self.fps = 20 self.framecount = 0 self.reset() def reset(self): namelist = ['Ground', 'Conveyor', 'Finger', 'Block', 'Scrambled Block', 'Not Rewardable', 'Teleport Me'] for child in self.render.getChildren(): for test in namelist: if child.node().name == test: self.world.remove(child.node()) child.removeNode() break # Plane self.plane_shape = BulletPlaneShape(Vec3(0, 0, 1), 1) self.plane_node = BulletRigidBodyNode('Ground') self.plane_node.addShape(self.plane_shape) self.plane_np = self.render.attachNewNode(self.plane_node) self.plane_np.setPos(0.0, 0.0, -1.0) self.world.attachRigidBody(self.plane_node) # Conveyor self.conv_node = BulletRigidBodyNode('Conveyor') self.conv_node.setFriction(1.0) self.conv_np = self.render.attachNewNode(self.conv_node) self.conv_shape = BulletBoxShape(Vec3(100.0, 1.0, 0.05)) self.conv_node.setMass(1000.0) self.conv_np.setPos(-95.0, 0.0, 0.1) self.conv_node.addShape(self.conv_shape) self.world.attachRigidBody(self.conv_node) self.model = loader.loadModel('assets/conv.egg') self.model.flattenLight() self.model.reparentTo(self.conv_np) # Finger self.finger_node = BulletRigidBodyNode('Finger') self.finger_node.setFriction(1.0) self.finger_np = self.render.attachNewNode(self.finger_node) self.finger_shape = BulletCylinderShape(0.1, 0.25, ZUp) self.finger_node.setMass(0) self.finger_np.setPos(1.8, 0.0, 0.24 + 0.0254*3.5) self.finger_node.addShape(self.finger_shape) self.world.attachRigidBody(self.finger_node) self.model = loader.loadModel('assets/finger.egg') self.model.flattenLight() self.model.reparentTo(self.finger_np) self.blocks = [] for block_num in range(15): new_block = self.spawn_block(Vec3(28, random.uniform(-3, 3), (0.2 * block_num) + 2.0), 2, random.choice([4, 4, 6]), random.choice([10, 11, 12, 13, 14, 15, 15, 16, 16, 17, 17, 18, 18, 18, 18, 19, 19, 19, 19, 20, 20, 20, 20, 20, 21, 21, 21, 21, 22, 22, 22, 23, 23, 23, 23, 23, 24])) # new_block = self.spawn_block(Vec3(18, 0, (0.2 * block_num) + 2.0), # 2, 4, 24) self.blocks.append(new_block) self.finger_speed_mps = 0.0 self.penalty_applied = False self.teleport_cooled_down = True self.fps = 20 self.framecount = 0 self.last_teleport_time = 0.0 self.time_to_teleport = False return self.step(1)[0] def spawn_block(self, location, z_inches, y_inches, x_inches): """ Spawns a block """ node = BulletRigidBodyNode('Block') node.setFriction(1.0) block_np = self.render.attachNewNode(node) shape = BulletBoxShape(Vec3(0.0254*y_inches, 0.0254*x_inches, 0.0254*z_inches)) node.setMass(1.0) block_np.setPos(location) block_np.setHpr(random.uniform(-60, 60), 0.0, 0.0) node.addShape(shape) self.world.attachRigidBody(node) model = loader.loadModel('assets/bullet-samples/models/box.egg') model.setH(90) model.setSx(0.0254*x_inches*2) model.setSy(0.0254*y_inches*2) model.setSz(0.0254*z_inches*2) model.flattenLight() model.reparentTo(block_np) block_np.node().setTag('scrambled', 'False') return block_np def get_camera_image(self, requested_format=None): """ Returns the camera's image, which is of type uint8 and has values between 0 and 255. The 'requested_format' argument should specify in which order the components of the image must be. For example, valid format strings are "RGBA" and "BGRA". By default, Panda's internal format "BGRA" is used, in which case no data is copied over. """ tex = self.dr.getScreenshot() if requested_format is None: data = tex.getRamImage() else: data = tex.getRamImageAs(requested_format) image = np.frombuffer(data, np.uint8) # use data.get_data() instead of data in python 2 image.shape = (tex.getYSize(), tex.getXSize(), tex.getNumComponents()) image = np.flipud(image) return image[:,:,:3] def reset_conv(self): conveyor_dist_left = 1 - self.conv_np.getPos()[0] if conveyor_dist_left < 10: self.conv_np.setX(-95.0) self.conv_np.setY(0.0) def check_rewards(self): reward = 0 for block in self.blocks: block_x, block_y, block_z = block.getPos() if block_z > 0.16 and block_x > -1 and block_x < 0: block.node().setTag('scrambled', 'True') if block_x < 2.3 and block_z < 0.16 and block.node().getTag('scrambled') == 'True': block.node().setTag('scrambled', 'False') reward = 1 return reward def check_teleportable(self, blocks_per_minute): self.time = self.framecount/self.fps # if self.time % (1/(blocks_per_minute/60)) < 0.1: # self.time_to_teleport = True # else: # self.time_to_teleport = False # self.teleport_cooled_down = True teleport_cooled_down = True if self.last_teleport_time + 0.4 < self.time else False if random.choice([True, False, False, False]) and teleport_cooled_down: self.last_teleport_time = self.time self.time_to_teleport = True for block in self.blocks: block_x = block.getPos()[0] if block_x > 5: block.node().setTag('scrambled', 'False') if self.time_to_teleport is True: self.time_to_teleport = False block.setX(-3) block.setY(0.0) block.setZ(2.0) block.setHpr(random.uniform(-60, 60), 0.0, 0.0) def step(self, action): dt = 1/self.fps self.framecount += 1 max_dist = 1.1 # Move finger finger_max_speed = 2 finger_accel = 10.0 finger_deccel = 10.0 self.action_buffer.pop(0) self.action_buffer.append(action) action = self.action_buffer[0] if action == 0: self.finger_speed_mps += dt * finger_accel if self.finger_speed_mps > finger_max_speed: self.finger_speed_mps = 2 if action == 1: if self.finger_speed_mps > 0.01: self.finger_speed_mps -= finger_deccel * dt if self.finger_speed_mps < -0.01: self.finger_speed_mps += finger_deccel * dt if action == 2: self.finger_speed_mps -= dt * finger_accel if self.finger_speed_mps < -finger_max_speed: self.finger_speed_mps = -finger_max_speed real_displacement = self.finger_speed_mps * dt self.finger_np.setY(self.finger_np.getY() + real_displacement) if self.finger_np.getY() > max_dist: self.finger_np.setY(max_dist) self.finger_speed_mps = 0 if self.finger_np.getY() < -max_dist: self.finger_np.setY(-max_dist) self.finger_speed_mps = 0 # self.world.doPhysics(dt, 5, 1.0/120.0) self.world.doPhysics(dt, 20, 1.0/240.0) self.reset_conv() self.check_teleportable(blocks_per_minute=59) # Keep the conveyor moving self.conv_np.node().setLinearVelocity(Vec3(1.0, 0.0, 0.0)) if self.human_playable is False: self.graphicsEngine.renderFrame() TransformState.garbageCollect() RenderState.garbageCollect() image = self.get_camera_image() else: image = None score = 0 score += self.check_rewards() done = False return image, score, done def update(self, task): is_down = self.mouseWatcherNode.is_button_down next_act = 1 if is_down(self.forward_button): next_act = 0 if is_down(self.backward_button): next_act = 2 _, reward, _ = self.step(next_act) if reward != 0: print(reward) last_frame_duration = time.time() - self.last_frame_start_time if last_frame_duration < (1/self.fps): time.sleep((1/self.fps) - last_frame_duration) self.last_frame_start_time = time.time() return task.cont
class TestApplication(ShowBase): def __init__(self): ShowBase.__init__(self) # game objects self.controlled_obj_index_ = 0 self.level_sectors_ = [] self.controlled_objects_ = [] # active objects self.active_sector_ = None self.setupRendering() self.setupControls() self.setupPhysics() self.clock_ = ClockObject() self.kinematic_mode_ = False # Task logging.info("TestSectors demo started") taskMgr.add(self.update, 'updateWorld') def setupRendering(self): self.setBackgroundColor(0.1, 0.1, 0.8, 1) self.setFrameRateMeter(True) # Light alight = AmbientLight('ambientLight') alight.setColor(Vec4(0.5, 0.5, 0.5, 1)) alightNP = self.render.attachNewNode(alight) dlight = DirectionalLight('directionalLight') dlight.setDirection(Vec3(1, 1, -1)) dlight.setColor(Vec4(0.7, 0.7, 0.7, 1)) dlightNP = self.render.attachNewNode(dlight) self.render.clearLight() self.render.setLight(alightNP) self.render.setLight(dlightNP) self.setupBackgroundImage() def setupBackgroundImage(self): image_file = Filename(rospack.RosPack().get_path(BACKGROUND_IMAGE_PACKAGE) + '/resources/backgrounds/' + BACKGROUND_IMAGE_PATH) # check if image can be loaded img_head = PNMImageHeader() if not img_head.readHeader(image_file ): raise IOError, "PNMImageHeader could not read file %s. Try using absolute filepaths"%(image_file.c_str()) sys.exit() # Load the image with a PNMImage w = img_head.getXSize() h = img_head.getYSize() img = PNMImage(w,h) #img.alphaFill(0) img.read(image_file) texture = Texture() texture.setXSize(w) texture.setYSize(h) texture.setZSize(1) texture.load(img) texture.setWrapU(Texture.WM_border_color) # gets rid of odd black edges around image texture.setWrapV(Texture.WM_border_color) texture.setBorderColor(LColor(0,0,0,0)) # creating CardMaker to hold the texture cm = CardMaker('background') cm.setFrame(-0.5*w,0.5*w,-0.5*h,0.5*h) # This configuration places the image's topleft corner at the origin (left, right, bottom, top) background_np = NodePath(cm.generate()) background_np.setTexture(texture) background_np.reparentTo(self.render) background_np.setPos(BACKGROUND_POSITION) background_np.setScale(BACKGROUND_IMAGE_SCALE) def setupControls(self): # Input (Events) 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) self.accept('n', self.selectNextControlledObject) self.accept('k', self.toggleKinematicMode) # Inputs (Polling) self.input_state_ = InputState() self.input_state_.watchWithModifiers("right","arrow_right") self.input_state_.watchWithModifiers('left', 'arrow_left') self.input_state_.watchWithModifiers('jump', 'arrow_up') self.input_state_.watchWithModifiers('stop', 'arrow_down') self.input_state_.watchWithModifiers('roll_right', 'c') self.input_state_.watchWithModifiers('roll_left', 'z') self.input_state_.watchWithModifiers('roll_stop', 'x') self.title = addTitle("Panda3D: Kinematic Objects") self.inst1 = addInstructions(0.06, "ESC: Quit") self.inst2 = addInstructions(0.12, "Up/Down: Jump/Stop") self.inst3 = addInstructions(0.18, "Left/Right: Move Left / Move Rigth") self.inst4 = addInstructions(0.24, "z/x/c : Rotate Left/ Rotate Stop / Rotate Right") self.inst5 = addInstructions(0.30, "n: Select Next Character") self.inst6 = addInstructions(0.36, "k: Toggle Kinematic Mode") self.inst7 = addInstructions(0.42, "f1: Toggle Wireframe") self.inst8 = addInstructions(0.48, "f2: Toggle Texture") self.inst9 = addInstructions(0.54, "f3: Toggle BulletDebug") self.inst10 = addInstructions(0.60, "f5: Capture Screenshot") def processCollisions(self): contact_manifolds = self.physics_world_.getManifolds() for cm in contact_manifolds: node0 = cm.getNode0() node1 = cm.getNode1() col_mask1 = node0.getIntoCollideMask() col_mask2 = node1.getIntoCollideMask() if (col_mask1 == col_mask2) and \ (col_mask1 == SECTOR_ENTERED_BITMASK) and \ self.active_sector_.getSectorPlaneNode().getName() != node0.getName(): logging.info('Collision between %s and %s'%(node0.getName(),node1.getName())) sector = [s for s in self.level_sectors_ if s.getSectorPlaneNode().getName() == node0.getName()] sector = [s for s in self.level_sectors_ if s.getSectorPlaneNode().getName() == node1.getName()] + sector self.switchToSector(sector[0]) logging.info("Sector %s entered"%(self.active_sector_.getName())) def switchToSector(self,sector): obj_index = self.controlled_obj_index_ character_obj = self.controlled_objects_[obj_index] if self.active_sector_ is not None: self.active_sector_.releaseCharacter() character_obj.setY(sector,0) character_obj.setH(sector,0) self.active_sector_ = sector self.active_sector_.constrainCharacter(character_obj) self.active_sector_.enableDetection(False) sectors = [s for s in self.level_sectors_ if s != self.active_sector_] for s in sectors: s.enableDetection(True) def setupPhysics(self): # setting up physics world and parent node path self.physics_world_ = BulletWorld() self.world_node_ = self.render.attachNewNode('world') self.cam.reparentTo(self.world_node_) self.physics_world_.setGravity(Vec3(0, 0, -9.81)) self.debug_node_ = self.world_node_.attachNewNode(BulletDebugNode('Debug')) self.debug_node_.node().showWireframe(True) self.debug_node_.node().showConstraints(True) self.debug_node_.node().showBoundingBoxes(True) self.debug_node_.node().showNormals(True) self.physics_world_.setDebugNode(self.debug_node_.node()) self.debug_node_.hide() # setting up collision groups self.physics_world_.setGroupCollisionFlag(GAME_OBJECT_BITMASK.getLowestOnBit(),GAME_OBJECT_BITMASK.getLowestOnBit(),True) self.physics_world_.setGroupCollisionFlag(SECTOR_ENTERED_BITMASK.getLowestOnBit(),SECTOR_ENTERED_BITMASK.getLowestOnBit(),True) self.physics_world_.setGroupCollisionFlag(GAME_OBJECT_BITMASK.getLowestOnBit(),SECTOR_ENTERED_BITMASK.getLowestOnBit(),False) # setting up ground self.ground_ = self.world_node_.attachNewNode(BulletRigidBodyNode('Ground')) self.ground_.node().addShape(BulletPlaneShape(Vec3(0, 0, 1), 0)) self.ground_.setPos(0,0,0) self.ground_.setCollideMask(GAME_OBJECT_BITMASK) self.physics_world_.attachRigidBody(self.ground_.node()) # creating level objects self.setupLevelSectors() # creating controlled objects diameter = 0.4 sphere_visual = loader.loadModel('models/ball.egg') bounds = sphere_visual.getTightBounds() # start of model scaling extents = Vec3(bounds[1] - bounds[0]) scale_factor = diameter/max([extents.getX(),extents.getY(),extents.getZ()]) sphere_visual.clearModelNodes() sphere_visual.setScale(scale_factor,scale_factor,scale_factor) # end of model scaling sphere_visual.setTexture(loader.loadTexture('models/bowl.jpg'),1) sphere = NodePath(BulletRigidBodyNode('Sphere')) sphere.node().addShape(BulletSphereShape(0.5*diameter)) sphere.node().setMass(1.0) #sphere.node().setLinearFactor((1,0,1)) #sphere.node().setAngularFactor((0,1,0)) sphere.setCollideMask(GAME_OBJECT_BITMASK) sphere_visual.instanceTo(sphere) self.physics_world_.attachRigidBody(sphere.node()) self.controlled_objects_.append(sphere) sphere.reparentTo(self.world_node_) sphere.setPos(self.level_sectors_[0],Vec3(0,0,diameter+10)) sphere.setHpr(self.level_sectors_[0],Vec3(0,0,0)) # creating bullet ghost for detecting entry into other sectors player_ghost = sphere.attachNewNode(BulletGhostNode('player-ghost-node')) ghost_box_shape = BulletSphereShape(PLAYER_GHOST_DIAMETER/2) ghost_box_shape.setMargin(SECTOR_COLLISION_MARGIN) ghost_sphere_shape = BulletSphereShape(PLAYER_GHOST_DIAMETER*0.5) ghost_sphere_shape.setMargin(SECTOR_COLLISION_MARGIN) player_ghost.node().addShape(ghost_box_shape) player_ghost.setPos(sphere,Vec3(0,0,0)) player_ghost.node().setIntoCollideMask(SECTOR_ENTERED_BITMASK) self.physics_world_.attach(player_ghost.node()) # creating mobile box size = Vec3(0.2,0.2,0.4) mbox_visual = loader.loadModel('models/box.egg') bounds = mbox_visual.getTightBounds() extents = Vec3(bounds[1] - bounds[0]) scale_factor = 1/max([extents.getX(),extents.getY(),extents.getZ()]) mbox_visual.setScale(size.getX()*scale_factor,size.getY()*scale_factor,size.getZ()*scale_factor) mbox = NodePath(BulletRigidBodyNode('MobileBox')) mbox.node().addShape(BulletBoxShape(size/2)) mbox.node().setMass(1.0) #mbox.node().setLinearFactor((1,0,1)) #mbox.node().setAngularFactor((0,1,0)) mbox.setCollideMask(GAME_OBJECT_BITMASK) mbox_visual.instanceTo(mbox) self.physics_world_.attachRigidBody(mbox.node()) self.controlled_objects_.append(mbox) mbox.reparentTo(self.level_sectors_[0]) mbox.setPos(Vec3(1,0,size.getZ()+1)) # switching to sector 1 self.switchToSector(self.level_sectors_[0]) def setupLevelSectors(self): start_pos = Vec3(0,-20,0) for i in range(0,4): sector = Sector('sector' + str(i),self.physics_world_) sector.reparentTo(self.world_node_) #sector.setHpr(Vec3(60*(i+1),0,0)) sector.setHpr(Vec3(90*i,0,0)) sector.setPos(sector,Vec3(0,-20,i*4)) self.level_sectors_.append(sector) sector.setup() def update(self, task): self.clock_.tick() dt = self.clock_.getDt() self.processInput(dt) self.physics_world_.doPhysics(dt, 5, 1.0/180.0) self.processCollisions() self.updateCamera() return task.cont def processInput(self,dt): activate = False obj = self.controlled_objects_[self.controlled_obj_index_] if self.kinematic_mode_: obj.node().setKinematic(True) return else: obj.node().setKinematic(False) vel = self.active_sector_.getRelativeVector(self.world_node_,obj.node().getLinearVelocity()) w = self.active_sector_.getRelativeVector(self.world_node_, obj.node().getAngularVelocity()) vel.setY(0) #w.setX(0) #w.setZ(0) if self.input_state_.isSet('right'): vel.setX(2) activate = True if self.input_state_.isSet('left'): vel.setX(-2) activate = True if self.input_state_.isSet('jump'): vel.setZ(4) activate = True if self.input_state_.isSet('stop'): vel.setX(0) if self.input_state_.isSet('roll_right'): w.setY(ROTATIONAl_SPEED) activate = True if self.input_state_.isSet('roll_left'): w.setY(-ROTATIONAl_SPEED) activate = True if self.input_state_.isSet('roll_stop'): w.setY(0) if activate : obj.node().setActive(True,True) vel = self.world_node_.getRelativeVector(self.active_sector_,vel) w = self.world_node_.getRelativeVector(self.active_sector_,w) obj.node().setLinearVelocity(vel) obj.node().setAngularVelocity(w) #logging.info("Linear Velocity: %s"%(str(vel))) def updateCamera(self): if len(self.controlled_objects_) > 0: obj = self.controlled_objects_[self.controlled_obj_index_] self.cam.setPos(self.active_sector_,obj.getPos(self.active_sector_)) self.cam.setY(self.active_sector_,-CAM_DISTANCE/1) self.cam.lookAt(obj.getParent(),obj.getPos()) def cleanup(self): for i in range(0,len(self.controlled_objects_)): rb = self.controlled_objects_[i] self.physics_world_.removeRigidBody(rb.node()) rb.removeNode() self.object_nodes_ = [] self.controlled_objects_ = [] self.physics_world_.removeRigidBody(self.ground_.node()) self.ground_ = None self.physics_world_ = None self.debug_node_ = None self.cam.reparentTo(self.render) self.world_node_.removeNode() self.world_node_ = None # _____HANDLER_____ def toggleKinematicMode(self): self.kinematic_mode_ = not self.kinematic_mode_ print "Kinematic Mode %s"%('ON' if self.kinematic_mode_ else 'OFF') def selectNextControlledObject(self): self.controlled_obj_index_+=1 if self.controlled_obj_index_ >= len(self.controlled_objects_): self.controlled_obj_index_ = 0 # reset def doExit(self): self.cleanup() sys.exit(1) def doReset(self): self.cleanup() self.setupPhysics() def toggleDebug(self): if self.debug_node_.isHidden(): self.debug_node_.show() else: self.debug_node_.hide() def doScreenshot(self): self.screenshot('Bullet')
class Game(DirectObject): def __init__(self): base.setBackgroundColor(0.1, 0.1, 0.8, 1) base.setFrameRateMeter(True) base.cam.setPos(0, -40, 10) 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(5, 0, -2)) 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) # 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 update(self, task): dt = globalClock.getDt() self.world.doPhysics(dt, 10, 0.004) 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()) # Box shape = BulletBoxShape(Vec3(0.5, 0.5, 0.5) * 2.0) boxNP = self.worldNP.attachNewNode(BulletRigidBodyNode('Box')) boxNP.node().setMass(150.0) boxNP.node().addShape(shape) boxNP.setPos(0, 0, 2) boxNP.setCollideMask(BitMask32.allOn()) self.world.attachRigidBody(boxNP.node()) visualNP = loader.loadModel('models/box.egg') visualNP.clearModelNodes() visualNP.setScale(2.0) visualNP.reparentTo(boxNP) # Soft body world information info = self.world.getWorldInfo() info.setAirDensity(1.2) info.setWaterDensity(0) info.setWaterOffset(0) info.setWaterNormal(Vec3(0, 0, 0)) # Softbody nx = 31 ny = 31 p00 = Point3(-8, -8, 0) p10 = Point3(8, -8, 0) p01 = Point3(-8, 8, 0) p11 = Point3(8, 8, 0) bodyNode = BulletSoftBodyNode.makePatch(info, p00, p10, p01, p11, nx, ny, 1 + 2 + 4 + 8, True) material = bodyNode.appendMaterial() material.setLinearStiffness(0.4) bodyNode.generateBendingConstraints(2, material) bodyNode.setTotalMass(50.0) bodyNode.getShape(0).setMargin(0.5) bodyNP = self.worldNP.attachNewNode(bodyNode) self.world.attachSoftBody(bodyNode) # Rendering with Geom: fmt = GeomVertexFormat.getV3n3t2() geom = BulletHelper.makeGeomFromFaces(bodyNode, fmt, True) bodyNode.linkGeom(geom) visNode = GeomNode('') visNode.addGeom(geom) visNP = bodyNP.attachNewNode(visNode) # Now we want to have a texture and texture coordinates. # The geom's format has already a column for texcoords, so we just need # to write texcoords using a GeomVertexRewriter. tex = loader.loadTexture('models/panda.jpg') visNP.setTexture(tex) BulletHelper.makeTexcoordsForPatch(geom, nx, ny)
class Balls(ShowBase): def __init__(self): ShowBase.__init__(self) self.title = OnscreenText(text="0 balls", parent=base.a2dBottomRight, align=TextNode.ARight, fg=(1, 1, 1, 1), pos=(-0.1, 0.1), scale=.08, shadow=(0, 0, 0, 0.5)) # exit on esc self.accept('escape', sys.exit) # disable standart mouse based camera control self.disableMouse() # set camera position self.camera.setPos(0, -30, 25) self.camera.lookAt(0, 0, 0) # self.world = BulletWorld() self.world.setGravity(Vec3(0, 0, -9.81)) self.taskMgr.add(self.updateWorld, 'updateWorld') self.setupLight() # down self.makePlane(0, Vec3(0, 0, 1), (0, 0, 0), (0, 0, 0)) # up self.makePlane(1, Vec3(0, 0, -1), (0, 0, 10), (0, 0, 0)) # left self.makePlane(2, Vec3(1, 0, 0), (-5, 0, 5), (0, 0, 90)) # right self.makePlane(3, Vec3(-1, 0, 0), (5, 0, 5), (0, 0, -90)) # top self.makePlane(4, Vec3(0, 1, 0), (0, -5, 5), (0, 90, 0)) # buttom self.makePlane(5, Vec3(0, -1, 0), (0, 5, 5), (0, -90, 0)) self.accept('mouse1', self.pickBall) self.accept('mouse3', self.releaseBall) self.accept('arrow_up', partial(self.rotateCube, hpr = (0, ROTATE_SPEED, 0))) self.accept('arrow_down', partial(self.rotateCube, hpr = (0, -ROTATE_SPEED, 0))) self.accept('arrow_left', partial(self.rotateCube, hpr = (0, 0, -ROTATE_SPEED))) self.accept('arrow_right', partial(self.rotateCube, hpr = (0, 0, ROTATE_SPEED))) self.accept('space', self.shakeBalls) self.accept('page_up', self.addRandomBall) self.accept('page_down', self.rmBall) self.ballCnt = 0 self.ballColors = {} for num in xrange(DEFAULT_BALLS): self.addRandomBall() self.picked = set([]) def setupLight(self): ambientLight = AmbientLight("ambientLight") ambientLight.setColor((.8, .8, .8, 1)) directionalLight = DirectionalLight("directionalLight") directionalLight.setDirection(LVector3(0, 45, -45)) directionalLight.setColor((0.2, 0.2, 0.2, 1)) render.setLight(render.attachNewNode(directionalLight)) render.setLight(render.attachNewNode(ambientLight)) def updateWorld(self, task): dt = globalClock.getDt() # bug #1455084, simple doPhysics(dt) does nothing # looks like fixed already self.world.doPhysics(dt, 1, 1. / 60.) return task.cont def rayCollision(self): if self.mouseWatcherNode.hasMouse(): mouse = self.mouseWatcherNode.getMouse() pointFrom, pointTo = Point3(), Point3() self.camLens.extrude(mouse, pointFrom, pointTo) pointFrom = render.getRelativePoint(self.cam, pointFrom) pointTo = render.getRelativePoint(self.cam, pointTo) hits = self.world.rayTestAll(pointFrom, pointTo).getHits() return sorted(hits, key = lambda x: (x.getHitPos() - pointFrom).length()) return [] def pickBall(self): hits = self.rayCollision() for hit in hits: hit_node = hit.getNode() if 'ball' in hit_node.getName(): self.picked.add(hit_node.getName()) NodePath(hit_node.getChild(0).getChild(0)).setColor(HIGHLIGHT) def releaseBall(self): hits = self.rayCollision() if hits: foundBall = False for picked in hits: hit_node = picked.getNode() if 'ball' in hit_node.getName(): foundBall = True x, y, z = picked.getHitPos() bodies = self.world.getRigidBodies() for elem in bodies: name = elem.getName() if name in self.picked: # apply some physics node = NodePath(elem.getChild(0).getChild(0)) node_x, node_y, node_z = node.getPos(render) ix = (x - node_x) iy = (y - node_y) dir = atan2(iy, ix) dx, dy = SPEED * cos(dir), SPEED * sin(dir) elem.applyCentralImpulse(LVector3(dx, dy, z)) node.setColor(self.ballColors[elem.getName()]) if foundBall: self.picked = set([]) def rotateCube(self, hpr = (0, 0, 0)): # h, p, r = z, x, y # FIXME: something completely wrong goes here # need some time to figure it out planes = render.findAllMatches('**/plane*') for plane in planes: oldParent = plane.getParent() pivot = render.attachNewNode('pivot') pivot.setPos(render, 0, 0, 5) plane.wrtReparentTo(pivot) pivot.setHpr(render, Vec3(hpr)) if oldParent.getName() != 'render': oldParent.removeNode() def shakeBalls(self): balls = filter(lambda x: 'ball' in x.getName(), self.world.getRigidBodies()) for ball in balls: dx = uniform(-SHAKE_SPEED, SHAKE_SPEED) dy = uniform(-SHAKE_SPEED, SHAKE_SPEED) dz = uniform(-SHAKE_SPEED, SHAKE_SPEED) ball.applyCentralImpulse(LVector3(dx, dy, dz)) def updateBallsCounter(self, num): self.ballCnt += num self.title.setText('%d balls' % (self.ballCnt)) def addRandomBall(self): planes = render.findAllMatches('**/plane*') x, y, z = zip(*[tuple(plane.getPos()) for plane in planes]) xPos = uniform(min(x), max(x)) yPos = uniform(min(y), max(y)) zPos = uniform(min(z), max(z)) self.makeBall(self.ballCnt, (xPos, yPos, zPos)) self.updateBallsCounter(1) def rmBall(self): if self.ballCnt != 0: ball = render.find('**/ball_' + str(self.ballCnt - 1)) self.ballColors.pop(ball.getName()) ball.removeNode() self.updateBallsCounter(-1) def makePlane(self, num, norm, pos, hpr): shape = BulletPlaneShape(norm, 0) node = BulletRigidBodyNode('plane_' + str(num)) node.addShape(shape) physics = render.attachNewNode(node) physics.setPos(*pos) self.world.attachRigidBody(node) model = loader.loadModel('models/square') model.setScale(10, 10, 10) model.setHpr(*hpr) model.setTransparency(TransparencyAttrib.MAlpha) model.setColor(1, 1, 1, 0.25) model.reparentTo(physics) def makeColor(self): return (random(), random(), random(), 1) def makeBall(self, num, pos = (0, 0, 0)): shape = BulletSphereShape(0.5) node = BulletRigidBodyNode('ball_' + str(num)) node.setMass(1.0) node.setRestitution(.9) node.setDeactivationEnabled(False) node.addShape(shape) physics = render.attachNewNode(node) physics.setPos(*pos) self.world.attachRigidBody(node) model = loader.loadModel('models/ball') color = self.makeColor() model.setColor(color) self.ballColors['ball_' + str(num)] = color model.reparentTo(physics)
def step_physics(task): dt = globalClock.getDt() physics.doPhysics(dt) return task.cont s.taskMgr.add(step_physics, 'physics simulation') # A physical object in the simulation node = BulletRigidBodyNode('Box') node.setMass(1.0) node.addShape(BulletSphereShape(1)) # Attaching the physical object in the scene graph and adding # a visible model to it np = s.render.attachNewNode(node) np.set_pos(0, 0, 0) np.set_hpr(45, 0, 45) m = loader.loadModel("models/smiley") m.reparentTo(np) physics.attachRigidBody(node) # Let's actually see what's happening base.cam.setPos(0, -10, 0) base.cam.lookAt(0, 0, 0) # Give the object a nudge and run the program # the impulse vector is in world space, the position at which it is # applied is in object space. node.apply_impulse(Vec3(0, 0, 1), Point3(1, 0, 0)) node.apply_impulse(Vec3(0, 0, -1), Point3(-1, 0, 0)) s.run()
class Game(DirectObject): def __init__(self): base.setBackgroundColor(0.1, 0.1, 0.8, 1) base.setFrameRateMeter(True) base.cam.setPos(0, -10, 5) base.cam.lookAt(0, 0, 0.2) # 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('up', 'w') inputState.watchWithModifiers('left', 'a') inputState.watchWithModifiers('down', 's') inputState.watchWithModifiers('right', 'd') # 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): force = Vec3(0, 0, 0) if inputState.isSet('up'): force.setY( 1.0) if inputState.isSet('down'): force.setY(-1.0) if inputState.isSet('left'): force.setX(-1.0) if inputState.isSet('right'): force.setX( 1.0) force *= 300.0 self.bowlNP.node().setActive(True) self.bowlNP.node().applyCentralForce(force) def update(self, task): dt = globalClock.getDt() self.processInput(dt) self.world.doPhysics(dt) 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.debugNP.node().showWireframe(True) self.debugNP.node().showConstraints(True) self.debugNP.node().showBoundingBoxes(False) self.debugNP.node().showNormals(False) self.world = BulletWorld() self.world.setGravity(Vec3(0, 0, -9.81)) self.world.setDebugNode(self.debugNP.node()) # Ground shape = BulletPlaneShape(Vec3(0, 0, 1), 0) body = BulletRigidBodyNode('Ground') bodyNP = self.worldNP.attachNewNode(body) bodyNP.node().addShape(shape) bodyNP.setPos(0, 0, 0) bodyNP.setCollideMask(BitMask32.allOn()) self.world.attachRigidBody(bodyNP.node()) # Bowl visNP = loader.loadModel('models/bowl.egg') geom = visNP.findAllMatches('**/+GeomNode').getPath(0).node().getGeom(0) mesh = BulletTriangleMesh() mesh.addGeom(geom) shape = BulletTriangleMeshShape(mesh, dynamic=True) body = BulletRigidBodyNode('Bowl') bodyNP = self.worldNP.attachNewNode(body) bodyNP.node().addShape(shape) bodyNP.node().setMass(10.0) bodyNP.setPos(0, 0, 0) bodyNP.setCollideMask(BitMask32.allOn()) self.world.attachRigidBody(bodyNP.node()) visNP.reparentTo(bodyNP) self.bowlNP = bodyNP self.bowlNP.setScale(2) # Eggs self.eggNPs = [] for i in range(5): x = random.gauss(0, 0.1) y = random.gauss(0, 0.1) z = random.gauss(0, 0.1) + 1 h = random.random() * 360 p = random.random() * 360 r = random.random() * 360 visNP = loader.loadModel('models/egg.egg') geom = visNP.findAllMatches('**/+GeomNode').getPath(0).node().getGeom(0) shape = BulletConvexHullShape() shape.addGeom(geom) body = BulletRigidBodyNode('Egg-%i' % i) bodyNP = self.worldNP.attachNewNode(body) bodyNP.node().setMass(1.0) bodyNP.node().addShape(shape) bodyNP.node().setDeactivationEnabled(False) bodyNP.setCollideMask(BitMask32.allOn()) bodyNP.setPosHpr(x, y, z, h, p, r) #bodyNP.setScale(1.5) self.world.attachRigidBody(bodyNP.node()) visNP.reparentTo(bodyNP) self.eggNPs.append(bodyNP)
class Simulation(ShowBase): scale = 1 height = 500 lateralError = 200 dt = 0.1 steps = 0 #Test Controllers fuelPID = PID(10, 0.5, 10, 0, 100) heightPID = PID(0.08, 0, 0.3, 0.1, 1) pitchPID = PID(10, 0, 1000, -10, 10) rollPID = PID(10, 0, 1000, -10, 10) XPID = PID(0.2, 0, 0.8, -10, 10) YPID = PID(0.2, 0, 0.8, -10, 10) vulcain = NeuralNetwork() tau = 0.5 Valves=[] CONTROL = False EMPTY = False LANDED = False DONE = False gimbalX = 0 gimbalY = 0 targetAlt = 150 R = RE(200 * 9.806, 250 * 9.806, 7607000 / 9 * scale, 0.4) throttle = 0.0 fuelMass_full = 417000 * scale fuelMass_init = 0.10 radius = 1.8542 * scale length = 47 * scale Cd = 1.5 def __init__(self, VISUALIZE=False): self.VISUALIZE = VISUALIZE if VISUALIZE is True: ShowBase.__init__(self) self.setBackgroundColor(0.2, 0.2, 0.2, 1) self.setFrameRateMeter(True) self.render.setShaderAuto() self.cam.setPos(-120 * self.scale, -120 * self.scale, 120 * self.scale) self.cam.lookAt(0, 0, 10 * self.scale) alight = AmbientLight('ambientLight') alight.setColor(Vec4(0.5, 0.5, 0.5, 1)) alightNP = self.render.attachNewNode(alight) dlight = DirectionalLight('directionalLight') dlight.setDirection(Vec3(1, -1, -1)) dlight.setColor(Vec4(0.7, 0.7, 0.7, 1)) dlightNP = self.render.attachNewNode(dlight) self.render.clearLight() self.render.setLight(alightNP) self.render.setLight(dlightNP) 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') self.ostData = OnscreenText(text='ready', pos=(-1.3, 0.9), scale=0.07, fg=Vec4(1, 1, 1, 1), align=TextNode.ALeft) self.fuelMass = self.fuelMass_full * self.fuelMass_init self.vulcain.load_existing_model(path="LandingRockets/",model_name="140k_samples_1024neurons_3layers_l2-0.000001") # Physics self.setup() # _____HANDLER_____ def doExit(self): self.cleanup() sys.exit(1) def doReset(self): self.cleanup() self.setup() def toggleWireframe(self): ...#self.toggleWireframe() def toggleTexture(self): ...#self.toggleTexture() def toggleDebug(self): if self.debugNP.isHidden(): self.debugNP.show() else: self.debugNP.hide() def doScreenshot(self): self.screenshot('Bullet') # ____TASK___ def processInput(self): throttleChange = 0.0 if inputState.isSet('forward'): self.gimbalY = 10 if inputState.isSet('reverse'): self.gimbalY = -10 if inputState.isSet('left'): self.gimbalX = 10 if inputState.isSet('right'): self.gimbalX = -10 if inputState.isSet('turnLeft'): throttleChange = -1.0 if inputState.isSet('turnRight'): throttleChange = 1.0 self.throttle += throttleChange / 100.0 self.throttle = min(max(self.throttle, 0), 1) def processContacts(self): result = self.world.contactTestPair(self.rocketNP.node(), self.groundNP.node()) #print(result.getNumContacts()) if result.getNumContacts() != 0: self.LANDED = True #self.DONE = True def update(self,task): #self.control(0.1,0.1,0.1) #self.processInput() #self.processContacts() # self.terrain.update() return task.cont def cleanup(self): self.world.removeRigidBody(self.groundNP.node()) self.world.removeRigidBody(self.rocketNP.node()) self.world = None self.debugNP = None self.groundNP = None self.rocketNP = None self.worldNP.removeNode() self.LANDED = False self.EMPTY = False self.DONE = False self.steps = 0 self.fuelMass = self.fuelMass_full*self.fuelMass_init def setup(self): self.targetAlt = r.randrange(100,300) self.Valves = np.array([0.15,0.2,0.15]) self.EngObs = self.vulcain.predict_data_point(np.array(self.Valves).reshape(1,-1)) if self.VISUALIZE is True: self.worldNP = self.render.attachNewNode('World') else: self.root = NodePath(PandaNode("world root")) self.worldNP = self.root.attachNewNode('World') self.world = BulletWorld() self.world.setGravity(Vec3(0, 0, -9.80665)) # World self.debugNP = self.worldNP.attachNewNode(BulletDebugNode('Debug')) self.debugNP.node().showWireframe(True) self.debugNP.node().showConstraints(True) self.debugNP.node().showBoundingBoxes(False) self.debugNP.node().showNormals(True) self.debugNP.show() self.world.setDebugNode(self.debugNP.node()) # self.debugNP.showTightBounds() # self.debugNP.showBounds() # Ground (static) shape = BulletPlaneShape(Vec3(0, 0, 1), 1) self.groundNP = self.worldNP.attachNewNode(BulletRigidBodyNode('Ground')) self.groundNP.node().addShape(shape) self.groundNP.setPos(0, 0, 0) self.groundNP.setCollideMask(BitMask32.allOn()) self.world.attachRigidBody(self.groundNP.node()) # Rocket shape = BulletCylinderShape(self.radius, self.length, ZUp) self.rocketNP = self.worldNP.attachNewNode(BulletRigidBodyNode('Cylinder')) self.rocketNP.node().setMass(27200 * self.scale) self.rocketNP.node().addShape(shape) #self.rocketNP.setPos(20,20,250) self.rocketNP.setPos(20, 20, r.randrange(100, 300)+200) #self.rocketNP.setPos(r.randrange(-self.lateralError, self.lateralError, 1), r.randrange(-self.lateralError, self.lateralError, 1), self.height) # self.rocketNP.setPos(0, 0, self.length*10) self.rocketNP.setCollideMask(BitMask32.allOn()) # self.rocketNP.node().setCollisionResponse(0) self.rocketNP.node().notifyCollisions(True) self.world.attachRigidBody(self.rocketNP.node()) for i in range(4): leg = BulletCylinderShape(0.1 * self.radius, 0.5 * self.length, XUp) self.rocketNP.node().addShape(leg, TransformState.makePosHpr( Vec3(6 * self.radius * math.cos(i * math.pi / 2), 6 * self.radius * math.sin(i * math.pi / 2), -0.6 * self.length), Vec3(i * 90, 0, 30))) shape = BulletConeShape(0.75 * self.radius, 0.5 * self.radius, ZUp) self.rocketNP.node().addShape(shape, TransformState.makePosHpr(Vec3(0, 0, -1 / 2 * self.length), Vec3(0, 0, 0))) # Fuel self.fuelRadius = 0.9 * self.radius shape = BulletCylinderShape(self.fuelRadius, 0.01 * self.length, ZUp) self.fuelNP = self.worldNP.attachNewNode(BulletRigidBodyNode('Cone')) self.fuelNP.node().setMass(self.fuelMass_full * self.fuelMass_init) self.fuelNP.node().addShape(shape) self.fuelNP.setPos(0, 0, self.rocketNP.getPos().getZ() - self.length * 0.5 * (1 - self.fuelMass_init)) self.fuelNP.setCollideMask(BitMask32.allOn()) self.fuelNP.node().setCollisionResponse(0) self.world.attachRigidBody(self.fuelNP.node()) frameA = TransformState.makePosHpr(Point3(0, 0, 0), Vec3(0, 0, 90)) frameB = TransformState.makePosHpr(Point3(0, 0, 0), Vec3(0, 0, 90)) self.fuelSlider = BulletSliderConstraint(self.rocketNP.node(), self.fuelNP.node(), frameA, frameB, 1) self.fuelSlider.setTargetLinearMotorVelocity(0) self.fuelSlider.setDebugDrawSize(2.0) self.fuelSlider.set_lower_linear_limit(0) self.fuelSlider.set_upper_linear_limit(0) self.world.attachConstraint(self.fuelSlider) self.npThrustForce = LineNodePath(self.rocketNP, 'Thrust', thickness=4, colorVec=Vec4(1, 0.5, 0, 1)) self.npDragForce = LineNodePath(self.rocketNP, 'Drag', thickness=4, colorVec=Vec4(1, 0, 0, 1)) self.npLiftForce = LineNodePath(self.rocketNP, 'Lift', thickness=4, colorVec=Vec4(0, 0, 1, 1)) self.npFuelState = LineNodePath(self.fuelNP, 'Fuel', thickness=20, colorVec=Vec4(0, 1, 0, 1)) self.rocketCSLon = self.radius ** 2 * math.pi self.rocketCSLat = self.length * 2 * self.radius if self.VISUALIZE is True: self.terrain = loader.loadModel("LZGrid2.egg") self.terrain.setScale(10) self.terrain.reparentTo(self.render) self.terrain.setColor(Vec4(0.1, 0.2, 0.1, 1)) self.toggleTexture() #self.fuelNP.setPos(0, 0, self.rocketNP.getPos().getZ() - self.length * 0.4 * (1 - self.fuelMass_init)) for i in range(5): self.world.doPhysics(self.dt, 5, 1.0 / 180.0) self.fuelSlider.set_lower_linear_limit(-self.length * 0.5 * (1 - self.fuelMass_init)) self.fuelSlider.set_upper_linear_limit(self.length * 0.5 * (1 - self.fuelMass_init)) for i in range(100): self.world.doPhysics(self.dt, 5, 1.0 / 180.0) self.rocketNP.node().applyForce(Vec3(0,0,300000), Vec3(0, 0, 0)) def updateRocket(self, mdot, dt): # Fuel Update self.fuelMass = self.fuelNP.node().getMass() - dt * mdot if self.fuelMass <= 0: self.EMPTY is True fuel_percent = self.fuelMass / self.fuelMass_full self.fuelNP.node().setMass(self.fuelMass) fuelHeight = self.length * fuel_percent I1 = 1 / 2 * self.fuelMass * self.fuelRadius ** 2 I2 = 1 / 4 * self.fuelMass * self.fuelRadius ** 2 + 1 / 12 * self.fuelMass * fuelHeight * fuelHeight self.fuelNP.node().setInertia(Vec3(I2, I2, I1)) # Shift fuel along slider constraint fuelTargetPos = 0.5 * (self.length - fuelHeight) fuelPos = self.fuelSlider.getLinearPos() self.fuelSlider.set_upper_linear_limit(fuelTargetPos) self.fuelSlider.set_lower_linear_limit(-fuelTargetPos) self.npFuelState.reset() self.npFuelState.drawArrow2d(Vec3(0, 0, -0.5 * fuelHeight), Vec3(0, 0, 0.5 * fuelHeight), 45, 2) self.npFuelState.create() def observe(self): pos = self.rocketNP.getPos() vel = self.rocketNP.node().getLinearVelocity() quat = self.rocketNP.getTransform().getQuat() Roll, Pitch, Yaw = quat.getHpr() rotVel = self.rocketNP.node().getAngularVelocity() offset = self.targetAlt-pos.getZ() return pos, vel, Roll, Pitch, Yaw, rotVel, self.fuelMass / self.fuelMass_full, self.EMPTY, self.DONE, self.LANDED, offset, self.EngObs[0], self.Valves def control(self,ValveCommands): self.gimbalX = 0 self.gimbalY = 0 self.Valves = ValveCommands-(ValveCommands-self.Valves)*np.exp(-self.dt/self.tau) self.EngObs = self.vulcain.predict_data_point(np.array(self.Valves).reshape(1,-1 )) #Brennkammerdruck, Gaskammertemp, H2Massenstrom, LOX MAssentrom, Schub #Bar,K,Kg/s,Kg/s,kN #self.dt = globalClock.getDt() pos = self.rocketNP.getPos() vel = self.rocketNP.node().getLinearVelocity() quat = self.rocketNP.getTransform().getQuat() Roll, Pitch, Yaw = quat.getHpr() rotVel = self.rocketNP.node().getAngularVelocity() # CHECK STATE if self.fuelMass <= 0: self.EMPTY = True #if pos.getZ() <= 36: # self.LANDED = True self.LANDED = False self.processContacts() P, T, rho = air_dens(pos[2]) rocketZWorld = quat.xform(Vec3(0, 0, 1)) AoA = math.acos(min(max(dot(norm(vel), norm(-rocketZWorld)), -1), 1)) dynPress = 0.5 * dot(vel, vel) * rho dragArea = self.rocketCSLon + (self.rocketCSLat - self.rocketCSLon) * math.sin(AoA) drag = norm(-vel) * dynPress * self.Cd * dragArea time = globalClock.getFrameTime() liftVec = norm(vel.project(rocketZWorld) - vel) if AoA > 0.5 * math.pi: liftVec = -liftVec lift = liftVec * (math.sin(AoA * 2) * self.rocketCSLat * dynPress) if self.CONTROL: self.throttle = self.heightPID.control(pos.getZ(), vel.getZ(), 33) pitchTgt = self.XPID.control(pos.getX(), vel.getX(), 0) self.gimbalX = -self.pitchPID.control(Yaw, rotVel.getY(), pitchTgt) rollTgt = self.YPID.control(pos.getY(), vel.getY(), 0) self.gimbalY = -self.rollPID.control(Pitch, rotVel.getX(), -rollTgt) self.thrust = self.EngObs[0][4]*1000 #print(self.EngObs) quat = self.rocketNP.getTransform().getQuat() quatGimbal = TransformState.makeHpr(Vec3(0, self.gimbalY, self.gimbalX)).getQuat() thrust = quatGimbal.xform(Vec3(0, 0, self.thrust)) thrustWorld = quat.xform(thrust) #print(thrustWorld) self.npDragForce.reset() self.npDragForce.drawArrow2d(Vec3(0, 0, 0), quat.conjugate().xform(drag) / 1000, 45, 2) self.npDragForce.create() self.npLiftForce.reset() self.npLiftForce.drawArrow2d(Vec3(0, 0, 0), quat.conjugate().xform(lift) / 1000, 45, 2) self.npLiftForce.create() self.npThrustForce.reset() if self.EMPTY is False & self.LANDED is False: self.npThrustForce.drawArrow2d(Vec3(0, 0, -0.5 * self.length), Vec3(0, 0, -0.5 * self.length) - thrust / 30000, 45, 2) self.npThrustForce.create() self.rocketNP.node().applyForce(drag, Vec3(0, 0, 0)) self.rocketNP.node().applyForce(lift, Vec3(0, 0, 0)) #print(self.EMPTY,self.LANDED) if self.EMPTY is False & self.LANDED is False: self.rocketNP.node().applyForce(thrustWorld, quat.xform(Vec3(0, 0, -1 / 2 * self.length))) self.updateRocket(self.EngObs[0][2]+self.EngObs[0][3], self.dt) self.rocketNP.node().setActive(True) self.fuelNP.node().setActive(True) self.processInput() self.world.doPhysics(self.dt) self.steps+=1 if self.steps > 1000: self.DONE = True telemetry = [] telemetry.append('Thrust: {}'.format(int(self.EngObs[0][4]))) telemetry.append('Fuel: {}%'.format(int(self.fuelMass / self.fuelMass_full * 100.0))) telemetry.append('Gimbal: {}'.format(int(self.gimbalX)) + ',{}'.format(int(self.gimbalY))) telemetry.append('AoA: {}'.format(int(AoA / math.pi * 180.0))) telemetry.append('\nPos: {},{}'.format(int(pos.getX()), int(pos.getY()))) telemetry.append('Height: {}'.format(int(pos.getZ()))) telemetry.append('RPY: {},{},{}'.format(int(Roll), int(Pitch), int(Yaw))) telemetry.append('Speed: {}'.format(int(np.linalg.norm(vel)))) telemetry.append('Vel: {},{},{}'.format(int(vel.getX()), int(vel.getY()), int(vel.getZ()))) telemetry.append('Rot: {},{},{}'.format(int(rotVel.getX()), int(rotVel.getY()), int(rotVel.getZ()))) telemetry.append('LANDED: {}'.format(self.LANDED)) telemetry.append('Time: {}'.format(self.steps*self.dt)) telemetry.append('TARGET: {}'.format(self.targetAlt)) #print(pos) if self.VISUALIZE is True: self.ostData.setText('\n'.join(telemetry)) self.cam.setPos(pos[0] - 120 * self.scale, pos[1] - 120 * self.scale, pos[2] + 80 * self.scale) self.cam.lookAt(pos[0], pos[1], pos[2]) self.taskMgr.step() """
class Mecha01Client(ShowBase): def __init__(self): ShowBase.__init__(self) self.taskMgr.add(self.spinCameraTask, "SpinCameraTask") self.plight = PointLight('plight') self.pnlp = self.camera.attachNewNode(self.plight) render.setLight(self.pnlp) self.debugNode = BulletDebugNode('Debug') self.debugNode.showWireframe(True) self.debugNode.showConstraints(True) self.debugNode.showBoundingBoxes(False) self.debugNode.showNormals(False) self.debugNP = render.attachNewNode(self.debugNode) self.debugNP.show() self.world = BulletWorld() self.world.setDebugNode(self.debugNode) self.planet = Planet() self.planet.perlin_terrain(5, 0.5) self.planet.build_node_path(render, self.world) shape = BulletCapsuleShape(0.25, 0.5) node = BulletRigidBodyNode('Capsule') node.addShape(shape) np = render.attachNewNode(node) np.setPos(Vec3(0, 0.8, 0.8) * self.planet.get_radius()) new_up_vector = Vec3(np.getPos() - self.planet.get_node_path().getPos()) old_up_vector = Vec3(0, 0, 1) q = self.__quatFromTo(old_up_vector, new_up_vector) np.setQuat(q) self.np01 = np self.world.attachRigidBody(node) #node.applyCentralImpulse(Vec3(0.4, 0.6, 1.0)) self.taskMgr.add(self.physicsUpdateTask, "PhysicsUpdateTask") self.accept('arrow_up', self.test01, ["shalala"]) def test01(self, x): print(x) def gravity(self, position): down_vector = Vec3(0, 0, 0) - position down_vector.normalize() gravity = LVector3f(down_vector * 9.81) def physicsUpdateTask(self, task): dt = globalClock.getDt() # simulating spherical gravity node = self.np01.getNode(0) pos = self.np01.getPos() gravity = self.gravity(pos) #self.np01.setQuat(Quat(0, 1.1, 1.1, 0)) self.world.doPhysics(dt) return Task.cont def spinCameraTask(self, task): # angleDegrees = task.time * 6.0 # angleRadians = angleDegrees * (pi / 180.0) # self.camera.setPos( # 100.0 * sin(angleRadians), # -100.0 * cos(angleRadians), 0.0) # self.camera.setHpr(angleDegrees, 0, 0) self.camera.setPos(self.np01.getPos() * 5.0) self.camera.lookAt(self.np01) return Task.cont def __quatFromTo(self, v0, v1): print(v0, v1) q = Quat( sqrt(v0.length()**2 * v1.length()**2) + v0.dot(v1), v0.cross(v1)) q.normalize() return q
class EscapeFromJCMB(object, DirectObject): def __init__(self): print "Loading..." self.init_window() self.init_music() self.init_bullet() self.init_key() self.load_world() self.init_player() self.init_objects() render.prepareScene(base.win.getGsg()) print "Done" self.start_physics() def init_window(self): # Hide the mouse cursor props = WindowProperties() props.setCursorHidden(True) base.win.requestProperties(props) def init_music(self): music = base.loader.loadSfx("../data/snd/Bent_and_Broken.mp3") music.play() self.playferscream = base.loader.loadSfx("../data/snd/deadscrm.wav") def init_bullet(self): self.world = BulletWorld() self.world.setGravity(Vec3(0, 0, -9.81)) # debugNode = BulletDebugNode('Debug') # debugNode.showWireframe(True) # debugNode.showConstraints(True) # debugNode.showBoundingBoxes(False) # debugNode.showNormals(False) # debugNP = render.attachNewNode(debugNode) # debugNP.show() # self.world.setDebugNode(debugNP.node()) alight = AmbientLight('alight') alight.setColor(VBase4(0.05, 0.05, 0.05, 1)) alnp = render.attachNewNode(alight) render.setLight(alnp) def init_key(self): # Stores the state of the keys, 1 for pressed and 0 for unpressed self.key_state = { 'up': False, 'right': False, 'down': False, 'left': False } # Assign the key event handler self.accept('w', self.set_key_state, ['up', True]) self.accept('w-up', self.set_key_state, ['up', False]) self.accept('d', self.set_key_state, ['right', True]) self.accept('d-up', self.set_key_state, ['right', False]) self.accept('s', self.set_key_state, ['down', True]) self.accept('s-up', self.set_key_state, ['down', False]) self.accept('a', self.set_key_state, ['left', True]) self.accept('a-up', self.set_key_state, ['left', False]) # Stores the state of the mouse buttons, 1 for pressed and 0 for unpressed self.mouse_state = {'left_click': False, 'right_click': False} # Assign the mouse click event handler self.accept('mouse1', self.set_mouse_state, ['left_click', True]) self.accept('mouse1-up', self.set_mouse_state, ['left_click', False]) self.accept('mouse2', self.set_mouse_state, ['right_click', True]) self.accept('mouse2-up', self.set_mouse_state, ['right_click', False]) self.accept('z', self.print_pos) # Esc self.accept('escape', sys.exit) def set_key_state(self, key, state): self.key_state[key] = state def set_mouse_state(self, button, state): self.mouse_state[button] = state def print_pos(self): print self.playernp.getPos() def egg_to_BulletTriangleMeshShape(self, modelnp): mesh = BulletTriangleMesh() for collisionNP in modelnp.findAllMatches('**/+CollisionNode'): collisionNode = collisionNP.node() for collisionPolygon in collisionNode.getSolids(): tri_points = collisionPolygon.getPoints() mesh.addTriangle(tri_points[0], tri_points[1], tri_points[2]) shape = BulletTriangleMeshShape(mesh, dynamic=False) return shape def load_world(self): stairwell = loader.loadModel('../data/mod/jcmbstairs.egg') stairwell.reparentTo(render) stairwell_shape = self.egg_to_BulletTriangleMeshShape(stairwell) stairwellnode = BulletRigidBodyNode('stairwell') stairwellnode.addShape(stairwell_shape) self.world.attachRigidBody(stairwellnode) def init_player(self): # Stop the default mouse behaviour base.disableMouse() # Character has a capsule shape shape = BulletCapsuleShape(0.2, 1, ZUp) self.player = BulletRigidBodyNode('Player') self.player.setMass(80.0) self.player.addShape(shape) self.playernp = render.attachNewNode(self.player) self.playernp.setPos(0, 0, 1) self.world.attachRigidBody(self.player) self.player.setLinearSleepThreshold(0.0) self.player.setAngularFactor(0.0) self.player_speed = 3 self.player_is_grabbing = False # self.head = BulletRigidBodyNode('Player Head') # self.head.addShape(BulletSphereShape(0.2)) # self.head.setMass(10) # self.head.setInertia(Vec3(1,1,1)) # self.head.setAngularFactor(1.0) # self.head.setLinearFactor(0.0) # self.headnp = self.playernp.attachNewNode(self.head) # self.headnp.setPos(0,0,0) # self.headnp.setCollideMask(BitMask32.allOff()) # self.world.attachRigidBody(self.head) # Attach the camera to the player's head base.camera.reparentTo(self.playernp) # base.camera.setPos(0,0,.5) base.camLens.setFov(80) base.camLens.setNear(0.01) # Make the torch and attach it to our character torch = Spotlight('torch') torch.setColor(VBase4(1, 1, 1, 1)) lens = PerspectiveLens() lens.setFov(80) lens.setNearFar(20, 100) torch.setLens(lens) self.torchnp = base.camera.attachNewNode(torch) self.torchnp.setPos(0, 0, 0) # Allow the world to be illuminated by our torch render.setLight(self.torchnp) self.cs = None # Player's "hand" in the world hand_model = loader.loadModel('../data/mod/hand.egg') hand_model.setScale(1) hand_model.setBillboardPointEye() self.hand = BulletRigidBodyNode('Hand') self.hand.addShape(BulletSphereShape(0.1)) self.hand.setLinearSleepThreshold(0.0) self.hand.setLinearDamping(0.9999999) self.hand.setAngularFactor(0.0) self.hand.setMass(1.0) self.world.attachRigidBody(self.hand) self.handnp = render.attachNewNode(self.hand) self.handnp.setCollideMask(BitMask32.allOff()) # hand_model.reparentTo(self.handnp) # Create a text node to display object identification information and attach it to the player's "hand" self.hand_text = TextNode('Hand Text') self.hand_text.setText("Ch-ch-chek yoself befo yo rek yoself, bro.") self.hand_text.setAlign(TextNode.ACenter) self.hand_text.setTextColor(1, 1, 1, 1) self.hand_text.setShadow(0.05, 0.05) self.hand_text.setShadowColor(0, 0, 0, 1) self.hand_text_np = render.attachNewNode(self.hand_text) self.hand_text_np.setScale(0.03) self.hand_text_np.setBillboardPointEye() self.hand_text_np.hide() # Disable the depth testing for the hand and the text - we always want these things on top, with no clipping self.handnp.setDepthTest(False) self.hand_text_np.setDepthTest(False) # Add the player update task taskMgr.add(self.update, 'update_player_task') def init_objects(self): # Make Playfer Box shape = BulletBoxShape(Vec3(0.25, 0.25, 0.25)) node = BulletRigidBodyNode('Playfer Box') node.setMass(110.0) node.setFriction(1.0) node.addShape(shape) node.setAngularDamping(0.0) np = render.attachNewNode(node) np.setPos(-1.4, 1.7, -1.7) self.world.attachRigidBody(node) playferboxmodel = loader.loadModel('../data/mod/playferbox.egg') playferboxmodel.reparentTo(np) # Make Pendlepot shape = BulletBoxShape(Vec3(0.2, 0.15, 0.1)) node = BulletRigidBodyNode('Pendlepot') node.setMass(5.0) node.setFriction(1.0) node.addShape(shape) node.setAngularDamping(0.0) np = render.attachNewNode(node) np.setPos(-1.4, 1.7, -1.5) self.world.attachRigidBody(node) pendlepotmodel = loader.loadModel('../data/mod/pendlepot.egg') pendlepotmodel.reparentTo(np) def update(self, task): # Update camera orientation md = base.win.getPointer(0) mouse_x = md.getX() mouse_y = md.getY() centre_x = base.win.getXSize() / 2 centre_y = base.win.getYSize() / 2 if base.win.movePointer(0, centre_x, centre_y): new_H = base.camera.getH() + (centre_x - mouse_x) new_P = base.camera.getP() + (centre_y - mouse_y) if new_P < -90: new_P = -90 elif new_P > 90: new_P = 90 base.camera.setH(new_H) base.camera.setP(new_P) # Update player position speed = 3 self.player_is_moving = False if (self.key_state["up"] == True): self.player_is_moving = True dir = 0 if (self.key_state["down"] == True): self.player_is_moving = True dir = 180 if (self.key_state["left"] == True): self.player_is_moving = True dir = 90 if (self.key_state["right"] == True): self.player_is_moving = True dir = 270 self.player.clearForces() old_vel = self.player.getLinearVelocity() new_vel = Vec3(0, 0, 0) if self.player_is_moving == True: new_vel.setX(-speed * math.sin( (base.camera.getH() + dir) * 3.1415 / 180.0)) new_vel.setY(speed * math.cos( (base.camera.getH() + dir) * 3.1415 / 180.0)) timescale = 0.001 linear_force = (new_vel - old_vel) / (timescale) linear_force.setZ(0.0) self.player.applyCentralForce(linear_force) if self.player_is_grabbing == False: new_hand_pos = LPoint3f( render.getRelativePoint(base.camera, Vec3(0, 0.2, 0))) self.handnp.setPos(new_hand_pos) else: new_hand_pos = LPoint3f( render.getRelativePoint(base.camera, Vec3(0, 0.5, 0))) diff = new_hand_pos - self.handnp.getPos() self.hand.applyCentralForce(diff * 1000 - self.hand.getLinearVelocity() * 100) if diff.length() > .5: self.player.setLinearVelocity(Vec3(0, 0, 0)) # Identify what lies beneath the player's hand (unless player is holding something) if self.player_is_grabbing == False: ray_from = self.playernp.getPos() ray_to = LPoint3f( render.getRelativePoint(base.camera, Vec3(0, 1, 0))) result = self.world.rayTestClosest(ray_from, ray_to) if result.hasHit() == True: self.hand_text.setText(result.getNode().getName()) self.hand_text_np.setPos(result.getHitPos()) self.hand_text_np.show() # If player clicks, grab the object by the nearest point (as chosen by ray) if self.mouse_state["left_click"] == True: if result.getNode().getNumChildren() == 1: obj = NodePath(result.getNode().getChild(0)) if self.player_is_grabbing == False: self.player_is_grabbing = True # Find the position of contact in terms of the object's local coordinates. # Parent the player's hand to the grabbed object at that position. pos = obj.getRelativePoint(render, result.getHitPos()) self.grabbed_node = result.getNode() self.grabbed_node.setActive(True) print self.grabbed_node frameA = TransformState.makePosHpr( Vec3(0, 0, 0), Vec3(0, 0, 0)) frameB = TransformState.makePosHpr( Vec3(0, 0, 0), Vec3(0, 0, 0)) swing1 = 20 # degrees swing2 = 20 # degrees twist = 20 # degrees self.cs = BulletConeTwistConstraint( self.hand, result.getNode(), frameA, frameB) self.cs.setLimit(swing1, swing2, twist) self.world.attachConstraint(self.cs) # Stop the held object swinging all over the place result.getNode().setAngularDamping(0.7) else: self.hand_text_np.hide() self.player_is_grabbing = False if self.mouse_state["left_click"] == False: self.player_is_grabbing = False if self.cs != None: self.world.remove_constraint(self.cs) self.cs = None self.grabbed_node.setAngularDamping(0.0) if self.player_is_grabbing == True and self.mouse_state[ "right_click"] == True: self.world.remove_constraint(self.cs) self.cs = None self.grabbed_node.setAngularDamping(0.0) self.grabbed_node.setActive(True) self.grabbed_node.applyCentralImpulse(1000, 0, 0) if self.player_is_grabbing == True: self.hand_text_np.hide() return task.cont def update_physics(self, task): dt = globalClock.getDt() self.world.doPhysics(dt) return task.cont def start_physics(self): taskMgr.add(self.update_physics, 'update_physics')
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)
def __init__(self, args): # Instance variables self.camPos = Vec3(0, 0, 0) self.faceVec = Vec2(0, 0) self.velocity = 0 self.episodeCounter = 0 self.episodeFrameCounter = 0 self.historyFrameCounter = 0 self.deviationCounter = 0 self.accSignal = 0 self.turnSignal = 0 self.signalGameFeedback = Event() self.signalPlayerReady = Event() self.lock = Lock() self.bufferFeedback = bytearray() self.signalShutdown = Event() self.recordVideo = args['record'] # Panda3D-related initialization # Car-view Camera base.camLens.setNear(0.05) base.camLens.setFov(70) # A fixed, top-view Camera _ = Camera("top_cam") topCam = render.attachNewNode(_) topCam.setName("top_cam") topCam.setPos(0, 0, 40) topCam.setHpr(0, -90, 0) dr = base.camNode.getDisplayRegion(0) dr.setActive(0) window = dr.getWindow() drTop = window.makeDisplayRegion(0.5, 1, 0, 0.5) drTop.setSort(dr.getSort()) drTop.setCamera(topCam) self.drCar = window.makeDisplayRegion(0, 0.5, 0.5, 1) self.drCar.setSort(dr.getSort()) self.drCar.setCamera(base.cam) # BulletWorld world = BulletWorld() world.setGravity(Vec3(0, 0, 0)) props = WindowProperties() props.setSize(1024, 768) base.win.requestProperties(props) # Floor shapeGround = BulletPlaneShape(Vec3(0, 0, 1), 0.1) nodeGround = BulletRigidBodyNode('Ground') nodeGround.addShape(shapeGround) npGround = render.attachNewNode(nodeGround) npGround.setPos(0, 0, -2) world.attachRigidBody(nodeGround) cmGround = CardMaker("Ground") cmGround.setFrame(-10, 10, -10, 10); npGroundTex = render.attachNewNode(cmGround.generate()) npGroundTex.reparentTo(npGround) ts = TextureStage("ts") ts.setMode(TextureStage.M_modulate) groundTexture = loader.loadTexture(args['map']) npGroundTex.setP(270) npGroundTex.setTexScale(ts, 1, 1) npGround.setTexture(ts, groundTexture) # Car shapeBox = BulletBoxShape(Vec3(0.5, 0.5, 0.5)) nodeBox = BulletRigidBodyNode('Box') nodeBox.setMass(1.0) nodeBox.addShape(shapeBox) self.npBox = render.attachNewNode(nodeBox) self.npBox.setPos(self.camPos) world.attachRigidBody(nodeBox) modelBox = loader.loadModel('models/box.egg') modelBox.flattenLight() modelBox.setScale(0.5) modelBox.reparentTo(self.npBox)
class Freegrip(fgcp.FreegripContactpairs): def __init__(self, objpath, handpkg, readser=False, torqueresist=50): """ initialization :param objpath: path of the object :param ser: True use pre-computed template file for debug (in order to debug large models like tool.stl :param torqueresist: the maximum allowable distance to com (see FreegripContactpairs.planContactpairs) author: weiwei date: 20161201, osaka """ super(self.__class__, self).__init__(objpath, readser) if readser is False: self.removeBadSamples() self.clusterFacetSamplesRNN(reduceRadius=10) self.planContactpairs(torqueresist) self.saveSerialized("tmpcp.pickle") else: self.loadSerialized("tmpcp.pickle", objpath) self.handpkg = handpkg self.hand = handpkg.newHandNM(hndcolor=[0, 1, 0, .1]) self.handfgrpcc_uninstanced = handpkg.newHandFgrpcc() self.handname = handpkg.getHandName() # gripcontactpairs_precc is the gripcontactpairs ([[p0,p1,p2],[p0',p1',p2']] pairs) after precc (collision free) # gripcontactpairnormals_precc is the gripcontactpairnormals ([[n0,n1,n2],[n0',n1',n2']] pairs) after precc # likewise, gripcontactpairfacets_precc is the [faceid0, faceid1] pair corresponding to the upper two self.gripcontactpairs_precc = None self.gripcontactpairnormals_precc = None self.gripcontactpairfacets_precc = None # the final results: gripcontacts: a list of [cct0, cct1] # griprotmats: a list of Mat4 # gripcontactnormals: a list of [nrml0, nrml1] self.gripcontacts = None self.griprotmats = None self.gripjawwidth = None self.gripcontactnormals = None self.bulletworld = BulletWorld() # prepare the model for collision detection self.objgeom = pandageom.packpandageom(self.objtrimesh.vertices, self.objtrimesh.face_normals, self.objtrimesh.faces) self.objmeshbullnode = cd.genCollisionMeshGeom(self.objgeom) self.bulletworld.attachRigidBody(self.objmeshbullnode) # for plot self.rtq85plotlist = [] self.counter2 = 0 # for dbupdate self.dbobjname = os.path.splitext(os.path.basename(objpath))[0] # def loadRtq85Models(self): # """ # load the rtq85 model and its fgrprecc model # :return: # """ # self.rtq85hnd = rtq85nm.Rtq85NM(hndcolor=[1, 0, 0, .1]) # handfgrpccpath = Filename.fromOsSpecific(os.path.join(this_dir, "robotiq85/rtq85egg", "robotiq_85_tip_precc.egg")) # self.handfgrpcc_uninstanced = loader.loadModel(handfgrpccpath) def removeHndcc(self, base, discretesize=8): """ Handcc means hand collision detection :param discretesize: the number of hand orientations :return: author: weiwei date: 20161212, tsukuba """ # isplotted = 0 # if self.rtq85plotlist: # for rtq85plotnode in self.rtq85plotlist: # rtq85plotnode.removeNode() # self.rtq85plotlist = [] self.gripcontacts = [] self.griprotmats = [] self.gripjawwidth = [] self.gripcontactnormals = [] plotoffsetfp = 6 self.counter = 0 while self.counter < self.facetpairs.shape[0]: # print str(self.counter) + "/" + str(self.facetpairs.shape[0]-1) # print self.gripcontactpairs_precc facetpair = self.facetpairs[self.counter] facetidx0 = facetpair[0] facetidx1 = facetpair[1] for j, contactpair in enumerate( self.gripcontactpairs_precc[self.counter]): for angleid in range(discretesize): cctpnt0 = contactpair[ 0] + plotoffsetfp * self.facetnormals[facetidx0] cctpnt1 = contactpair[ 1] + plotoffsetfp * self.facetnormals[facetidx1] cctnormal0 = self.gripcontactpairnormals_precc[ self.counter][j][0] cctnormal1 = [ -cctnormal0[0], -cctnormal0[1], -cctnormal0[2] ] tmphand = self.hand # tmprtq85 = rtq85nm.Rtq85NM(hndcolor=[1, 0, 0, .1]) # save initial hand pose initmat = tmphand.getMat() fgrdist = np.linalg.norm((cctpnt0 - cctpnt1)) tmphand.setJawwidth(fgrdist) tmphand.lookAt(cctnormal0[0], cctnormal0[1], cctnormal0[2]) rotax = [0, 1, 0] rotangle = 360.0 / discretesize * angleid rotmat = rm.rodrigues(rotax, rotangle) tmphand.setMat( pandageom.cvtMat4(rotmat) * tmphand.getMat()) axx = tmphand.getMat().getRow3(0) # 130 is the distance from hndbase to fingertip cctcenter = (cctpnt0 + cctpnt1) / 2 + 145 * np.array( [axx[0], axx[1], axx[2]]) tmphand.setPos( Point3(cctcenter[0], cctcenter[1], cctcenter[2])) # collision detection hndbullnode = cd.genCollisionMeshMultiNp( tmphand.handnp, base.render) result = self.bulletworld.contactTest(hndbullnode) if not result.getNumContacts(): self.gripcontacts.append(contactpair) self.griprotmats.append(tmphand.getMat()) self.gripjawwidth.append(fgrdist) self.gripcontactnormals.append( self.gripcontactpairnormals_precc[self.counter][j]) # pg.plotDumbbell(base.render, (cctpnt0+cctpnt1)/2, cctcenter, length=245, thickness=5, rgba=[.4,.4,.4,1]) # pg.plotAxisSelf(base.render, (cctpnt0+cctpnt1)/2+245*np.array([axx[0], axx[1], axx[2]]), # tmprtq85.getMat(), length=30, thickness=2) # tmprtq85.setColor([.5, .5, .5, 1]) # tmprtq85.reparentTo(base.render) # self.rtq85plotlist.append(tmprtq85) # isplotted = 1 # reset initial hand pose tmphand.setMat(initmat) self.counter += 1 self.counter = 0 def removeFgrpcc(self, base): """ Fgrpcc means finger pre collision detection :return: author: weiwei date: 20161212, tsukuba """ self.gripcontactpairs_precc = [] self.gripcontactpairnormals_precc = [] self.gripcontactpairfacets_precc = [] plotoffsetfp = 6 self.counter = 0 while self.counter < self.facetpairs.shape[0]: print str(self.counter) + "/" + str(self.facetpairs.shape[0] - 1) print self.gripcontactpairs self.gripcontactpairs_precc.append([]) self.gripcontactpairnormals_precc.append([]) self.gripcontactpairfacets_precc.append([]) facetpair = self.facetpairs[self.counter] facetidx0 = facetpair[0] facetidx1 = facetpair[1] for j, contactpair in enumerate( self.gripcontactpairs[self.counter]): cctpnt0 = contactpair[ 0] + plotoffsetfp * self.facetnormals[facetidx0] cctpnt1 = contactpair[ 1] + plotoffsetfp * self.facetnormals[facetidx1] cctnormal0 = self.facetnormals[facetidx0] cctnormal1 = [-cctnormal0[0], -cctnormal0[1], -cctnormal0[2]] handfgrpcc0 = NodePath("handfgrpcc0") self.handfgrpcc_uninstanced.instanceTo(handfgrpcc0) handfgrpcc0.setPos(cctpnt0[0], cctpnt0[1], cctpnt0[2]) handfgrpcc0.lookAt(cctpnt0[0] + cctnormal0[0], cctpnt0[1] + cctnormal0[1], cctpnt0[2] + cctnormal0[2]) handfgrpcc1 = NodePath("handfgrpcc1") self.handfgrpcc_uninstanced.instanceTo(handfgrpcc1) handfgrpcc1.setPos(cctpnt1[0], cctpnt1[1], cctpnt1[2]) handfgrpcc1.lookAt(cctpnt1[0] + cctnormal1[0], cctpnt1[1] + cctnormal1[1], cctpnt1[2] + cctnormal1[2]) handfgrpcc = NodePath("handfgrpcc") handfgrpcc0.reparentTo(handfgrpcc) handfgrpcc1.reparentTo(handfgrpcc) # prepare the model for collision detection facetmeshbullnode = cd.genCollisionMeshMultiNp(handfgrpcc) result = self.bulletworld.contactTest(facetmeshbullnode) if not result.getNumContacts(): self.gripcontactpairs_precc[-1].append(contactpair) self.gripcontactpairnormals_precc[-1].append( self.gripcontactpairnormals[self.counter][j]) self.gripcontactpairfacets_precc[-1].append( self.gripcontactpairfacets[self.counter]) self.counter += 1 self.counter = 0 def saveToDB(self, gdb): """ save the result to mysqldatabase :param gdb: is an object of the GraspDB class in the database package :return: author: weiwei date: 20170110 """ # save to database gdb = db.GraspDB() idhand = gdb.loadIdHand(self.handname) sql = "SELECT * FROM freeairgrip, object WHERE freeairgrip.idobject = object.idobject AND \ object.name LIKE '%s' AND freeairgrip.idhand LIKE '%s'" % ( self.dbobjname, idhand) result = gdb.execute(sql) if not result: sql = "SELECT idobject FROM object WHERE name LIKE '%s'" % self.dbobjname returnlist = gdb.execute(sql) if len(returnlist) != 0: idobject = returnlist[0][0] else: sql = "INSERT INTO object(name) VALUES('%s')" % self.dbobjname idobject = gdb.execute(sql) print self.gripcontacts for i in range(len(self.gripcontacts)): sql = "INSERT INTO freeairgrip(idobject, contactpnt0, contactpnt1, \ contactnormal0, contactnormal1, rotmat, jawwidth, idhand) \ VALUES('%s', '%s', '%s', '%s', '%s', '%s', '%s', %d)" % \ (idobject, dc.v3ToStr(self.gripcontacts[i][0]), dc.v3ToStr(self.gripcontacts[i][1]), dc.v3ToStr(self.gripcontactnormals[i][0]), dc.v3ToStr(self.gripcontactnormals[i][1]), dc.mat4ToStr(self.griprotmats[i]), str(self.gripjawwidth[i]), idhand) gdb.execute(sql) else: print "Grasps already saved or duplicated filename!" def removeFgrpccShow(self, base): """ Fgrpcc means finger pre collision detection This one is specially written for demonstration :return: author: weiwei date: 20161201, osaka """ # 6 is used because I am supposing 4+2 where 4 is the default # margin of bullet in panda3d. (NOTE: This is a guess) plotoffsetfp = 6 # np0 = base.render.find("**/pair0") # if np0: # np0.removeNode() # np1 = base.render.find("**/pair1") # if np1: # np1.removeNode() # # np0collection = base.render.findAllMatches("**/rtq85fgrpcc0") # for np0 in np0collection: # np0.removeNode() # np1collection = base.render.findAllMatches("**/rtq85fgrpcc1") # for np1 in np1collection: # np1.removeNode() # # npscollection = base.render.findAllMatches("**/sphere") # for nps in npscollection: # nps.removeNode() npbrchild = base.render.find("**/tempplot") if npbrchild: npbrchild.removeNode() # for fast delete brchild = NodePath('tempplot') brchild.reparentTo(base.render) self.counter += 1 if self.counter >= self.facetpairs.shape[0]: self.counter = 0 facetpair = self.facetpairs[self.counter] facetidx0 = facetpair[0] facetidx1 = facetpair[1] geomfacet0 = pandageom.packpandageom( self.objtrimesh.vertices + np.tile(plotoffsetfp * self.facetnormals[facetidx0], [self.objtrimesh.vertices.shape[0], 1]), self.objtrimesh.face_normals[self.facets[facetidx0]], self.objtrimesh.faces[self.facets[facetidx0]]) geomfacet1 = pandageom.packpandageom( self.objtrimesh.vertices + np.tile(plotoffsetfp * self.facetnormals[facetidx1], [self.objtrimesh.vertices.shape[0], 1]), self.objtrimesh.face_normals[self.facets[facetidx1]], self.objtrimesh.faces[self.facets[facetidx1]]) # show the facetpair node0 = GeomNode('pair0') node0.addGeom(geomfacet0) star0 = NodePath('pair0') star0.attachNewNode(node0) facetcolorarray = self.facetcolorarray star0.setColor( Vec4(facetcolorarray[facetidx0][0], facetcolorarray[facetidx0][1], facetcolorarray[facetidx0][2], facetcolorarray[facetidx0][3])) star0.setTwoSided(True) star0.reparentTo(brchild) node1 = GeomNode('pair1') node1.addGeom(geomfacet1) star1 = NodePath('pair1') star1.attachNewNode(node1) star1.setColor( Vec4(facetcolorarray[facetidx1][0], facetcolorarray[facetidx1][1], facetcolorarray[facetidx1][2], facetcolorarray[facetidx1][3])) star1.setTwoSided(True) star1.reparentTo(brchild) for j, contactpair in enumerate(self.gripcontactpairs[self.counter]): cctpnt0 = contactpair[ 0] + plotoffsetfp * self.facetnormals[facetidx0] cctpnt1 = contactpair[ 1] + plotoffsetfp * self.facetnormals[facetidx1] # the following two choices decide the way to detect contacts cctnormal00 = np.array( self.gripcontactpairnormals[self.counter][j][0]) cctnormal01 = -np.array( self.gripcontactpairnormals[self.counter][j][1]) cctnormal0raw = (cctnormal00 + cctnormal01) cctnormal0 = (cctnormal0raw / np.linalg.norm(cctnormal0raw)).tolist() # the following two choices decide the way to detect contacts cctnormal10 = -cctnormal00 cctnormal11 = -cctnormal01 cctnormal1raw = (cctnormal10 + cctnormal11) cctnormal1 = (cctnormal1raw / np.linalg.norm(cctnormal1raw)).tolist() handfgrpcc0 = NodePath("handfgrpcc0") self.handfgrpcc_uninstanced.instanceTo(rtq85pcc0) handfgrpcc0.setPos(cctpnt0[0], cctpnt0[1], cctpnt0[2]) handfgrpcc0.lookAt(cctpnt0[0] + cctnormal0[0], cctpnt0[1] + cctnormal0[1], cctpnt0[2] + cctnormal0[2]) handfgrpcc1 = NodePath("handfgrpcc1") self.handfgrpcc_uninstanced.instanceTo(rtq85pcc1) handfgrpcc1.setPos(cctpnt1[0], cctpnt1[1], cctpnt1[2]) handfgrpcc1.lookAt(cctpnt1[0] + cctnormal1[0], cctpnt1[1] + cctnormal1[1], cctpnt1[2] + cctnormal1[2]) handfgrpcc = NodePath("handfgrpcc") handfgrpcc0.reparentTo(handfgrpcc) handfgrpcc1.reparentTo(handfgrpcc) # prepare the model for collision detection facetmeshbullnode = cd.genCollisionMeshMultiNp(handfgrpcc, brchild) result = self.bulletworld.contactTest(facetmeshbullnode) for contact in result.getContacts(): cp = contact.getManifoldPoint() pandageom.plotSphere(brchild, pos=cp.getLocalPointA(), radius=3, rgba=Vec4(1, 0, 0, 1)) pandageom.plotSphere(brchild, pos=cp.getLocalPointB(), radius=3, rgba=Vec4(0, 0, 1, 1)) if result.getNumContacts(): handfgrpcc0.setColor(1, 0, 0, .3) handfgrpcc1.setColor(1, 0, 0, .3) else: handfgrpcc0.setColor(1, 1, 1, .3) handfgrpcc1.setColor(1, 1, 1, .3) handfgrpcc0.setTransparency(TransparencyAttrib.MAlpha) handfgrpcc1.setTransparency(TransparencyAttrib.MAlpha) handfgrpcc0.reparentTo(brchild) handfgrpcc1.reparentTo(brchild) pandageom.plotArrow(star0, spos=cctpnt0, epos=cctpnt0 + plotoffsetfp * self.facetnormals[facetidx0] + cctnormal0, rgba=[ facetcolorarray[facetidx0][0], facetcolorarray[facetidx0][1], facetcolorarray[facetidx0][2], facetcolorarray[facetidx0][3] ], length=10) pandageom.plotArrow(star1, spos=cctpnt1, epos=cctpnt1 + plotoffsetfp * self.facetnormals[facetidx1] + cctnormal1, rgba=[ facetcolorarray[facetidx1][0], facetcolorarray[facetidx1][1], facetcolorarray[facetidx1][2], facetcolorarray[facetidx1][3] ], length=10) def removeFgrpccShowLeft(self, base): """ Fgrpcc means finger pre collision detection This one is specially written for demonstration Plot the available grips :return: author: weiwei date: 20161212, tsukuba """ plotoffsetfp = 6 self.counter += 1 if self.counter >= self.facetpairs.shape[0]: return else: print str(self.counter) + "/" + str(self.facetpairs.shape[0] - 1) facetpair = self.facetpairs[self.counter] facetidx0 = facetpair[0] facetidx1 = facetpair[1] for j, contactpair in enumerate( self.gripcontactpairs[self.counter]): cctpnt0 = contactpair[ 0] + plotoffsetfp * self.facetnormals[facetidx0] cctpnt1 = contactpair[ 1] + plotoffsetfp * self.facetnormals[facetidx1] cctnormal0 = self.facetnormals[facetidx0] cctnormal1 = [-cctnormal0[0], -cctnormal0[1], -cctnormal0[2]] handfgrpcc0 = NodePath("handfgrpcc0") self.handfgrpcc_uninstanced.instanceTo(handfgrpcc0) handfgrpcc0.setPos(cctpnt0[0], cctpnt0[1], cctpnt0[2]) handfgrpcc0.lookAt(cctpnt0[0] + cctnormal0[0], cctpnt0[1] + cctnormal0[1], cctpnt0[2] + cctnormal0[2]) handfgrpcc1 = NodePath("rtq85fgrpcc1") self.handfgrpcc_uninstanced.instanceTo(handfgrpcc1) handfgrpcc1.setPos(cctpnt1[0], cctpnt1[1], cctpnt1[2]) handfgrpcc1.lookAt(cctpnt1[0] + cctnormal1[0], cctpnt1[1] + cctnormal1[1], cctpnt1[2] + cctnormal1[2]) handfgrpcc = NodePath("handfgrpcc") handfgrpcc0.reparentTo(handfgrpcc) handfgrpcc1.reparentTo(handfgrpcc) # prepare the model for collision detection facetmeshbullnode = cd.genCollisionMeshMultiNp(handfgrpcc) result = self.bulletworld.contactTest(facetmeshbullnode) if not result.getNumContacts(): handfgrpcc0.setColor(1, 1, 1, .3) handfgrpcc1.setColor(1, 1, 1, .3) handfgrpcc0.setTransparency(TransparencyAttrib.MAlpha) handfgrpcc1.setTransparency(TransparencyAttrib.MAlpha) handfgrpcc0.reparentTo(base.render) handfgrpcc1.reparentTo(base.render) def removeHndccShow(self, base, discretesize=8): """ Handcc means hand collision detection This one is developed for demonstration This function should be called after executing removeHndcc :param discretesize: the number of hand orientations :return: delayTime author: weiwei date: 20161212, tsukuba """ # isplotted = 0 if self.rtq85plotlist: for rtq85plotnode in self.rtq85plotlist: rtq85plotnode.removeNode() self.rtq85plotlist = [] self.gripcontacts = [] self.griprotmats = [] self.gripjawwidth = [] self.gripcontactnormals = [] plotoffsetfp = 6 if self.counter2 == 0: self.counter += 1 if self.counter >= self.facetpairs.shape[0]: self.counter = 0 self.counter2 += 1 if self.counter2 >= discretesize: self.counter2 = 0 print str(self.counter) + "/" + str(self.facetpairs.shape[0] - 1) facetpair = self.facetpairs[self.counter] facetidx0 = facetpair[0] facetidx1 = facetpair[1] for j, contactpair in enumerate( self.gripcontactpairs_precc[self.counter]): if j == 0: print j, contactpair # for angleid in range(discretesize): angleid = self.counter2 cctpnt0 = contactpair[ 0] + plotoffsetfp * self.facetnormals[facetidx0] cctpnt1 = contactpair[ 1] + plotoffsetfp * self.facetnormals[facetidx1] cctnormal0 = self.gripcontactpairnormals_precc[ self.counter][j][0] cctnormal1 = [-cctnormal0[0], -cctnormal0[1], -cctnormal0[2]] tmprtq85 = rtq85nm.Rtq85NM(hndcolor=[1, 0, 0, .1]) # save initial hand pose fgrdist = np.linalg.norm((cctpnt0 - cctpnt1)) # TODO setting jawwidth to 80 is enough # since fgrpcc already detects inner collisions tmprtq85.setJawwidth(fgrdist) tmprtq85.lookAt(cctnormal0[0], cctnormal0[1], cctnormal0[2]) rotax = [0, 1, 0] rotangle = 360.0 / discretesize * angleid rotmat = rm.rodrigues(rotax, rotangle) tmprtq85.setMat(pandageom.cvtMat4(rotmat) * tmprtq85.getMat()) axx = tmprtq85.getMat().getRow3(0) # 130 is the distance from hndbase to fingertip cctcenter = (cctpnt0 + cctpnt1) / 2 + 145 * np.array( [axx[0], axx[1], axx[2]]) tmprtq85.setPos( Point3(cctcenter[0], cctcenter[1], cctcenter[2])) # collision detection self.hndbullnode = cd.genCollisionMeshMultiNp( tmprtq85.rtq85np, base.render) result = self.bulletworld.contactTest(self.hndbullnode) if not result.getNumContacts(): self.gripcontacts.append(contactpair) self.griprotmats.append(tmprtq85.getMat()) self.gripjawwidth.append(fgrdist) self.gripcontactnormals.append( self.gripcontactpairnormals_precc[self.counter][j]) # pandageom.plotDumbbell(base.render, (cctpnt0+cctpnt1)/2, cctcenter, length=245, thickness=5, rgba=[.4,.4,.4,1]) # pandageom.plotAxisSelf(base.render, (cctpnt0+cctpnt1)/2+245*np.array([axx[0], axx[1], axx[2]]), # tmprtq85.getMat(), length=30, thickness=2) tmprtq85.setColor([1, 1, 1, .3]) tmprtq85.reparentTo(base.render) self.rtq85plotlist.append(tmprtq85) # isplotted = 1 else: for contact in result.getContacts(): # cp = contact.getManifoldPoint() # pandageom.plotSphere(brchild, pos=cp.getLocalPointA(), radius=3, rgba=Vec4(1, 0, 0, 1)) # pandageom.plotSphere(brchild, pos=cp.getLocalPointB(), radius=3, rgba=Vec4(0, 0, 1, 1)) tmprtq85.setColor([.5, 0, 0, .3]) tmprtq85.reparentTo(base.render) self.rtq85plotlist.append(tmprtq85) def plotObj(self): geomnodeobj = GeomNode('obj') geomnodeobj.addGeom(self.objgeom) npnodeobj = NodePath('obj') npnodeobj.attachNewNode(geomnodeobj) npnodeobj.reparentTo(base.render) def showAllGrips(self): """ showAllGrips :return: author: weiwei date: 20170206 """ for i in range(len(self.gripcontacts)): # for i in range(2,3): hndrotmat = self.griprotmats[i] hndjawwidth = self.gripjawwidth[i] # show grasps # tmprtq85 = rtq85nm.Rtq85NM(hndcolor=[.7, .7, 0.7, .7]) tmprtq85 = rtq85nm.Rtq85NM(hndcolor=[0, 1, 0, .5]) tmprtq85.setMat(hndrotmat) tmprtq85.setJawwidth(hndjawwidth) # tmprtq85.setJawwidth(80) tmprtq85.reparentTo(base.render)
class App(ShowBase): def __init__(self, width, height): ShowBase.__init__(self) globalClock.set_mode(ClockObject.MNonRealTime) globalClock.set_dt(0.02) # 20ms per frame self.setBackgroundColor(0.9, 0.9, 0.9) self.createLighting() self.setupCamera(width, height) self.world = BulletWorld() self.world.setGravity(Vec3(0, 0, -9.81 * 10.0)) self.setupDebug() self.createPlane() self.animated_rig = ExposedJointRig('walking', { 'walk': 'walking-animation.egg' }) self.animated_rig.reparentTo(self.render) self.animated_rig.setPos(0, 0, -98) self.animated_rig.createLines(VBase4(0.5, 0.75, 1.0, 1.0)) self.animated_rig.pose('walk', 0) self.physical_rig = RigidBodyRig() self.physical_rig.reparentTo(self.render) self.physical_rig.setPos(0, 0, -98) self.physical_rig.createColliders(self.animated_rig) self.physical_rig.createConstraints() self.physical_rig.setCollideMask(BitMask32.bit(1)) self.physical_rig.attachRigidBodies(self.world) self.physical_rig.attachConstraints(self.world) self.control_rig = ControlJointRig('walking') self.control_rig.reparentTo(self.render) self.control_rig.setPos(0, 0, -98) self.control_rig.createLines(VBase4(1.0, 0.75, 0.5, 1.0)) self.control_rig.matchPose(self.animated_rig) self.disable_collisions() self.is_paused = True self.accept('escape', sys.exit) self.accept('space', self.toggle_pause) def toggle_pause(self): if self.is_paused: self.is_paused = False self.taskMgr.add(self.update, 'update') else: self.is_paused = True self.taskMgr.remove('update') def disable_collisions(self): for i in range(32): self.world.setGroupCollisionFlag(i, i, False) self.world.setGroupCollisionFlag(0, 1, True) def setupDebug(self): node = BulletDebugNode('Debug') node.showWireframe(True) self.world.setDebugNode(node) np = self.render.attachNewNode(node) np.show() def createLens(self, aspect_ratio, fov=60.0, near=1.0, far=1000.0): lens = PerspectiveLens() lens.setFov(fov) lens.setAspectRatio(aspect_ratio) lens.setNearFar(near, far) return lens def setupCamera(self, width, height): self.cam.setPos(-200, -100, 0) self.cam.lookAt(0, -100, 0) self.cam.node().setLens(self.createLens(width / height)) def createLighting(self): light = DirectionalLight('light') light.set_color(VBase4(0.2, 0.2, 0.2, 1)) np = self.render.attach_new_node(light) np.setPos(0, -200, 0) np.lookAt(0, 0, 0) self.render.set_light(np) light = AmbientLight('ambient') light.set_color(VBase4(0.4, 0.4, 0.4, 1)) np = self.render.attachNewNode(light) self.render.setLight(np) def createPlane(self): rb = BulletRigidBodyNode('Ground') rb.addShape(BulletPlaneShape(Vec3(0, 0, 1), 1)) rb.setFriction(1.0) np = self.render.attachNewNode(rb) np.setPos(Vec3(0, 0, -100)) np.setCollideMask(BitMask32.bit(0)) self.world.attachRigidBody(rb) return np def update(self, task): self.animated_rig.pose('walk', globalClock.getFrameCount() * 0.5) self.physical_rig.matchPose(self.animated_rig) self.control_rig.matchPhysicalPose(self.physical_rig) self.world.doPhysics(globalClock.getDt(), 10, 1.0 / 180.0) return task.cont
def planContactpairs(self, torqueresist=50, fgrtipdist=82): """ find the grasps using parallel pairs :param: torqueresist the maximum allowable distance to com :param: fgrtipdist the maximum dist between finger tips :return: author: weiwei date: 20161130, harada office @ osaka university """ # note that pairnormals and pairfacets are duplicated for each contactpair # the duplication is performed on purpose for convenient access # also, each contactpair"s" corresponds to a facetpair # it is empty when no contactpair is available self.gripcontactpairs = [] # gripcontactpairnormals and gripcontactpairfacets are not helpful # they are kept for convenience (they could be accessed using facetnormals and facetpairs) self.gripcontactpairnormals = [] self.gripcontactpairfacets = [] # for precollision detection # bulletworldprecc = BulletWorld() # # build the collision shape of objtrimesh # geomobj = pandageom.packpandageom(self.objtrimesh.vertices, self.objtrimesh.face_normals, # self.objtrimesh.faces) # objmesh = BulletTriangleMesh() # objmesh.addGeom(geomobj) # objmeshnode = BulletRigidBodyNode('objmesh') # objmeshnode.addShape(BulletTriangleMeshShape(objmesh, dynamic=True)) # bulletworldprecc.attachRigidBody(objmeshnode) # for raytracing bulletworldray = BulletWorld() nfacets = self.facets.shape[0] self.facetpairs = list(itertools.combinations(range(nfacets), 2)) for facetpair in self.facetpairs: # print "facetpair" # print facetpair, len(self.facetpairs) self.gripcontactpairs.append([]) self.gripcontactpairnormals.append([]) self.gripcontactpairfacets.append([]) # if one of the facet doesnt have samples, jump to next if self.objsamplepnts_refcls[facetpair[0]].shape[0] is 0 or \ self.objsamplepnts_refcls[facetpair[1]].shape[0] is 0: # print "no sampled points" continue # check if the faces are opposite and parallel dotnorm = np.dot(self.facetnormals[facetpair[0]], self.facetnormals[facetpair[1]]) if dotnorm < -0.95: # check if any samplepnts's projection from facetpairs[i][0] falls in the polygon of facetpairs[i][1] facet0pnts = self.objsamplepnts_refcls[facetpair[0]] facet0normal = self.facetnormals[facetpair[0]] facet1normal = self.facetnormals[facetpair[1]] # generate collision mesh facetmesh = BulletTriangleMesh() faceidsonfacet = self.facets[facetpair[1]] geom = pandageom.packpandageom( self.objtrimesh.vertices, self.objtrimesh.face_normals[faceidsonfacet], self.objtrimesh.faces[faceidsonfacet]) facetmesh.addGeom(geom) facetmeshbullnode = BulletRigidBodyNode('facet') facetmeshbullnode.addShape( BulletTriangleMeshShape(facetmesh, dynamic=True)) bulletworldray.attachRigidBody(facetmeshbullnode) # check the projection of a ray for facet0pnt in facet0pnts: pFrom = Point3(facet0pnt[0], facet0pnt[1], facet0pnt[2]) pTo = pFrom + Vec3(facet1normal[0], facet1normal[1], facet1normal[2]) * 9999 result = bulletworldray.rayTestClosest(pFrom, pTo) if result.hasHit(): hitpos = result.getHitPos() if np.linalg.norm( np.array(facet0pnt.tolist()) - np.array([hitpos[0], hitpos[1], hitpos[2]]) ) < fgrtipdist: fgrcenter = ( np.array(facet0pnt.tolist()) + np.array( [hitpos[0], hitpos[1], hitpos[2]])) / 2.0 # avoid large torque if np.linalg.norm(self.objtrimesh.center_mass - fgrcenter) < torqueresist: self.gripcontactpairs[-1].append([ facet0pnt.tolist(), [hitpos[0], hitpos[1], hitpos[2]] ]) self.gripcontactpairnormals[-1].append( [[ facet0normal[0], facet0normal[1], facet0normal[2] ], [ facet1normal[0], facet1normal[1], facet1normal[2] ]]) self.gripcontactpairfacets[-1].append( facetpair) # pre collision checking: it takes one finger as a cylinder and # computes its collision with the object ## first finger # cylindernode0 = BulletRigidBodyNode('cylindernode0') # cylinder0height = 50 # cylinder0normal = Vec3(facet0normal[0], facet0normal[1], facet0normal[2]) # cylindernode0.addShape(BulletCylinderShape(radius=self.preccradius, # height=cylinder0height, # up=cylinder0normal), # TransformState.makePos(pFrom+cylinder0normal*cylinder0height*.6)) # bulletworldprecc.attachRigidBody(cylindernode0) # ## second finger # cylindernode1 = BulletRigidBodyNode('cylindernode1') # cylinder1height = cylinder1height # # use the inverse of facet0normal instead of facet1normal to follow hand orientation # cylinder1normal = Vec3(-facet0normal[0], -facet0normal[1], -facet0normal[2]) # cylindernode1.addShape(BulletCylinderShape(radius=self.preccradius, # height=cylinder1height, # up=cylinder1normal), # TransformState.makePos(pFrom+cylinder1normal*cylinder1height*.6)) # bulletworldprecc.attachRigidBody(cylindernode1) # if bulletworldprecc.contactTestPair(cylindernode0, objmeshnode) and \ # bulletworldprecc.contactTestPair(cylindernode1, objmeshnode): bulletworldray.removeRigidBody(facetmeshbullnode) # update the pairs availablepairids = np.where(self.gripcontactpairs)[0] self.facetpairs = np.array(self.facetpairs)[availablepairids] self.gripcontactpairs = np.array( self.gripcontactpairs)[availablepairids] self.gripcontactpairnormals = np.array( self.gripcontactpairnormals)[availablepairids] self.gripcontactpairfacets = np.array( self.gripcontactpairfacets)[availablepairids]
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): force = Vec3(0, 0, 0) torque = Vec3(0, 0, 0) if inputState.isSet('forward'): force.setY(1.0) if inputState.isSet('reverse'): force.setY(-1.0) if inputState.isSet('left'): force.setX(-1.0) if inputState.isSet('right'): force.setX(1.0) if inputState.isSet('turnLeft'): torque.setZ(1.0) if inputState.isSet('turnRight'): torque.setZ(-1.0) force *= 30.0 torque *= 10.0 self.boxNP.node().setActive(True) self.boxNP.node().applyCentralForce(force) self.boxNP.node().applyTorque(torque) def update(self, task): dt = globalClock.getDt() self.processInput(dt) self.world.doPhysics(dt) 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()) # Ground shape = BulletPlaneShape(Vec3(0, 0, 1), -1) mask = BitMask32(0x0f) print('ground: ', mask) np = self.worldNP.attachNewNode(BulletRigidBodyNode('Ground')) np.node().addShape(shape) np.setPos(0, 0, -1) np.setCollideMask(mask) self.world.attachRigidBody(np.node()) ## Box 1 #shape = BulletBoxShape(Vec3(0.5, 0.5, 0.5)) #mask = BitMask32.allOff() #print 'box-1: ', mask #np = self.worldNP.attachNewNode(BulletRigidBodyNode('Box-1')) #np.node().setMass(1.0) #np.node().addShape(shape) #np.setPos(-6, 0, 4) #np.setCollideMask(mask) #self.world.attachRigidBody(np.node()) ## Box 2 #shape = BulletBoxShape(Vec3(0.5, 0.5, 0.5)) #mask = BitMask32.bit(0) #print 'box-2: ', mask #np = self.worldNP.attachNewNode(BulletRigidBodyNode('Box-2')) #np.node().setMass(1.0) #np.node().addShape(shape) #np.setPos(-2, 0, 4) #np.setCollideMask(mask) #self.world.attachRigidBody(np.node()) ## Box 3 #shape = BulletBoxShape(Vec3(0.5, 0.5, 0.5)) #mask = BitMask32.bit(2) #print 'box-3: ', mask #np = self.worldNP.attachNewNode(BulletRigidBodyNode('Box-3')) #np.node().setMass(1.0) #np.node().addShape(shape) #np.setPos(2, 0, 4) #np.setCollideMask(mask) #self.world.attachRigidBody(np.node()) # Box 4 shape = BulletBoxShape(Vec3(0.5, 0.5, 0.5)) mask = BitMask32(0x3) print('box-4: ', mask) np = self.worldNP.attachNewNode(BulletRigidBodyNode('Box-4')) np.node().setMass(1.0) np.node().addShape(shape) np.setPos(6, 0, 4) np.setCollideMask(mask) self.world.attachRigidBody(np.node()) self.boxNP = np visualNP = loader.loadModel('models/box.egg') visualNP.reparentTo(self.boxNP)
def planContactpairs(self, torqueresist = 50, fgrtipdist = 82): """ find the grasps using parallel pairs :param: torqueresist the maximum allowable distance to com :param: fgrtipdist the maximum dist between finger tips :return: author: weiwei date: 20161130, harada office @ osaka university """ # note that pairnormals and pairfacets are duplicated for each contactpair # the duplication is performed on purpose for convenient access # also, each contactpair"s" corresponds to a facetpair # it is empty when no contactpair is available self.gripcontactpairs = [] # gripcontactpairnormals and gripcontactpairfacets are not helpful # they are kept for convenience (they could be accessed using facetnormals and facetpairs) self.gripcontactpairnormals = [] self.gripcontactpairfacets = [] # for precollision detection # bulletworldprecc = BulletWorld() # # build the collision shape of objtrimesh # geomobj = pandageom.packpandageom(self.objtrimesh.vertices, self.objtrimesh.face_normals, # self.objtrimesh.faces) # objmesh = BulletTriangleMesh() # objmesh.addGeom(geomobj) # objmeshnode = BulletRigidBodyNode('objmesh') # objmeshnode.addShape(BulletTriangleMeshShape(objmesh, dynamic=True)) # bulletworldprecc.attachRigidBody(objmeshnode) # for raytracing bulletworldray = BulletWorld() nfacets = self.facets.shape[0] self.facetpairs = list(itertools.combinations(range(nfacets), 2)) for facetpair in self.facetpairs: # print "facetpair" # print facetpair, len(self.facetpairs) self.gripcontactpairs.append([]) self.gripcontactpairnormals.append([]) self.gripcontactpairfacets.append([]) # if one of the facet doesnt have samples, jump to next if self.objsamplepnts_refcls[facetpair[0]].shape[0] is 0 or \ self.objsamplepnts_refcls[facetpair[1]].shape[0] is 0: # print "no sampled points" continue # check if the faces are opposite and parallel dotnorm = np.dot(self.facetnormals[facetpair[0]], self.facetnormals[facetpair[1]]) if dotnorm < -0.95: # check if any samplepnts's projection from facetpairs[i][0] falls in the polygon of facetpairs[i][1] facet0pnts = self.objsamplepnts_refcls[facetpair[0]] facet0normal = self.facetnormals[facetpair[0]] facet1normal = self.facetnormals[facetpair[1]] # generate collision mesh facetmesh = BulletTriangleMesh() faceidsonfacet = self.facets[facetpair[1]] geom = pandageom.packpandageom(self.objtrimesh.vertices, self.objtrimesh.face_normals[faceidsonfacet], self.objtrimesh.faces[faceidsonfacet]) facetmesh.addGeom(geom) facetmeshbullnode = BulletRigidBodyNode('facet') facetmeshbullnode.addShape(BulletTriangleMeshShape(facetmesh, dynamic=True)) bulletworldray.attachRigidBody(facetmeshbullnode) # check the projection of a ray for facet0pnt in facet0pnts: pFrom = Point3(facet0pnt[0], facet0pnt[1], facet0pnt[2]) pTo = pFrom + Vec3(facet1normal[0], facet1normal[1], facet1normal[2])*9999 result = bulletworldray.rayTestClosest(pFrom, pTo) if result.hasHit(): hitpos = result.getHitPos() if np.linalg.norm(np.array(facet0pnt.tolist())-np.array([hitpos[0], hitpos[1], hitpos[2]])) < fgrtipdist: fgrcenter = (np.array(facet0pnt.tolist())+np.array([hitpos[0], hitpos[1], hitpos[2]]))/2.0 # avoid large torque if np.linalg.norm(self.objtrimesh.center_mass - fgrcenter) < torqueresist: self.gripcontactpairs[-1].append([facet0pnt.tolist(), [hitpos[0], hitpos[1], hitpos[2]]]) self.gripcontactpairnormals[-1].append([[facet0normal[0], facet0normal[1], facet0normal[2]], [facet1normal[0], facet1normal[1], facet1normal[2]]]) self.gripcontactpairfacets[-1].append(facetpair) # pre collision checking: it takes one finger as a cylinder and # computes its collision with the object ## first finger # cylindernode0 = BulletRigidBodyNode('cylindernode0') # cylinder0height = 50 # cylinder0normal = Vec3(facet0normal[0], facet0normal[1], facet0normal[2]) # cylindernode0.addShape(BulletCylinderShape(radius=self.preccradius, # height=cylinder0height, # up=cylinder0normal), # TransformState.makePos(pFrom+cylinder0normal*cylinder0height*.6)) # bulletworldprecc.attachRigidBody(cylindernode0) # ## second finger # cylindernode1 = BulletRigidBodyNode('cylindernode1') # cylinder1height = cylinder1height # # use the inverse of facet0normal instead of facet1normal to follow hand orientation # cylinder1normal = Vec3(-facet0normal[0], -facet0normal[1], -facet0normal[2]) # cylindernode1.addShape(BulletCylinderShape(radius=self.preccradius, # height=cylinder1height, # up=cylinder1normal), # TransformState.makePos(pFrom+cylinder1normal*cylinder1height*.6)) # bulletworldprecc.attachRigidBody(cylindernode1) # if bulletworldprecc.contactTestPair(cylindernode0, objmeshnode) and \ # bulletworldprecc.contactTestPair(cylindernode1, objmeshnode): bulletworldray.removeRigidBody(facetmeshbullnode) # update the pairs availablepairids = np.where(self.gripcontactpairs)[0] self.facetpairs = np.array(self.facetpairs)[availablepairids] self.gripcontactpairs = np.array(self.gripcontactpairs)[availablepairids] self.gripcontactpairnormals = np.array(self.gripcontactpairnormals)[availablepairids] self.gripcontactpairfacets = np.array(self.gripcontactpairfacets)[availablepairids]
class RegripTpp(): #def __init__(self, objpath,workcellpath, robot, handpkg, gdb, offset = -55): def __init__(self, objpath, workcellpath, robot, handpkg, gdb, offset=-55): self.objtrimesh=trimesh.load_mesh(objpath) self.handpkg = handpkg self.handname = handpkg.getHandName() # for dbaccess self.dbobjname = os.path.splitext(os.path.basename(objpath))[0] # regg = regrip graph self.regg = nx.Graph() self.ndiscreterot = 0 self.nplacements = 0 self.globalgripids = [] # for removing the grasps at start and goal self.robothand = handpkg.newHandNM(hndcolor=[1, 0, 0, .1]) # plane to remove hand self.bulletworld = BulletWorld() self.planebullnode = cd.genCollisionPlane(offset =offset) self.bulletworld.attachRigidBody(self.planebullnode) self.startnodeids = None self.goalnodeids = None self.shortestpaths = None self.gdb = gdb self.robot = robot # load retraction distances self.rethandx, self.retworldz, self.retworlda, self.worldz = self.gdb.loadIKRet() # worlda is default for the general grasps on table top # for assembly at start and goal, worlda is computed by assembly planner self.worlda = Vec3(0,0,1) # loadfreeairgrip #self.__loadDropFreeGrip() self.__loadFreeAirGrip() self.__loadGripsToBuildGraph() def __loadFreeAirGrip(self): """ load self.freegripid, etc. from mysqldatabase :param gdb: an object of the database.GraspDB class :return: author: weiwei date: 20170110 """ freeairgripdata = self.gdb.loadFreeAirGrip(self.dbobjname, self.handname) if freeairgripdata is None: raise ValueError("Plan the freeairgrip first!") self.freegripid1 = freeairgripdata[0] self.freegripcontacts1 = freeairgripdata[1] self.freegripnormals1 = freeairgripdata[2] self.freegriprotmats1 = freeairgripdata[3] self.freegripjawwidth1 = freeairgripdata[4] def __loadDropFreeGrip(self): """ load self.freegripid, etc. from mysqldatabase :param gdb: an object of the database.GraspDB class :return: author: jiayao date: 20170821 """ # freeairgripdata = self.gdb.loadDropFreeGrip(self.dbobjname, self.handname) # if freeairgripdata is None: # raise ValueError("Plan the DropFreeGrip first!") # # self.freegripid = freeairgripdata[0] # self.freegripcontacts = freeairgripdata[1] # self.freegripnormals = freeairgripdata[2] # self.freegriprotmats = freeairgripdata[3] # self.freegripjawwidth = freeairgripdata[4] handname = "rtq85" freegripid = [] freegripcontacts = [] freegripnormals = [] freegriprotmats = [] freegripjawwidth = [] # access to db gdb = db.GraspDB() sql = "SELECT dropfreegrip.iddropfreegrip, dropfreegrip.contactpnt0, dropfreegrip.contactpnt1, \ dropfreegrip.contactnormal0, dropfreegrip.contactnormal1, dropfreegrip.rotmat, \ dropfreegrip.jawwidth FROM dropfreegrip, hand, object\ WHERE dropfreegrip.idobject = object.idobject AND object.name like '%s' \ AND dropfreegrip.idhand = hand.idhand AND hand.name like '%s' \ " % (self.dbobjname, handname) data = gdb.execute(sql) if len(data) != 0: for i in range(len(data)): freegripid.append(int(data[i][0])) freegripcontacts.append([dc.strToV3(data[i][1]), dc.strToV3(data[i][2])]) freegripnormals.append([dc.strToV3(data[i][3]), dc.strToV3(data[i][4])]) freegriprotmats.append(dc.strToMat4(data[i][5])) freegripjawwidth.append(float(data[i][6])) else: print "no DropFreeGrip select" self.freegripid = freegripid self.freegripcontacts = freegripcontacts self.freegripnormals = freegripnormals self.freegriprotmats = freegriprotmats self.freegripjawwidth = freegripjawwidth print "ok" def __loadGripsToBuildGraph(self, armname = "rgt"): """ load tabletopgrips retraction distance are also loaded from database :param robot: an robot defined in robotsim.hrp5 or robotsim.nextage :param gdb: an object of the database.GraspDB class :param idarm: value = 1 "lft" or 2 "rgt", which arm to use :return: author: weiwei date: 20170112 """ # load idarm idarm = self.gdb.loadIdArm(armname) idhand = self.gdb.loadIdHand(self.handname) # get the global grip ids # and prepare the global edges # for each globalgripid, find all its tabletopids (pertaining to placements) globalidsedges = {} sql = "SELECT idfreeairgrip FROM freeairgrip,object WHERE freeairgrip.idobject=object.idobject AND \ object.name LIKE '%s' AND freeairgrip.idhand = %d" % (self.dbobjname, idhand) # sql = "SELECT dropfreegrip.iddropfreegrip FROM dropfreegrip,object WHERE dropfreegrip.idobject=object.idobject AND \ # object.name LIKE '%s' AND dropfreegrip.idhand = %d" % (self.dbobjname, idhand) # sql = "SELECT dropworkcellgrip.iddropworkcellgrip FROM dropworkcellgrip,object WHERE dropworkcellgrip.idobject=object.idobject AND \ # object.name LIKE '%s' AND dropworkcellgrip.idhand = %d" % (self.dbobjname, idhand) result = self.gdb.execute(sql) if len(result) == 0: raise ValueError("Plan freeairgrip first!") for ggid in result: globalidsedges[str(ggid[0])] = [] self.globalgripids.append(ggid[0]) sql = "SELECT dropstablepos.iddropstablepos,dropstablepos.rot,dropstablepos.pos,angle_drop.value FROM \ dropstablepos,object,angle_drop WHERE \ dropstablepos.idobject=object.idobject AND \ object.name LIKE '%s' " % self.dbobjname result = self.gdb.execute(sql) if len(result) != 0: tpsrows = np.array(result) #self.angles = list([0.0]) self.angles = list(set(map(float, tpsrows[:, 3]))) # for plotting self.fttpsids = list(set(map(int, tpsrows[:, 0]))) self.nfttps = len(self.fttpsids) idrobot = self.gdb.loadIdRobot(self.robot) for i, idtps in enumerate(tpsrows[:,0]): sql = "SELECT dropworkcellgrip.iddropworkcellgrip, dropworkcellgrip.contactpnt0, dropworkcellgrip.contactpnt1, \ dropworkcellgrip.rotmat, dropworkcellgrip.jawwidth ,dropworkcellgrip.idfreeairgrip\ FROM dropworkcellgrip,dropfreegrip,freeairgrip,ik_drop\ WHERE \ dropworkcellgrip.iddropstablepos = %d \ AND dropworkcellgrip.iddropworkcellgrip=ik_drop.iddropworkcellgrip AND ik_drop.idrobot=%d AND ik_drop.idarm = %d AND\ ik_drop.feasibility='True' AND ik_drop.feasibility_handx='True' AND ik_drop.feasibility_handxworldz='True' \ AND ik_drop.feasibility_worlda='True' AND ik_drop.feasibility_worldaworldz='True' \ AND dropworkcellgrip.idfreeairgrip = freeairgrip.idfreeairgrip \ AND dropworkcellgrip.idhand = % d AND dropworkcellgrip.iddropfreegrip = dropfreegrip.iddropfreegrip " \ % (int(idtps),idrobot, idarm, idhand) resultttgs = self.gdb.execute(sql) if len(resultttgs)==0: print "no result" continue localidedges = [] for ttgsrow in resultttgs: ttgsid = int(ttgsrow[0]) ttgscct0 = dc.strToV3(ttgsrow[1]) ttgscct1 = dc.strToV3(ttgsrow[2]) ttgsrotmat = dc.strToMat4(ttgsrow[3]) ttgsjawwidth = float(ttgsrow[4]) ttgsidfreeair = int(ttgsrow[5]) ttgsfgrcenter = (ttgscct0+ttgscct1)/2 handx = ttgsrotmat.getRow3(0) ttgsfgrcenterhandx = ttgsfgrcenter + handx*self.rethandx ttgsfgrcenterhandxworldz = ttgsfgrcenterhandx + self.worldz*self.retworldz ttgsfgrcenterworlda = ttgsfgrcenter + self.worlda*self.retworlda ttgsfgrcenterworldaworldz = ttgsfgrcenterworlda+ self.worldz*self.retworldz ttgsfgrcenternp = pg.v3ToNp(ttgsfgrcenter) ttgsfgrcenternp_handx = pg.v3ToNp(ttgsfgrcenterhandx) ttgsfgrcenternp_handxworldz = pg.v3ToNp(ttgsfgrcenterhandxworldz) ttgsfgrcenternp_worlda = pg.v3ToNp(ttgsfgrcenterworlda) ttgsfgrcenternp_worldaworldz = pg.v3ToNp(ttgsfgrcenterworldaworldz) ttgsrotmat3np = pg.mat3ToNp(ttgsrotmat.getUpper3()) objrotmat4 = pg.npToMat4(np.transpose(pg.mat3ToNp(dc.strToMat3(tpsrows[:, 1][i]))), pg.v3ToNp(dc.strToV3(tpsrows[:, 2][i]))) objrotmat4worlda = Mat4(objrotmat4) objrotmat4worlda.setRow(3, objrotmat4.getRow3(3)+self.worlda*self.retworlda) objrotmat4worldaworldz = Mat4(objrotmat4worlda) objrotmat4worldaworldz.setRow(3, objrotmat4worlda.getRow3(3)+self.worldz*self.retworldz) self.regg.add_node('mid' + str(ttgsid), fgrcenter=ttgsfgrcenternp, fgrcenterhandx = ttgsfgrcenternp_handx, fgrcenterhandxworldz = ttgsfgrcenternp_handxworldz, fgrcenterworlda = ttgsfgrcenternp_worlda, fgrcenterworldaworldz = ttgsfgrcenternp_worldaworldz, jawwidth=ttgsjawwidth, hndrotmat3np=ttgsrotmat3np, globalgripid = ttgsidfreeair, #freetabletopplacementid = int(tpsrows[:,2][i]), freetabletopplacementid=int(tpsrows[:, 0][i]), tabletopplacementrotmat = objrotmat4, tabletopplacementrotmathandx = objrotmat4, tabletopplacementrotmathandxworldz = objrotmat4, tabletopplacementrotmatworlda = objrotmat4worlda, tabletopplacementrotmatworldaworldz = objrotmat4worldaworldz, angle = float(tpsrows[:,3][i]), tabletopposition = dc.strToV3(tpsrows[:,2][i])) print "str(ttgsidfreeair),str(ttgsid)",str(ttgsidfreeair),str(ttgsid) globalidsedges[str(ttgsidfreeair)].append('mid' + str(ttgsid)) localidedges.append('mid' + str(ttgsid)) for edge in list(itertools.combinations(localidedges, 2)): self.regg.add_edge(*edge, weight=1, edgetype = 'transit') #toc = time.clock() #print "(toc - tic2)", (toc - tic) if len(globalidsedges) == 0: raise ValueError("Plan tabletopgrips first!") #tic = time.clock() for globalidedgesid in globalidsedges: for edge in list(itertools.combinations(globalidsedges[globalidedgesid], 2)): self.regg.add_edge(*edge, weight=1, edgetype = 'transfer') #toc = time.clock() #print "(toc - tic3)", (toc - tic) else: print ('No placements planned!') assert('No placements planned!') def __addstartgoal(self, startrotmat4, goalrotmat4, base): """ add start and goal for the regg :param startrotmat4 and goalrotmat4: both are 4by4 panda3d matrix :return: author: weiwei date: 20161216, sapporo """ ### start # the node id of a globalgripid in startnode nodeidofglobalidinstart= {} # the startnodeids is also for quick access self.startnodeids = [] for j, rotmat in enumerate(self.freegriprotmats1): #print j, len(self.freegriprotmats) # print rotmat ttgsrotmat = rotmat * startrotmat4 # for collision detection, we move the object back to x=0,y=0 ttgsrotmatx0y0 = Mat4(startrotmat4) ttgsrotmatx0y0.setCell(3,0,0) ttgsrotmatx0y0.setCell(3,1,0) ttgsrotmatx0y0 = rotmat * ttgsrotmatx0y0 # check if the hand collide with tabletop # tmphnd = self.robothand tmphnd = self.handpkg.newHandNM(hndcolor=[1, 0, 0, .1]) initmat = tmphnd.getMat() initjawwidth = tmphnd.jawwidth # set jawwidth to 80 to avoid collision with surrounding obstacles # set to gripping with is unnecessary tmphnd.setJawwidth(80) tmphnd.setMat(ttgsrotmatx0y0) # add hand model to bulletworld hndbullnode = cd.genCollisionMeshMultiNp(tmphnd.handnp) result = self.bulletworld.contactTest(hndbullnode) # tmphnd.setMat(ttgsrotmat) # tmphnd.reparentTo(base.render) # if j > 3: # base.run() if not result.getNumContacts(): ttgscct0=startrotmat4.xformPoint(self.freegripcontacts1[j][0]) ttgscct1=startrotmat4.xformPoint(self.freegripcontacts1[j][1]) ttgsfgrcenter = (ttgscct0+ttgscct1)/2 handx = ttgsrotmat.getRow3(0) ttgsfgrcenterhandx = ttgsfgrcenter + handx*self.rethandx # handxworldz is not necessary for start # ttgsfgrcenterhandxworldz = ttgsfgrcenterhandx + self.worldz*self.retworldz ttgsfgrcenterworlda = ttgsfgrcenter + self.worlda*self.retworlda ttgsfgrcenterworldaworldz = ttgsfgrcenterworlda+ self.worldz*self.retworldz ttgsjawwidth = self.freegripjawwidth1[j] ttgsidfreeair = self.freegripid1[j] ttgsfgrcenternp = pg.v3ToNp(ttgsfgrcenter) ttgsfgrcenternp_handx = pg.v3ToNp(ttgsfgrcenterhandx) # handxworldz is not necessary for start # ttgsfgrcenternp_handxworldz = pg.v3ToNp(ttgsfgrcenterhandxworldz) ttgsfgrcenternp_worlda = pg.v3ToNp(ttgsfgrcenterworlda) ttgsfgrcenternp_worldaworldz = pg.v3ToNp(ttgsfgrcenterworldaworldz) ttgsrotmat3np = pg.mat3ToNp(ttgsrotmat.getUpper3()) #print "solving starting iks" ikc = self.robot.numikr(ttgsfgrcenternp, ttgsrotmat3np) ikcx = self.robot.numikr(ttgsfgrcenternp_handx, ttgsrotmat3np) ikca = self.robot.numikr(ttgsfgrcenternp_worlda, ttgsrotmat3np) ikcaz = self.robot.numikr(ttgsfgrcenternp_worldaworldz, ttgsrotmat3np) if (ikc is not None) and (ikcx is not None) and (ikca is not None) and (ikcaz is not None): # note the tabletopposition here is not the contact for the intermediate states # it is the zero pos tabletopposition = startrotmat4.getRow3(3) startrotmat4worlda = Mat4(startrotmat4) startrotmat4worlda.setRow(3, startrotmat4.getRow3(3)+self.worlda*self.retworlda) startrotmat4worldaworldz = Mat4(startrotmat4worlda) startrotmat4worldaworldz.setRow(3, startrotmat4worlda.getRow3(3)+self.worldz*self.retworldz) self.regg.add_node('start'+str(j), fgrcenter=ttgsfgrcenternp, fgrcenterhandx = ttgsfgrcenternp_handx, fgrcenterhandxworldz = 'na', fgrcenterworlda = ttgsfgrcenternp_worlda, fgrcenterworldaworldz = ttgsfgrcenternp_worldaworldz, jawwidth=ttgsjawwidth, hndrotmat3np=ttgsrotmat3np, globalgripid = ttgsidfreeair, freetabletopplacementid = 'na', tabletopplacementrotmat = startrotmat4, tabletopplacementrotmathandx = startrotmat4, tabletopplacementrotmathandxworldz = 'na', tabletopplacementrotmatworlda = startrotmat4worlda, tabletopplacementrotmatworldaworldz = startrotmat4worldaworldz, angle = 'na', tabletopposition = tabletopposition) nodeidofglobalidinstart[ttgsidfreeair]='start'+str(j) self.startnodeids.append('start'+str(j)) # tmprtq85.reparentTo(base.render) tmphnd.setMat(initmat) tmphnd.setJawwidth(initjawwidth) if len(self.startnodeids) == 0: #raise ValueError("No available starting grip!") print ("No available start grip!") return False # add start transit edge for edge in list(itertools.combinations(self.startnodeids, 2)): self.regg.add_edge(*edge, weight = 1, edgetype = 'starttransit') # add start transfer edge for reggnode, reggnodedata in self.regg.nodes(data=True): if reggnode.startswith('mid'): globalgripid = reggnodedata['globalgripid'] if globalgripid in nodeidofglobalidinstart.keys(): startnodeid = nodeidofglobalidinstart[globalgripid] self.regg.add_edge(startnodeid, reggnode, weight=1, edgetype = 'starttransfer') ### goal # the node id of a globalgripid in goalnode, for quick setting up edges nodeidofglobalidingoal= {} # the goalnodeids is also for quick access self.goalnodeids = [] for j, rotmat in enumerate(self.freegriprotmats1): #print j, len(self.freegriprotmats) ttgsrotmat = rotmat * goalrotmat4 # for collision detection, we move the object back to x=0,y=0 ttgsrotmatx0y0 = Mat4(goalrotmat4) ttgsrotmatx0y0.setCell(3,0,0) ttgsrotmatx0y0.setCell(3,1,0) ttgsrotmatx0y0 = rotmat * ttgsrotmatx0y0 # check if the hand collide with tabletop tmphnd = self.robothand initmat = tmphnd.getMat() initjawwidth = tmphnd.jawwidth tmphnd.setJawwidth(self.freegripjawwidth1[j]) tmphnd.setMat(ttgsrotmatx0y0) # add hand model to bulletworld hndbullnode = cd.genCollisionMeshMultiNp(tmphnd.handnp) result = self.bulletworld.contactTest(hndbullnode) if not result.getNumContacts(): ttgscct0=goalrotmat4.xformPoint(self.freegripcontacts1[j][0]) ttgscct1=goalrotmat4.xformPoint(self.freegripcontacts1[j][1]) ttgsfgrcenter = (ttgscct0+ttgscct1)/2 handx = ttgsrotmat.getRow3(0) ttgsfgrcenterhandx = ttgsfgrcenter + handx*self.rethandx ttgsfgrcenterhandxworldz = ttgsfgrcenterhandx + self.worldz*self.retworldz ttgsfgrcenterworlda = ttgsfgrcenter + self.worlda*self.retworlda ttgsfgrcenterworldaworldz = ttgsfgrcenterworlda+ self.worldz*self.retworldz ttgsjawwidth = self.freegripjawwidth1[j] ttgsidfreeair = self.freegripid1[j] ttgsfgrcenternp = pg.v3ToNp(ttgsfgrcenter) ttgsfgrcenternp_handx = pg.v3ToNp(ttgsfgrcenterhandx) ttgsfgrcenternp_handxworldz = pg.v3ToNp(ttgsfgrcenterhandxworldz) ttgsfgrcenternp_worlda = pg.v3ToNp(ttgsfgrcenterworlda) ttgsfgrcenternp_worldaworldz = pg.v3ToNp(ttgsfgrcenterworldaworldz) ttgsrotmat3np = pg.mat3ToNp(ttgsrotmat.getUpper3()) #print "solving goal iks" ikc = self.robot.numikr(ttgsfgrcenternp, ttgsrotmat3np) ikcx = self.robot.numikr(ttgsfgrcenternp_handx, ttgsrotmat3np) ikcxz = self.robot.numikr(ttgsfgrcenternp_handxworldz, ttgsrotmat3np) ikca = self.robot.numikr(ttgsfgrcenternp_worlda, ttgsrotmat3np) ikcaz = self.robot.numikr(ttgsfgrcenternp_worldaworldz, ttgsrotmat3np) if (ikc is not None) and (ikcx is not None) and (ikcxz is not None) \ and (ikca is not None) and (ikcaz is not None): # note the tabletopposition here is not the contact for the intermediate states # it is the zero pos tabletopposition = goalrotmat4.getRow3(3) goalrotmat4worlda = Mat4(goalrotmat4) goalrotmat4worlda.setRow(3, goalrotmat4.getRow3(3)+self.worlda*self.retworlda) goalrotmat4worldaworldz = Mat4(goalrotmat4worlda) goalrotmat4worldaworldz.setRow(3, goalrotmat4worlda.getRow3(3)+self.worldz*self.retworldz) self.regg.add_node('goal'+str(j), fgrcenter=ttgsfgrcenternp, fgrcenterhandx = ttgsfgrcenternp_handx, fgrcenterhandxworldz = ttgsfgrcenternp_handxworldz, fgrcenterworlda = ttgsfgrcenternp_worlda, fgrcenterworldaworldz = ttgsfgrcenternp_worldaworldz, jawwidth=ttgsjawwidth, hndrotmat3np=ttgsrotmat3np, globalgripid = ttgsidfreeair, freetabletopplacementid = 'na', tabletopplacementrotmat = goalrotmat4, tabletopplacementrotmathandx = goalrotmat4, tabletopplacementrotmathandxworldz = goalrotmat4, tabletopplacementrotmatworlda = goalrotmat4worlda, tabletopplacementrotmatworldaworldz = goalrotmat4worldaworldz, angleid = 'na', tabletopposition = tabletopposition) nodeidofglobalidingoal[ttgsidfreeair]='goal'+str(j) self.goalnodeids.append('goal'+str(j)) tmphnd.setMat(initmat) tmphnd.setJawwidth(initjawwidth) if len(self.goalnodeids) == 0: #raise ValueError("No available goal grip!") print ("No available goal grip!") return False # add goal transit edges for edge in list(itertools.combinations(self.goalnodeids, 2)): self.regg.add_edge(*edge, weight = 1, edgetype = 'goaltransit') # add goal transfer edge for reggnode, reggnodedata in self.regg.nodes(data=True): if reggnode.startswith('mid'): globalgripid = reggnodedata['globalgripid'] if globalgripid in nodeidofglobalidingoal.keys(): goalnodeid = nodeidofglobalidingoal[globalgripid] self.regg.add_edge(goalnodeid, reggnode, weight=1, edgetype = 'goaltransfer') # add start to goal direct edges for startnodeid in self.startnodeids: for goalnodeid in self.goalnodeids: # startnodeggid = start node global grip id startnodeggid = self.regg.node[startnodeid]['globalgripid'] goalnodeggid = self.regg.node[goalnodeid]['globalgripid'] if startnodeggid == goalnodeggid: self.regg.add_edge(startnodeid, goalnodeid, weight=1, edgetype = 'startgoaltransfer') return True def findshortestpath(self, startrotmat4, goalrotmat4, base): self.directshortestpaths = [] if self.__addstartgoal(startrotmat4, goalrotmat4, base) == False: print "No path found! add start goal false" self.directshortestpaths = [] return False # startgrip = random.select(self.startnodeids) # goalgrip = random.select(self.goalnodeids) startgrip = self.startnodeids[0] goalgrip = self.goalnodeids[0] self.shortestpaths = nx.all_shortest_paths(self.regg, source = startgrip, target = goalgrip) self.directshortestpaths = [] # directshortestpaths removed the repeated start and goal transit try: for path in self.shortestpaths: print path for i, pathnode in enumerate(path): if pathnode.startswith('start') and i < len(path)-1: continue else: self.directshortestpaths.append(path[i-1:]) break for i, pathnode in enumerate(self.directshortestpaths[-1]): if i > 0 and pathnode.startswith('goal'): self.directshortestpaths[-1]=self.directshortestpaths[-1][:i+1] break except: print "No path found!" pass def plotgraph(self, pltfig): """ plot the graph without start and goal :param pltfig: the matplotlib object :return: author: weiwei date: 20161217, sapporos """ # biggest circle: grips; big circle: rotation; small circle: placements # radiusplacement = 30 # radiusrot = 6 # radiusgrip = 1 radiusplacement = 0 radiusrot = 0 radiusgrip = 0 xyplacementspos = {} xydiscreterotspos = {} self.xyzglobalgrippos = {} for i, ttpsid in enumerate(self.fttpsids): xydiscreterotspos[ttpsid]={} self.xyzglobalgrippos[ttpsid]={} xypos = [radiusplacement*math.cos(2*math.pi/self.nfttps*i), radiusplacement*math.sin(2*math.pi/self.nfttps*i)] xyplacementspos[ttpsid] = xypos for j, anglevalue in enumerate(self.angles): self.xyzglobalgrippos[ttpsid][anglevalue]={} xypos = [radiusrot*math.cos(math.radians(anglevalue)), radiusrot*math.sin(math.radians(anglevalue))] xydiscreterotspos[ttpsid][anglevalue] = \ [xyplacementspos[ttpsid][0]+xypos[0], xyplacementspos[ttpsid][1]+xypos[1]] for k, globalgripid in enumerate(self.globalgripids): xypos = [radiusgrip*math.cos(2*math.pi/len(self.globalgripids)* k), radiusgrip*math.sin(2*math.pi/len(self.globalgripids)*k)] self.xyzglobalgrippos[ttpsid][anglevalue][globalgripid]=\ [xydiscreterotspos[ttpsid][anglevalue][0]+xypos[0], xydiscreterotspos[ttpsid][anglevalue][1]+xypos[1], 0] # for start and goal grasps poses: self.xyzlobalgrippos={} for k, globalgripid in enumerate(self.globalgripids): xypos = [radiusgrip * math.cos(2 * math.pi / len(self.globalgripids) * k), radiusgrip * math.sin(2 * math.pi / len(self.globalgripids) * k)] self.xyzlobalgrippos[globalgripid] = [xypos[0],xypos[1],0] transitedges = [] transferedges = [] starttransferedges = [] goaltransferedges = [] starttransitedges = [] goaltransitedges = [] for nid0, nid1, reggedgedata in self.regg.edges(data=True): if (reggedgedata['edgetype'] is 'transit') or (reggedgedata['edgetype'] is 'transfer'): fttpid0 = self.regg.node[nid0]['freetabletopplacementid'] anglevalue0 = self.regg.node[nid0]['angle'] ggid0 = self.regg.node[nid0]['globalgripid'] fttpid1 = self.regg.node[nid1]['freetabletopplacementid'] anglevalue1 = self.regg.node[nid1]['angle'] ggid1 = self.regg.node[nid1]['globalgripid'] tabletopposition0 = self.regg.node[nid0]['tabletopposition'] tabletopposition1 = self.regg.node[nid1]['tabletopposition'] xyzpos0 = map(add, self.xyzglobalgrippos[fttpid0][anglevalue0][ggid0], [tabletopposition0[0], tabletopposition0[1], tabletopposition0[2]]) xyzpos1 = map(add, self.xyzglobalgrippos[fttpid1][anglevalue1][ggid1], [tabletopposition1[0], tabletopposition1[1], tabletopposition1[2]]) # 3d # if reggedgedata['edgetype'] is 'transit': # transitedges.append([xyzpos0, xyzpos1]) # if reggedgedata['edgetype'] is 'transfer': # transferedges.append([xyzpos0, xyzpos1]) #2d if reggedgedata['edgetype'] is 'transit': transitedges.append([xyzpos0[:2], xyzpos1[:2]]) if reggedgedata['edgetype'] is 'transfer': transferedges.append([xyzpos0[:2], xyzpos1[:2]]) else: if (reggedgedata['edgetype'] is 'starttransit') or (reggedgedata['edgetype'] is 'goaltransit'): gid0 = self.regg.node[nid0]['globalgripid'] gid1 = self.regg.node[nid1]['globalgripid'] tabletopposition0 = self.regg.node[nid0]['tabletopposition'] tabletopposition1 = self.regg.node[nid1]['tabletopposition'] xyzpos0 = map(add, self.xyzlobalgrippos[gid0], [tabletopposition0[0], tabletopposition0[1], tabletopposition0[2]]) xyzpos1 = map(add, self.xyzlobalgrippos[gid1], [tabletopposition1[0], tabletopposition1[1], tabletopposition1[2]]) if reggedgedata['edgetype'] is 'starttransit': starttransitedges.append([xyzpos0[:2], xyzpos1[:2]]) if reggedgedata['edgetype'] is 'goaltransit': goaltransitedges.append([xyzpos0[:2], xyzpos1[:2]]) else: # start or goal transfer if nid0.startswith('mid'): fttpid0 = self.regg.node[nid0]['freetabletopplacementid'] anglevalue0 = self.regg.node[nid0]['angle'] gid0 = self.regg.node[nid0]['globalgripid'] tabletopposition0 = self.regg.node[nid0]['tabletopposition'] gid1 = self.regg.node[nid1]['globalgripid'] tabletopposition1 = self.regg.node[nid1]['tabletopposition'] xyzpos0 = map(add, self.xyzglobalgrippos[fttpid0][anglevalue0][gid0], [tabletopposition0[0], tabletopposition0[1], tabletopposition0[2]]) xyzpos1 = map(add, self.xyzlobalgrippos[gid1], [tabletopposition1[0], tabletopposition1[1], tabletopposition1[2]]) if nid1[:4] == 'goal': goaltransferedges.append([xyzpos0[:2], xyzpos1[:2]]) else: starttransferedges.append([xyzpos0[:2], xyzpos1[:2]]) if nid1.startswith('mid'): gid0 = self.regg.node[nid0]['globalgripid'] tabletopposition0 = self.regg.node[nid0]['tabletopposition'] fttpid1 = self.regg.node[nid1]['freetabletopplacementid'] anglevalue1 = self.regg.node[nid1]['angle'] gid1 = self.regg.node[nid1]['globalgripid'] tabletopposition1 = self.regg.node[nid1]['tabletopposition'] xyzpos0 = map(add, self.xyzlobalgrippos[gid0], [tabletopposition0[0], tabletopposition0[1], tabletopposition0[2]]) xyzpos1 = map(add, self.xyzglobalgrippos[fttpid1][anglevalue1][gid1], [tabletopposition1[0], tabletopposition1[1], tabletopposition1[2]]) if nid0[:4] == 'goal': goaltransferedges.append([xyzpos0[:2], xyzpos1[:2]]) else: starttransferedges.append([xyzpos0[:2], xyzpos1[:2]]) #3d # transitec = mc3d.Line3DCollection(transitedges, colors=[0,1,1,1], linewidths=1) # transferec = mc3d.Line3DCollection(transferedges, colors=[0,0,0,.1], linewidths=1) #2d transitec = mc.LineCollection(transitedges, colors=[0,1,1,1], linewidths=1) transferec = mc.LineCollection(transferedges, colors=[0,0,0,.1], linewidths=1) starttransferec = mc.LineCollection(starttransferedges, colors=[1,0,0,.3], linewidths=1) goaltransferec = mc.LineCollection(goaltransferedges, colors=[0,0,1,.3], linewidths=1) starttransitec = mc.LineCollection(starttransitedges, colors=[.5,0,0,.3], linewidths=1) goaltransitec = mc.LineCollection(goaltransitedges, colors=[0,0,.5,.3], linewidths=1) ax = pltfig.add_subplot(111) ax.add_collection(transferec) ax.add_collection(transitec) ax.add_collection(starttransferec) ax.add_collection(goaltransferec) ax.add_collection(starttransitec) ax.add_collection(goaltransitec) # for reggnode, reggnodedata in self.regg.nodes(data=True): # placementid = reggnodedata['placementid'] # angleid = reggnodedata['angleid'] # globalgripid = reggnodedata['globalgripid'] # tabletopposition = reggnodedata['tabletopposition'] # xyzpos = map(add, xyzglobalgrippos[placementid][angleid][str(globalgripid)],[tabletopposition[0], tabletopposition[1], tabletopposition[2]]) # plt.plot(xyzpos[0], xyzpos[1], 'ro') def plotshortestpath(self, pltfig, id = 0): """ plot the shortest path about transit and transfer: The tabletoppositions of start and goal are the local zero of the mesh model in contrast, the tabletoppositions of the other nodes in the graph are the local zero of the supporting facet if tabletopposition start == tabletop position goal there are two possibilities: 1) start and goal are the same, then it is transit 2) start and goal are different, then it is tranfer Note that start and the second will never be the same since they are in different coordinate systems. It is reasonable since the shortest path will never let the start go to the same position again. if the second item is not the goal, the path between the first and second items is sure to be a transfer path :param id: :return: """ for i,path in enumerate(self.directshortestpaths): if i is id: pathedgestransit = [] pathedgestransfer = [] pathlength = len(path) for pnidx in range(pathlength-1): nid0 = path[pnidx] nid1 = path[pnidx+1] if pnidx == 0 and pnidx+1 == pathlength-1: gid0 = self.regg.node[nid0]['globalgripid'] tabletopposition0 = self.regg.node[nid0]['tabletopposition'] gid1 = self.regg.node[nid1]['globalgripid'] tabletopposition1 = self.regg.node[nid1]['tabletopposition'] xyzpos0 = map(add, self.xyzlobalgrippos[gid0], [tabletopposition0[0], tabletopposition0[1], tabletopposition0[2]]) xyzpos1 = map(add, self.xyzlobalgrippos[gid1], [tabletopposition1[0], tabletopposition1[1], tabletopposition1[2]]) if tabletopposition0 == tabletopposition1: pathedgestransit.append([xyzpos0[:2], xyzpos1[:2]]) else: pathedgestransfer.append([xyzpos0[:2], xyzpos1[:2]]) else: if pnidx == 0: # this is sure to be transfer gid0 = self.regg.node[nid0]['globalgripid'] tabletopposition0 = self.regg.node[nid0]['tabletopposition'] fttpid1 = self.regg.node[nid1]['freetabletopplacementid'] anglevalue1 = self.regg.node[nid1]['angle'] gid1 = self.regg.node[nid1]['globalgripid'] tabletopposition1 = self.regg.node[nid1]['tabletopposition'] xyzpos0 = map(add, self.xyzlobalgrippos[gid0], [tabletopposition0[0], tabletopposition0[1], tabletopposition0[2]]) xyzpos1 = map(add, self.xyzglobalgrippos[fttpid1][anglevalue1][gid1], [ tabletopposition1[0], tabletopposition1[1], tabletopposition1[2]]) pathedgestransfer.append([xyzpos0[:2], xyzpos1[:2]]) if pnidx+1 == pathlength-1: # also definitely transfer fttpid0 = self.regg.node[nid0]['freetabletopplacementid'] anglevalue0 = self.regg.node[nid0]['angle'] gid0 = self.regg.node[nid0]['globalgripid'] tabletopposition0 = self.regg.node[nid0]['tabletopposition'] gid1 = self.regg.node[nid1]['globalgripid'] tabletopposition1 = self.regg.node[nid1]['tabletopposition'] xyzpos0 = map(add, self.xyzglobalgrippos[fttpid0][anglevalue0][gid0], [tabletopposition0[0], tabletopposition0[1], tabletopposition0[2]]) xyzpos1 = map(add, self.xyzlobalgrippos[gid1], [tabletopposition1[0], tabletopposition1[1], tabletopposition1[2]]) pathedgestransfer.append([xyzpos0[:2], xyzpos1[:2]]) if pnidx > 0 and pnidx+1 < pathlength-1: fttpid0 = self.regg.node[nid0]['freetabletopplacementid'] anglevalue0 = self.regg.node[nid0]['angle'] gid0 = self.regg.node[nid0]['globalgripid'] tabletopposition0 = self.regg.node[nid0]['tabletopposition'] fttpid1 = self.regg.node[nid1]['freetabletopplacementid'] anglevalue1 = self.regg.node[nid1]['angle'] gid1 = self.regg.node[nid1]['globalgripid'] tabletopposition1 = self.regg.node[nid1]['tabletopposition'] xyzpos0 = map(add, self.xyzglobalgrippos[fttpid0][anglevalue0][gid0], [tabletopposition0[0], tabletopposition0[1], tabletopposition0[2]]) xyzpos1 = map(add, self.xyzglobalgrippos[fttpid1][anglevalue1][gid1], [tabletopposition1[0], tabletopposition1[1], tabletopposition1[2]]) if tabletopposition0 == tabletopposition1: pathedgestransit.append([xyzpos0[:2], xyzpos1[:2]]) else: pathedgestransfer.append([xyzpos0[:2], xyzpos1[:2]]) pathtransitec = mc.LineCollection(pathedgestransit, colors=[.5, 1, 0, 1], linewidths=5) pathtransferec = mc.LineCollection(pathedgestransfer, colors=[0, 1, 0, 1], linewidths=5) ax = pltfig.gca() ax.add_collection(pathtransitec) ax.add_collection(pathtransferec) def plotgraphp3d(self, base): """ draw the graph in panda3d :param base: :return: author: weiwei date: 20161216, osaka itami airport """ # big circle: rotation; small circle: placements radiusplacement = 30 radiusrot = 6 radiusgrip = 1 xyplacementspos = [] xydiscreterotspos = [] xyzglobalgrippos = [] for i in range(self.nplacements): xydiscreterotspos.append([]) xyzglobalgrippos.append([]) xypos = [radiusplacement*math.cos(2*math.pi/self.nplacements*i), radiusplacement*math.sin(2*math.pi/self.nplacements*i)] xyplacementspos.append(xypos) for j in range(self.ndiscreterot): xyzglobalgrippos[-1].append({}) xypos = [radiusrot*math.cos(2*math.pi/self.ndiscreterot* j), radiusrot*math.sin(2*math.pi/self.ndiscreterot * j)] xydiscreterotspos[-1].append([xyplacementspos[-1][0]+xypos[0],xyplacementspos[-1][1]+xypos[1]]) for k, globalgripid in enumerate(self.globalgripids): xypos = [radiusgrip*math.cos(2*math.pi/len(self.globalgripids)* k), radiusgrip*math.sin(2*math.pi/len(self.globalgripids)*k)] xyzglobalgrippos[-1][-1][globalgripid]=[xydiscreterotspos[-1][-1][0]+xypos[0],xydiscreterotspos[-1][-1][1]+xypos[1], 0] transitedges = [] transferedges = [] for nid0, nid1, reggedgedata in self.regg.edges(data=True): fttpid0 = self.regg.node[nid0]['freetabletopplacementid'] anglevalue0 = self.regg.node[nid0]['angle'] gid0 = self.regg.node[nid0]['globalgripid'] fttpid1 = self.regg.node[nid1]['freetabletopplacementid'] angelvalue1 = self.regg.node[nid1]['angle'] gid1 = self.regg.node[nid1]['globalgripid'] tabletopposition0 = self.regg.node[nid0]['tabletopposition'] tabletopposition1 = self.regg.node[nid1]['tabletopposition'] xyzpos0 = map(add, xyzglobalgrippos[fttpid0][anglevalue0][str(gid0)], [tabletopposition0[0], tabletopposition0[1], tabletopposition0[2]]) xyzpos1 = map(add, xyzglobalgrippos[fttpid1][angelvalue1][str(gid1)], [tabletopposition1[0], tabletopposition1[1], tabletopposition1[2]]) # 3d if reggedgedata['edgetype'] is 'transit': transitedges.append([xyzpos0, xyzpos1]) if reggedgedata['edgetype'] is 'transfer': transferedges.append([xyzpos0, xyzpos1]) #3d transitecnp = pg.makelsnodepath(transitedges, rgbacolor=[0,1,1,1]) transferecnp = pg.makelsnodepath(transferedges, rgbacolor=[0,0,0,.1]) transitecnp.reparentTo(base.render) transferecnp.reparentTo(base.render)
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 RegripTppAss(object): def __init__(self, objpath, nxtrobot, handpkg, gdb): self.graphtpp0 = GraphTpp(objpath, nxtrobot, handpkg, gdb, 'rgt') self.armname = armname self.gdb = gdb self.robot = nxtrobot self.hand = handpkg.newHandNM(hndcolor=[1, 0, 0, .1]) # plane to remove hand self.bulletworld = BulletWorld() self.planebullnode = cd.genCollisionPlane() self.bulletworld.attachRigidBody(self.planebullnode) # add tabletop plane model to bulletworld this_dir, this_filename = os.path.split(__file__) ttpath = Filename.fromOsSpecific( os.path.join( os.path.split(this_dir)[0] + os.sep, "grip", "supports", "tabletop.egg")) self.ttnodepath = NodePath("tabletop") ttl = loader.loadModel(ttpath) ttl.instanceTo(self.ttnodepath) # self.endnodeids is a dictionary where # self.endnodeids['rgt'] equals self.startnodeids # self.endnodeids['lft'] equals self.endnodeids # in right->left order self.endnodeids = {} # load retraction distances self.rethandx, self.retworldz, self.retworlda, self.worldz = self.gdb.loadIKRet( ) # worlda is default for the general grasps on table top # for assembly at start and goal, worlda is computed by assembly planner self.worlda = Vec3(0, 0, 1) self.gnodesplotpos = {} self.freegripid = [] self.freegripcontacts = [] self.freegripnormals = [] self.freegriprotmats = [] self.freegripjawwidth = [] # for start and goal grasps poses: radiusgrip = 1 self.__xyzglobalgrippos_startgoal = {} for k, globalgripid in enumerate(self.graphtpp.globalgripids): xypos = [ radiusgrip * math.cos(2 * math.pi / len(self.graphtpp.globalgripids) * k), radiusgrip * math.sin(2 * math.pi / len(self.graphtpp.globalgripids) * k) ] self.__xyzglobalgrippos_startgoal[globalgripid] = [ xypos[0], xypos[1], 0 ] self.__loadFreeAirGrip() @property def dbobjname(self): # read-only property return self.graphtpp.dbobjname def __loadFreeAirGrip(self): """ load self.freegripid, etc. from mysqldatabase :param gdb: an object of the database.GraspDB class :return: author: weiwei date: 20170110 """ freeairgripdata = self.gdb.loadFreeAirGrip(self.dbobjname) if freeairgripdata is None: raise ValueError("Plan the freeairgrip first!") self.freegripid = freeairgripdata[0] self.freegripcontacts = freeairgripdata[1] self.freegripnormals = freeairgripdata[2] self.freegriprotmats = freeairgripdata[3] self.freegripjawwidth = freeairgripdata[4] def addEnds(self, rotmat4, armname): # the node id of a globalgripid in end nodeidofglobalidinend = {} # the endnodeids is also for quick access self.endnodeids[armname] = [] for j, rotmat in enumerate(self.freegriprotmats): assgsrotmat = rotmat * rotmat4 # for collision detection, we move the object back to x=0,y=0 assgsrotmatx0y0 = Mat4(rotmat4) assgsrotmatx0y0.setCell(3, 0, 0) assgsrotmatx0y0.setCell(3, 1, 0) assgsrotmatx0y0 = rotmat * assgsrotmatx0y0 # check if the hand collide with tabletop tmphand = self.hand initmat = tmphand.getMat() initjawwidth = tmphand.jawwidth # set jawwidth to 80 to avoid collision with surrounding obstacles # set to gripping with is unnecessary # tmphand.setJawwidth(self.freegripjawwidth[j]) tmphand.setJawwidth(tmphand.jawwidthopen) tmphand.setMat(assgsrotmatx0y0) # add hand model to bulletworld hndbullnode = cd.genCollisionMeshMultiNp(tmphand.handnp) result = self.bulletworld.contactTest(hndbullnode) if not result.getNumContacts(): assgscct0 = rotmat4.xformPoint(self.freegripcontacts[j][0]) assgscct1 = rotmat4.xformPoint(self.freegripcontacts[j][1]) assgsfgrcenter = (assgscct0 + assgscct1) / 2 handx = assgsrotmat.getRow3(0) assgsfgrcenterhandx = assgsfgrcenter + handx * self.rethandx # handxworldz is not necessary for start # assgsfgrcenterhandxworldz = assgsfgrcenterhandx + self.worldz*self.retworldz assgsfgrcenterworlda = assgsfgrcenter + self.worlda * self.retworlda assgsfgrcenterworldaworldz = assgsfgrcenterworlda + self.worldz * self.retworldz assgsjawwidth = self.freegripjawwidth[j] assgsidfreeair = self.freegripid[j] assgsfgrcenternp = pg.v3ToNp(assgsfgrcenter) assgsfgrcenternp_handx = pg.v3ToNp(assgsfgrcenterhandx) # handxworldz is not necessary for start # assgsfgrcenternp_handxworldz = pg.v3ToNp(assgsfgrcenterhandxworldz) assgsfgrcenternp_worlda = pg.v3ToNp(assgsfgrcenterworlda) assgsfgrcenternp_worldaworldz = pg.v3ToNp( assgsfgrcenterworldaworldz) assgsrotmat3np = pg.mat3ToNp(assgsrotmat.getUpper3()) ikc = self.robot.numikr(assgsfgrcenternp, assgsrotmat3np) ikcx = self.robot.numikr(assgsfgrcenternp_handx, assgsrotmat3np) ikca = self.robot.numikr(assgsfgrcenternp_worlda, assgsrotmat3np) ikcaz = self.robot.numikr(assgsfgrcenternp_worldaworldz, assgsrotmat3np) if (ikc is not None) and (ikcx is not None) and ( ikca is not None) and (ikcaz is not None): # note the tabletopposition here is not the contact for the intermediate states # it is the zero pos tabletopposition = rotmat4.getRow3(3) startrotmat4worlda = Mat4(rotmat4) startrotmat4worlda.setRow( 3, rotmat4.getRow3(3) + self.worlda * self.retworlda) startrotmat4worldaworldz = Mat4(startrotmat4worlda) startrotmat4worldaworldz.setRow( 3, startrotmat4worlda.getRow3(3) + self.worldz * self.retworldz) self.graphtpp.regg.add_node( 'end' + armname + str(j), fgrcenter=assgsfgrcenternp, fgrcenterhandx=assgsfgrcenternp_handx, fgrcenterhandxworldz='na', fgrcenterworlda=assgsfgrcenternp_worlda, fgrcenterworldaworldz=assgsfgrcenternp_worldaworldz, jawwidth=assgsjawwidth, hndrotmat3np=assgsrotmat3np, globalgripid=assgsidfreeair, freetabletopplacementid='na', tabletopplacementrotmat=rotmat4, tabletopplacementrotmathandx=rotmat4, tabletopplacementrotmathandxworldz='na', tabletopplacementrotmatworlda=startrotmat4worlda, tabletopplacementrotmatworldaworldz= startrotmat4worldaworldz, angle='na', tabletopposition=tabletopposition) nodeidofglobalidinend[assgsidfreeair] = 'start' + str(j) self.endnodeids[armname].append('start' + str(j)) tmphand.setMat(initmat) tmphand.setJawwidth(initjawwidth) if len(self.endnodeids[armname]) == 0: raise ValueError("No available end grip at " + armname) # add start transit edge for edge in list(itertools.combinations(self.endnodeids[armname], 2)): self.graphtpp.regg.add_edge(*edge, weight=1, edgetype='endtransit') # add start transfer edge for reggnode, reggnodedata in self.graphtpp.regg.nodes(data=True): if reggnode.startswith(self.armname): globalgripid = reggnodedata['globalgripid'] if globalgripid in nodeidofglobalidinend.keys(): endnodeid = nodeidofglobalidinend[globalgripid] self.graphtpp.regg.add_edge(endnodeid, reggnode, weight=1, edgetype='endtransfer') for nid in self.graphtpp.regg.nodes(): if nid.startswith('end'): ggid = self.graphtpp.regg.node[nid]['globalgripid'] tabletopposition = self.graphtpp.regg.node[nid][ 'tabletopposition'] xyzpos = map(add, self.__xyzglobalgrippos_startgoal[ggid], [ tabletopposition[0], tabletopposition[1], tabletopposition[2] ]) self.gnodesplotpos[nid] = xyzpos[:2] def plotGraph(self, pltax, endname='start', gtppoffset=[0, 0]): """ :param pltax: :param endname: :param gtppoffset: where to plot graphtpp, see the plotGraph function of GraphTpp class :return: """ self.graphtpp.plotGraph(pltax, offset=gtppoffset) self.__plotEnds(pltax, endname) def __plotEnds(self, pltax, endname='end'): transitedges = [] transferedges = [] for nid0, nid1, reggedgedata in self.graphtpp.regg.edges(data=True): if nid0.startswith('end'): pos0 = self.gnodesplotpos[nid0][:2] else: pos0 = self.graphtpp.gnodesplotpos[nid0][:2] if nid1.startswith('end'): pos1 = self.gnodesplotpos[nid1][:2] else: pos1 = self.graphtpp.gnodesplotpos[nid1][:2] if (reggedgedata['edgetype'] == 'endtransit'): transitedges.append([pos0, pos1]) elif (reggedgedata['edgetype'] is 'endtransfer'): transferedges.append([pos0, pos1]) transitec = mc.LineCollection(transitedges, colors=[.5, 0, 0, .3], linewidths=1) transferec = mc.LineCollection(transferedges, colors=[1, 0, 0, .3], linewidths=1) pltax.add_collection(transferec) pltax.add_collection(transitec)
class Game(DirectObject): def __init__(self): base.setBackgroundColor(0.1, 0.1, 0.8, 1) base.setFrameRateMeter(True) base.cam.setPos(0, -60, 20) 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) # 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 update(self, task): dt = globalClock.getDt() self.world.doPhysics(dt, 10, 0.008) 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.debugNP.showTightBounds() #self.debugNP.showBounds() self.world = BulletWorld() self.world.setGravity(Vec3(0, 0, -9.81)) self.world.setDebugNode(self.debugNP.node()) # Ground p0 = Point3(-20, -20, 0) p1 = Point3(-20, 20, 0) p2 = Point3(20, -20, 0) p3 = Point3(20, 20, 0) mesh = BulletTriangleMesh() mesh.addTriangle(p0, p1, p2) mesh.addTriangle(p1, p2, p3) shape = BulletTriangleMeshShape(mesh, dynamic=False) np = self.worldNP.attachNewNode(BulletRigidBodyNode('Mesh')) np.node().addShape(shape) np.setPos(0, 0, -2) np.setCollideMask(BitMask32.allOn()) self.world.attachRigidBody(np.node()) # Stair origin = Point3(0, 0, 0) size = Vec3(2, 10, 1) shape = BulletBoxShape(size * 0.5) for i in range(10): pos = origin + size * i pos.setY(0) np = self.worldNP.attachNewNode(BulletRigidBodyNode('Stair%i' % i)) np.node().addShape(shape) np.setPos(pos) np.setCollideMask(BitMask32.allOn()) npV = loader.loadModel('models/box.egg') npV.reparentTo(np) npV.setScale(size) self.world.attachRigidBody(np.node()) # Soft body world information info = self.world.getWorldInfo() info.setAirDensity(1.2) info.setWaterDensity(0) info.setWaterOffset(0) info.setWaterNormal(Vec3(0, 0, 0)) # Softbody center = Point3(0, 0, 0) radius = Vec3(1, 1, 1) * 1.5 node = BulletSoftBodyNode.makeEllipsoid(info, center, radius, 128) node.setName('Ellipsoid') node.getMaterial(0).setLinearStiffness(0.1) node.getCfg().setDynamicFrictionCoefficient(1) node.getCfg().setDampingCoefficient(0.001) node.getCfg().setPressureCoefficient(1500) node.setTotalMass(30, True) node.setPose(True, False) np = self.worldNP.attachNewNode(node) np.setPos(15, 0, 12) #np.setH(90.0) #np.showBounds() #np.showTightBounds() self.world.attachSoftBody(np.node()) geom = BulletHelper.makeGeomFromFaces(node) node.linkGeom(geom) nodeV = GeomNode('EllipsoidVisual') nodeV.addGeom(geom) npV = np.attachNewNode(nodeV)
from panda3d.bullet import BulletBoxShape base.cam.setPos(0, -10, 0) base.cam.lookAt(0, 0, 0) # World world = BulletWorld() world.setGravity(Vec3(0, 0, -9.81)) # Plane shape = BulletPlaneShape(Vec3(0, 0, 1), 1) node = BulletRigidBodyNode('Ground') node.addShape(shape) np = render.attachNewNode(node) np.setPos(0, 0, -2) world.attachRigidBody(node) # Box shape = BulletBoxShape(Vec3(0.5, 0.5, 0.5)) node = BulletRigidBodyNode('Box') node.setMass(1.0) node.addShape(shape) np = render.attachNewNode(node) np.setPos(0, 0, 2) world.attachRigidBody(node) model = loader.loadModel('models/box.egg') model.flattenLight() model.reparentTo(np) # Update def update(task):
class LabTask03(DirectObject): #define the state of the game and level gameState = 'INIT' gameLevel = 1 cameraState = 'STARTUP' count = 0 attempts = 3 posX = -200 posY = 20 posZ = 30 score = 0 contacts = 0 pause = False fire = True desiredCamPos = Vec3(-200,30,20) def __init__(self): self.imageObject = OnscreenImage(image = 'models/splashscreen.png', pos=(0,0,0), scale=(1.4,1,1)) preloader = Preloader() self.musicLoop = loader.loadSfx("music/loop/EndlessBliss.mp3") self.snowmansHit = loader.loadSfx("music/effects/snowball_hit.wav") self.candleThrow = loader.loadSfx("music/effects/snowball_throw.wav") self.presentHit = loader.loadSfx("music/effects/present_hit.wav") self.loseSound = loader.loadSfx("music/effects/Failure-WahWah.mp3") self.winSound = loader.loadSfx("music/effects/Ta Da-SoundBible.com-1884170640.mp3") self.nextLevelSound = loader.loadSfx("music/effects/button-17.wav") self.loseScreen = OnscreenImage(image = 'models/losescreen.png', pos=(0,0,0), scale=(1.4,1,1)) self.loseScreen.hide() self.winScreen = OnscreenImage(image = 'models/winscreen.png', pos=(0,0,0), scale=(1.4,1,1)) self.winScreen.hide() self.helpScreen = OnscreenImage(image = 'models/helpscreen.jpg', pos=(0,0,0.1), scale=(1,1,0.8)) self.helpScreen.hide() self.backBtn = DirectButton(text=("Back"), scale = 0.1, pos = (0,0,-0.8), command = self.doBack) 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.doBack) self.menuBtn.hide() self.backBtn.hide() base.setBackgroundColor(0.1, 0.1, 0.8, 1) #base.setFrameRateMeter(True) # Position the camera base.cam.setPos(0, 30, 20) base.cam.lookAt(0, 30, 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) self.accept('f', self.doShoot, [True]) self.accept('p', self.doPause) inputState.watchWithModifiers('forward', 'w') inputState.watchWithModifiers('left', 'a') inputState.watchWithModifiers('reverse', 's') inputState.watchWithModifiers('right', 'd') inputState.watchWithModifiers('turnLeft', 'q') inputState.watchWithModifiers('turnRight', 'e') inputState.watchWithModifiers('moveLineUp', 'i') inputState.watchWithModifiers('moveLineDown','k') inputState.watchWithModifiers('moveLineRight','l') inputState.watchWithModifiers('moveLineLeft','j') self.font = loader.loadFont('models/SHOWG.TTF') self.font.setPixelsPerUnit(60) self.attemptText = OnscreenText(text='', pos = (0.9,0.8), scale = 0.07, font = self.font) self.levelText = OnscreenText(text='', pos=(-0.9,0.9), scale = 0.07, font = self.font ) self.scoreText = OnscreenText(text='', pos = (0.9,0.9), scale = 0.07, font = self.font) self.text = OnscreenText(text = '', pos = (0, 0), scale = 0.07, font = self.font) self.pauseText = OnscreenText(text='P: Pause', pos= (0.9,0.7), scale = 0.05, font = self.font) self.pauseText.hide() # Task taskMgr.add(self.update, 'updateWorld') # Physics self.setup() # _____HANDLER_____ def doRetry(self): self.loseScreen.hide() self.levelText.clearText() self.scoreText.clearText() self.attemptText.clearText() self.playGame() self.retryBtn.hide() self.cleanup() self.setup() def doExit(self): self.cleanup() sys.exit(1) def doReset(self): self.attempts = 3 self.cameraState = 'STARTUP' base.cam.setPos(0,30,20) self.arrow.removeNode() self.scoreText.clearText() self.levelText.clearText() self.attemptText.clearText() 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') def doShoot(self, ccd): if(self.fire and self.attempts > 0 and self.gameState == 'PLAY'): self.cameraState = 'LOOK' self.fire = False self.candleThrow.play() self.attempts -= 1 #get from/to points from mouse click ## pMouse = base.mouseWatcherNode.getMouse() ## pFrom = Point3() ## pTo = Point3() ## base.camLens.extrude(pMouse, pFrom, pTo) ## pFrom = render.getRelativePoint(base.cam, pFrom) ## pTo = render.getRelativePoint(base.cam, pTo) # calculate initial velocity v = self.pTo - self.pFrom ratio = v.length() / 40 v.normalize() v *= 70.0 * ratio torqueOffset = random.random() * 10 #create bullet shape = BulletSphereShape(0.5) shape01 = BulletSphereShape(0.5) shape02 = BulletSphereShape(0.5) shape03 = BulletSphereShape(0.5) body = BulletRigidBodyNode('Candle') bodyNP = self.worldNP.attachNewNode(body) bodyNP.node().addShape(shape, TransformState.makePos(Point3(0,0,0))) bodyNP.node().addShape(shape01, TransformState.makePos(Point3(0,0.5,-0.5))) bodyNP.node().addShape(shape02,TransformState.makePos(Point3(0,1,-1))) bodyNP.node().addShape(shape03,TransformState.makePos(Point3(0,1.5,-1.5))) bodyNP.node().setMass(100) bodyNP.node().setFriction(1.0) bodyNP.node().setLinearVelocity(v) bodyNP.node().applyTorque(v*torqueOffset) bodyNP.setPos(self.pFrom) bodyNP.setCollideMask(BitMask32.allOn()) visNP = loader.loadModel('models/projectile.X') visNP.setScale(0.7) visNP.clearModelNodes() visNP.reparentTo(bodyNP) #self.bird = bodyNP.node() if ccd: bodyNP.node().setCcdMotionThreshold(1e-7) bodyNP.node().setCcdSweptSphereRadius(0.5) self.world.attachRigidBody(bodyNP.node()) #remove the bullet again after 1 sec self.bullets = bodyNP taskMgr.doMethodLater(5, self.removeBullet, 'removeBullet', extraArgs=[bodyNP], appendTask = True) def removeBullet(self, bulletNP, task): self.cameraState = 'STAY' self.fire = True self.world.removeRigidBody(bulletNP.node()) bulletNP.removeNode() if(self.attempts <= 0 and len(self.snowmans)>0): self.gameState = 'LOSE' self.doContinue() return task.done # ____TASK___ def processInput(self, dt): force = Vec3(0, 0, 0) torque = Vec3(0, 0, 0) #print self.pTo.getY() if inputState.isSet('forward'): if(self.pTo.getZ() < 40): self.pTo.addZ(0.5) if inputState.isSet('reverse'): if(self.pTo.getZ() > 0 ): self.pTo.addZ(-0.5) if inputState.isSet('left'): if(self.pTo.getY() < 100): self.pTo.addY(0.5) self.arrow.setScale(self.arrow.getSx(),self.arrow.getSy()-0.006,self.arrow.getSz()) if inputState.isSet('right'): if(self.pTo.getY() > 60): self.pTo.addY(-0.5) self.arrow.setScale(self.arrow.getSx(),self.arrow.getSy()+0.006,self.arrow.getSz()) self.arrow.lookAt(self.pTo) def processContacts(self, dt): for box in self.boxes: for snowman in self.snowmans: result = self.world.contactTestPair(box, snowman.node()) if (result.getNumContacts() > 0): self.snowmansHit.play() self.score += 100 self.text.setPos(0,0.7) self.text.setText("HIT") self.snowmans.remove(snowman) self.world.removeRigidBody(snowman.node()) snowman.removeNode() if(len(self.snowmans) <= 0): self.gameState = "NEXT" self.text.setText('Nice! Press space to continue') for box in self.boxes: for present in self.presents: result01 = self.world.contactTestPair(box,present.node()) if(result01.getNumContacts() > 0): self.presents.remove(present) self.world.removeRigidBody(present.node()) present.removeNode() self.presentHit.play() self.score += 500 def doContinue(self): if(self.gameState == 'INIT'): self.gameState = 'MENU' self.text.clearText() self.musicLoop.setLoop(True) self.musicLoop.setVolume(0.5) self.musicLoop.play() self.startBtn = DirectButton(text=("Play"), scale = 0.1, pos = (0,0,0),command=self.playGame) self.helpBtn = DirectButton(text=("Help"), scale = 0.1, pos = (0,0,-0.2),command=self.help) self.exitBtn = DirectButton(text=("Exit"), scale = 0.1, pos = (0,0,-0.4), command = self.doExit) return if self.gameState == 'NEXT': self.nextLevelSound.play() self.fire = True self.gameLevel += 1 self.score += (self.attempts * 100) self.doReset() self.gameState = 'PLAY' return if self.gameState == 'LOSE': self.loseSound.play() self.arrow.removeNode() self.loseScreen.show() self.levelText.hide() self.attemptText.hide() self.scoreText.hide() self.text.hide() self.pauseText.hide() self.retryBtn.show() return if self.gameState == 'WIN': self.levelText.hide() self.attemptText.hide() self.scoreText.clearText() self.scoreText.setPos(0,0.6) self.scoreText.setText("%s"%self.score) self.winScreen.show() self.winSound.play() self.menuBtn.show() self.pauseText.hide() return def playGame(self): print "Play Game" self.attempts = 3 self.score = 0 self.gameLevel = 1 self.gameState = 'PLAY' self.musicLoop.setVolume(0.3) self.imageObject.hide() self.startBtn.hide() self.exitBtn.hide() self.helpBtn.hide() self.cleanup() self.setup() def doBack(self): self.gameState = 'MENU' self.scoreText.setPos(0.9,0.9) self.scoreText.hide() self.imageObject.show() self.startBtn.show() self.exitBtn.show() self.helpBtn.show() self.helpScreen.hide() self.backBtn.hide() self.menuBtn.hide() self.winScreen.hide() def help(self): self.gameState = 'HELP' self.startBtn.hide() self.exitBtn.hide() self.helpBtn.hide() self.backBtn.show() self.helpScreen.show() return def doPause(self): self.pause = not self.pause if(self.pause): self.text.setText("Pause") else: self.text.clearText() def update(self, task): dt = globalClock.getDt() if(not self.pause): if self.gameState == 'INIT': self.text.setPos(0,0) self.text.setText('Press space to continue') self.accept('space',self.doContinue) if self.gameState == 'PLAY': #print self.posZ #if(self.posX < -120): # self.posX += 0.03 # self.posZ -= 0.02 #elif(self.posZ < 70): # self.posZ += 0.02; #elif(self.posZ > 70 and self.posX > -120): # self.posZ -= 0.02 # self.posX -= 0.03 #base.cam.setPos(self.posX, self.posZ, self.posY) self.levelText.setText('Level: %s'%self.gameLevel) self.attemptText.setText('Tries Left: %s'%self.attempts) self.scoreText.setText('Score: %s'%self.score) self.pauseText.show() self.processContacts(dt) self.world.doPhysics(dt, 20, 1.0/180.0) #self.drawLines() self.processInput(dt) if self.cameraState == 'STARTUP': oldPos = base.cam.getPos() pos = (oldPos*0.9) + (self.desiredCamPos*0.1) base.cam.setPos(pos) base.cam.lookAt(0,30,0) elif self.cameraState == 'STAY': #oldPos = base.cam.getPos() #currPos = (oldPos*0.9) + (self.desiredCamPos*0.1) #base.cam.setPos(currPos) base.cam.lookAt(0,30,0) elif self.cameraState == 'LOOK': base.cam.lookAt(self.bullets) #base.cam.setPos(-200,self.bullets.getZ(),20) if self.gameState == 'NEXT': self.world.doPhysics(dt, 20, 1.0/180.0) ## self.raycast() return task.cont def raycast(self): # Raycast for closest hit tsFrom = TransformState.makePos(Point3(0,0,0)) tsTo = TransformState.makePos(Point3(10,0,0)) shape = BulletSphereShape(0.5) penetration = 1.0 result = self.world.sweepTestClosest(shape, tsFrom, tsTo, penetration) if result.hasHit(): torque = Vec3(10,0,0) force = Vec3(0,0,100) ## for hit in result.getHits(): ## hit.getNode().applyTorque(torque) ## hit.getNode().applyCentralForce(force) result.getNode().applyTorque(torque) result.getNode().applyCentralForce(force) ## print result.getClosestHitFraction() ## print result.getHitFraction(), \ ## result.getNode(), \ ## result.getHitPos(), \ ## result.getHitNormal() def cleanup(self): self.world = None self.worldNP.removeNode() self.arrow.removeNode() self.lines.reset() self.text.clearText() def setup(self): self.attemptText.show() self.levelText.show() self.scoreText.show() self.text.show() 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()) #arrow self.scaleY = 10 self.arrow = loader.loadModel('models/arrow.X') self.arrow.setScale(0.5,0.5,0.5) self.arrow.setAlphaScale(0.5) #self.arrow.setTransparency(TransparencyAttrib.MAlpha) self.arrow.reparentTo(render) #SkyBox skybox = loader.loadModel('models/skybox.X') skybox.reparentTo(render) # Ground 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()) visualNP = loader.loadModel('models/ground.X') visualNP.clearModelNodes() visualNP.reparentTo(np) #some boxes self.boxes = [] self.snowmans = [] self.platforms = [] self.presents = [] #TODO: Make a table #Table Top #self.createBox(Vec3(),Vec3()) if(self.gameLevel == 1): self.createBox(Vec3(5,7,1),Vec3(0,5,10),1.0) #2 legs self.createBox(Vec3(4,1,4),Vec3(0,11,5),1.0) self.createBox(Vec3(4,1,4),Vec3(0,-1,5),1.0) # Pigs self.createSnowman(2.0,Vec3(0, 5, 2.0),10.0) self.createSnowman(1.5, Vec3(0,-10,4.0),10.0) if(self.gameLevel == 2): #table01 self.createBox(Vec3(5,14,1),Vec3(0,-2,12),2.0) self.createBox(Vec3(5,7,1),Vec3(0,5,10),2.0) self.createBox(Vec3(4,1,4),Vec3(0,11,5),1.0) self.createBox(Vec3(4,1,4),Vec3(0,-1,5),1.0) #table02 self.createBox(Vec3(5,7,1),Vec3(0,-9,10),2.0) self.createBox(Vec3(4,1,4),Vec3(0,-3,5),1.0) self.createBox(Vec3(4,1,4),Vec3(0,-15,5),1.0) #table03 self.createBox(Vec3(1,1,1), Vec3(0,-1,14), 2.0) self.createBox(Vec3(1,1,1), Vec3(0,-3,14), 2.0) self.createBox(Vec3(1,1,1), Vec3(0,3,14), 2.0) self.createBox(Vec3(1,1,1), Vec3(0,-5,14), 2.0) self.createBox(Vec3(1,1,1), Vec3(0,5,14), 2.0) #pigs self.createSnowman(2.0,Vec3(0, 5, 2.0),10.0) self.createSnowman(2.0, Vec3(0,-9,2.0), 10.0) self.createSnowman(2.0,Vec3(0,-23,2.0),10.0) if(self.gameLevel == 3): #table01 self.createBox(Vec3(4,2,2),Vec3(0,12,12),1.0) self.createBox(Vec3(4,1,4),Vec3(0,11,5),1.0) self.createBox(Vec3(4,1,4),Vec3(0,13,5),1.0) #table02 self.createBox(Vec3(4,2,2),Vec3(0,-15,12),1.0) self.createBox(Vec3(4,1,4),Vec3(0,-14,5),1.0) self.createBox(Vec3(4,1,4),Vec3(0,-16,5),1.0) #table03 self.createBox(Vec3(4,10,1),Vec3(0,-1,12),1.0) self.createBox(Vec3(4,10,1),Vec3(0,-1,14),1.0) self.createBox(Vec3(4,2,4),Vec3(0,-2,5),0.1) #table04 self.createPlatform(Vec3(4,8,1),Vec3(0,7,16),1.0) self.createPlatform(Vec3(4,8,1),Vec3(0,-9,16),1.0) self.createBox(Vec3(4,1,3),Vec3(0,13,20),1.0) self.createBox(Vec3(4,1,3),Vec3(0,-16,20),1.0) #table05 self.createBox(Vec3(4,15,1),Vec3(0,-1,24),1.0) self.createStoneBox(Vec3(1,1,1),Vec3(0,2,20),5.0) self.createStoneBox(Vec3(1,1,1),Vec3(0,-2,20),5.0) self.createStoneBox(Vec3(1,1,1),Vec3(0,4,20),5.0) self.createStoneBox(Vec3(1,1,1),Vec3(0,8,20),5.0) self.createStoneBox(Vec3(1,1,1),Vec3(0,6,20),5.0) #pigs self.createSnowman(2.0,Vec3(0, 5, 2.0),10.0) self.createSnowman(2.0,Vec3(0,-8.5,2.0),10.0) self.createSnowman(1.5, Vec3(0,-9,19.5), 7.0) #presents self.createPresent(Vec3(2,2,2),Vec3(0,-20,5)) if(self.gameLevel == 4): #wall self.createStoneBox(Vec3(4,1.5,10), Vec3(0,20,10),20) #table01 self.createBox(Vec3(4,1,5), Vec3(0,7,7),1) self.createBox(Vec3(4,1,5), Vec3(0,0,7),1) self.createBox(Vec3(4,1,4), Vec3(0,3,7),1) self.createPlatform(Vec3(5,8,1), Vec3(0,4,13),1) self.createBox(Vec3(4,1,3), Vec3(0,11,18),1) self.createBox(Vec3(4,1,3), Vec3(0,-3,18),1) self.createBox(Vec3(4,8,1), Vec3(0,4,25),1) self.createStoneBox(Vec3(1,1,1), Vec3(0,4,27),2) self.createStoneBox(Vec3(1,1,1), Vec3(0,7,27),2) self.createStoneBox(Vec3(1,1,1), Vec3(0,2,27),2) self.createStoneBox(Vec3(1,1,1), Vec3(0,2,29),2) #stairs self.createPlatform(Vec3(4,50,4), Vec3(0,-55,5),100) #table02 self.createBox(Vec3(4,1,5), Vec3(0,-13,15),1) self.createBox(Vec3(4,1,5), Vec3(0,-20,15),1) self.createBox(Vec3(4,1,4), Vec3(0,-17,15),1) self.createPlatform(Vec3(4,8,1), Vec3(0,-16,22),1) self.createBox(Vec3(4,1,3), Vec3(0,-9,28),1) self.createBox(Vec3(4,1,3), Vec3(0,-23,28),1) self.createBox(Vec3(4,8,1), Vec3(0,-16,33),1) self.createStoneBox(Vec3(1,1,1), Vec3(0,-16,35),2) self.createStoneBox(Vec3(1,1,1), Vec3(0,-13,35),2) self.createStoneBox(Vec3(1,1,1), Vec3(0,-18,35),2) self.createStoneBox(Vec3(1,1,1), Vec3(0,-18,37),2) self.createStoneBox(Vec3(1,1,1), Vec3(0,-14,37),2) #snowman self.createSnowman(2.0,Vec3(0,30,5),1.0) self.createSnowman(1.5,Vec3(0,4,18),1.0) self.createSnowman(1.5,Vec3(0,-13,26),1.0) self.createSnowman(1.5,Vec3(0,-19,26),1.0) self.createSnowman(2.0,Vec3(0,12,5),1.0) self.createPresent(Vec3(2,2,2),Vec3(0,-25,13)) self.createPresent(Vec3(3,3,3),Vec3(0,-30,13)) self.createPresent(Vec3(4,4,4),Vec3(0,-36,13)) #self.createSnowman(1.5,Vec3(0,4,20),1.0) if(self.gameLevel == 5): #table01 self.createStoneBox(Vec3(4,7,3), Vec3(0,30,5),10.0) self.createStoneBox(Vec3(4,7,3), Vec3(0,-30,5),10.0) self.createBox(Vec3(4,1,3), Vec3(0,0,5), 1.0) self.createSnowman(1.5,Vec3(0,-6,5),1.0) self.createSnowman(1.5,Vec3(0,6,5),1.0) self.createBox(Vec3(4,1,3), Vec3(0,-12,5), 1.0) self.createBox(Vec3(4,1,3), Vec3(0,12,5), 1.0) self.createSnowman(1.5,Vec3(0,-18,5),1.0) self.createSnowman(1.5,Vec3(0,18,5),1.0) self.createStoneBox(Vec3(4,6,1),Vec3(0,-18,10), 0.1) self.createStoneBox(Vec3(4,6,1),Vec3(0,-6,10), 0.1) self.createStoneBox(Vec3(4,6,1),Vec3(0,6,10), 0.1) self.createStoneBox(Vec3(4,6,1),Vec3(0,18,10), 0.1) self.createStoneBox(Vec3(4,1,3),Vec3(0,23,14), 1.0) self.createStoneBox(Vec3(4,1,3),Vec3(0,-23,14), 1.0) self.createBox(Vec3(4,1,3),Vec3(0,18,14),1.0) self.createBox(Vec3(4,1,3),Vec3(0,-18,14),1.0) self.createStoneBox(Vec3(4,1,7),Vec3(0,13,20), 2.0) self.createStoneBox(Vec3(4,1,7),Vec3(0,-13,20), 2.0) self.createBox(Vec3(4,1,3),Vec3(0,8,14),1.0) self.createBox(Vec3(4,1,3),Vec3(0,-8,14),1.0) self.createStoneBox(Vec3(4,1,3),Vec3(0,3,14), 1.0) self.createStoneBox(Vec3(4,1,3),Vec3(0,-3,14), 1.0) self.createPlatform(Vec3(4,3.5,1),Vec3(0,-20 ,20), 1.0) self.createPlatform(Vec3(4,3.5,1),Vec3(0,20 ,20), 1.0) self.createPlatform(Vec3(4,3.5,1),Vec3(0,-5 ,20), 1.0) self.createPlatform(Vec3(4,3.5,1),Vec3(0,5 ,20), 1.0) self.createStoneBox(Vec3(4,1,3.5),Vec3(0,-18,25), 2.0) self.createStoneBox(Vec3(4,1,3.5),Vec3(0,18,25), 2.0) self.createStoneBox(Vec3(4,1,3.5),Vec3(0,-7.5,25), 2.0) self.createStoneBox(Vec3(4,1,3.5),Vec3(0,7.5,25), 2.0) self.createStoneBox(Vec3(4,6,1),Vec3(0,-12,30), 2.0) self.createStoneBox(Vec3(4,6,1),Vec3(0,12,30), 2.0) self.createBox(Vec3(4,1,5),Vec3(0,-5,30), 2.0) self.createBox(Vec3(4,1,5),Vec3(0,5,30), 2.0) self.createBox(Vec3(4,5,1),Vec3(0,0,40), 2.0) self.createPlatform(Vec3(4,2,0.5),Vec3(0,0,42), 2.0) self.createStoneBox(Vec3(4,0.5,2),Vec3(0,-3.5,45), 2.0) self.createStoneBox(Vec3(4,0.5,2),Vec3(0,3.5,45), 2.0) self.createStoneBox(Vec3(4,4,0.5),Vec3(0,0,48), 2.0) self.createPresent(Vec3(1.5,1.5,1.5),Vec3(0,22,30)) self.createPresent(Vec3(1.5,1.5,1.5),Vec3(0,-22,30)) self.createPresent(Vec3(2,2,1),Vec3(0,0,44)) self.createPresent(Vec3(3,3,4),Vec3(0,0,33)) if(self.gameLevel > 5): self.gameState = 'WIN' self.doContinue() return # drawing lines between the points ## self.lines = LineNodePath(parent = render, thickness = 3.0, colorVec = Vec4(1, 0, 0, 1)) ## self.pFrom = Point3(-4, 0, 0.5) ## self.pTo = Point3(4, 0, 0.5) # Aiming line self.lines = LineNodePath(parent = render, thickness = 3.0, colorVec = Vec4(1, 0, 0, 1)) self.pFrom = Point3(0, 100, 0.5) self.pTo = Point3(0, 60, 10) self.arrow.setPos(self.pFrom) def drawLines(self): # Draws lines for the ray. self.lines.reset() self.lines.drawLines([(self.pFrom,self.pTo)]) self.lines.create() def createBox(self,size,pos,mass): shape = BulletBoxShape(size) body = BulletRigidBodyNode('Obstacle') bodyNP = self.worldNP.attachNewNode(body) bodyNP.node().addShape(shape) bodyNP.node().setMass(mass) bodyNP.node().setFriction(1.0) bodyNP.node().setDeactivationEnabled(False) bodyNP.setPos(pos) bodyNP.setCollideMask(BitMask32.allOn()) self.world.attachRigidBody(bodyNP.node()) visNP = loader.loadModel('models/crate.X') visNP.setScale(size*2) visNP.clearModelNodes() visNP.reparentTo(bodyNP) self.boxes.append(body) def createStoneBox(self,size,pos,mass): shape = BulletBoxShape(size) body = BulletRigidBodyNode('Obstacle') bodyNP = self.worldNP.attachNewNode(body) bodyNP.node().addShape(shape) bodyNP.node().setMass(mass) bodyNP.node().setFriction(1.0) bodyNP.node().setDeactivationEnabled(False) bodyNP.setPos(pos) bodyNP.setCollideMask(BitMask32.allOn()) self.world.attachRigidBody(bodyNP.node()) visNP = loader.loadModel('models/stone.X') visNP.setScale(size*2) visNP.clearModelNodes() visNP.reparentTo(bodyNP) self.boxes.append(body) def createSnowman(self, size, pos, mass): shape = BulletBoxShape(Vec3(size,size,size)) shape01 = BulletSphereShape(size/2) body = BulletRigidBodyNode('Snowman') np = self.worldNP.attachNewNode(body) np.node().setMass(mass) np.node().addShape(shape, TransformState.makePos(Point3(0,0,-1))) np.node().addShape(shape01, TransformState.makePos(Point3(0,0,1))) np.node().setFriction(10.0) np.node().setDeactivationEnabled(False) np.setPos(pos) np.setCollideMask(BitMask32.allOn()) self.world.attachRigidBody(np.node()) visualNP = loader.loadModel('models/snowman.X') visualNP.setScale(size) visualNP.clearModelNodes() visualNP.reparentTo(np) self.snowmans.append(np) def createPlatform(self,size,pos,mass): shape = BulletBoxShape(size) body = BulletRigidBodyNode('Platform') bodyNP = self.worldNP.attachNewNode(body) bodyNP.node().addShape(shape) bodyNP.node().setMass(mass) bodyNP.node().setFriction(1.0) bodyNP.node().setDeactivationEnabled(False) bodyNP.setPos(pos) bodyNP.setCollideMask(BitMask32.allOn()) self.world.attachRigidBody(bodyNP.node()) visNP = loader.loadModel('models/crate.X') visNP.setScale(size*2) visNP.clearModelNodes() visNP.reparentTo(bodyNP) self.platforms.append(body) def createPresent(self,size,pos): shape = BulletBoxShape(size*0.7) body = BulletRigidBodyNode('Present') bodyNP = self.worldNP.attachNewNode(body) bodyNP.node().addShape(shape) bodyNP.node().setMass(1.0) bodyNP.node().setFriction(1.0) bodyNP.node().setDeactivationEnabled(False) bodyNP.setPos(pos) bodyNP.setCollideMask(BitMask32.allOn()) self.world.attachRigidBody(bodyNP.node()) visNP = loader.loadModel('models/present.X') visNP.setScale(size*2) visNP.clearModelNodes() visNP.reparentTo(bodyNP) self.presents.append(bodyNP)
class App(ShowBase): def __init__(self, args): ShowBase.__init__(self) headless = args["--headless"] width = args["--width"] height = args["--height"] globalClock.setMode(ClockObject.MNonRealTime) globalClock.setDt(0.02) # 20ms per frame self.setBackgroundColor(0.9, 0.9, 0.9) self.createLighting() if not headless: self.setupCamera(width, height, Vec3(200, -200, 0), Vec3(0, 0, 0)) # self.render.setAntialias(AntialiasAttrib.MAuto) # filters = CommonFilters(self.win, self.cam) # filters.setAmbientOcclusion(numsamples=16, radius=0.01, amount=1.0, strength=2.0, falloff=0.002) # filters.setAmbientOcclusion(radius=0.01, amount=0.5, strength=0.5) self.render.setShaderAuto() self.world = BulletWorld() self.world.setGravity(Vec3(0, 0, -9.81 * 100.0)) # self.world.setGravity(Vec3(0, 0, 0)) # self.setupDebug() self.createPlane(Vec3(0, 0, -100)) self.animated_rig = ExposedJointRig("walking", {"walk": "walking-animation.egg"}) self.animated_rig.reparentTo(self.render) self.animated_rig.setPos(0, 0, 0) # self.animated_rig.createLines(VBase4(0.5, 0.75, 1.0, 1.0)) self.physical_rig = RigidBodyRig() self.physical_rig.reparentTo(self.render) self.physical_rig.setPos(0, 0, 0) self.physical_rig.createColliders(self.animated_rig) self.physical_rig.createConstraints() self.physical_rig.setCollideMask(BitMask32.bit(1)) self.physical_rig.attachRigidBodies(self.world) self.physical_rig.attachConstraints(self.world) self.physical_rig.attachCubes(self.loader) self.target_physical_rig = RigidBodyRig() self.target_physical_rig.reparentTo(self.render) self.target_physical_rig.setPos(0, 0, 0) self.target_physical_rig.createColliders(self.animated_rig) self.target_physical_rig.createConstraints() self.target_physical_rig.setCollideMask(BitMask32.bit(2)) self.target_physical_rig.attachRigidBodies(self.world) self.target_physical_rig.attachConstraints(self.world) self.target_physical_rig.clearMasses() self.disableCollisions() # self.animated_rig.pose('walk', 0) self.physical_rig.matchPose(self.animated_rig) self.target_physical_rig.matchPose(self.animated_rig) self.world.doPhysics(globalClock.getDt(), 10, 1.0 / 180.0) self.physical_rig.clearForces() self.target_physical_rig.clearForces() self.num_frames = self.animated_rig.getNumFrames("walk") self.model = load_model("model.json", "weights.hdf5") self.err_sum = 0.0 self.accept("escape", sys.exit) self.taskMgr.add(self.update, "update") def disableCollisions(self): for i in range(32): self.world.setGroupCollisionFlag(i, i, False) self.world.setGroupCollisionFlag(0, 1, True) self.world.setGroupCollisionFlag(0, 2, True) def setupDebug(self): node = BulletDebugNode("Debug") node.showWireframe(True) self.world.setDebugNode(node) np = self.render.attachNewNode(node) np.show() def createLens(self, aspect_ratio, fov=60.0, near=1.0, far=1000.0): lens = PerspectiveLens() lens.setFov(fov) lens.setAspectRatio(aspect_ratio) lens.setNearFar(near, far) return lens def setupCamera(self, width, height, pos, look): self.cam.setPos(pos) self.cam.lookAt(look) self.cam.node().setLens(self.createLens(width / height)) def createLighting(self): light = DirectionalLight("light") light.setColor(VBase4(0.4, 0.4, 0.4, 1)) light.setShadowCaster(True) light.getLens().setNearFar(100.0, 400.0) light.getLens().setFilmSize(400, 400) # light.showFrustum() np = self.render.attachNewNode(light) np.setPos(100, -100, 200) np.lookAt(0, 0, -100) self.render.setLight(np) light = AmbientLight("ambient") light.setColor(VBase4(0.2, 0.2, 0.2, 1)) np = self.render.attachNewNode(light) self.render.setLight(np) def createPlane(self, pos): rb = BulletRigidBodyNode("GroundPlane") rb.addShape(BulletPlaneShape(Vec3(0, 0, 1), 1)) rb.setFriction(1.0) rb.setAnisotropicFriction(1.0) rb.setRestitution(1.0) np = self.render.attachNewNode(rb) np.setPos(pos) np.setCollideMask(BitMask32.bit(0)) plane = self.loader.loadModel("cube") plane.setScale(Vec3(100, 100, 1)) plane.setPos(Vec3(0, 0, -0.5)) plane.setColor(VBase4(0.8, 0.8, 0.8, 1.0)) plane.reparentTo(np) self.world.attachRigidBody(rb) return np def update(self, task): # self.world.doPhysics(globalClock.getDt(), 10, 1.0 / 180.0) # return task.cont frame_count = globalClock.getFrameCount() joint_positions = self.physical_rig.getJointPositions() joint_rotations = self.physical_rig.getJointRotations() linear_velocities = self.physical_rig.getLinearVelocities() angular_velocities = self.physical_rig.getAngularVelocities() # pause_count = 1 # if frame_count % pause_count == 0: # self.animated_rig.pose('walk', int(frame_count / pause_count) % self.num_frames) # self.target_physical_rig.matchPose(self.animated_rig) next_joint_positions = self.target_physical_rig.getJointPositions() next_joint_rotations = self.target_physical_rig.getJointRotations() target_directions = next_joint_positions - joint_positions target_rotations = get_angle_vec(next_joint_rotations - joint_rotations) X_max = [ 123.79363250732422, 123.79363250732422, 123.79363250732422, 123.79363250732422, 123.79363250732422, 123.79363250732422, 123.79363250732422, 123.79363250732422, 123.79363250732422, 123.79363250732422, 123.79363250732422, 123.79363250732422, 123.79363250732422, 123.79363250732422, 123.79363250732422, 123.79363250732422, 123.79363250732422, 123.79363250732422, 123.79363250732422, 123.79363250732422, 123.79363250732422, 123.79363250732422, 123.79363250732422, 123.79363250732422, 123.79363250732422, 123.79363250732422, 123.79363250732422, 123.79363250732422, 123.79363250732422, 123.79363250732422, 123.79363250732422, 123.79363250732422, 123.79363250732422, 123.79363250732422, 123.79363250732422, 123.79363250732422, 123.79363250732422, 123.79363250732422, 123.79363250732422, 123.79363250732422, 123.79363250732422, 123.79363250732422, 123.79363250732422, 123.79363250732422, 123.79363250732422, 123.79363250732422, 123.79363250732422, 123.79363250732422, 123.79363250732422, 123.79363250732422, 123.79363250732422, 123.79363250732422, 123.79363250732422, 123.79363250732422, 180.0, 180.0, 180.0, 180.0, 180.0, 180.0, 180.0, 180.0, 180.0, 180.0, 180.0, 180.0, 180.0, 180.0, 180.0, 180.0, 180.0, 180.0, 180.0, 180.0, 180.0, 180.0, 180.0, 180.0, 180.0, 180.0, 180.0, 180.0, 180.0, 180.0, 180.0, 180.0, 180.0, 180.0, 180.0, 180.0, 180.0, 180.0, 180.0, 180.0, 180.0, 180.0, 180.0, 180.0, 180.0, 180.0, 180.0, 180.0, 180.0, 180.0, 180.0, 180.0, 180.0, 180.0, 1886.330810546875, 1886.330810546875, 1886.330810546875, 1886.330810546875, 1886.330810546875, 1886.330810546875, 1886.330810546875, 1886.330810546875, 1886.330810546875, 1886.330810546875, 1886.330810546875, 1886.330810546875, 1886.330810546875, 1886.330810546875, 1886.330810546875, 1886.330810546875, 1886.330810546875, 1886.330810546875, 1886.330810546875, 1886.330810546875, 1886.330810546875, 1886.330810546875, 1886.330810546875, 1886.330810546875, 1886.330810546875, 1886.330810546875, 1886.330810546875, 1886.330810546875, 1886.330810546875, 1886.330810546875, 1886.330810546875, 1886.330810546875, 1886.330810546875, 1886.330810546875, 1886.330810546875, 1886.330810546875, 1886.330810546875, 1886.330810546875, 1886.330810546875, 1886.330810546875, 1886.330810546875, 1886.330810546875, 1886.330810546875, 1886.330810546875, 1886.330810546875, 1886.330810546875, 1886.330810546875, 1886.330810546875, 1886.330810546875, 1886.330810546875, 1886.330810546875, 1886.330810546875, 1886.330810546875, 1886.330810546875, 179.98973083496094, 179.98973083496094, 179.98973083496094, 179.98973083496094, 179.98973083496094, 179.98973083496094, 179.98973083496094, 179.98973083496094, 179.98973083496094, 179.98973083496094, 179.98973083496094, 179.98973083496094, 179.98973083496094, 179.98973083496094, 179.98973083496094, 179.98973083496094, 179.98973083496094, 179.98973083496094, 179.98973083496094, 179.98973083496094, 179.98973083496094, 179.98973083496094, 179.98973083496094, 179.98973083496094, 179.98973083496094, 179.98973083496094, 179.98973083496094, 179.98973083496094, 179.98973083496094, 179.98973083496094, 179.98973083496094, 179.98973083496094, 179.98973083496094, 179.98973083496094, 179.98973083496094, 179.98973083496094, 179.98973083496094, 179.98973083496094, 179.98973083496094, 179.98973083496094, 179.98973083496094, 179.98973083496094, 179.98973083496094, 179.98973083496094, 179.98973083496094, 179.98973083496094, 179.98973083496094, 179.98973083496094, 179.98973083496094, 179.98973083496094, 179.98973083496094, 179.98973083496094, 179.98973083496094, 179.98973083496094, 33.925838470458984, 33.925838470458984, 33.925838470458984, 33.925838470458984, 33.925838470458984, 33.925838470458984, 33.925838470458984, 33.925838470458984, 33.925838470458984, 33.925838470458984, 33.925838470458984, 33.925838470458984, 33.925838470458984, 33.925838470458984, 33.925838470458984, 33.925838470458984, 33.925838470458984, 33.925838470458984, 33.925838470458984, 33.925838470458984, 33.925838470458984, 33.925838470458984, 33.925838470458984, 33.925838470458984, 33.925838470458984, 33.925838470458984, 33.925838470458984, 33.925838470458984, 33.925838470458984, 33.925838470458984, 33.925838470458984, 33.925838470458984, 33.925838470458984, 33.925838470458984, 33.925838470458984, 33.925838470458984, 33.925838470458984, 33.925838470458984, 33.925838470458984, 33.925838470458984, 33.925838470458984, 33.925838470458984, 33.925838470458984, 33.925838470458984, 33.925838470458984, 33.925838470458984, 33.925838470458984, 33.925838470458984, 33.925838470458984, 33.925838470458984, 33.925838470458984, 33.925838470458984, 33.925838470458984, 33.925838470458984, 180.0, 180.0, 180.0, 180.0, 180.0, 180.0, 180.0, 180.0, 180.0, 180.0, 180.0, 180.0, 180.0, 180.0, 180.0, 180.0, 180.0, 180.0, 180.0, 180.0, 180.0, 180.0, 180.0, 180.0, 180.0, 180.0, 180.0, 180.0, 180.0, 180.0, 180.0, 180.0, 180.0, 180.0, 180.0, 180.0, 180.0, 180.0, 180.0, 180.0, 180.0, 180.0, 180.0, 180.0, 180.0, 180.0, 180.0, 180.0, 180.0, 180.0, 180.0, 180.0, 180.0, 180.0, ] Y_max = 200000.0 X = ( np.concatenate( [ joint_positions, joint_rotations, linear_velocities, angular_velocities, target_directions, target_rotations, ] ) / X_max ) Y = np.clip(self.model.predict(np.array([X])), -1.0, 1.0) * Y_max self.physical_rig.apply_forces(Y[0]) self.world.doPhysics(globalClock.getDt(), 10, 1.0 / 180.0) err = self.target_physical_rig.compareTo(self.physical_rig) self.err_sum += math.pow(err, 2.0) err_rmse = math.sqrt(self.err_sum / frame_count) print err_rmse return task.cont
class App(ShowBase): def __init__(self, args): ShowBase.__init__(self) headless = args["--headless"] width = args["--width"] height = args["--height"] self.save_path = args["<save_path>"] globalClock.setMode(ClockObject.MNonRealTime) globalClock.setDt(0.02) # 20ms per frame self.setBackgroundColor(0.9, 0.9, 0.9) self.createLighting() if not headless: self.setupCamera(width, height, Vec3(0, -20, 0), Vec3(0, 0, 0)) self.world = BulletWorld() self.world.setGravity(Vec3(0, 0, -9.81)) self.setupDebug() self.createPlane(Vec3(0, 0, -11)) self.animated_rig = ExposedJointRig("walking", {"walk": "walking-animation.egg"}) self.animated_rig.reparentTo(self.render) self.animated_rig.setPos(0, 0, 0) self.animated_rig.setScale(0.1, 0.1, 0.1) self.animated_rig.createLines(VBase4(0.5, 0.75, 1.0, 1.0)) self.physical_rig = RigidBodyRig() self.physical_rig.reparentTo(self.render) self.physical_rig.setPos(0, 0, 0) self.physical_rig.setScale(0.1, 0.1, 0.1) self.physical_rig.createColliders(self.animated_rig) self.physical_rig.createConstraints(offset_scale=0.1) self.physical_rig.setCollideMask(BitMask32.bit(1)) self.physical_rig.attachRigidBodies(self.world) self.physical_rig.attachConstraints(self.world) self.disableCollisions() # self.setAnimationFrame(0) self.physical_rig.matchPose(self.animated_rig) self.frame_count = self.animated_rig.getNumFrames("walk") self.babble_count = 10 self.train_count = self.frame_count * self.babble_count * 1000 self.test_count = self.frame_count * self.babble_count * 100 widgets = [ progressbar.SimpleProgress(), " ", progressbar.Percentage(), " ", progressbar.Bar(), " ", progressbar.FileTransferSpeed(format="%(scaled)5.1f/s"), " ", progressbar.ETA(), " ", progressbar.Timer(format="Elapsed: %(elapsed)s"), ] self.progressbar = progressbar.ProgressBar(widgets=widgets, max_value=self.train_count + self.test_count) self.progressbar.start() self.X_train = [] self.Y_train = [] self.X_test = [] self.Y_test = [] self.accept("escape", sys.exit) self.taskMgr.add(self.update, "update") def disableCollisions(self): for i in range(32): self.world.setGroupCollisionFlag(i, i, False) self.world.setGroupCollisionFlag(0, 1, True) def setupDebug(self): node = BulletDebugNode("Debug") node.showWireframe(True) self.world.setDebugNode(node) np = self.render.attachNewNode(node) np.show() def createLens(self, aspect_ratio, fov=60.0, near=1.0, far=1000.0): lens = PerspectiveLens() lens.setFov(fov) lens.setAspectRatio(aspect_ratio) lens.setNearFar(near, far) return lens def setupCamera(self, width, height, pos, look): self.cam.setPos(pos) self.cam.lookAt(look) self.cam.node().setLens(self.createLens(width / height)) def createLighting(self): light = DirectionalLight("light") light.setColor(VBase4(0.2, 0.2, 0.2, 1)) np = self.render.attachNewNode(light) np.setPos(10, -10, 20) np.lookAt(0, 0, 0) self.render.setLight(np) light = AmbientLight("ambient") light.setColor(VBase4(0.4, 0.4, 0.4, 1)) np = self.render.attachNewNode(light) self.render.setLight(np) def createPlane(self, pos): rb = BulletRigidBodyNode("plane") rb.addShape(BulletPlaneShape(Vec3(0, 0, 1), 1)) rb.setFriction(1.0) rb.setAnisotropicFriction(1.0) rb.setRestitution(1.0) np = self.render.attachNewNode(rb) np.setPos(pos) np.setCollideMask(BitMask32.bit(0)) self.world.attachRigidBody(rb) return np def save(self): data = ((self.X_train, self.Y_train), (self.X_test, self.Y_test)) f = open(self.save_path, "wb") pickle.dump(data, f) f.close() self.progressbar.finish() def setAnimationFrame(self, frame): self.animated_rig.pose("walk", frame) self.physical_rig.matchPose(self.animated_rig) def generate(self, count): if count % self.babble_count == 0: # self.setAnimationFrame(int(count / self.babble_count)) self.physical_rig.matchPose(self.animated_rig) joint_positions = self.physical_rig.getJointPositions() joint_rotations = self.physical_rig.getJointRotations() linear_velocities = self.physical_rig.getLinearVelocities() angular_velocities = self.physical_rig.getAngularVelocities() Y = self.physical_rig.babble() self.world.doPhysics(globalClock.getDt(), 10, 1.0 / 180.0) next_joint_positions = self.physical_rig.getJointPositions() next_joint_rotations = self.physical_rig.getJointRotations() target_directions = next_joint_positions - joint_positions target_rotations = get_angle_vec(next_joint_rotations - joint_rotations) X = np.concatenate( [ joint_positions, joint_rotations, linear_velocities, angular_velocities, target_directions, target_rotations, ] ) return X, Y def update(self, task): # if globalClock.getFrameCount() % self.babble_count == 0: # self.setAnimationFrame(globalClock.getFrameCount() / self.babble_count) # self.physical_rig.babble() self.world.doPhysics(globalClock.getDt(), 10, 1.0 / 180.0) return task.cont curr_train_count = len(self.X_train) curr_test_count = len(self.X_test) train_complete = curr_train_count == self.train_count test_complete = curr_test_count == self.test_count if not train_complete: # motor babbling X, Y = self.generate(curr_train_count) self.X_train.append(X) self.Y_train.append(Y) elif not test_complete: # move positions X, Y = self.generate(curr_test_count) self.X_test.append(X) self.Y_test.append(Y) if train_complete and test_complete: self.save() sys.exit() return task.done self.progressbar.update(curr_train_count + curr_test_count) return task.cont
def step_physics(task): dt = globalClock.getDt() physics.doPhysics(dt) return task.cont s.taskMgr.add(step_physics, 'physics simulation') # A physical object in the simulation node = BulletRigidBodyNode('Box') node.setMass(1.0) node.addShape(BulletSphereShape(1)) physics.attachRigidBody(node) # Attaching the physical object in the scene graph and adding # a visible model to it np = s.render.attachNewNode(node) np.setPos(0, 0, 0) m = loader.loadModel("models/smiley") m.reparentTo(np) # Let's actually see what's happening base.cam.setPos(0, -10, 0) base.cam.lookAt(0, 0, 0) # Give the object a nudge and run the program node.apply_impulse(Vec3(0, 1, 0), Point3(0, 0, 0)) s.run()
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) self.accept('1', self.doSelect, [0,]) self.accept('2', self.doSelect, [1,]) self.accept('3', self.doSelect, [2,]) self.accept('4', self.doSelect, [3,]) self.accept('5', self.doSelect, [4,]) self.accept('6', self.doSelect, [5,]) 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') def doSelect(self, i): self.boxNP = self.boxes[i] # ____TASK___ def processInput(self, dt): force = Vec3(0, 0, 0) torque = Vec3(0, 0, 0) if inputState.isSet('forward'): force.setY( 1.0) if inputState.isSet('reverse'): force.setY(-1.0) if inputState.isSet('left'): force.setX(-1.0) if inputState.isSet('right'): force.setX( 1.0) if inputState.isSet('turnLeft'): torque.setZ( 1.0) if inputState.isSet('turnRight'): torque.setZ(-1.0) force *= 30.0 torque *= 10.0 self.boxNP.node().setActive(True) self.boxNP.node().applyCentralForce(force) self.boxNP.node().applyTorque(torque) def update(self, task): dt = globalClock.getDt() self.processInput(dt) self.world.doPhysics(dt) 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()) # Ground shape = BulletPlaneShape(Vec3(0, 0, 1), -1) np = self.worldNP.attachNewNode(BulletRigidBodyNode('Ground')) np.node().addShape(shape) np.setPos(0, 0, -1) np.setCollideMask(BitMask32.bit(0)) self.world.attachRigidBody(np.node()) # Boxes self.boxes = [None,]*6 for i in range(6): shape = BulletBoxShape(Vec3(0.5, 0.5, 0.5)) np = self.worldNP.attachNewNode(BulletRigidBodyNode('Box-1')) np.node().setMass(1.0) np.node().addShape(shape) self.world.attachRigidBody(np.node()) self.boxes[i] = np visualNP = loader.loadModel('models/box.egg') visualNP.reparentTo(np) self.boxes[0].setPos(-3, -3, 0) self.boxes[1].setPos( 0, -3, 0) self.boxes[2].setPos( 3, -3, 0) self.boxes[3].setPos(-3, 3, 0) self.boxes[4].setPos( 0, 3, 0) self.boxes[5].setPos( 3, 3, 0) self.boxes[0].setCollideMask(BitMask32.bit(1)) self.boxes[1].setCollideMask(BitMask32.bit(2)) self.boxes[2].setCollideMask(BitMask32.bit(3)) self.boxes[3].setCollideMask(BitMask32.bit(1)) self.boxes[4].setCollideMask(BitMask32.bit(2)) self.boxes[5].setCollideMask(BitMask32.bit(3)) self.boxNP = self.boxes[0] self.world.setGroupCollisionFlag(0, 1, True) self.world.setGroupCollisionFlag(0, 2, True) self.world.setGroupCollisionFlag(0, 3, True) self.world.setGroupCollisionFlag(1, 1, False) self.world.setGroupCollisionFlag(1, 2, True)