def checkgrasp(self):
grasp = gpa.load_pickle_file(self.objname, None, "rtqhe.pickle")
grasp_info_list = grasp
print("number of grasp 1", len(grasp))
hndfa = rtqhe.RobotiqHE(enable_cc=True)
rtqHE = copy.deepcopy(hndfa)
rtqHE.jaw_to(jawwidth=0.035)
slope = self.slopelist_high
obj = cm.CollisionModel(initor=self.objpath)
obj.set_scale((0.001, 0.001, 0.001))
cfreegrasp = []
for i, rotmat4 in enumerate(self.pRotMat):
temcfreegrasp = []
if self.collisionlist[i] == False and self.stablelist[i] == True:
obstaclelist = slope
obj.set_homomat(da.pdmat4_to_npmat4(rotmat4))
obstaclelist.append(obj)
for grasp in grasp_info_list:
jaw_width, jaw_center_pos, jaw_center_rotmat, hnd_pos, hnd_rotmat = grasp
rotmatgraspnp = np.dot(
da.pdmat4_to_npmat4(rotmat4),
rm.homomat_from_posrot(hnd_pos, hnd_rotmat))
rotmat4panda = da.npmat4_to_pdmat4(rotmatgraspnp)
rtqHE.fix_to(pos=rotmatgraspnp[:3, 3],
rotmat=rotmatgraspnp[:3, :3],
jawwidth=jaw_width)
if rtqHE.is_mesh_collided(obstaclelist) == False:
temcfreegrasp.append([rotmat4panda, False])
else:
temcfreegrasp.append([rotmat4panda, True])
print("grasp in collision")
cfreegrasp.append(temcfreegrasp)
print("number of grasp 2", len(temcfreegrasp))
self.cfreegrasppose = cfreegrasp
def rot_top(self, angle):
rot = rm.rotmat_from_axangle((0, 0, 1), angle / 180 * np.pi)
mat = da.npv3mat3_to_pdmat4((0, 0, 0), rot)
for i, cube in enumerate(self.node_list):
if round(cube.getPos()[2], 2) == 0.02:
print(i)
self.node_list[i].setMat(
da.npmat4_to_pdmat4(
np.dot(da.pdmat4_to_npmat4(mat),
da.pdmat4_to_npmat4(
self.node_list[i].getMat()))))
def copy_cdnp_to(self, nodepath, homomat=None, clearmask=False):
"""
Return a nodepath including the cdcn,
the returned nodepath is attached to the given one
:param nodepath: parent np
:param homomat: allow specifying a special homomat to virtually represent a pose that is different from the mesh
:return:
author: weiwei
date: 20180811
"""
returnnp = nodepath.attachNewNode(copy.deepcopy(self.cdnp.getNode(0)))
if clearmask:
returnnp.node().setCollideMask(0x00)
else:
returnnp.node().setCollideMask(self.cdnp.getCollideMask())
if homomat is None:
returnnp.setMat(self._objpdnp.getMat())
else:
returnnp.setMat(da.npmat4_to_pdmat4(homomat)) # scale is reset to 1 1 1 after setMat to the given homomat
returnnp.setScale(self._objpdnp.getScale())
return returnnp
def getplacementRotMat(self):
self.p = []
for i in range(len(self.rotmat)):
for j in range(len(self.rotmat[i])):
com = copy.deepcopy(self.com)
holdingpairsnormals = self.hpairsnormals[i]
holdingpairscenters = self.hpairscenters[i]
temholdingpairsvertices = self.hpairsvertices[i]
# sloperot = None
a = self.rotmat[i][j]
b = self.holdingpairsSptPnt[i]
addvector = np.array([[1], [1], [1]])
holdingpairscenters = np.concatenate(
(holdingpairscenters, addvector), axis=1)
addvector_vertices = np.array([1])
addvector_com = np.array([1])
print(com)
com = np.concatenate((np.array(com), addvector_com), axis=0)
holdingpairsvertices = []
for temvertices in temholdingpairsvertices:
tempverticeonSFC = []
for vertices in temvertices:
vertices = np.concatenate(
(vertices, addvector_vertices), axis=0)
tempverticeonSFC.append(vertices)
holdingpairsvertices.append(tempverticeonSFC)
sloperot = rm.homomat_from_posrot(rot=np.dot(
rm.rotmat_from_axangle([0, 1, 0], np.radians(-54.74)),
rm.rotmat_from_axangle([0, 0, 1], np.radians(-45))))
# sloperot = np.eye(4)
# sloperot2 = da.npmat4_to_pdmat4(sloperot1)
# sloperot3 = da.pdmat4_to_npmat4(sloperot2)
p = da.npmat4_to_pdmat4(rm.homomat_from_posrot(b, a))
self.p.append(p)
p = da.pdmat4_to_npmat4(p)
p = np.linalg.inv(p)
p = np.dot(sloperot, p)
placementNormals0 = np.dot(p[0:3, 0:3],
holdingpairsnormals[0].T).T
placementNormals1 = np.dot(p[0:3, 0:3],
holdingpairsnormals[1].T).T
placementNormals2 = np.dot(p[0:3, 0:3],
holdingpairsnormals[2].T).T
placementfacetpairscenter0 = np.dot(p,
holdingpairscenters[0].T).T
placementfacetpairscenter1 = np.dot(p,
holdingpairscenters[1].T).T
placementfacetpairscenter2 = np.dot(p,
holdingpairscenters[2].T).T
#change CoM
# x_e=5
# y_e = 5
# z_e = 5
# CoMRotMat = np.array([[1,0,0,x_e],[0,1,0,y_e],[0,0,1,z_e],[0,0,0,1]])
# com = np.dot(CoMRotMat,com.T)
#-----------------------
print(com.T)
placementCom0 = np.dot(p, com.T).T
dtemplacementVertices = []
for vertices in holdingpairsvertices:
templacementVertices = []
for vertice in vertices:
placementfacetpairvertices = np.dot(p, vertice.T).T
templacementVertices.append(placementfacetpairvertices)
dtemplacementVertices.append(templacementVertices)
ddtemplacementface = []
for placement in self.hpairsfaces_triple[i][j]:
dtemplacementface = []
for face in placement:
templacementface = []
for faceverticeID in face:
faceverticePos = np.concatenate(
(self.vertices[faceverticeID], np.array([1])),
axis=0)
rotatedfaceveritce = np.dot(p, faceverticePos.T).T
templacementface.append(
rotatedfaceveritce) #every small face
dtemplacementface.append(
templacementface) #every facet
ddtemplacementface.append(
dtemplacementface) #every placement
p = da.npmat4_to_pdmat4(p)
self.pRotMat.append(p)
self.pNormals.append([
placementNormals0[0:3], placementNormals1[0:3],
placementNormals2[0:3]
])
self.pFacetpairscenter.append([
placementfacetpairscenter0[0:3],
placementfacetpairscenter1[0:3],
placementfacetpairscenter2[0:3]
])
self.pVertices.append(dtemplacementVertices)
self.pCoM.append(placementCom0)
self.pFaces.append(ddtemplacementface)
def __init__(self,
initor,
cdtype="triangles",
mass=.3,
restitution=0,
allow_deactivation=False,
allow_ccd=True,
friction=.2,
dynamic=True,
name="rbd"):
"""
TODO: triangles do not seem to work (very slow) in the github version (20210418)
Use convex if possible
:param initor: could be itself (copy), or an instance of collision model
:param type: triangle or convex
:param mass:
:param restitution: bounce parameter
:param friction:
:param dynamic: only applicable to triangle type, if an object does not move with force, it is not dynamic
:param name:
author: weiwei
date: 20190626, 20201119
"""
super().__init__(name)
if isinstance(initor, gm.GeometricModel):
if initor._objtrm is None:
raise ValueError("Only applicable to models with a trimesh!")
self.com = initor.objtrm.center_mass * base.physics_scale
self.setMass(mass)
self.setRestitution(restitution)
self.setFriction(friction)
self.setLinearDamping(.3)
self.setAngularDamping(.3)
if allow_deactivation:
self.setDeactivationEnabled(True)
self.setLinearSleepThreshold(.01 * base.physics_scale)
self.setAngularSleepThreshold(.01 * base.physics_scale)
else:
self.setDeactivationEnabled(False)
if allow_ccd: # continuous collision detection
self.setCcdMotionThreshold(1e-7)
self.setCcdSweptSphereRadius(0.0005 * base.physics_scale)
geom_np = initor.objpdnp.getChild(0).find("+GeomNode")
geom = copy.deepcopy(geom_np.node().getGeom(0))
vdata = geom.modifyVertexData()
vertices = copy.deepcopy(
np.frombuffer(vdata.modifyArrayHandle(0).getData(),
dtype=np.float32))
vertices.shape = (-1, 6)
vertices[:, :3] = vertices[:, :3] * base.physics_scale - self.com
vdata.modifyArrayHandle(0).setData(
vertices.astype(np.float32).tobytes())
geomtf = geom_np.getTransform()
geomtf = geomtf.setPos(geomtf.getPos() * base.physics_scale)
if cdtype == "triangles":
geombmesh = BulletTriangleMesh()
geombmesh.addGeom(geom)
bulletshape = BulletTriangleMeshShape(geombmesh,
dynamic=dynamic)
bulletshape.setMargin(1e-6)
self.addShape(bulletshape, geomtf)
elif cdtype == "convex":
bulletshape = BulletConvexHullShape(
) # TODO: compute a convex hull?
bulletshape.addGeom(geom, geomtf)
bulletshape.setMargin(1e-6)
self.addShape(bulletshape, geomtf)
elif cdtype == 'box':
minx = min(vertices[:, 0])
miny = min(vertices[:, 1])
minz = min(vertices[:, 2])
maxx = max(vertices[:, 0])
maxy = max(vertices[:, 1])
maxz = max(vertices[:, 2])
pcd_box = CollisionBox(Point3(minx, miny, minz),
Point3(maxx, maxy, maxz))
bulletshape = BulletBoxShape.makeFromSolid(pcd_box)
bulletshape.setMargin(1e-6)
self.addShape(bulletshape, geomtf)
else:
raise NotImplementedError
pd_homomat = geomtf.getMat()
pd_com_pos = pd_homomat.xformPoint(
Vec3(self.com[0], self.com[1], self.com[2]))
np_homomat = dh.pdmat4_to_npmat4(pd_homomat)
np_com_pos = dh.pdv3_to_npv3(pd_com_pos)
np_homomat[:3, 3] = np_com_pos # update center to com
self.setTransform(
TransformState.makeMat(dh.npmat4_to_pdmat4(np_homomat)))
elif isinstance(initor, BDBody):
self.com = initor.com.copy()
self.setMass(initor.getMass())
self.setRestitution(initor.restitution)
self.setFriction(initor.friction)
self.setLinearDamping(.3)
self.setAngularDamping(.3)
if allow_deactivation:
self.setDeactivationEnabled(True)
self.setLinearSleepThreshold(.01 * base.physics_scale)
self.setAngularSleepThreshold(.01 * base.physics_scale)
else:
self.setDeactivationEnabled(False)
if allow_ccd:
self.setCcdMotionThreshold(1e-7)
self.setCcdSweptSphereRadius(0.0005 * base.physics_scale)
np_homomat = copy.deepcopy(initor.get_homomat())
np_homomat[:3, 3] = np_homomat[:3, 3] * base.physics_scale
self.setTransform(
TransformState.makeMat(dh.npmat4_to_pdmat4(np_homomat)))
self.addShape(initor.getShape(0), initor.getShapeTransform(0))
def __init__(self, objinit, cdtype="triangle", mass=.3, restitution=0, allowdeactivation=False, allowccd=True,
friction=.2, dynamic=True, name="rbd"):
"""
:param objinit: could be itself (copy), or an instance of collision model
:param type: triangle or convex
:param mass:
:param restitution: bounce parameter
:param friction:
:param dynamic: only applicable to triangle type, if an object does not move with force, it is not dynamic
:param name:
author: weiwei
date: 20190626, 20201119
"""
super().__init__(name)
if isinstance(objinit, gm.GeometricModel):
if objinit._objtrm is None:
raise ValueError("Only applicable to models with a trimesh!")
self.com = objinit.objtrm.center_mass
self.setMass(mass)
self.setRestitution(restitution)
self.setFriction(friction)
self.setLinearDamping(.3)
self.setAngularDamping(.3)
if allowdeactivation:
self.setDeactivationEnabled(True)
self.setLinearSleepThreshold(0.001)
self.setAngularSleepThreshold(0.001)
else:
self.setDeactivationEnabled(False)
if allowccd: # continuous collision detection
self.setCcdMotionThreshold(1e-6)
self.setCcdSweptSphereRadius(0.0005)
gnd = objinit.objpdnp.getChild(0).find("+GeomNode")
geom = copy.deepcopy(gnd.node().getGeom(0))
vdata = geom.modifyVertexData()
vertrewritter = GeomVertexRewriter(vdata, 'vertex')
while not vertrewritter.isAtEnd(): # shift local coordinate to geom to correctly update dynamic changes
v = vertrewritter.getData3f()
vertrewritter.setData3f(v[0] - self.com[0], v[1] - self.com[1], v[2] - self.com[2])
geomtf = gnd.getTransform()
if cdtype is "triangle":
geombmesh = BulletTriangleMesh()
geombmesh.addGeom(geom)
bulletshape = BulletTriangleMeshShape(geombmesh, dynamic=dynamic)
bulletshape.setMargin(1e-6)
self.addShape(bulletshape, geomtf)
elif cdtype is "convex":
bulletshape = BulletConvexHullShape() # TODO: compute a convex hull?
bulletshape.addGeom(geom, geomtf)
bulletshape.setMargin(1e-6)
self.addShape(bulletshape, geomtf)
else:
raise NotImplementedError
pdmat4 = geomtf.getMat()
pdv3 = pdmat4.xformPoint(Vec3(self.com[0], self.com[1], self.com[2]))
homomat = dh.pdmat4_to_npmat4(pdmat4)
pos = dh.pdv3_to_npv3(pdv3)
homomat[:3, 3] = pos # update center to com
self.setTransform(TransformState.makeMat(dh.npmat4_to_pdmat4(homomat)))
elif isinstance(objinit, BDBody):
self.com = objinit.com.copy()
self.setMass(objinit.getMass())
self.setRestitution(objinit.restitution)
self.setFriction(objinit.friction)
self.setLinearDamping(.3)
self.setAngularDamping(.3)
if allowdeactivation:
self.setDeactivationEnabled(True)
self.setLinearSleepThreshold(0.001)
self.setAngularSleepThreshold(0.001)
else:
self.setDeactivationEnabled(False)
if allowccd:
self.setCcdMotionThreshold(1e-6)
self.setCcdSweptSphereRadius(0.0005)
self.setTransform(TransformState.makeMat(dh.npmat4_to_pdmat4(objinit.gethomomat())))
self.addShape(objinit.getShape(0), objinit.getShapeTransform(0))