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]
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 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 isSelfCollided(self, robot):
"""
check the collision of a single arm
:param armid: 'lft' or 'rgt'
:return:
author: weiwei
date: 20170608
"""
dualmnps = self.robotmesh.genmnp_list(robot)
# single arm check
sglmnps = dualmnps[0]
sglbullnodesrgt = []
sglbullnodeslft = []
# armbase is not examined
for sglmnp in sglmnps[1:len(sglmnps)-1]:
sglbullnode = cd.genCollisionMeshNp(sglmnp, basenodepath=None, name='autogen')
sglbullnodesrgt.append(sglbullnode)
# hand is multinp
sglbullnode = cd.genCollisionMeshMultiNp(sglmnps[-1], basenodepath=None, name='autogen')
sglbullnodesrgt.append(sglbullnode)
if self.__isSglArmCollided(sglbullnodesrgt):
print "right arm self collision!"
return True
else:
sglmnps = dualmnps[1]
# armbase is not examined
for sglmnp in sglmnps[1:len(sglmnps)-1]:
sglbullnode = cd.genCollisionMeshNp(sglmnp, basenodepath=None, name='autogen')
sglbullnodeslft.append(sglbullnode)
# hand is multinp
sglbullnode = cd.genCollisionMeshMultiNp(sglmnps[-1], basenodepath=None, name='autogen')
sglbullnodeslft.append(sglbullnode)
if self.__isSglArmCollided(sglbullnodeslft):
print "left arm self collision!"
return True
# dual arm check
if self.__isDualArmCollided(sglbullnodesrgt, sglbullnodeslft):
print "left-right arm self collision!"
return True
#
# arm body check
bodymnps = dualmnps[2]
bdybullnodes = []
if bodymnps:
for bodymnp in bodymnps:
bodybullnode = cd.genCollisionMeshMultiNp(bodymnp, basenodepath=None, name='autogen')
bdybullnodes.append(bodybullnode)
if self.__isSglArmBdyCollided(sglbullnodesrgt, bdybullnodes):
print "right arm body self collision!"
return True
if self.__isSglArmBdyCollided(sglbullnodeslft, bdybullnodes):
print "left right body arm self collision!"
return True
# for bullnode in sglbullnodesrgt[-4:]:
# self.bulletworld.attachRigidBody(bullnode)
# for bullnode in sglbullnodeslft:
# self.bulletworld.attachRigidBody(bullnode)
# for bullnode in bdybullnodes:
# self.bulletworld.attachRigidBody(bullnode)
return False
def isCollided(self, robot, obstaclelist):
"""
simultaneously check self-collision and robot-obstacle collision
this function should be faster than calling isselfcollided and isobstaclecollided one after another
:param robot:
:param obstaclelist:
:return:
author: weiwei
date: 20170613
"""
print self.counter
if self.counter > 200:
self.counter = 0
self.bulletworld = BulletWorld()
gc.collect()
self.counter += 1
dualmnps = self.robotmesh.genmnp_list(robot)
sglbullnodesrgt = []
sglbullnodeslft = []
# rgt arm
# single arm check
sglmnps = dualmnps[0]
# armbase is not examined
for sglmnp in sglmnps[1:len(sglmnps)-1]:
sglbullnode = cd.genCollisionMeshNp(sglmnp, basenodepath=None, name='autogen')
sglbullnodesrgt.append(sglbullnode)
# hand is multinp
sglbullnode = cd.genCollisionMeshMultiNp(sglmnps[-1], basenodepath=None, name='autogen')
sglbullnodesrgt.append(sglbullnode)
# lft arm
sglmnps = dualmnps[1]
# armbase is not examined
for sglmnp in sglmnps[1:len(sglmnps)-1]:
sglbullnode = cd.genCollisionMeshNp(sglmnp, basenodepath=None, name='autogen')
sglbullnodeslft.append(sglbullnode)
# hand is multinp
sglbullnode = cd.genCollisionMeshMultiNp(sglmnps[-1], basenodepath=None, name='autogen')
sglbullnodeslft.append(sglbullnode)
# arm obstacle check
# only check hands
nlinkrgt = len(sglbullnodesrgt)
nlinklft = len(sglbullnodeslft)
for obstaclemnp in obstaclelist:
obstaclebullnode = cd.genCollisionMeshNp(obstaclemnp, basenodepath=None, name='autogen')
for i in range(nlinkrgt - 1, nlinkrgt):
result = self.bulletworld.contactTestPair(sglbullnodesrgt[i], obstaclebullnode)
if result.getNumContacts():
print "rgtarm-obstacle collision!"
return True
for i in range(nlinklft - 1, nlinklft):
result = self.bulletworld.contactTestPair(sglbullnodeslft[i], obstaclebullnode)
if result.getNumContacts():
print "lftarm-obstacle collision!"
return True
#
# # sgl arm check
# if self.__isSglArmCollided(sglbullnodesrgt):
# print "right arm self collision!"
# return True
# else:
# if self.__isSglArmCollided(sglbullnodeslft):
# print "left arm self collision!"
# return True
#
# # dual arm check
# if self.__isDualArmCollided(sglbullnodesrgt, sglbullnodeslft):
# print "left-right arm self collision!"
# return True
# #
# # arm body check
# bodymnps = dualmnps[2]
# bdybullnodes = []
# if bodymnps:
# for bodymnp in bodymnps:
# bodybullnode = cd.genCollisionMeshMultiNp(bodymnp, basenodepath=None, name='autogen')
# bdybullnodes.append(bodybullnode)
# if self.__isSglArmBdyCollided(sglbullnodesrgt, bdybullnodes):
# print "right arm body self collision!"
# return True
# if self.__isSglArmBdyCollided(sglbullnodeslft, bdybullnodes):
# print "left right body arm self collision!"
# return True
# for bullnode in sglbullnodesrgt:
# self.bulletworld.attachRigidBody(bullnode)
# for bullnode in sglbullnodeslft:
# self.bulletworld.attachRigidBody(bullnode)
# for bullnode in bdybullnodes:
# self.bulletworld.attachRigidBody(bullnode)
return False
# print result1
# print result1.getContacts()
# print result2
# print result2.getContacts()
# for contact in result.getContacts():
# cp = contact.getManifoldPoint()
# print cp.getLocalPointA()
return task.cont
base = pandactrl.World()
# first hand
sdmbs = SdmbsSd()
sdmbs.attachTo(0,0,0,0,1,1,15)
print sdmbs.getHpr()
# sdmbs.reparentTo(base.render)
handbullnp = cd.genCollisionMeshNp(sdmbs.handnp)
# second hand
sdmbs1 = SdmbsSd()
sdmbs1.reparentTo(base.render)
hand1bullnp = cd.genCollisionMeshNp(sdmbs1.handnp)
pg.plotAxisSelf(base.render, Vec3(0,0,0))
bullcldrnp = base.render.attachNewNode("bulletcollider")
base.world = BulletWorld()
base.taskMgr.add(updateworld, "updateworld", extraArgs=[base.world], appendTask=True)
result = base.world.contactTestPair(handbullnp, hand1bullnp)
for contact in result.getContacts():
cp = contact.getManifoldPoint()
# print cp.getLocalPointA()
# print result1
# print result1.getContacts()
# print result2
# print result2.getContacts()
# for contact in result.getContacts():
# cp = contact.getManifoldPoint()
# print cp.getLocalPointA()
return task.cont
base = pandactrl.World()
# first hand
sdmbs = Sdmbs()
sdmbs.attachTo(0,0,0,0,0,1, 75)
print sdmbs.getHpr()
# sdmbs.reparentTo(base.render)
handbullnp = cd.genCollisionMeshNp(sdmbs.handnp)
# second hand
sdmbs1 = Sdmbs()
sdmbs1.reparentTo(base.render)
hand1bullnp = cd.genCollisionMeshNp(sdmbs1.handnp)
pg.plotAxisSelf(base.render, Vec3(0,0,0))
bullcldrnp = base.render.attachNewNode("bulletcollider")
base.world = BulletWorld()
base.taskMgr.add(updateworld, "updateworld", extraArgs=[base.world], appendTask=True)
result = base.world.contactTestPair(handbullnp, hand1bullnp)
for contact in result.getContacts():
cp = contact.getManifoldPoint()
# print cp.getLocalPointA()
# node.setMass(1.0)
# node.setAngularVelocity(Vec3(1,1,1))
# node.addShape(shape)
# np = base.render.attachNewNode(node)
# np.setPos(0, 0, 0)
this_dir, this_filename = os.path.split(__file__)
model_filepath = Filename.fromOsSpecific(
os.path.join(this_dir, "models", "top2.egg"))
model = loader.loadModel(model_filepath)
rbdnodepath = base.render.attachNewNode("topnode")
model.instanceTo(rbdnodepath)
rbdnodepath.setMat(Mat4.rotateMat(10, Vec3(1, 0, 0)))
rbdnodepath.setPos(0, 0, 300)
topbullnode = cd.genCollisionMeshNp(rbdnodepath)
topbullnode.setMass(.5)
topbullnode.setLinearVelocity(Vec3(0, 0, 0))
# topbullnode.setAngularVelocity(rbdnodepath.getMat().getRow3(2)*50.0)
# topbullnode.applyTorqueImpulse(rbdnodepath.getMat().getRow3(2)*50000.0)
topbullnode.applyTorqueImpulse(Vec3(0, 0, 1) * 50000.0)
print topbullnode.getInertia()
world = BulletWorld()
world.setGravity(Vec3(0, 0, 0))
world.attachRigidBody(topbullnode)
pg.plotAxisSelf(base.render)
def update(task):
dt = globalClock.getDt()
# node = BulletRigidBodyNode('Box')
# node.setMass(1.0)
# node.setAngularVelocity(Vec3(1,1,1))
# node.addShape(shape)
# np = base.render.attachNewNode(node)
# np.setPos(0, 0, 0)
this_dir, this_filename = os.path.split(__file__)
model_filepath = Filename.fromOsSpecific(os.path.join(this_dir, "models", "top2.egg"))
model = loader.loadModel(model_filepath)
rbdnodepath = base.render.attachNewNode("topnode")
model.instanceTo(rbdnodepath)
rbdnodepath.setMat(Mat4.rotateMat(10, Vec3(1,0,0)))
rbdnodepath.setPos(0,0,300)
topbullnode = cd.genCollisionMeshNp(rbdnodepath)
topbullnode.setMass(.5)
topbullnode.setLinearVelocity(Vec3(0,0,0))
# topbullnode.setAngularVelocity(rbdnodepath.getMat().getRow3(2)*50.0)
# topbullnode.applyTorqueImpulse(rbdnodepath.getMat().getRow3(2)*50000.0)
topbullnode.applyTorqueImpulse(Vec3(0,0,1)*50000.0)
print topbullnode.getInertia()
world = BulletWorld()
world.setGravity(Vec3(0, 0, 0))
world.attachRigidBody(topbullnode)
pg.plotAxisSelf(base.render)
def update(task):
dt = globalClock.getDt()
world.doPhysics(dt)