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)
from motion import smoother as sm
from motion import checker as ctcb
from motion import collisioncheckerball as cdck
from robot_sim.robots.dualarm.yumi import yumi
from robot_sim.robots.dualarm.yumi import yumimesh, yumiball
from pandaplotutils import pandactrl
import manipulation.grip.yumiintegrated.yumiintegrated as yi
import numpy as np
import environment.suitayuminotop as yumisetting
import os
import copy
import tubepuzzle as tp
base = pandactrl.World(camp=[2700, -2000, 2000], lookatpos=[0, 0, 0])
env = yumisetting.Env()
env.reparentTo(base.render)
obscmlist = env.getstationaryobslist()
for obscm in obscmlist:
obscm.showcn()
_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)