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)
