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)
obscmlist = env.getstationaryobslist() for obscm in obscmlist: obscm.showcn() objcm = env.loadobj("bunnysim.stl") objcm.setColor(.2, .5, 0, 1) objcm.setPos(400, -200, 100) objcm.reparentTo(base.render) objcm.showcn() objpos = np.array([400, -300, 100]) objrot = rm.rodrigues([0, 1, 0], 45) objcm2 = env.loadobj("housing.stl") objcm2.setColor(1, .5, 0, 1) env.addchangableobs(base.render, objcm2, objpos, objrot) objcm3 = cm.CollisionModel(trimesh.primitives.Box(box_center=[500,100,100], box_extents=[200,50,100])) objcm3.setColor(1,0,0,1) objcm3.reparentTo(base.render) objcm3.showcn() hndfa = yumiintegrated.YumiIntegratedFactory() rgthnd = hndfa.genHand() lfthnd = hndfa.genHand() rbt = robot.YumiRobot(rgthnd=rgthnd, lfthnd=lfthnd) rbtmg = robotmesh.YumiMesh() rbtmnp = rbtmg.genmnp(rbt, toggleendcoord=True) rbtmnp.reparentTo(base.render) base.run()
_this_dir, _ = os.path.split(__file__) _smalltubepath = os.path.join(_this_dir, "objects", "tubesmall.stl") _largetubepath = os.path.join(_this_dir, "objects", "tubelarge.stl") _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]])