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')