def estimate(yhx, lctr, onscreennodepaths, task):
if yhx.base.inputmgr.keyMap['space'] is True:
yhx.base.inputmgr.keyMap['space'] = False
for idosnp, onscreennp in enumerate(onscreennodepaths):
if onscreennp is not None:
onscreennp.removeNode()
onscreennodepaths[idosnp] = None
else:
break
objpcd = lctr.capturecorrectedpcd(yhx.pxc)
homomat = loc.findtubestand_matchonobb(objpcd, toggledebug=False)
elearray, eleconfidencearray = loc.findtubes(homomat,
objpcd,
toggledebug=False)
framenp = yhx.p3dh.genframe(pos=homomat[:3, 3],
rotmat=homomat[:3, :3])
framenp.reparentTo(yhx.base.render)
onscreennodepaths[0] = framenp
rbtnp = yhx.rbtmesh.genmnp(yhx.robot_s)
rbtnp.reparentTo(yhx.base.render)
onscreennodepaths[1] = rbtnp
pcdnp = p3dh.genpointcloudnodepath(objpcd, pntsize=5)
pcdnp.reparentTo(yhx.base.render)
onscreennodepaths[2] = pcdnp
tbscm = loc.gentubestand(homomat=homomat)
tbscm.reparentTo(yhx.base.render)
tbscm.showcn()
onscreennodepaths[3] = tbscm
tubecms = loc.gentubes(elearray,
tubestand_homomat=homomat,
eleconfidencearray=eleconfidencearray)
for i, tbcm in enumerate(tubecms):
tbcm.reparentTo(yhx.base.render)
hmat = tbcm.get_homomat()
hmat[:3, 3] -= hmat[:3, 2] * 50
tbcm.set_homomat(hmat)
tbcm.showcn()
onscreennodepaths[i + 4] = tbcm
return task.again
else:
return task.again
def findtubes(self, tubestand_homomat, tgtpcdnp, toggledebug=False):
"""
:param tubestand_homomat:
:param tgtpcdnp:
:return:
"""
elearray = np.zeros((5, 10))
eleconfidencearray = np.zeros((5, 10))
tgtpcdo3d = o3dh.nparray_to_o3dpcd(tgtpcdnp)
tgtpcdo3d_removed = o3dh.remove_outlier(tgtpcdo3d,
downsampling_voxelsize=None,
nb_points=90,
radius=5)
tgtpcdnp = o3dh.o3dpcd_to_parray(tgtpcdo3d_removed)
# transform tgtpcdnp back
tgtpcdnp_normalized = rm.homotransformpointarray(
rm.homoinverse(tubestand_homomat), tgtpcdnp)
if toggledebug:
cm.CollisionModel(tgtpcdnp_normalized).reparentTo(base.render)
tscm2 = cm.CollisionModel("../objects/tubestand.stl")
tscm2.reparentTo(base.render)
for i in range(5):
for j in range(10):
holepos = self.tubeholecenters[i][j]
# squeeze the hole size by half
tmppcd = tgtpcdnp_normalized[
tgtpcdnp_normalized[:, 0] < holepos[0] +
self.tubeholesize[0] / 1.9]
tmppcd = tmppcd[tmppcd[:, 0] > holepos[0] -
self.tubeholesize[0] / 1.9]
tmppcd = tmppcd[tmppcd[:, 1] < holepos[1] +
self.tubeholesize[1] / 1.9]
tmppcd = tmppcd[tmppcd[:, 1] > holepos[1] -
self.tubeholesize[1] / 1.9]
tmppcd = tmppcd[tmppcd[:, 2] > 70]
if len(tmppcd) > 100:
print(
"------more than 100 raw points, start a new test------"
)
# use core tmppcd to decide tube types (avoid noises)
coretmppcd = tmppcd[tmppcd[:, 0] < holepos[0] +
self.tubeholesize[0] / 4]
coretmppcd = coretmppcd[coretmppcd[:, 0] > holepos[0] -
self.tubeholesize[0] / 4]
coretmppcd = coretmppcd[coretmppcd[:, 1] < holepos[1] +
self.tubeholesize[1] / 4]
coretmppcd = coretmppcd[coretmppcd[:, 1] > holepos[1] -
self.tubeholesize[1] / 4]
print("testing the number of core points...")
print(len(coretmppcd[:, 2]))
if len(coretmppcd[:, 2]) < 10:
print("------the new test is done------")
continue
if np.max(tmppcd[:, 2]) > 100:
candidatetype = 1
tmppcd = tmppcd[tmppcd[:, 2] >
100] # crop tmppcd for better charge
else:
candidatetype = 2
tmppcd = tmppcd[tmppcd[:, 2] < 90]
if len(tmppcd) < 10:
continue
print("passed the core points test, rotate around...")
rejflag = False
for angle in np.linspace(0, 180, 20):
tmphomomat = np.eye(4)
tmphomomat[:3, :3] = rm.rodrigues(
tubestand_homomat[:3, 2], angle)
newtmppcd = rm.homotransformpointarray(
tmphomomat, tmppcd)
minstd = np.min(np.std(newtmppcd[:, :2], axis=0))
print(minstd)
if minstd < 1.3:
rejflag = True
print("rotate round done")
if rejflag:
continue
else:
tmpangles = np.arctan2(tmppcd[:, 1], tmppcd[:, 0])
tmpangles[
tmpangles < 0] = 360 + tmpangles[tmpangles < 0]
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)
print("------the new test is done------")
return elearray, eleconfidencearray
import manipulation.suction.freesuc as fs
import manipulation.suction.sandmmbs.sdmbs as sds
import manipulation.suction.hlabbig.hlabbig as hlb
rhx = robothelper.RobotHelperX(usereal=True, startworld=True)
# rhx.movetox(rhx.robot_s.initrgtjnts, arm_name="rgt")
# rhx.movetox(rhx.robot_s.initlftjnts, arm_name="lft")
# rhx.closegripperx(arm_name="rgt")
# rhx.closegripperx(arm_name="lft")
# rhx.opengripperx(arm_name="rgt")
# rhx.opengripperx(arm_name="lft")
pg = PcdGrab()
nppcd = pg.capturecorrectedpcd(pxc=rhx.pxc)
p3dpcd = p3dh.genpointcloudnodepath(nppcd, pntsize=1.57)
# p3dpcd.reparentTo(rhx.base.render)
effa = hlb.EFFactory()
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)