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 computehomomat(RotMatnozero,height,error):
roterror_x = rm.rotmat_from_axangle(axis=(1, 0, 0), angle=np.radians(error[3]))
roterror_y = rm.rotmat_from_axangle(axis=(0, 1, 0), angle=np.radians(error[4]))
roterror_z = rm.rotmat_from_axangle(axis=(0, 0, 1), angle=np.radians(error[5]))
roterror = rm.homomat_from_posrot(rot = np.dot(roterror_x, np.dot(roterror_y, roterror_z)))
rotating=rm.rotmat_from_axangle(axis=(0,0,1),angle=np.radians(180))
moving=np.array([0.600+error[0],0+error[1],height+error[2]])
objT=rm.homomat_from_posrot(moving, rotating)
objhomomat=np.dot(objT,np.dot(da.pdmat4_to_npmat4(RotMatnozero),roterror))
return objhomomat
def checkcollision(self):
# checker = cdchecker.MCMchecker(toggledebug=True)
temobj = cm.CollisionModel(initor=self.objpath)
temobj.set_scale((0.001, 0.001, 0.001))
collisionlist = []
for i in range(len(self.pRotMat)):
temobj.set_homomat(da.pdmat4_to_npmat4(self.pRotMat[i]))
checkresult = temobj.is_mcdwith(self.slopelist)
# checkresult = checker.isMeshMeshListCollided(temobj, self.slopelist)
collisionlist.append(checkresult)
return collisionlist
def gethomomat(self):
"""
get the homomat considering the original local frame
the dynamic body moves in a local frame defined at com (line 46 of this file), instead of returning the
homomat of the dynamic body, this file returns the pose of original local frame
the returned homomat can be used by collision bodies for rendering.
:return:
author: weiwei
date: 2019?, 20201119
"""
pdmat4 = self.getTransform().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
return homomat
def update(objmnp, counter, testnode, normalnode, graspnode, textNPose,
textNPose_lp, picnode, task):
if base.inputmgr.keymap['space'] is True:
hndfa = rtqhe.RobotiqHE(enable_cc=True)
if counter[0] < len(self.pRotMat):
print("It's No.", counter[0], "placement")
print("Is it collision?", self.collisionlist[counter[0]])
if objmnp[0] is not None:
objmnp[0].detach()
normalnode[0].detachNode()
graspnode[0].detachNode()
textNPose[0].detachNode()
textNPose[1].detachNode()
# textNPose_lp[0].detachNode()
objmnp[0] = cm.CollisionModel(initor=self.objpath)
objmnp[0].set_scale((0.001, 0.001, 0.001))
objmnp[0].set_homomat(
da.pdmat4_to_npmat4(self.pRotMat[counter[0]]))
normalnode[0] = NodePath("normal")
graspnode[0] = NodePath("graspnode")
textNPose[0] = NodePath("text")
textNPose[1] = NodePath("text")
textNPose_lp[0] = NodePath("text")
# graspnode[0].reparentTo(base.render)
normalnode[0].reparentTo(base.render)
for j in range(len(self.cfreegrasppose[counter[0]])):
if j % 30 != 0:
if not self.cfreegrasppose[counter[0]][j][1]:
hndfa.gen_meshmodel(rgba=(0, 1, 0,
0.15)).attach_to(
graspnode[0])
else:
continue
print("number of grasp 3",
len(self.cfreegrasppose[counter[0]]))
hndfa.fix_to(
pos=da.pdmat4_to_npmat4(
self.cfreegrasppose[counter[0]][j][0])[:3, 3],
rotmat=da.pdmat4_to_npmat4(
self.cfreegrasppose[counter[0]][j][0])[:3, :3])
if self.cfreegrasppose[counter[0]][j][1]:
hndfa.gen_meshmodel(rgba=(1, 0, 0,
0.05)).attach_to(
graspnode[0])
else:
hndfa.gen_meshmodel(rgba=(0, 1, 0,
0.15)).attach_to(
graspnode[0])
graspnode[0].reparentTo(base.render)
self.showfeatures(
node=normalnode[0],
normal=self.pNormals[counter[0]],
placementfacetpairscenter=self.pFacetpairscenter[
counter[0]],
placementVertices=self.pVertices[counter[0]],
placementCoM=self.pCoM[counter[0]],
showCoM=True,
showVertices=False,
showNormal=False)
if self.stablelist is not None:
print("stable?", self.stablelist[counter[0]])
# textNPose[0] = OnscreenText(
# text="Stability (uniform) is:"+str(self.stabilityindex[0][counter[0]]), pos=(-.9, -.8, 0),
# scale=0.1,
# fg=(1., 0, 0, 1),
# align=TextNode.ALeft, mayChange=1)
# textNPose_lp[0] = OnscreenText(
# text="Stability (lp u=0.3) is:" + str(self.stabilityindex[1][counter[0]]), pos=(-.9, -.9, 0),
# scale=0.1,
# fg=(1., 0, 0, 1),
# align=TextNode.ALeft, mayChange=1)
# checker = cdchecker.MCMchecker(toggledebug=True)
# checker.showMesh(objmnp[0])
if self.collisionlist[counter[0]]:
objmnp[0].set_rgba((1, 0, 0, 1))
# objmnp[0].set_rgba((0.1, 0.1, 0.1, 1))
else:
if self.stablelist[counter[0]]:
objmnp[0].set_rgba((0, 191 / 255, 1, 1))
objmnp[0].set_rgba((0.1, 0.1, 0.1, 0.5))
else:
objmnp[0].set_rgba((0.1, 0.1, 0.1, 1))
objmnp[0].attach_to(base)
counter[0] += 1
else:
counter[0] = 0
if base.inputmgr.keymap['w'] is True:
if counter[0] < len(self.pRotMat):
print("It's No.", counter[0], "placement")
print("Is it collision?", self.collisionlist[counter[0]])
if objmnp[0] is not None:
objmnp[0].detach()
normalnode[0].detachNode()
graspnode[0].detachNode()
textNPose[0].detachNode()
textNPose[1].detachNode()
# picnode[0].detachNode()
# picnode[1].detachNode()
objmnp[0] = cm.CollisionModel(initor=self.objpath)
objmnp[0].set_scale((0.001, 0.001, 0.001))
objmnp[0].set_homomat(
da.pdmat4_to_npmat4(self.pRotMat[counter[0]]))
normalnode[0] = NodePath("normal")
graspnode[0] = NodePath("graspnode")
textNPose[0] = NodePath("text")
textNPose[1] = NodePath("text")
graspnode[0].reparentTo(base.render)
normalnode[0].reparentTo(base.render)
# picnode[0] = NodePath("obj")
# picnode[0].setMat(da.npmat4_to_pdmat4(rm.homomat_from_posrot(rot=rm.rotmat_from_axangle([1, 0, 0], np.radians(-45)))))
# picnode[0].reparentTo(base.render)
# picnode[1] = OnscreenImage(picnode[0], pos=(150, 0, 0), scale=100, parent=testnode[0])
# image = self.picnodepath[counter[0]]
# picnode[1].setImage(image=image)
textNPose[1] = OnscreenText(text="ID" + str([counter[0]]),
pos=(-.9, -.6, 0),
scale=0.1,
fg=(0, 0, 0, 1),
align=TextNode.ALeft,
mayChange=1)
# for j in range(len(self.cfreegrasppose[counter[0]])):
# rtqHE = hndfa.genHand()
# rtqHE.setMat(self.cfreegrasppose[counter[0]][j])
# rtqHE.reparentTo(graspnode[0])
# self.showsingleNormal(node = normalnode[0], normal = self.placementNormals[counter[0]],
# placementfacetpairscenter = self.placementfacetpairscenter[counter[0]],
# placementVertices = self.placementVertices[counter[0]],
# placementCoM = self.placementCoM[counter[0]])
self.showfeatures(
node=normalnode[0],
normal=self.pNormals[counter[0]],
placementfacetpairscenter=self.pFacetpairscenter[
counter[0]],
placementVertices=self.pVertices[counter[0]],
placementCoM=self.pCoM[counter[0]],
showCoM=True,
showVertices=False,
showNormal=False)
if self.stablelist is not None:
print("stable?", self.stablelist[counter[0]])
textNPose[0] = OnscreenText(
text="Stability (uniform) is:" +
str(self.stabilityindex_nolp[counter[0]]),
pos=(-.9, -.8, 0),
scale=0.1,
fg=(1., 0, 0, 1),
align=TextNode.ALeft,
mayChange=1)
# textNPose_lp[0] = OnscreenText(
# text="Stability (lp u=0.3) is:" + str(self.stabilityindex[1][counter[0]]), pos=(-.9, -.9, 0),
# scale=0.1,
# fg=(1., 0, 0, 1),
# align=TextNode.ALeft, mayChange=1)
# checker = cdchecker.MCMchecker(toggledebug=True)
# checker.showMesh(objmnp[0])
if self.collisionlist[counter[0]]:
objmnp[0].set_rgba((1, 0, 0, 0.5))
else:
if self.stablelist is not None:
if self.stablelist[counter[0]]:
objmnp[0].set_rgba((0, 191 / 255, 1, 0.5))
objmnp[0].set_rgba((0.1, 0.1, 0.1, 0.5))
# objmnp[0].set_rgba((1, 0, 0, 1))
else:
objmnp[0].set_rgba((0.1, 0.1, 0.1, 0.5))
# objmnp[0].set_rgba((1, 0, 0, 1))
else:
objmnp[0].set_rgba((0, 191 / 255, 1, 0.5))
objmnp[0].set_rgba((0.1, 0.1, 0.1, 0.5))
# objmnp[0].set_rgba((1, 0, 0, 1))
objmnp[0].attach_to(base)
counter[0] += 1
else:
counter[0] = 0
return task.again
RotMat[i] for i in range(len(stablecklist)) if stablecklist[i] is True
]
RotMatnozeroID = [
i for i in range(len(stablecklist)) if stablecklist[i] is True
]
comlist = [
comlistall[i] for i in range(len(stablecklist))
if stablecklist[i] is True
]
object_fixture = object.copy()
object_fixture_pos = fixture_start_pos + np.array([0, 0, 0.030])
object_fixture_rotmat = np.eye(3)
object_fixture_homomat = rm.homomat_from_posrot(
object_fixture_pos,
object_fixture_rotmat).dot(da.pdmat4_to_npmat4(RotMatnozero[1]))
object_fixture.set_homomat(object_fixture_homomat)
# object_fixture.attach_to(base)
# object_goal = object_fixture
object_start = object.copy()
# object_start_pos = np.array([0.900, -.350, 0.800])
object_start_pos = np.array([0.950, -.350, 0.800])
object_start_rotmat = rm.rotmat_from_axangle(
(1, 0, 0),
np.radians(-90)).dot(rm.rotmat_from_axangle((0, 0, 1),
np.radians(180)))
object_start_homomat = rm.homomat_from_posrot(object_start_pos,
object_start_rotmat)
object_start.set_pos(object_start_pos)
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))
def setMat(self, pandamat4):
self._bdb.sethomomat(da.pdmat4_to_npmat4(pandamat4))
self._objcm.objnp.setMat(pandamat4)
objname = "test_long_small"
with open(address + "/" + objname + "/" + "placementrotmat.pickle", "rb") as f:
RotMat = pickle.load(f)
with open(address + "/" + objname + "/" + "stablecklist.pickle", "rb") as f:
stablecklist = pickle.load(f)
with open(address + "/" + objname + "/" + "placementcom.pickle", "rb") as f:
comlistall = pickle.load(f)
RotMatnozero = [RotMat[i] for i in range(len(stablecklist)) if stablecklist[i] is True]
RotMatnozeroID = [i for i in range(len(stablecklist)) if stablecklist[i] is True]
comlist = [comlistall[i] for i in range(len(stablecklist)) if stablecklist[i] is True]
object_fixture = object.copy()
object_fixture_pos = fixture_start_pos + np.array([0,0,0.030])
object_fixture_rotmat = np.eye(3)
object_fixture_homomat = rm.homomat_from_posrot(object_fixture_pos, object_fixture_rotmat).dot(da.pdmat4_to_npmat4(RotMatnozero[1]))
object_fixture.set_homomat(object_fixture_homomat)
# object_fixture.attach_to(base)
# object_goal = object_fixture
object_start = object.copy()
object_start_pos = np.array([0.900, -.350, 0.800])
object_start_rotmat = rm.rotmat_from_axangle((1,0,0), np.radians(-90)).dot(rm.rotmat_from_axangle((0,0,1),np.radians(180)))
object_start_homomat = rm.homomat_from_posrot(object_start_pos, object_start_rotmat)
object_start.set_pos(object_start_pos)
object_start.set_rotmat(object_start_rotmat)
# object_start.attach_to(base)
grasp_info_list = gpa.load_pickle_file('test_long', './', 'rtqhe.pickle')
