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