tubepos = rm.homotransformpoint(tubestand_homomat,
tubepos_normalized)
tubemat[:3, 3] = tubepos
newtubecm.set_homomat(tubemat)
newtubecm.setColor(rgba[0], rgba[1], rgba[2], rgba[3])
tubecmlist.append(newtubecm)
return tubecmlist
if __name__ == '__main__':
import robothelper
import numpy as np
import environment.collisionmodel as cm
yhx = robothelper.RobotHelperX(usereal=False, startworld=False)
loc = Locator(standtype="light")
bgdepth = pickle.load(open("./databackground/bgdepth.pkl", "rb"))
bgpcd = pickle.load(open("./databackground/bgpcd.pkl", "rb"))
# objpcdmerged = None
# for i in range(1):
# yhx.pxc.triggerframe()
# fgdepth = yhx.pxc.getdepthimg()
# fgpcd = yhx.pxc.getpcd()
#
# substracteddepth = bgdepth - fgdepth
# substracteddepth = substracteddepth.clip(40, 300)
# substracteddepth[substracteddepth == 40] = 0
# substracteddepth[substracteddepth == 300] = 0
author: weiwei
date: 20191229osaka
"""
return rm.homotransformpointarray(self.sensorhomomat, pcdarray)
if __name__ == '__main__':
import robothelper
import utiltools.misc.p3dhtils as p3dh
import environment.collisionmodel as cm
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()
import robothelper
import numpy as np
import cv2
import pickle
import environment.collisionmodel as cm
if __name__ == '__main__':
yhx = robothelper.RobotHelperX(usereal=False)
yhx.pxc.triggerframe()
bgdepth = yhx.pxc.getdepthimg()
pickle.dump(bgdepth, open("../../databackground/bgdepthlow.pkl", "wb"))
bgpcd = yhx.pxc.getpcd()
pickle.dump(bgpcd, open("../../databackground/bgpcddepthlow.pkl", "wb"))
tubepos = rm.homotransformpoint(tubemat, tubepos_normalized)
tubemat[:3, 3] = tubepos
newtubecm.set_homomat(tubemat)
newtubecm.setColor(rgba[0], rgba[1], rgba[2], rgba[3])
tubecmlist.append(newtubecm)
return tubecmlist
if __name__ == '__main__':
import robothelper
import numpy as np
import environment.collisionmodel as cm
yhx = robothelper.RobotHelperX(usereal=False,
startworld=True,
autorotate=True)
loc = LocatorFixed(homomatfilename="rightfixture_homomat1")
# bgdepth = pickle.load(open("./databackground/bgdepth.pkl", "rb"))
# bgpcd = pickle.load(open("./databackground/bgpcd.pkl", "rb"))
# objpcdmerged = None
# for i in range(1):
# yhx.pxc.triggerframe()
# fgdepth = yhx.pxc.getdepthimg()
# fgpcd = yhx.pxc.getpcd()
#
# substracteddepth = bgdepth - fgdepth
# substracteddepth = substracteddepth.clip(40, 300)
# substracteddepth[substracteddepth == 40] = 0
if armname == "rgt":
handtmp = self.rgthndfa.genHand()
else:
handtmp = self.lfthndfa.genHand()
handtmp.set_homomat(homomat)
handtmp.setjawwidth(handtmp.jawwidthopen)
iscollided = self.bcdchecker.isMeshListMeshListCollided(
handtmp.cmlist, obstaclecmlist)
return iscollided
if __name__ == "__main__":
import robothelper as yh
rhx = yh.RobotHelperX(usereal=False)
base = rhx.startworld()
objcm = rhx.cm.CollisionModel("./objects/tubebig.stl")
hmstr = HandoverPlanner(obj=objcm, rhx=rhx, retractdistance=100)
hmstr.genhvgpsgl(rhx.np.array([400, 0, 300]), rhx.np.eye(3))
identityglist_rgt, identityglist_lft, fpsnpmat4, identitygplist, fpsnestedglist_rgt, fpsnestedglist_lft, \
ikfid_fpsnestedglist_rgt,ikfid_fpsnestedglist_lft, \
ikjnts_fpsnestedglist_rgt, ikjnts_fpsnestedglist_lft = hmstr.gethandover()
print(ikfid_fpsnestedglist_lft.keys())
print(ikfid_fpsnestedglist_rgt.keys())
poseid = 0
for gp in identitygplist:
homopcd[:3, :] = pcd.T
realpcd = np.dot(amat, homopcd).T
return realpcd[:, :3]
if __name__ == '__main__':
import os
import pickle
import robotconn.rpc.phoxi.phoxi_client as pcdt
parameters = aruco.DetectorParameters_create()
aruco_dict = aruco.getPredefinedDictionary(aruco.DICT_4X4_250)
armname = "rgt"
yhx = robothelper.RobotHelperX(usereal=True)
yhx.env.reparentTo(yhx.base.render)
pxc = pcdt.PhxClient(host="192.168.125.100:18300")
# relpos = phoxi_computeboardcenterinhand(yhx, pxc, arm_name, parameters, aruco_dict)
# phoxi_calibbyestinee(yhx, pxc, arm_name, parameters, aruco_dict)
relpos = np.array([-12.03709376, 1.22814887, 37.36035265])
phoxi_calib(yhx, pxc, armname, relpos, parameters, aruco_dict)
# rawamat = pickle.load(open(os.path.join(yhx.root, "datacalibration", "calibmat.pkl"), "rb"))
# phoxi_calib_refine(yhx, pxc, rawamat, arm_name="rgt")
# yhx.robot_s.movearmfk(yhx.getarmjntsx("rgt"), arm_name="rgt")
# yhx.robot_s.movearmfk(yhx.getarmjntsx("lft"), arm_name="lft")
# rbtnp = yhx.rbtmesh.genmnp(yhx.robot_s)
# rbtnp.reparentTo(base.render)
#
# amat = get_amat()