Esempio n. 1
0
                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
Esempio n. 2
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()
Esempio n. 3
0
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"))
Esempio n. 4
0
                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
Esempio n. 5
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:
Esempio n. 6
0
    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()