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