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)
objlt.setColor(.3, .3, .3, .9)
objtsd = env.loadobj(_tubestand)
objtsd.setColor(0, .5, .7, 1.9)
objtsdpos = [300, 0, 0]
objtsd.setPos(objtsdpos[0], objtsdpos[1], objtsdpos[2])
objtsd.reparentTo(base.render)
hndfa = yi.YumiIntegratedFactory()
rgthnd = hndfa.genHand()
lfthnd = hndfa.genHand()
robot = yumi.YumiRobot(rgthnd, lfthnd)
robot.opengripper(armname="rgt")
robot.close_gripper(armname="lft")
robotball = yumiball.YumiBall()
robotmesh = yumimesh.YumiMesh()
robotnp = robotmesh.genmnp(robot)
robotnp.reparentTo(base.render)
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]])
def getPos(i, j, objtsdpos):
x = 300 + (i - 2) * 19
y = 9 + (j - 5) * 18
z = objtsdpos[2] + 2
return (x, y, z)