def __init__(self, startworld=True, autorotate=False): """ helper function to simplify everything :return: """ self.env = yumisetting.Env() self.obscmlist = self.env.getstationaryobslist() self.rgthndfa = yi.YumiIntegratedFactory() self.lfthndfa = self.rgthndfa self.rgthnd = self.rgthndfa.genHand() self.lfthnd = self.lfthndfa.genHand() self.rbt = yumi.YumiRobot(self.rgthnd, self.lfthnd) self.rbtball = yumiball.YumiBall() self.rbtmesh = yumimesh.YumiMesh() self.pcdchecker = cdck.CollisionCheckerBall(self.rbtball) self.bcdchecker = bch.MCMchecker(toggledebug=False) self.ctcallback = ctcb.CtCallback(self.rbt, self.pcdchecker, armname="rgt") self.smoother = sm.Smoother() self.root = os.path.abspath(os.path.dirname(__file__)) self.p3dh = p3dh self.cm = cm self.np = np self.rm = rm if startworld: self.base = self.startworld(autorotate=autorotate)
for i in range(5): for j in range(10): if elearray[i][j] == 1: objsttemp = copy.deepcopy(objst) pos = getPos(i, j, objtsdpos) objsttemp.setPos(pos[0], pos[1], pos[2]) objsttemp.reparentTo(base.render) if elearray[i][j] == 2: objsttemp = copy.deepcopy(objlt) pos = getPos(i, j, objtsdpos) objsttemp.setPos(pos[0], pos[1], pos[2]) objsttemp.reparentTo(base.render) base.run() armname = 'rgt' cdchecker = cdck.CollisionCheckerBall(robotball) ctcallback = ctcb.CtCallback(base, robot, cdchecker, armname=armname) smoother = sm.Smoother() # state = np.array([[1, 0, 0, 0, 0, 0, 1, 0, 0, 0], # [0, 0, 0, 0, 0, 0, 0, 2, 0, 2], # [0, 0, 0, 0, 0, 0, 0, 0, 2, 0], # [1, 0, 0, 0, 0, 0, 0, 0, 2, 2], # [1, 0, 0, 0, 0, 0, 0, 2, 0, 2]]) elearray = np.array([[0, 0, 0, 0, 0, 0, 0, 0, 0, 0], [0, 0, 0, 2, 2, 2, 2, 0, 0, 0], [0, 0, 2, 1, 1, 1, 0, 0, 0, 0], [0, 0, 2, 1, 2, 2, 0, 0, 0, 0], [0, 0, 0, 0, 2, 0, 0, 0, 0, 0]]) tpobj = tp.TubePuzzle(elearray) path = tpobj.atarSearch()