def __init__(self, directory=None):
"""
:param directory:
"""
srcpcdfilename = "vacuumhead_templatepcd.pkl"
srcmeshfilename = "vacuumhead.stl"
if directory is None:
self.bgdepth = pickle.load(
open("./databackground/bgdepth.pkl", "rb"))
self.bgpcd = pickle.load(open("./databackground/bgpcd.pkl", "rb"))
self.sensorhomomat = pickle.load(
open("./datacalibration/calibmat.pkl", "rb"))
self.srcpcdnp = pickle.load(
open("./dataobjtemplate/" + srcpcdfilename,
"rb")) # tstpcd, tube stand template
self.srccm = cm.CollisionModel("./objects/" + srcmeshfilename)
else:
self.bgdepth = pickle.load(
open(directory + "/databackground/bgdepth.pkl", "rb"))
self.bgpcd = pickle.load(
open(directory + "/databackground/bgpcd.pkl", "rb"))
self.sensorhomomat = pickle.load(
open(directory + "/datacalibration/calibmat.pkl", "rb"))
self.srcpcdnp = pickle.load(
open(directory + "/dataobjtemplate/" + srcpcdfilename,
"rb")) # tstpcd, tube stand template
self.srccm = cm.CollisionModel(directory + "/objects/" +
srcmeshfilename)
# for compatibility with locatorfixed
self.objhomomat = None
def gentubes(self, homomat, elearray=None, type="dumbbell"):
"""
:param elearray:
:param tubestand_homomat:
:param eleconfidencearray: None by default
:param alpha: only works when eleconfidencearray is None, it renders the array transparently
:return:
author: weiwei
date: 20191229osaka
"""
if elearray is None:
elearray = self.pattern
tubecmlist = []
for i in range(elearray.shape[0]):
for j in range(elearray.shape[1]):
if elearray[i,j] == 1:
if type is "dumbbell":
tubecm = cm.CollisionModel(p3dh.gendumbbell(spos=np.array([0,0,0]), epos = np.array([0, 0, 100])))
tubecm.setColor(.5,0,0,1)
elif type is "cad":
tubecm = self.tubebigcm
elif elearray[i,j] == 2:
if type is "dumbbell":
tubecm = cm.CollisionModel(p3dh.gendumbbell(spos=np.array([0,0,0]), epos = np.array([0, 0, 80])))
tubecm.setColor(0,.5,0,1)
elif type is "cad":
tubecm = self.tubesmall1cm
elif elearray[i,j] == 3:
if type is "dumbbell":
tubecm = cm.CollisionModel(p3dh.gendumbbell(spos=np.array([0,0,0]), epos = np.array([0, 0, 70])))
tubecm.setColor(0,0,.5,1)
elif type is "cad":
tubecm = self.tubesmall2cm
elif elearray[i,j] == -1:
tubecm = cm.CollisionModel(p3dh.gendumbbell(spos=np.array([0,0,0]), epos = np.array([0, 0, 70])))
tubecm.setColor(1,1,1,1)
elif elearray[i,j] == -2:
tubecm = cm.CollisionModel(p3dh.gendumbbell(spos=np.array([0,0,0]), epos = np.array([0, 0, 70])))
tubecm.setColor(0,0,0,1)
else:
continue
newtubecm = copy.deepcopy(tubecm)
tubemat = copy.deepcopy(homomat)
tubepos_normalized = np.array([self.tubeholecenters[i,j][0], self.tubeholecenters[i,j][1], 5])
tubepos = rm.homotransformpoint(homomat, tubepos_normalized)
tubemat[:3, 3] = tubepos
newtubecm.set_homomat(tubemat)
tubecmlist.append(newtubecm)
return tubecmlist
def __init__(self, betransparent=False):
"""
load obstacles model
separated by category
:param base:
author: weiwei
date: 20181205
"""
self.__this_dir, _ = os.path.split(__file__)
# table
self.__tablepath = os.path.join(self.__this_dir, "obstacles", "yumi_tablenotop.stl")
self.__tablecm = cm.CollisionModel(self.__tablepath, betransparent)
self.__tablecm.setColor(.55, .55, .5, 1.0)
self.__tablecm.setColor(.45, .45, .35, 1.0)
# housing
## housing pillar
## TODO these models could be replaced by trimesh.primitives
self.__beam2100path = os.path.join(self.__this_dir, "obstacles", "yumi_column60602100.stl")
self.__beam540path = os.path.join(self.__this_dir, "obstacles", "yumi_column6060540.stl")
self.__pillarrgtcm = cm.CollisionModel(self.__beam2100path)
self.__pillarrgtcm.setColor(.5, .5, .55, 1.0)
self.__pillarrgtcm.setPos(-327.0, -240.0, -1015.0)
self.__pillarlftcm = copy.deepcopy(self.__pillarrgtcm)
self.__pillarlftcm.setColor(.5, .5, .55, 1.0)
self.__pillarlftcm.setPos(-327.0, 240.0, -1015.0)
## housing rgt
self.__rowbackcm = cm.CollisionModel(self.__beam540path)
self.__rowbackcm.setColor(.5, .5, .55, 1.0)
self.__rowbackcm.setPos(-327.0, 0.0, 1085.0)
self.__rowrgtcm = copy.deepcopy(self.__rowbackcm)
self.__rowrgtcm.setColor(.5, .5, .55, 1.0)
homomat = self.__rowrgtcm.gethomomat()
homomat[:3,:3] = rm.rodrigues([0,0,1],90)
self.__rowrgtcm.sethomomat(homomat)
self.__rowrgtcm.setPos(-27.0, -240.0, 1085.0)
self.__rowlftcm = copy.deepcopy(self.__rowbackcm)
self.__rowlftcm.setColor(.5, .5, .55, 1.0)
homomat = self.__rowlftcm.gethomomat()
homomat[:3,:3] = rm.rodrigues([0,0,1],90)
self.__rowlftcm.sethomomat(homomat)
self.__rowlftcm.setPos(-27.0, 240.0, 1085.0)
self.__rowfrontcm = copy.deepcopy(self.__rowbackcm)
self.__rowfrontcm.setColor(.5, .5, .55, 1.0)
self.__rowfrontcm.setPos(273.0, -0.0, 1085.0)
self.__battached = False
self.__changableobslist = []
def gentubeandstandboxcm(self,
homomat,
wrapheight=120,
rgba=np.array([.5, .5, .5, .3])):
"""
gen a solid box to wrap both a stand and the tubes in it
:param homomat:
:return:
author: weiwei
date: 20191229osaka
"""
homomat_copy = copy.deepcopy(homomat)
homomat_copy[:3,
3] = homomat_copy[:3,
3] + homomat_copy[:3,
2] * wrapheight / 2
tubeandstandboxcm = cm.CollisionModel(
p3dh.genbox(
np.array([self.tubestandsize[0], self.tubestandsize[1], 120]),
homomat_copy))
tubeandstandboxcm.setColor(rgba[0], rgba[1], rgba[2], rgba[3])
return tubeandstandboxcm
def __init__(self):
this_dir, this_filename = os.path.split(__file__)
self.__name = "barrett"
# open and close is defined for antipodal grip (thumb vs index+middle)
self.__jawwidthopen = 100
self.__jawwidthclose = 0
self.__base = cm.CollisionModel(objinit=this_dir + "/stl/palm.stl",
betransparency=True)
self.__fingerprox = cm.CollisionModel(objinit=this_dir +
"/stl/finger_prox.stl",
betransparency=True)
self.__fingermed = cm.CollisionModel(objinit=this_dir +
"/stl/finger_med.stl",
betransparency=True)
self.__fingerdist = cm.CollisionModel(objinit=this_dir +
"/stl/finger_dist.stl",
betransparency=True)
def __init__(self, elearray=np.zeros((5,10)), root=".."):
self.tubestandcm = cm.CollisionModel(root+"/objects/tubestand_light.stl")
self.tubestandcm.setColor(0,.5,.7,1)
self.tubebigcm = cm.CollisionModel(root+"/objects/tubebig_capped.stl")
self.tubebigcm.setColor(.57,0,0,.7)
self.tubesmall1cm = cm.CollisionModel(root+"/objects/tubena.stl")
# self.tubesmall1cm.setColor(0,.57,0,.7)
self.tubesmall1cm.setColor(0,0,.57,.7)
self.tubesmall2cm = cm.CollisionModel(root+"/objects/tubeblue.stl")
self.tubesmall2cm.setColor(0,.57,0,.7)
self.pattern = elearray
tubeholecenters = []
for x in [-38,-19,0,19,38]:
tubeholecenters.append([])
for y in [-81, -63, -45, -27, -9, 9, 27, 45, 63, 81]:
tubeholecenters[-1].append([x,y])
self.tubeholecenters = np.array(tubeholecenters)
self.tubeholesize = np.array([15, 16])
def __init__(self,
directory=None,
homomatfilename_start="rightfixture_light_homomat1",
homomatfilename_goal="rightfixture_light_homomat2"):
self.__directory = directory
if directory is None:
self.bgdepth = pickle.load(
open("./databackground/bgdepth.pkl", "rb"))
self.bgpcd = pickle.load(open("./databackground/bgpcd.pkl", "rb"))
self.sensorhomomat = pickle.load(
open("./datacalibration/calibmat.pkl", "rb"))
self.tubestandcm = cm.CollisionModel(
"./objects/tubestand_light.stl")
self.tubebigcm = cm.CollisionModel("./objects/tubebig_capped.stl",
type="cylinder",
expand_radius=2)
self.tubesmallcm = cm.CollisionModel(
"./objects/tubesmall_capped.stl",
type="cylinder",
expand_radius=2)
self.tubestandhomomat_start = pickle.load(
open("./datafixture/" + homomatfilename_start + ".pkl", "rb"))
self.tubestandhomomat_goal = pickle.load(
open("./datafixture/" + homomatfilename_goal + ".pkl", "rb"))
else:
self.bgdepth = pickle.load(
open(directory + "/databackground/bgdepth.pkl", "rb"))
self.bgpcd = pickle.load(
open(directory + "/databackground/bgpcd.pkl", "rb"))
self.sensorhomomat = pickle.load(
open(directory + "/datacalibration/calibmat.pkl", "rb"))
self.tubestandcm = cm.CollisionModel(
directory + "/objects/tubestand_light.stl")
self.tubebigcm = cm.CollisionModel(directory +
"/objects/tubebig_capped.stl",
type="cylinder",
expand_radius=2)
self.tubesmallcm = cm.CollisionModel(
directory + "/objects/tubesmall_capped.stl",
type="cylinder",
expand_radius=2)
self.tubestandhomomat_start = pickle.load(
open("./datafixture/" + homomatfilename_start + ".pkl", "rb"))
self.tubestandhomomat_goal = pickle.load(
open("./datafixture/" + homomatfilename_goal + ".pkl", "rb"))
# down x, right y
tubeholecenters = []
for x in [-36, -18, 0, 18, 36]:
tubeholecenters.append([])
for y in [
-83.25, -64.75, -46.25, -27.75, -9.25, 9.25, 27.75, 46.25,
64.75, 83.25
]:
tubeholecenters[-1].append([x, y])
self.tubeholecenters = np.array(tubeholecenters)
self.tubeholesize = np.array([17, 16.5])
self.tubestandsize = np.array([97, 191])
# initialize the registered tubes, a dictionary with the template of each tube type in a list (multiple values allowed)
self.registeredtubetemps = {1: [], 2: []}
def __init__(self, directory=None, standtype = "light"):
"""
standtype could be normal, light, ...
:param directory:
:param standtype:
"""
if standtype is "normal":
tsfilename = "tubestand.stl"
tspcdfilename = "tubestandtemplatepcd.pkl"
# down x, right y
tubeholecenters = []
for x in [-38,-19,0,19,38]:
tubeholecenters.append([])
for y in [-81, -63, -45, -27, -9, 9, 27, 45, 63, 81]:
tubeholecenters[-1].append([x,y])
self.tubeholecenters = np.array(tubeholecenters)
self.tubeholesize = np.array([15, 16])
self.tubestandsize = np.array([96, 192])
elif standtype is "light":
tsfilename = "tubestand_light.stl"
tspcdfilename = "tubestand_light_templatepcd.pkl"
tubeholecenters = []
for x in [-36, -18, 0, 18, 36]:
tubeholecenters.append([])
for y in [-83.25, -64.75, -46.25, -27.75, -9.25, 9.25, 27.75, 46.25, 64.75, 83.25]:
tubeholecenters[-1].append([x, y])
self.tubeholecenters = np.array(tubeholecenters)
self.tubeholesize = np.array([17, 16.5])
self.tubestandsize = np.array([97, 191])
self.__directory = directory
if directory is None:
self.bgdepth = pickle.load(open("./databackground/bgdepth.pkl", "rb"))
self.bgpcd = pickle.load(open("./databackground/bgpcd.pkl", "rb"))
self.sensorhomomat = pickle.load(open("./datacalibration/calibmat.pkl", "rb"))
self.tstpcdnp = pickle.load(open("./dataobjtemplate/"+tspcdfilename, "rb"))# tstpcd, tube stand template
self.tubestandcm = cm.CollisionModel("./objects/"+tsfilename)
self.tubebigcm = cm.CollisionModel("./objects/tubebig_capped.stl", type="cylinder", expand_radius=0)
self.tubesmallcm = cm.CollisionModel("./objects/tubesmall_capped.stl", type="cylinder", expand_radius=0)
else:
self.bgdepth = pickle.load(open(directory+"/databackground/bgdepth.pkl", "rb"))
self.bgpcd = pickle.load(open(directory+"/databackground/bgpcd.pkl", "rb"))
self.sensorhomomat = pickle.load(open(directory+"/datacalibration/calibmat.pkl", "rb"))
self.tstpcdnp = pickle.load(open(directory+"/dataobjtemplate/"+tspcdfilename, "rb"))# tstpcd, tube stand template
self.tubestandcm = cm.CollisionModel(directory+"/objects/"+tsfilename)
self.tubebigcm = cm.CollisionModel(directory +"/objects/tubebig_capped.stl", type="cylinder", expand_radius=0)
self.tubesmallcm = cm.CollisionModel(directory +"/objects/tubesmall_capped.stl", type="cylinder", expand_radius=0)
# for compatibility with locatorfixed
self.tubestandhomomat = None
def findtubes(self, tubestand_homomat, tgtpcdnp, toggledebug=False):
"""
:param tgtpcdnp:
:return:
author: weiwei
date: 20200317
"""
elearray = np.zeros((5, 10))
eleconfidencearray = np.zeros((5, 10))
tgtpcdnp = o3dh.remove_outlier(tgtpcdnp,
downsampling_voxelsize=None,
nb_points=90,
radius=5)
# transform back to the local frame of the tubestand
tgtpcdnp_normalized = rm.homotransformpointarray(
rm.homoinverse(tubestand_homomat), tgtpcdnp)
if toggledebug:
cm.CollisionModel(tgtpcdnp_normalized).reparentTo(base.render)
tscm2 = copy.deepcopy(self.tubestandcm)
tscm2.reparentTo(base.render)
for i in range(5):
for j in range(10):
holepos = self.tubeholecenters[i][j]
tmppcd = self._crop_pcd_overahole(tgtpcdnp_normalized,
holepos[0], holepos[1])
if len(tmppcd) > 50:
if toggledebug:
print(
"------more than 50 raw points, start a new test------"
)
tmppcdover100 = tmppcd[tmppcd[:, 2] > 100]
tmppcdbelow90 = tmppcd[tmppcd[:, 2] < 90]
tmppcdlist = [tmppcdover100, tmppcdbelow90]
if toggledebug:
print("rotate around...")
rejflaglist = [False, False]
allminstdlist = [[], []]
newtmppcdlist = [None, None]
minstdlist = [None, None]
for k in range(2):
if toggledebug:
print("checking over 100 and below 90, now: ", j)
if len(tmppcdlist[k]) < 10:
rejflaglist[k] = True
continue
for angle in np.linspace(0, 180, 10):
tmphomomat = np.eye(4)
tmphomomat[:3, :3] = rm.rodrigues(
tubestand_homomat[:3, 2], angle)
newtmppcdlist[k] = rm.homotransformpointarray(
tmphomomat, tmppcdlist[k])
minstdlist[k] = np.min(
np.std(newtmppcdlist[k][:, :2], axis=0))
if toggledebug:
print(minstdlist[k])
allminstdlist[k].append(minstdlist[k])
if minstdlist[k] < 1.5:
rejflaglist[k] = True
if toggledebug:
print("rotate round done")
print("minstd ",
np.min(np.asarray(allminstdlist[k])))
if all(item for item in rejflaglist):
continue
elif all(not item for item in rejflaglist):
print("CANNOT tell if the tube is big or small")
raise ValueError()
else:
tmppcd = tmppcdbelow90 if rejflaglist[
0] else tmppcdover100
candidatetype = 2 if rejflaglist[0] else 1
tmpangles = np.arctan2(tmppcd[:, 1], tmppcd[:, 0])
tmpangles[
tmpangles < 0] = 360 + tmpangles[tmpangles < 0]
if toggledebug:
print(np.std(tmpangles))
print("ACCEPTED! ID: ", i, j)
elearray[i][j] = candidatetype
eleconfidencearray[i][j] = 1
if toggledebug:
# normalized
objnp = p3dh.genpointcloudnodepath(tmppcd,
pntsize=5)
rgb = np.random.rand(3)
objnp.setColor(rgb[0], rgb[1], rgb[2], 1)
objnp.reparentTo(base.render)
stick = p3dh.gendumbbell(
spos=np.array([holepos[0], holepos[1], 10]),
epos=np.array([holepos[0], holepos[1], 60]))
stick.setColor(rgb[0], rgb[1], rgb[2], 1)
stick.reparentTo(base.render)
# original
tmppcd_tr = rm.homotransformpointarray(
tubestand_homomat, tmppcd)
objnp_tr = p3dh.genpointcloudnodepath(tmppcd_tr,
pntsize=5)
objnp_tr.setColor(rgb[0], rgb[1], rgb[2], 1)
objnp_tr.reparentTo(base.render)
spos_tr = rm.homotransformpoint(
tubestand_homomat,
np.array([holepos[0], holepos[1], 0]))
stick_tr = p3dh.gendumbbell(
spos=np.array([spos_tr[0], spos_tr[1], 10]),
epos=np.array([spos_tr[0], spos_tr[1], 60]))
stick_tr.setColor(rgb[0], rgb[1], rgb[2], 1)
stick_tr.reparentTo(base.render)
# box normalized
center, bounds = rm.get_aabb(tmppcd)
boxextent = np.array([
bounds[0, 1] - bounds[0, 0],
bounds[1, 1] - bounds[1, 0],
bounds[2, 1] - bounds[2, 0]
])
boxhomomat = np.eye(4)
boxhomomat[:3, 3] = center
box = p3dh.genbox(
extent=boxextent,
homomat=boxhomomat,
rgba=np.array([rgb[0], rgb[1], rgb[2], .3]))
box.reparentTo(base.render)
# box original
center_r = rm.homotransformpoint(
tubestand_homomat, center)
boxhomomat_tr = copy.deepcopy(tubestand_homomat)
boxhomomat_tr[:3, 3] = center_r
box_tr = p3dh.genbox(
extent=boxextent,
homomat=boxhomomat_tr,
rgba=np.array([rgb[0], rgb[1], rgb[2], .3]))
box_tr.reparentTo(base.render)
if toggledebug:
print("------the new test is done------")
return elearray, eleconfidencearray
def gencm(self, startposobj, startrotobj, startposrbt, startrotrbt, isrotated = False, iscornered = False, isdrooped = False, isinclined = False):
"""
make the collision model of the object, set its pose according to the robot end effector pose
:param startposobj: object position
:param startrotobj: object orientation
:param startposrbt: robot position
:param startrotrbt: robot orientation
:param isrotated: is the object to be rotated 90 degrees about the vertical axis of the world frame
:param iscornered:is the object grasped from its corner
:param isdrooped: is the object drooped due to orientation
:param isinclined: is the object at an inclined pose
:return: collision model of the object with set pose
"""
#load the object and set its pose related to the robot pose
startposobj = startposobj
startrotobj = startrotobj
startpos = startposrbt
startrot = startrotrbt
#create the collision model
self.objcm = cm.CollisionModel(objinit=self.objpath)
# if required to flip the object and the robot pose
# startrot = rot_transform(startrot, 180)
# set the object pose in hand
# easy to be automated for planning
if isrotated:
self.torque = 9.81 * self.m * self.width / 2 / 1000 # Nm
print("rotated torque is set!")
print("self.torque is:", self.torque)
rotz = rm.rodrigues([0, 0, 1], -90)
tmp_rot = np.dot(rotz, startrotobj)
startrotobj = np.dot(tmp_rot, startrotobj)
startposobj[0] -= (self.length/2-self.width/2)
if isinclined:
incline_angle = 10
tmp_incline_rot = rm.rodrigues(startrot[:,2],incline_angle)
startrot = np.dot(tmp_incline_rot,startrot)
startrotobj = np.dot(tmp_incline_rot, startrotobj)
objcmcopy = copy.deepcopy(self.objcm)
objcmcopy.setMat(pg.npToMat4(startrotobj,startposobj))
##for visualization of the limit
# for incangle in range(0,20,10):
# print (incangle)
# tmp_incline_rot = rm.rodrigues(startrot[:, 2], incangle)
# startrotobjinc = np.dot(tmp_incline_rot, startrotobj)
# objcmcopy = copy.deepcopy(objcm)
# startrotrbt = np.dot(tmp_incline_rot, startrot)
# rbtangles = rbt.numik(startpos, startrotrbt, "lft")
# rbt.movearmfk(rbtangles, "lft")
# rbtmg.genmnp(rbt, jawwidthlft=objheight,toggleendcoord=False).reparentTo(base.render)
# objcmcopy.setMat(pg.npToMat4(startrotobjinc, startposobj))
# if incangle == 10:
# objcmcopy.setColor(.8, .0, .0, .5)
# objcmcopy.reparentTo(base.render)
# base.run()
if iscornered:
corner_angle = 45
rotz = rm.rodrigues([0,0,1],corner_angle)
tmp_rot = np.dot(rotz,startrotobj)
startrotobj = np.dot(tmp_rot,startrotobj)
grasppnt = [-self.length/2, self.width/2,0,1]
obj_w_t = np.eye(4)
obj_w_t[:3,:3] = startrotobj
obj_w_t[:3,3] = startposobj
print("obj_w_t",obj_w_t)
grasppnt_w = np.dot(obj_w_t,grasppnt)
print(grasppnt_w)
print(startpos)
pos_diff = [a-b for a,b in zip(grasppnt_w,startpos)]
print ("Position difference is: ", pos_diff)
print("Position difference is: ", [grasppnt_w[0],grasppnt_w[1],grasppnt_w[2]]-startpos)
startposobj-=pos_diff
if isdrooped == True:
droop_angle = -20
tmp_rot_ee_x = rm.rodrigues(startrot[:,0],droop_angle)
startrotobj = np.dot(tmp_rot_ee_x,startrotobj)
# grasppnt = [-objlength/2*np.cos(droop_angle), objwidth/2*np.sin(droop_angle), 0, 1]
# obj_w_t = np.eye(4)
# obj_w_t[:3, :3] = startrotobj
# obj_w_t[:3, 3] = startposobj
# grasppnt_w = np.dot(obj_w_t, grasppnt)
# print(grasppnt_w)
# print(startpos)
# pos_diff = [a - b for a, b in zip(grasppnt_w, startpos)]
# print("Position difference is: ", pos_diff)
# print("Position difference is: ", [grasppnt_w[0], grasppnt_w[1], grasppnt_w[2]] - startpos)
# startposobj += pos_diff
self.objcm.setMat(pg.npToMat4(startrotobj,startposobj))
return self.objcm
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()
def __init__(self, *args, **kwargs):
"""
load the robotiq85 model, set jaw_width
:param args:
:param kwargs:
'jaw_width' 0-85
'ftsensoroffset' the offset for forcesensor
'toggleframes' True, False
author: weiwei
date: 20160627, 20190518, 20190824osaka, 20200321osaka
"""
if 'jawwidthopen' in kwargs:
self.__jawwidthopen = kwargs['jawwidthopen']
else:
self.__jawwidthopen = 100
if 'jawwidthclose' in kwargs:
self.__jawwidthclose = kwargs['jawwidthclose']
else:
self.__jawwidthclose = 0
if 'jaw_width' in kwargs:
self.__jawwidth = kwargs['jaw_width']
else:
self.__jawwidth = self.__jawwidthopen
if 'ftsensoroffset' in kwargs:
self.__ftsensoroffset = kwargs['ftsensoroffset']
if 'toggleframes' in kwargs:
self.__toggleframes = kwargs['toggleframes']
if 'hndbase' in kwargs:
self.__hndbase = cp.deepcopy(kwargs['hndbase'])
# for copy
self.__hndbase_bk = cp.deepcopy(kwargs['hndbase'])
if 'hndfingerprox' in kwargs:
self.__finger1_prox = cp.deepcopy(kwargs['hndfingerprox'])
self.__finger2_prox = cp.deepcopy(kwargs['hndfingerprox'])
self.__finger3_prox = cp.deepcopy(kwargs['hndfingerprox'])
# for copy
self.__hndfingerprox_bk = cp.deepcopy(kwargs['hndfingerprox'])
if 'hndfingermed' in kwargs:
self.__finger1_med = cp.deepcopy(kwargs['hndfingermed'])
self.__finger2_med = cp.deepcopy(kwargs['hndfingermed'])
self.__finger3_med = cp.deepcopy(kwargs['hndfingermed'])
# for copy
self.__hndfingermed_bk = cp.deepcopy(kwargs['hndfingermed'])
if 'hndfingerdist' in kwargs:
self.__finger1_dist = cp.deepcopy(kwargs['hndfingerdist'])
self.__finger2_dist = cp.deepcopy(kwargs['hndfingerdist'])
self.__finger3_dist = cp.deepcopy(kwargs['hndfingerdist'])
# for copy
self.__hndfingerdist_bk = cp.deepcopy(kwargs['hndfingerdist'])
self.__name = "barrett-bh8-282"
self.__hndnp = NodePath(self.__name)
self.__angle_open = self._compute_jawangle(self.__jawwidthopen)
self.__angle_close = self._compute_jawangle(self.__jawwidthclose)
print(self.__angle_open, self.__angle_close)
baselength = 79.5
fingerlength = 100
# eetippos/eetiprot
self.__eetip = np.array([
0.0, 0.0, baselength + fingerlength
]) + np.array([0.0, 0.0, self.__ftsensoroffset]) # max length 136
# base
# self.__hndbase.setrotmat(rm.rodrigues(np.array([0,0,1]), 180))
self.__hndbase.setpos(np.array([0.0, 0.0, self.__ftsensoroffset]))
# ftsensor
if self.__ftsensoroffset > 0:
self.__ftsensor = cm.CollisionModel(tp.Cylinder(
height=self.__ftsensoroffset, radius=30),
name="ftsensor")
self.__ftsensor.setPos(0, 0, -self.__ftsensoroffset / 2)
self.__ftsensor.reparentTo(self.__hndbase)
else:
self.__ftsensor = None
# 1,2 are the index and middle fingers, 3 is the thumb
self.__finger1_prox_pos = np.array([-25, 0, 41.5])
self.__finger1_prox_rot = rm.rotmat_from_euler(0, 0, -90)
self.__finger1_med_pos = np.array([50, 0, 33.9])
self.__finger1_med_rot = rm.rotmat_from_euler(90, 0, 0)
self.__finger1_dist_pos = np.array([69.94, 3, 0])
self.__finger1_dist_rot = rm.rotmat_from_euler(0, 0, -3)
self.__finger2_prox_pos = np.array([25, 0, 41.5])
self.__finger2_prox_rot = rm.rotmat_from_euler(0, 0, -90)
self.__finger2_med_pos = np.array([50, 0, 33.9])
self.__finger2_med_rot = rm.rotmat_from_euler(90, 0, 0)
self.__finger2_dist_pos = np.array([69.94, 3, 0])
self.__finger2_dist_rot = rm.rotmat_from_euler(0, 0, -3)
self.__finger3_med_pos = np.array([0, 50, 75.4])
self.__finger3_med_rot = rm.rotmat_from_euler(90, 0, 90)
self.__finger3_dist_pos = np.array([69.94, 3, 0])
self.__finger3_dist_rot = rm.rotmat_from_euler(0, 0, -3)
# controllable angles
self.__angle_main = 0.0
self.__angle_finger1 = 0.0
self.__angle_finger2 = 0.0
self.__angle_finger3 = 0.0
# angle ranges
self.__am_range = {"rngmin": 0, "rngmax": 180}
self.__af1_range = {"rngmin": 0, "rngmax": 140}
self.__af2_range = {"rngmin": 0, "rngmax": 140}
self.__af3_range = {"rngmin": 0, "rngmax": 140}
# update finger positions
# finger 1
finger1_prox_pos = self.__finger1_prox_pos
finger1_prox_rot = np.dot(
self.__finger1_prox_rot,
rm.rodrigues(np.array([0, 0, 1]), -self.__angle_main))
self.__finger1_prox.sethomomat(
rm.homobuild(finger1_prox_pos, finger1_prox_rot))
finger1_med_pos = finger1_prox_pos + np.dot(finger1_prox_rot,
self.__finger1_med_pos)
finger1_med_rot = np.dot(
finger1_prox_rot,
np.dot(self.__finger1_med_rot,
rm.rodrigues(np.array([0, 0, 1]), self.__angle_finger1)))
self.__finger1_med.sethomomat(
rm.homobuild(finger1_med_pos, finger1_med_rot))
finger1_dist_pos = finger1_med_pos + np.dot(finger1_med_rot,
self.__finger1_dist_pos)
finger1_dist_rot = np.dot(
finger1_med_rot,
np.dot(self.__finger1_dist_rot,
rm.rodrigues(np.array([0, 0, 1]),
self.__angle_finger1 / 3)))
self.__finger1_dist.sethomomat(
rm.homobuild(finger1_dist_pos, finger1_dist_rot))
self.__finger1_prox.reparentTo(self.__hndbase)
self.__finger1_med.reparentTo(self.__hndbase)
self.__finger1_dist.reparentTo(self.__hndbase)
# finger 2
finger2_prox_pos = self.__finger2_prox_pos
finger2_prox_rot = np.dot(
self.__finger2_prox_rot,
rm.rodrigues(np.array([0, 0, 1]), self.__angle_main))
self.__finger2_prox.sethomomat(
rm.homobuild(finger2_prox_pos, finger2_prox_rot))
finger2_med_pos = finger2_prox_pos + np.dot(finger2_prox_rot,
self.__finger2_med_pos)
finger2_med_rot = np.dot(
finger2_prox_rot,
np.dot(self.__finger2_med_rot,
rm.rodrigues(np.array([0, 0, 1]), self.__angle_finger2)))
self.__finger2_med.sethomomat(
rm.homobuild(finger2_med_pos, finger2_med_rot))
finger2_dist_pos = finger2_med_pos + np.dot(finger2_med_rot,
self.__finger2_dist_pos)
finger2_dist_rot = np.dot(
finger2_med_rot,
np.dot(self.__finger2_dist_rot,
rm.rodrigues(np.array([0, 0, 1]),
self.__angle_finger2 / 3)))
self.__finger2_dist.sethomomat(
rm.homobuild(finger2_dist_pos, finger2_dist_rot))
self.__finger2_prox.reparentTo(self.__hndbase)
self.__finger2_med.reparentTo(self.__hndbase)
self.__finger2_dist.reparentTo(self.__hndbase)
# finger 3
finger3_med_pos = self.__finger3_med_pos
finger3_med_rot = np.dot(
self.__finger3_med_rot,
rm.rodrigues(np.array([0, 0, 1]), self.__angle_finger3))
self.__finger3_med.sethomomat(
rm.homobuild(finger3_med_pos, finger3_med_rot))
finger3_dist_pos = finger3_med_pos + np.dot(finger3_med_rot,
self.__finger3_dist_pos)
finger3_dist_rot = np.dot(
finger3_med_rot,
np.dot(self.__finger3_dist_rot,
rm.rodrigues(np.array([0, 0, 1]),
self.__angle_finger3 / 3)))
self.__finger3_dist.sethomomat(
rm.homobuild(finger3_dist_pos, finger3_dist_rot))
#
self.__finger3_med.reparentTo(self.__hndbase)
self.__finger3_dist.reparentTo(self.__hndbase)
self.__hndbase.reparentTo(self.__hndnp)
# self.setjawwidth(self.__jawwidth)
self.setDefaultColor()
if self.__toggleframes:
if self.__ftsensor is not None:
self.__ftsensorframe = p3dh.genframe()
self.__ftsensorframe.reparentTo(self.__hndnp)
self.__hndframe = p3dh.genframe()
self.__hndframe.reparentTo(self.__hndnp)
self.__baseframe = p3dh.genframe()
self.__baseframe.reparentTo(self.__hndbase.objnp)
self.__finger1_prox_frame = p3dh.genframe()
self.__finger1_prox_frame.reparentTo(self.__finger1_prox.objnp)
self.__finger1_med_frame = p3dh.genframe()
self.__finger1_med_frame.reparentTo(self.__finger1_med.objnp)
self.__finger1_dist_frame = p3dh.genframe()
self.__finger1_dist_frame.reparentTo(self.__finger1_dist.objnp)
self.__finger2_prox_frame = p3dh.genframe()
self.__finger2_prox_frame.reparentTo(self.__finger2_prox.objnp)
self.__finger2_med_frame = p3dh.genframe()
self.__finger2_med_frame.reparentTo(self.__finger2_med.objnp)
self.__finger2_dist_frame = p3dh.genframe()
self.__finger2_dist_frame.reparentTo(self.__finger2_dist.objnp)
self.__finger3_med_frame = p3dh.genframe()
self.__finger3_med_frame.reparentTo(self.__finger3_med.objnp)
self.__finger3_dist_frame = p3dh.genframe()
self.__finger3_dist_frame.reparentTo(self.__finger3_dist.objnp)
def __init__(self, betransparent=False):
"""
load obstacles model
separated by category
:param base:
author: weiwei
date: 20181205
"""
self.__this_dir, _ = os.path.split(__file__)
# body
self.__bodypath = os.path.join(self.__this_dir, "obstacles",
"ur3body.stl")
self.__bodycm = cm.CollisionModel(self.__bodypath, betransparent)
self.__bodycm.setColor(.3, .3, .3, 1.0)
# table
self.__tablepath = os.path.join(self.__this_dir, "obstacles",
"ur3dtablefree.stl")
self.__tablecm = cm.CollisionModel(self.__tablepath, betransparent)
self.__tablecm.setColor(.55, .55, .5, 1.0)
self.__tablecm.setPos(50.0, 0.0, 0.0)
# housing
## housing pillar
## TODO these models could be replaced by trimesh.primitives
self.__beam1200path = os.path.join(self.__this_dir, "obstacles",
"ur3housing1200.stl")
self.__beam1500path = os.path.join(self.__this_dir, "obstacles",
"ur3housing1500.stl")
self.__beam2100path = os.path.join(self.__this_dir, "obstacles",
"ur3housing2100.stl")
self.__pillarfrontrgtcm = cm.CollisionModel(self.__beam2100path)
self.__pillarfrontrgtcm.setColor(.5, .5, .55, 1.0)
self.__pillarfrontrgtcm.setPos(860.0 + 50.0 + 30.0, -780.0, 0.0)
self.__pillarfrontlftcm = cm.CollisionModel(self.__beam2100path)
self.__pillarfrontlftcm.setColor(.5, .5, .55, 1.0)
self.__pillarfrontlftcm.setPos(860.0 + 50.0 + 30.0, 780.0, 0.0)
self.__pillarbackrgtcm = cm.CollisionModel(self.__beam2100path)
self.__pillarbackrgtcm.setColor(.5, .5, .55, 1.0)
self.__pillarbackrgtcm.setPos(860 + 50 - 1230, -780.0, 0.0)
self.__pillarbacklftcm = cm.CollisionModel(self.__beam2100path)
self.__pillarbacklftcm.setColor(.5, .5, .55, 1.0)
self.__pillarbacklftcm.setPos(860 + 50 - 1230, 780.0, 0.0)
## housing rgt
self.__rowrgtbottomcm = cm.CollisionModel(self.__beam1200path)
self.__rowrgtbottomcm.setColor(.5, .5, .55, 1.0)
self.__rowrgtbottomcm.setPos(860 + 50 - 600, -780.0, 75.0)
self.__rowrgtmiddlecm = cm.CollisionModel(self.__beam1200path)
self.__rowrgtmiddlecm.setColor(.5, .5, .55, 1.0)
self.__rowrgtmiddlecm.setPos(860 + 50 - 600, -780.0, 867.0)
self.__rowrgttopcm = cm.CollisionModel(self.__beam1200path)
self.__rowrgttopcm.setColor(.5, .5, .55, 1.0)
self.__rowrgttopcm.setPos(860 + 50 - 600, -780.0, 2100.0 - 60.0)
## housing lft
self.__rowlftbottomcm = cm.CollisionModel(self.__beam1200path)
self.__rowlftbottomcm.setColor(.5, .5, .55, 1.0)
self.__rowlftbottomcm.setPos(860 + 50 - 600, 780.0, 75.0)
self.__rowlfttopcm = cm.CollisionModel(self.__beam1200path)
self.__rowlfttopcm.setColor(.5, .5, .55, 1.0)
self.__rowlfttopcm.setPos(860 + 50 - 600, 780.0, 2100.0 - 60.0)
## housing front
self.__rowfrontbottomcm = cm.CollisionModel(self.__beam1500path)
self.__rowfrontbottomcm.setColor(.5, .5, .55, 1.0)
self.__rowfrontbottomcm.setPos(860 + 50 + 30, 0.0, 75.0)
self.__rowfrontmiddlecm = cm.CollisionModel(self.__beam1500path)
self.__rowfrontmiddlecm.setColor(.5, .5, .55, 1.0)
self.__rowfrontmiddlecm.setPos(860 + 50 + 30, 0.0, 867.0)
self.__rowfronttopcm = cm.CollisionModel(self.__beam1500path)
self.__rowfronttopcm.setColor(.5, .5, .55, 1.0)
self.__rowfronttopcm.setPos(860 + 50 + 30, 0.0, 2100.0 - 60.0)
## housing back
self.__rowbackbottomcm = cm.CollisionModel(self.__beam1500path)
self.__rowbackbottomcm.setColor(.5, .5, .55, 1.0)
self.__rowbackbottomcm.setPos(860 + 50 - 1230, 0.0,
2100.0 - 530.0 - 60.0)
self.__rowbackmiddlecm = cm.CollisionModel(self.__beam1500path)
self.__rowbackmiddlecm.setColor(.5, .5, .55, 1.0)
self.__rowbackmiddlecm.setPos(860 + 50 - 1230, 0.0,
2100.0 - 264.0 - 60.0)
self.__rowbacktopcm = cm.CollisionModel(self.__beam1500path)
self.__rowbacktopcm.setColor(.5, .5, .55, 1.0)
self.__rowbacktopcm.setPos(860 + 50 - 1230, 0.0, 2100.0 - 60.0)
self.__battached = False
self.__changableobslist = []
# objpath = "../objects/025mug1000.stl"
# objpath = "../objects/025mug3000.stl"
# objpath = "../objects/025mug3000_tsdf.stl"
# objpath = "../objects/035drill3000_tsdf.stl"
# objpath = "../objects/072toyplanedrill3000_tsdf.stl"
# objpath = "../objects/housing.stl"
# objpath = "../objects/ttube.stl"
# objpath = "./objects/bunny12356.stl"
# objstpath = "./objects/bunny17432.stl"
pathname = "./objects/"
objname = ["housing", "planewheel", "tool2", "ttube", "sandpart", "planelowerbody", "planerearstay", "bunnysim"]
type = ["160over", "160noover"]
for objn in objname:
objpath = pathname+objn+".stl"
objcm = cm.CollisionModel(objinit = objpath)
hndfa = hnd.Robotiq85Factory()
for i in range(2):
hand= hndfa.genHand()
if i == 0:
freegriptst = fg.Freegrip(objpath, hand, faceangle = .9, segangle = .9, refine1min=5, fpairparallel=-0.9, togglebcdcdebug=True, useoverlap=True)
if i == 1:
freegriptst = fg.Freegrip(objpath, hand, faceangle = .9, segangle = .9, refine1min=5, fpairparallel=-0.9, togglebcdcdebug=True, useoverlap=False)
# facetsizes = []
# for j, faces in enumerate(freegriptst.facets):
# rgba = [np.random.random(), np.random.random(), np.random.random(), 1]
# # compute facet normal
# facetnormal = np.sum(freegriptst.objtrimesh.face_normals[faces], axis=0)
# facetnormal = facetnormal / np.linalg.norm(facetnormal)
import utiltools.thirdparty.p3dhelper as p3dh
import utiltools.thirdparty.o3dhelper as o3dh
import pickle
import tro.tro_pickplaceplanner as ppp
if __name__ == '__main__':
yhx = robothelper.RobotHelperX(usereal=True)
yhx.movetox(yhx.robot_s.initrgtjnts, armname="rgt")
yhx.movetox(yhx.robot_s.initlftjnts, armname="lft")
yhx.closegripperx(armname="rgt")
# yhx.closegripperx(arm_name="lft")
# yhx.opengripperx(arm_name="rgt")
# yhx.opengripperx(arm_name="lft")
objcm = cm.CollisionModel("../objects/vacuumhead.stl")
# yhx = robothelper.RobotHelper(startworld=True)
armjnts0 = np.array(
[120.18, -41.71, -160.45, 7.16, 148.59, 95.69, -210.39])
obstaclecmlist = []
armname = "rgt"
rgtinitarmjnts = yhx.robot_s.getarmjnts(armname=armname).tolist()
primitivedirection_backward = np.array([0, 0, 1])
primitivedistance_backward = 150
rgtupmotion = yhx.genmovebackwardmotion(primitivedirection_backward,
primitivedistance_backward,
armjnts0, obstaclecmlist, armname)
rgtpickmotion = rgtupmotion[::-1]
rgtrrtgotobeforepick = yhx.planmotion(rgtinitarmjnts, rgtpickmotion[0],
obstaclecmlist, armname)
freesuctst = fs.Freesuc()
reconstructedtrimeshlist, nppcdlist = o3dh.reconstruct_surfaces_bp(
nppcd, radii=(10, 12))
# for i, tmpnppcd in enumerate(nppcdlist):
# p3dpcd = p3dh.genpointcloudnodepath(tmpnppcd, pntsize=1.57)
# p3dpcd.reparentTo(rhx.base.render)
# if i == 0:
# p3dpcd.setColor(.7,0,0,1)
# elif i == 1:
# p3dpcd.setColor(0,0,.7,1)
# elif i == 2:
# p3dpcd.setColor(0,.7,0,1)
# else:
# p3dpcd.setColor(1,1,1,1)
for i, reconstructedtrimesh in enumerate(reconstructedtrimeshlist):
reconstructedmeshobjcm = cm.CollisionModel(reconstructedtrimesh)
reconstructedmeshobjcm.reparentTo(rhx.base.render)
if i == 0:
reconstructedmeshobjcm.setColor(.7, 0, 0, 1)
elif i == 1:
reconstructedmeshobjcm.setColor(0, 0, .7, 1)
elif i == 2:
reconstructedmeshobjcm.setColor(0, .7, 0, 1)
else:
reconstructedmeshobjcm.setColor(1, 1, 1, 1)
freesuctst.plansuctions(effa=effa,
objinit=reconstructedmeshobjcm,
faceangle=.85,
segangle=.85,
mindist=10,
yifa = yi.YumiIntegratedFactory()
reconstructedtrimeshlist, nppcdlist = o3dh.reconstruct_surfaces_bp(
mergednppcd, mergednppcdnrmls, radii=(7, 10))
# for i, tmpnppcd in enumerate(nppcdlist):
# p3dpcd = p3dh.genpointcloudnodepath(tmpnppcd, pntsize=1.57)
# p3dpcd.reparentTo(rhx.base.render)
# if i == 0:
# p3dpcd.setColor(.7,0,0,1)
# elif i == 1:
# p3dpcd.setColor(0,0,.7,1)
# elif i == 2:
# p3dpcd.setColor(0,.7,0,1)
# else:
# p3dpcd.setColor(1,1,1,1)
for i, reconstructedtrimesh in enumerate(reconstructedtrimeshlist):
reconstructedmeshobjcm = cm.CollisionModel(reconstructedtrimesh)
# reconstructedmeshobjcm.reparentTo(rhx.base.render)
# if i == 1:
# reconstructedmeshobjcm.setColor(.7,0,0,1)
# elif i == 0:
# reconstructedmeshobjcm.setColor(0,0,.7,1)
# elif i == 2:
# reconstructedmeshobjcm.setColor(0,.7,0,1)
# else:
# reconstructedmeshobjcm.setColor(1,1,1,1)
freegriptst = fg.Freegrip(reconstructedmeshobjcm,
yifa.genHand(),
faceangle=.9,
segangle=.9,
refine1min=7,
hmnmg = humanmesh.NxtMesh()
#build the cabinet
cabinet = buildCabinet(base, isrendercoord=True)
cabinet.reparentTo(base.render)
cabinetpose = cabinet.getMat()
cabinetposenp4 = base.pg.mat4ToNp(cabinetpose)
__this_dir, _ = os.path.split(__file__)
largeboardpath = os.path.join(__this_dir, "objects", "largeboard.stl")
middleboardpath = os.path.join(__this_dir, "objects", "middleboard.stl")
smallboardpath = os.path.join(__this_dir, "objects", "smallboard.stl")
# the boards
largeboard0 = cm.CollisionModel(largeboardpath,
radius=3,
betransparency=True)
middleboard0 = cm.CollisionModel(middleboardpath,
radius=3,
betransparency=True)
smallboard0 = cm.CollisionModel(smallboardpath,
radius=3,
betransparency=True)
# the obstacle boards
largeboardobstacle0 = copy.deepcopy(largeboard0)
largecompundrot = np.dot(rm.rodrigues([0, 1, 0], -90),
rm.rodrigues([1, 0, 0], 90))
largeboardobstacle0.sethomomat(
rm.homobuild(
np.array([0, 195, 293.5]) + cabinetposenp4[:3, 3],
def __init__(self, betransparent=False):
"""
load obstacles model
separated by category
:param base:
author: weiwei
date: 20181205
"""
self.__this_dir, this_filename = os.path.split(__file__)
# body
self.__bodypath = os.path.join(self.__this_dir, "obstacles", "ur3body.stl")
self.__bodycm = cm.CollisionModel(self.__bodypath, betransparent)
self.__bodycm.setColor(.3,.3,.3,1.0)
# table
self.__tablepath = os.path.join(self.__this_dir, "obstacles", "ur3dtable.stl")
self.__tablecm = cm.CollisionModel(self.__tablepath, betransparent)
self.__tablecm.setColor(.6,.6,.6,1.0)
self.__tablecm.setPos(50.0, 0.0, 0.0)
# housing
## housing pillar
self.__beam1200path = os.path.join(self.__this_dir, "obstacles", "ur3housing1200.stl")
self.__beam1500path = os.path.join(self.__this_dir, "obstacles", "ur3housing1500.stl")
self.__beam2100path = os.path.join(self.__this_dir, "obstacles", "ur3housing2100.stl")
self.__pillarfrontrgtcm = cm.CollisionModel(self.__beam2100path)
self.__pillarfrontrgtcm.setColor(.6,.6,.6,1.0)
self.__pillarfrontrgtcm.setPos(860.0+50.0+30.0, -780.0, 0.0)
self.__pillarfrontlftcm = cm.CollisionModel(self.__beam2100path)
self.__pillarfrontlftcm.setColor(.6,.6,.6,1.0)
self.__pillarfrontlftcm.setPos(860.0+50.0+30.0, 780.0, 0.0)
self.__pillarbackrgtcm = cm.CollisionModel(self.__beam2100path)
self.__pillarbackrgtcm.setColor(.6,.6,.6,1.0)
self.__pillarbackrgtcm.setPos(860+50-1200, -780.0, 0.0)
self.__pillarbacklftcm = cm.CollisionModel(self.__beam2100path)
self.__pillarbacklftcm.setColor(.6,.6,.6,1.0)
self.__pillarbacklftcm.setPos(860+50-1200, 780.0, 0.0)
## housing rgt
self.__rowrgtbottomcm = cm.CollisionModel(self.__beam1200path)
self.__rowrgtbottomcm.setColor(.6,.6,.6,1.0)
self.__rowrgtbottomcm.setPos(860+50-600, -780.0, 75.0)
self.__rowrgtmiddlecm = cm.CollisionModel(self.__beam1200path)
self.__rowrgtmiddlecm.setColor(.6,.6,.6,1.0)
self.__rowrgtmiddlecm.setPos(860+50-600, -780.0, 867.0)
self.__rowrgttopcm = cm.CollisionModel(self.__beam1200path)
self.__rowrgttopcm.setColor(.6,.6,.6,1.0)
self.__rowrgttopcm.setPos(860+50-600, -780.0, 2100.0-60.0)
## housing lft
self.__rowlftbottomcm = cm.CollisionModel(self.__beam1200path)
self.__rowlftbottomcm.setColor(.6,.6,.6,1.0)
self.__rowlftbottomcm.setPos(860+50-600, 780.0, 75.0)
self.__rowlfttopcm = cm.CollisionModel(self.__beam1200path)
self.__rowlfttopcm.setColor(.6,.6,.6,1.0)
self.__rowlfttopcm.setPos(860+50-600, 780.0, 2100.0-60.0)
## housing front
self.__rowfrontbottomcm = cm.CollisionModel(self.__beam1500path)
self.__rowfrontbottomcm.setColor(.6,.6,.6,1.0)
self.__rowfrontbottomcm.setPos(860+50+30, 0.0, 75.0)
self.__rowfrontmiddlecm = cm.CollisionModel(self.__beam1500path)
self.__rowfrontmiddlecm.setColor(.6,.6,.6,1.0)
self.__rowfrontmiddlecm.setPos(860+50+30, 0.0, 867.0)
# self.__rowfronttopcm = cm.CollisionModel(self.__beam1500path)
# self.__rowfronttopcm.setColor(.6,.6,.6,1.0)
# self.__rowfronttopcm.setPos(860+50+30, 0.0, 2100.0-60.0)
## housing back
self.__rowbackbottomcm = cm.CollisionModel(self.__beam1500path)
self.__rowbackbottomcm.setColor(.6,.6,.6,1.0)
self.__rowbackbottomcm.setPos(860+50-1200, 0.0, 2100.0-530.0-60.0)
self.__rowbackmiddlecm = cm.CollisionModel(self.__beam1500path)
self.__rowbackmiddlecm.setColor(.6,.6,.6,1.0)
self.__rowbackmiddlecm.setPos(860+50-1200, 0.0, 2100.0-264.0-60.0)
self.__rowbacktopcm = cm.CollisionModel(self.__beam1500path)
self.__rowbacktopcm.setColor(.6,.6,.6,1.0)
self.__rowbacktopcm.setPos(860+50-1200, 0.0, 2100.0-60.0)
# assebmly frame
self.__assshelfpath = os.path.join(self.__this_dir, "obstacles", "ur3dassshelf.stl")
self.__asssupportpath = os.path.join(self.__this_dir, "obstacles", "asssupport.stl")
self.__assbasepath = os.path.join(self.__this_dir, "obstacles", "assbase.stl")
self.__assvertplanebigpath = os.path.join(self.__this_dir, "obstacles", "assvertplanebig.stl")
self.__assshelfcm = cm.CollisionModel(self.__assshelfpath)
self.__assshelfcm.setColor(.7,.7,.6,1.0)
self.__assshelfcm.setPos(50.0, 65.0,1000.0)
self.__asssupportcm = cm.CollisionModel(self.__asssupportpath)
self.__asssupportcm.setColor(.2,.2,.2,1.0)
self.__asssupportcm.setPos(110.0, 65.0,1190.0)
self.__assbasecm = cm.CollisionModel(self.__assbasepath)
self.__assbasecm.setColor(.7,.72,.75,1.0)
self.__assbasecm.setPos(130.0, 65.0,1220.0)
self.__assvertplanebigcm= cm.CollisionModel(self.__assvertplanebigpath)
self.__assvertplanebigcm.setColor(.7,.72,.75,1.0)
self.__assvertplanebigcm.setPos(330.0, 42.0,1230.0)
self.__battached = False
self.__changableobslist = []
def loadobj(self, name):
self.__objpath = os.path.join(self.__this_dir, "objects", name)
self.__objcm = cm.CollisionModel(self.__objpath, type="ball")
return self.__objcm
def buildCabinet(base,
cabpos=np.array([773.5, -55.17, 1035]),
cabrotangle=0,
isrendercoord=False):
__this_dir, _ = os.path.split(__file__)
cabinet = NodePath("cabinet")
#build the cabinet
#the middle board, 3 pieces
middleboardpath = os.path.join(__this_dir, "objects", "middleboard.stl")
middleboard0 = cm.CollisionModel(middleboardpath,
radius=3,
betransparency=True)
middleboard0.setColor(1, 0, 0, 1)
temprotmiddleboard = rm.rodrigues([0, 0, 1], 90)
rotmiddleboardmat4 = middleboard0.getMat()
rotmiddleboardnp4 = base.pg.mat4ToNp(rotmiddleboardmat4)
rotmiddleboardnp4[:3, :3] = np.dot(temprotmiddleboard,
rotmiddleboardnp4[:3, :3])
rotmiddleboardmat4 = base.pg.np4ToMat4(rotmiddleboardnp4)
middleboard0.setMat(rotmiddleboardmat4)
# temprotsmallboard = np.dot()
middleboard1 = copy.deepcopy(middleboard0)
middleboard1.setPos(0, 0, 288.5)
middleboard2 = copy.deepcopy(middleboard0)
temprotmiddleboard = rm.rodrigues([1, 0, 0], 180)
rotmiddleboardnp4[:3, :3] = np.dot(temprotmiddleboard,
rotmiddleboardnp4[:3, :3])
rotmiddleboardnp4[:3, 3] = np.array([0, 0, 587])
middleboard2.sethomomat(rotmiddleboardnp4)
# middleboard2.setPos(0,0,577)
middleboard0.setColor(.4, .4, .4, .7) #this 0 doesn't need to setpos
middleboard0.reparentTo(cabinet)
middleboard1.setColor(.4, .4, .4, .7)
middleboard1.reparentTo(cabinet)
middleboard2.setColor(.4, .4, .4, .7)
middleboard2.reparentTo(cabinet)
largeboardpath = os.path.join(__this_dir, "objects", "largeboard.stl")
largeboard0 = cm.CollisionModel(largeboardpath,
radius=3,
betransparency=True)
largeboard1 = copy.deepcopy(largeboard0)
largecompundrot0 = np.dot(rm.rodrigues([0, 1, 0], -90),
rm.rodrigues([1, 0, 0], 90))
largecompundrot1 = np.dot(rm.rodrigues([-1, 0, 0], 180), largecompundrot0)
largeboard0.sethomomat(
rm.homobuild(np.array([0, 195, 293.5]), largecompundrot0))
largeboard1.sethomomat(
rm.homobuild(np.array([0, -195, 293.5]), largecompundrot1))
largeboard0.setColor(.4, .4, .4, .7)
largeboard0.reparentTo(cabinet)
largeboard1.setColor(.4, .4, .4, .7)
largeboard1.reparentTo(cabinet)
# the small board, 2 pieces
smallboardpath = os.path.join(__this_dir, "objects", "smallboard.stl")
smallboard0 = cm.CollisionModel(smallboardpath,
radius=3,
betransparency=True)
smallboard1 = copy.deepcopy(smallboard0)
smallcompundrot = np.dot(rm.rodrigues([0, 1, 0], 90),
rm.rodrigues([0, 0, 1], -90))
smallboard0.sethomomat(
rm.homobuild(np.array([-142.5, 0, 149.25]), smallcompundrot))
smallboard1.sethomomat(
rm.homobuild(np.array([-142.5, 0, 587 - 149.25]), smallcompundrot))
smallboard0.setColor(.4, .4, .4, .7)
smallboard0.reparentTo(cabinet)
smallboard1.setColor(.4, .4, .4, .7)
smallboard1.reparentTo(cabinet)
if isrendercoord == True:
middleboard0.showLocalFrame()
middleboard1.showLocalFrame()
middleboard2.showLocalFrame()
largeboard0.showLocalFrame()
largeboard1.showLocalFrame()
smallboard0.showLocalFrame()
smallboard1.showLocalFrame()
#rotate the cabinet
cabinetassemblypos = cabpos # on the edge is 773, a bit outside is 814
cabinetassemblyrot = rm.rodrigues([0, 0, 1], cabrotangle)
cabinet.setMat(
base.pg.np4ToMat4(rm.homobuild(cabinetassemblypos,
cabinetassemblyrot)))
return cabinet
return effect_grasps
except:
print("load failed, create new graqsp file or grasp first.")
raise ValueError("File or data not found!")
if __name__ == '__main__':
import manipulation.grip.freegrip as fg
import environment.bulletcdhelper as bcd
base = pc.World(camp=[2000, -2000, 1500],
lookatpos=[0, 0, 100],
up=[0, -1, 1],
autocamrotate=False)
hndfa = yi.YumiIntegratedFactory()
objcm = cm.CollisionModel(objinit="../objects/" + "vacuumhead.stl")
gp = fg.Freegrip(objinit="../objects/" + "vacuumhead.stl",
hand=hndfa.genHand())
bmc = bcd.MCMchecker(toggledebug=True)
gp.segShow(base,
togglesamples=False,
togglenormals=False,
togglesamples_ref=False,
togglenormals_ref=False,
togglesamples_refcls=False,
togglenormals_refcls=False,
alpha=1)
predefinedgrasps = load_pickle_file_grip(objcm.name)
counter = [0]
import environment.collisionmodel as cm
import environment.bulletcdhelper as bch
base = pc.World(camp = [3000,0,3000], lookatpos= [0, 0, 0])
base.pggen.plotAxis(base.render)
# shape = BulletBoxShape(Vec3(50, 200, 450))
# node = BulletRigidBodyNode('Box')
# node.setMass(1.0)
# node.setAngularVelocity(Vec3(1,1,1))
# node.addShape(shape)
# np = base.render.attachNewNode(node)
# np.setPos(0, 0, 0)
model = cm.CollisionModel("./objects/bunnysim.meshes")
# model.reparentTo(base.render)
# model.setMat(Mat4.rotateMat(10, Vec3(1,0,0)))
# model.setPos(0,0,300)
bulletnode = bch.genBulletCDMesh(model)
bulletnode.setMass(1)
rigidbody = base.render.attachNewNode(bulletnode)
model.reparentTo(rigidbody)
world = BulletWorld()
world.setGravity(Vec3(0, 0, -9.8))
world.attach(bulletnode)
def update(task):
dt = globalClock.getDt()
def __init__(self, betransparent=False):
"""
load obstacles model
separated by category
:param base:
author: weiwei
date: 20181205
"""
self.__this_dir, this_filename = os.path.split(__file__)
# table
self.__tablepath = os.path.join(self.__this_dir, "obstacles",
"nxttable.stl")
self.__tablecm = cm.CollisionModel(self.__tablepath, betransparent)
self.__tablecm.setColor(.6, .6, .6, 1.0)
self.__tablecm.setPos(190.0, 0.0, 0.0)
# shelf
self.__belowfrontbeampath = os.path.join(
self.__this_dir, "obstacles", "nxt_belowfront_beam_800x30x30.stl")
self.__belowleftbeampath = os.path.join(
self.__this_dir, "obstacles", "nxt_belowleft_beam_600x30x30.stl")
self.__frontleftbeampath = os.path.join(
self.__this_dir, "obstacles", "nxt_frontleft_beam_850x30x30.stl")
self.__topleftbeampath = os.path.join(
self.__this_dir, "obstacles", "nxt_topleft_beam_300x30x30.stl")
self.__framebelowrgtcm = cm.CollisionModel(self.__belowleftbeampath)
self.__framebelowrgtcm.setColor(.6, .6, .6, 1.0)
self.__framebelowrgtcm.setPos(300.0 + 190.0, -415.0, 912.0)
self.__framebelowlftcm = cm.CollisionModel(self.__belowleftbeampath)
self.__framebelowlftcm.setColor(.6, .6, .6, 1.0)
self.__framebelowlftcm.setPos(300.0 + 190.0, 415.0, 912.0)
self.__framebelowfrontcm = cm.CollisionModel(self.__belowfrontbeampath)
self.__framebelowfrontcm.setColor(.6, .6, .6, 1.0)
self.__framebelowfrontcm.setPos(600.0 - 15.0 + 190.0, 0.0, 912.0)
self.__framebelowbackcm = cm.CollisionModel(self.__belowfrontbeampath)
self.__framebelowbackcm.setColor(.6, .6, .6, 1.0)
self.__framebelowbackcm.setPos(15.0 + 190.0, 0.0, 912.0)
self.__framefrontrgtcm = cm.CollisionModel(self.__frontleftbeampath)
self.__framefrontrgtcm.setColor(.6, .6, .6, 1.0)
self.__framefrontrgtcm.setPos(600.0 - 15.0 + 190.0, -415.0,
912.0 + 425.0)
self.__framefrontlftcm = cm.CollisionModel(self.__frontleftbeampath)
self.__framefrontlftcm.setColor(.6, .6, .6, 1.0)
self.__framefrontlftcm.setPos(600.0 - 15.0 + 190.0, 415.0,
912.0 + 425.0)
self.__frametoprgtcm = cm.CollisionModel(self.__topleftbeampath)
self.__frametoprgtcm.setColor(.6, .6, .6, 1.0)
self.__frametoprgtcm.setPos(600.0 - 150.0 + 190.0, -415.0,
912.0 + 850.0 + 15.0)
self.__frametoplftcm = cm.CollisionModel(self.__topleftbeampath)
self.__frametoplftcm.setColor(.6, .6, .6, 1.0)
self.__frametoplftcm.setPos(600.0 - 150.0 + 190.0, 415.0,
912.0 + 850.0 + 15.0)
self.__frametopfrontcm = cm.CollisionModel(self.__belowfrontbeampath)
self.__frametopfrontcm.setColor(.6, .6, .6, .3)
self.__frametopfrontcm.setPos(600.0 - 15.0 + 190.0, 0.0,
912.0 + 850.0 + 15.0)
self.__frametopbackcm = cm.CollisionModel(self.__belowfrontbeampath)
self.__frametopbackcm.setColor(.6, .6, .6, .3)
self.__frametopbackcm.setPos(600.0 - 300.0 + 15.0 + 190.0, 0.0,
912.0 + 850.0 + 15.0)
# housing
## housing pillar
# self.__beam1200path = os.path.join(self.__this_dir, "obstacles", "ur3housing1200.stl")
# self.__beam1500path = os.path.join(self.__this_dir, "obstacles", "ur3housing1500.stl")
# self.__beam2100path = os.path.join(self.__this_dir, "obstacles", "ur3housing2100.stl")
#
# self.__pillarfrontrgtcm = cm.CollisionModel(self.__beam2100path)
# self.__pillarfrontrgtcm.setColor(.6,.6,.6,1.0)
# self.__pillarfrontrgtcm.setPos(860.0+50.0+30.0, -780.0, 0.0)
# self.__pillarfrontlftcm = cm.CollisionModel(self.__beam2100path)
# self.__pillarfrontlftcm.setColor(.6,.6,.6,1.0)
# self.__pillarfrontlftcm.setPos(860.0+50.0+30.0, 780.0, 0.0)
# self.__pillarbackrgtcm = cm.CollisionModel(self.__beam2100path)
# self.__pillarbackrgtcm.setColor(.6,.6,.6,1.0)
# self.__pillarbackrgtcm.setPos(860+50-1200, -780.0, 0.0)
# self.__pillarbacklftcm = cm.CollisionModel(self.__beam2100path)
# self.__pillarbacklftcm.setColor(.6,.6,.6,1.0)
# self.__pillarbacklftcm.setPos(860+50-1200, 780.0, 0.0)
# ## housing rgt
# self.__rowrgtbottomcm = cm.CollisionModel(self.__beam1200path)
# self.__rowrgtbottomcm.setColor(.6,.6,.6,1.0)
# self.__rowrgtbottomcm.setPos(860+50-600, -780.0, 75.0)
# self.__rowrgtmiddlecm = cm.CollisionModel(self.__beam1200path)
# self.__rowrgtmiddlecm.setColor(.6,.6,.6,1.0)
# self.__rowrgtmiddlecm.setPos(860+50-600, -780.0, 867.0)
# self.__rowrgttopcm = cm.CollisionModel(self.__beam1200path)
# self.__rowrgttopcm.setColor(.6,.6,.6,1.0)
# self.__rowrgttopcm.setPos(860+50-600, -780.0, 2100.0-60.0)
# ## housing lft
# self.__rowlftbottomcm = cm.CollisionModel(self.__beam1200path)
# self.__rowlftbottomcm.setColor(.6,.6,.6,1.0)
# self.__rowlftbottomcm.setPos(860+50-600, 780.0, 75.0)
# self.__rowlfttopcm = cm.CollisionModel(self.__beam1200path)
# self.__rowlfttopcm.setColor(.6,.6,.6,1.0)
# self.__rowlfttopcm.setPos(860+50-600, 780.0, 2100.0-60.0)
# ## housing front
# self.__rowfrontbottomcm = cm.CollisionModel(self.__beam1500path)
# self.__rowfrontbottomcm.setColor(.6,.6,.6,1.0)
# self.__rowfrontbottomcm.setPos(860+50+30, 0.0, 75.0)
# self.__rowfrontmiddlecm = cm.CollisionModel(self.__beam1500path)
# self.__rowfrontmiddlecm.setColor(.6,.6,.6,1.0)
# self.__rowfrontmiddlecm.setPos(860+50+30, 0.0, 867.0)
# # self.__rowfronttopcm = cm.CollisionModel(self.__beam1500path)
# # self.__rowfronttopcm.setColor(.6,.6,.6,1.0)
# # self.__rowfronttopcm.setPos(860+50+30, 0.0, 2100.0-60.0)
# ## housing back
# self.__rowbackbottomcm = cm.CollisionModel(self.__beam1500path)
# self.__rowbackbottomcm.setColor(.6,.6,.6,1.0)
# self.__rowbackbottomcm.setPos(860+50-1200, 0.0, 2100.0-530.0-60.0)
# self.__rowbackmiddlecm = cm.CollisionModel(self.__beam1500path)
# self.__rowbackmiddlecm.setColor(.6,.6,.6,1.0)
# self.__rowbackmiddlecm.setPos(860+50-1200, 0.0, 2100.0-264.0-60.0)
# self.__rowbacktopcm = cm.CollisionModel(self.__beam1500path)
# self.__rowbacktopcm.setColor(.6,.6,.6,1.0)
# self.__rowbacktopcm.setPos(860+50-1200, 0.0, 2100.0-60.0)
self.__battached = False
self.__changableobslist = []
if __name__ == '__main__':
yhx = robothelper.RobotHelperX(usereal=False)
# tubestand
# tscm = cm.CollisionModel(os.path.join(yhx.root, "objects", "tubestand.stl"))
# tspcd, _ = tscm.samplesurface(radius=2)
# tspcd = tspcd[tspcd[:,2]>55]
#
# pickle.dump(tspcd, open(os.path.join(yhx.root, "dataobjtemplate", "tubestandtemplatepcd.pkl"), "wb"))
# base = yhx.startworld()
# pcdcm = cm.CollisionModel(tspcd)
# pcdcm.reparentTo(base.render)
# base.run()
# tubestand_light
vhcm = cm.CollisionModel(
os.path.join(yhx.root, "objects", "vacuumhead.stl"))
vhpcd, _ = vhcm.samplesurface(radius=2)
vhpcd = vhpcd[vhpcd[:, 1] < -5]
#
pickle.dump(
vhpcd,
open(
os.path.join(yhx.root, "dataobjtemplate",
"vacuumhead_templatepcd.pkl"), "wb"))
pcdcm = cm.CollisionModel(vhpcd)
pcdcm.reparentTo(yhx.base.render)
yhx.base.run()
# tubestand_light
tscm = cm.CollisionModel(
os.path.join(yhx.root, "objects", "tubestand_light.stl"))
