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)
_tubestand = os.path.join(_this_dir, "objects", "tubestand.stl") objst = env.loadobj(_smalltubepath) objst.setColor(.7, .7, .7, .9) objlt = env.loadobj(_largetubepath) 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