def rgtcapture(self): """ capture one goal pose using the rgt hndcam system :return: a 4x4 h**o numpy mat author: weiwei date: 20180926 """ # goalT = self.hc.getObjPoseRgtCam(self.rgtwatchpos, self.rgtwatchrotmat, marker_id=self.markerid) virtualgoalpos0 = np.array([450, 350, 1200]) virtualgoalrot0 = rm.rodrigues([ 1, 0, 0, ], 0) virtualgoalpos1 = np.array([450, 250, 1200]) virtualgoalrot1 = rm.rodrigues([ 1, 0, 0, ], 0) virtualgoalpos2 = np.array([490, 250, 1200]) virtualgoalrot2 = rm.rodrigues([ 1, 0, 0, ], 0) virtualgoalpos3 = np.array([490, 350, 1200]) virtualgoalrot3 = rm.rodrigues([ 1, 0, 0, ], 0) goalTlist = [ rm.homobuild(virtualgoalpos0, virtualgoalrot0), rm.homobuild(virtualgoalpos1, virtualgoalrot1), rm.homobuild(virtualgoalpos2, virtualgoalrot2), rm.homobuild(virtualgoalpos3, virtualgoalrot3) ] ngoal = len(self.goallist) if ngoal >= len(goalTlist): return False goalT = goalTlist[ngoal] if goalT is None: return False else: self.goallist.append(goalT) tmpobjcm = copy.deepcopy(self.objcm) tmpobjcm.setMat(base.pg.np4ToMat4(goalT)) tmpobjcm.reparentTo(base.render) tmpobjcm.showLocalFrame() self.objrenderlist.append(tmpobjcm) for id, tmpobjcm in enumerate(self.objrenderlist): tmpobjcm.setColor(0, (id) / float(len(self.objrenderlist)), (id + 1) / float(len(self.objrenderlist)), 1) return True
def __updatefk(self, armlj): """ Update the structure of hrp5's arm links and joints (single) Note that this function should not be called explicitly It is called automatically by functions like movexxx :param armlj: the rgtlj or lftlj robot structure :return: null author: weiwei date: 20161202 """ i = 1 while i != -1: j = armlj[i]['mother'] armlj[i]['linkpos'] = armlj[j]['linkend'] if i != 3 and i != 5: armlj[i]['rotmat'] = np.dot( np.dot(armlj[j]['rotmat'], armlj[i]['inherentR']), rm.rodrigues(armlj[i]['rotax'], armlj[i]['rotangle'])) else: armlj[i]['rotmat'] = np.dot( armlj[j]['rotmat'], rm.rodrigues(armlj[i]['rotax'], armlj[i]['rotangle'])) armlj[i]['linkend'] = np.squeeze( np.dot(armlj[i]['rotmat'], armlj[i]['linkvec'].reshape( (-1, 1))).reshape((1, -1))) + armlj[i]['linkpos'] i = armlj[i]['child'] return armlj
def movewaist(self, rotangle=0): """ rotate the base of the robot :param rotangle: in degree :return: null author: weiwei date: 20161107 """ # right arm self.rgtarm[0]['rotangle'] = rotangle self.rgtarm[0]['rotmat'] = np.dot( self.rgtarm[0]['refcs'], rm.rodrigues(self.rgtarm[0]['rotax'], self.rgtarm[0]['rotangle'])) self.rgtarm[0]['linkend'] = np.dot( self.rgtarm[0]['rotmat'], self.rgtarm[0]['linkvec']) + self.rgtarm[0]['linkpos'] self.__updatefk(self.rgtarm) # left arm self.lftarm[0]['rotangle'] = rotangle self.lftarm[0]['rotmat'] = np.dot( self.lftarm[0]['refcs'], rm.rodrigues(self.lftarm[0]['rotax'], self.lftarm[0]['rotangle'])) self.lftarm[0]['linkend'] = np.dot( self.lftarm[0]['rotmat'], self.lftarm[0]['linkvec']) + self.lftarm[0]['linkpos'] self.__updatefk(self.lftarm)
def rot_transform(rot_mat, incline_angle): """ Set the inclination angle of the object according :param rot_mat: initial rotation matrix of the robot arm :param incline_angle: inclination angle about the end effector Z axis :return: rotation matrix of the required inclination """ tmp_rot1 = rm.rodrigues(rot_mat[:, 2], 90) tmp_rot1 = np.dot(tmp_rot1, rot_mat) tmp_rot2 = rm.rodrigues(tmp_rot1[:, 2], incline_angle) return np.dot(tmp_rot2, tmp_rot1)
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 define_grasp_with_rotation(hndfa, grasp_center, finger_normal, hand_normal, jawwidth, objcm, rotation_interval=15, rotation_range=(-90, 90), toggleflip = True): """ :param hndfa: :param grasp_center: :param finger_normal: :param hand_normal: :param jawwidth: :param objcm: :param rotation_interval: :param rotation_range: :param toggleflip: :return: author: chenhao, revised by weiwei date: 20200104 """ effect_grasp = [] for rotate_angle in range(rotation_range[0], rotation_range[1], rotation_interval): hnd = hndfa.genHand() hand_normal_rotated = np.dot(rm.rodrigues(finger_normal, rotate_angle), np.asarray(hand_normal)) grasp = hnd.approachat(grasp_center[0], grasp_center[1], grasp_center[2], finger_normal[0], finger_normal[1], finger_normal[2], hand_normal_rotated[0], hand_normal_rotated[1], hand_normal_rotated[2], jawwidth=jawwidth) # if not ishndobjcollided(hndfa, grasp[0], grasp[2], objcm) == False: effect_grasp.append(grasp) if toggleflip: grasp_flipped = hnd.approachat(grasp_center[0], grasp_center[1], grasp_center[2], -finger_normal[0], -finger_normal[1], -finger_normal[2], hand_normal_rotated[0], hand_normal_rotated[1], hand_normal_rotated[2], jawwidth=jawwidth) if not ishndobjcollided(hndfa, grasp_flipped[0], grasp_flipped[2], objcm): effect_grasp.append(grasp_flipped) return effect_grasp
def __updatefk(self, armlj): """ Update the structure of hiro's arm links and joints (single) Note that this function should not be called explicitly It is called automatically by functions like movexxx :param armlj: the rgtlj or lftlj robot structure :return: null author: weiwei date: 20160615 """ i = 1 while i != -1: j = armlj[i]['mother'] armlj[i]['linkpos'] = armlj[j]['linkend'] armlj[i]['refcs'] = np.dot(armlj[j]['rotmat'], armlj[i]['inherentR']) armlj[i]['rotmat'] = np.dot( armlj[i]['refcs'], rm.rodrigues(armlj[i]['rotax'], armlj[i]['rotangle'])) armlj[i]['linkend'] = np.dot( armlj[i]['rotmat'], armlj[i]['linkvec']) + armlj[i]['linkpos'] i = armlj[i]['child'] return armlj
def rgtcapturestart(self): """ capture the starting pose using the rgt hndcam system :return: author: weiwei date: 20180926 """ # startT = None # while startT is None: # startT = self.hc.getObjPoseRgtCam(self.rgtwatchpos, self.rgtwatchrotmat, marker_id=self.markerid) # startT = np.array([[1,0,0,0],[0,1,0,0],[0,0,1,0],[500,-350,1100,1.0]]).T virtualgoalpos0 = np.array([500, -350, 1100]) virtualgoalrot0 = rm.rodrigues([ 1, 0, 0, ], 0) startT = rm.homobuild(virtualgoalpos0, virtualgoalrot0) self.goallist = [startT] + self.goallist self.startobjcm = copy.deepcopy(self.objcm) self.startobjcm.setMat(base.pg.np4ToMat4(startT)) self.startobjcm.reparentTo(base.render) self.startobjcm.showLocalFrame() self.startobjcm.setColor(1, 0, 0, 1)
def gripat(self, fcx, fcy, fcz, c0nx, c0ny, c0nz, rotangle=0, jawwidth=None): """ set the hand to grip at fcx, fcy, fcz, fc = finger center the normal of the sglfgr contact is set to be c0nx, c0ny, c0nz the rotation around the normal is set to rotangle the jaw_width is set to jaw_width date: 20170322 author: weiwei """ # x is the opening direction of the hand, _org means the value when rotangle=0 standardx_org = np.array([1, 0, 0]) newx_org = np.array([c0nx, c0ny, c0nz]) rotangle_org = rm.degree_betweenvector(newx_org, standardx_org) rotaxis_org = np.array([0, 0, 1]) if not (np.isclose(rotangle_org, 180.0) or np.isclose(rotangle_org, 0.0)): rotaxis_org = rm.unit_vector(np.cross(standardx_org, newx_org)) newrotmat_org = rm.rodrigues(rotaxis_org, rotangle_org) # rotate to the given rotangle hnd_rotmat4 = np.eye(4) hnd_rotmat4[:3, :3] = np.dot(rm.rodrigues(newx_org, rotangle), newrotmat_org) handtipnpvec3 = np.array([fcx, fcy, fcz]) - np.dot( hnd_rotmat4[:3, :3], self.__eetip) hnd_rotmat4[:3, 3] = handtipnpvec3 self.__hndnp.setMat(p3dh.npmat4_to_pdmat4(hnd_rotmat4)) if jawwidth is None: jawwidth = self.__jawwidthopen self.setjawwidth(jawwidth) return [jawwidth, np.array([fcx, fcy, fcz]), hnd_rotmat4]
def rot_uv(uv, angle=30, toggledebug=False): plt.scatter(np.asarray([v[0] for v in uv]), np.asarray([v[1] for v in uv]), color='red', marker='.') uv_3d = np.asarray([(v[0], v[1], 0) for v in uv]) uv_3d = pcdu.trans_pcd( uv_3d, rm.homobuild(np.asarray([0, 0, 0]), rm.rodrigues((0, 0, 1), angle))) uv_new = np.asarray([(v[0], v[1]) for v in uv_3d]) if toggledebug: plt.scatter(np.asarray([v[0] for v in uv_new]), np.asarray([v[1] for v in uv_new]), color='green', marker='.') plt.show() return uv_new
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
objgpos = np.array([304.5617667638, -277.1026725769, 1101.0]) objgrot = np.array([[1.0, 0.0, 0.0], [0.0, 6.12323426293e-17, -1.0], [0.0, 1.0, 6.12323426293e-17]]).T objcmcopys = copy.deepcopy(objcm) objcmcopys.setColor(0.0, 0.0, 1.0, .3) objcmcopys.setMat(base.pg.npToMat4(npmat3=objstrot, npvec3=objstpos)) objcmcopys.reparentTo(base.render) startpos = np.array([354.5617667638, -256.0889005661, 1060.5]) startrot = np.array([[1, 0, 0], [0, -1, 0], [0, 0, -1]]).T goalpos3 = np.array([354.5617667638, -256.0889005661, 1150.5]) goalrot3 = np.array([[1, 0, 0], [0, -0.92388, -0.382683], [0, 0.382683, -0.92388]]).T goalpos3 = goalpos3 - 70.0 * goalrot3[:, 2] goalpos4 = np.array([454.5617667638, -100, 1150.5]) goalrot4 = np.dot(rm.rodrigues([0, 0, 1], -120), goalrot3) goalpos4 = goalpos4 - 250.0 * goalrot4[:, 2] # goalpos2 = goalpos2 - 150.0*goalrot2[:,2] start = robot.numik(startpos, startrot, armname) goal3 = robot.numikmsc(goalpos3, goalrot3, msc=start, armname=armname) print(goal3) goal4 = robot.numikmsc(goalpos4, goalrot4, msc=start, armname=armname) print(goal4) planner = rrtc.RRTConnect(start=goal3, goal=goal4, ctcallback=ctcallback, starttreesamplerate=starttreesamplerate, endtreesamplerate=endtreesamplerate, expanddis=5, maxiter=2000, maxtime=100.0)
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
from direct.filter.CommonFilters import CommonFilters import manipulation.grip.robotiq85.rtq85 as rtq85 from robotsim.ur3dual import ur3dual from robotsim.ur3dual import ur3dualmesh import utiltools.robotmath as rm # loadPrcFileData("", "want-directtools #t") # loadPrcFileData("", "want-tk #t") base = pandactrl.World(camp=[3000, 3000, 3000], lookatp=[0, 0, 1300]) rgthnd = rtq85.Rtq85() lfthnd = rtq85.Rtq85() ur3dualrobot = ur3dual.Ur3DualRobot(position=[0, 500, 0], rotmat=rm.rodrigues([0, 0, 1], 70)) # goalpos = np.array([200,120,1410]) # goalrot = np.array([[1,0,0],[0,0,-1],[0,1,0]]) # goal = ur3dualrobot.numik(goalpos, goalrot, armname = 'lft') # goal = np.array([-46.251876352032305, -41.418654079396184, 94.34173209615693, -15.917387039376576, 92.07172082393664, -25.134340575525133]) goal = np.array([ -209.13573775, -42.61307312, -283.86285256, 30.51538756, -231.85631837, -218.24860474 ]) ur3dualrobot.movearmfk(goal, armname='rgt') # ur3dualrobot.gozeropose() ur3dualrobotball = Ur3DualBall() # bcndict = ur3dualrobotball.genfullbcndict(ur3dualrobot) # bcndict = ur3dualrobotball.gensemibcndict(ur3dualrobot) # bcndict = ur3dualrobotball.genholdbcndict(ur3dualrobot, armname='rgt') # bcndict = ur3dualrobotball.genfullactivebcndict(ur3dualrobot)
def plotPortionCircArrow(self, nodepath, axis=None, torqueportion=0.0, center=None, radius=5.0, thickness=1.5, rgba=None, discretization=24, plotname="circarrow"): """ Draw a circule arrow on a solid circle Especially used for visualization of torque the arraw starts from (axis cross [1,0,0])*radius+center if axis cross [1,0,0] equals [0,0,0], the arrow starts from (axis cross [0,1,0])*radius+center the portion of the circ arrow indicates the magnitude of t, maxt is set to torque/25.0 if torque/25.0 is larger than 1.0, set it to 1.0 :param nodepath: :param torque portion: measured torque over maximum value 0.0~1.0 :param center: center of the circle :param radius: radius of the circle :param rgba: color of the arrow :param discretization: number sticks used :return: author: weiwei date: 20180227 """ if axis is None: axis = np.array([0.0, 0.0, 1.0]) if center is None: center = np.array([0.0, 0.0, 0.0]) else: center = np.array([center[0], center[1], center[2]]) if rgba is None: rgba = np.array([1.0, 1.0, 1.0, 1.0]) xaxis = np.array([1.0, 0.0, 0.0]) yaxis = np.array([0.0, 1.0, 0.0]) startingaxis = np.cross(axis, xaxis) if np.linalg.norm(startingaxis) < .1: startingaxis = np.cross(axis, yaxis) startingaxis = startingaxis / np.linalg.norm(startingaxis) startingpos = startingaxis * radius + center cirarrnp = NodePath(plotname) discretizedangle = 360.0 / discretization ndist = int(torqueportion * discretization) lastpos = startingpos if abs(ndist) > 1: for i in range(1 * np.sign(ndist), ndist, 1 * np.sign(ndist)): nxtpos = center + np.dot( rm.rodrigues(axis, i * discretizedangle), startingaxis.T).T * radius self.plotStick(cirarrnp, spos=lastpos, epos=nxtpos, thickness=thickness, rgba=rgba, plotname="circarrseg") lastpos = nxtpos nxtposend = center + np.dot( rm.rodrigues(axis, ndist * discretizedangle), startingaxis.T).T * radius self.plotArrow(cirarrnp, lastpos, nxtposend, length=1.0, thickness=thickness, rgba=rgba) cirarrnpbg = NodePath(plotname + "bg") discretizedangle = 360.0 / discretization lastpos = startingpos for i in range(1, discretization + 1): nxtpos = center + np.dot(rm.rodrigues(axis, i * discretizedangle), startingaxis.T).T * radius bgrgba = np.array([rgba[0], rgba[1], rgba[2], .1]) self.plotStick(cirarrnp, spos=lastpos, epos=nxtpos, thickness=thickness * .9, rgba=bgrgba, plotname="circarrseg") lastpos = nxtpos cirarrnpbg.reparentTo(cirarrnp) cirarrnp.reparentTo(nodepath) return cirarrnp
if __name__ == "__main__": import environment.pandacdhelper as cmcd import os import numpy as np import utiltools.robotmath as rm import pandaplotutils.pandactrl as pc base = pc.World(camp=[1000, 300, 1000], lookatp=[0, 0, 0]) this_dir, this_filename = os.path.split(__file__) objpath = os.path.join(this_dir, "objects", "bunnysim.stl") bunnycm = CollisionModel(objpath, type="box") bunnycm.setColor(0.7, 0.7, 0.0, 1.0) bunnycm.reparentTo(base.render) rotmat = rm.rodrigues([1, 0, 0], 90) bunnycm.setMat(base.pg.npToMat4(rotmat)) bunnycm1 = CollisionModel(objpath, type="box") bunnycm1.setColor(0.7, 0, 0.7, 1.0) bunnycm1.reparentTo(base.render) # rotmat = rm.rodrigues([0,0,1], 15) rotmat = rm.euler_matrix(0, 0, 15) bunnycm1.setMat(base.pg.npToMat4(rotmat, np.array([0, 10, 0]))) bunnycm2 = CollisionModel(objpath, type="box") bunnycm2.setColor(0, 0.7, 0.7, 1.0) bunnycm2.reparentTo(base.render) rotmat = rm.rodrigues([1, 0, 0], -45) bunnycm2.setMat(base.pg.npToMat4(rotmat, np.array([0, 200, 0])))
def __initlftlj(self): """ Init hrp5's lft arm links and joints ## note x is facing forward, y is facing left, z is facing upward each element of lftlj is a dictionary lftlj[i]['linkpos'] indicates the position of a link lftlj[i]['linkvec'] indicates the vector of a link that points from start to end lftlj[i]['rotmat'] indicates the frame of this link lftlj[i]['rotax'] indicates the rotation axis of the link lftlj[i]['rotangle'] indicates the rotation angle of the link around the rotax lftlj[i]['linkend'] indicates the end position of the link (passively computed) ## more note: lftlj[1]['linkpos'] is the position of the first joint lftlj[i]['linkend'] is the same as lftlj[i+1]['linkpos'], I am keeping this value for the eef (end-effector) ## even more note: joint is attached to the linkpos of a link for the first link, the joint is fixed and the rotax = 0,0,0 ## inherentR is only available at the first link :return: lftlj: a list of dictionaries with each dictionary holding name, mother, child, linkpos (link position), linkvec (link vector), rotmat (orientation), rotax (rotation axis of a joint), rotangle (rotation angle of the joint around rotax) linkend (the end position of a link) is computed using rotmat, rotax, linkpos, and linklen lj means link and joint, the joint attached to the link is at the linkend author: weiwei date: 20161202, tsukuba """ # create a arm with 6 joints lftlj = [dict() for i in range(7)] rngsafemargin = 0 # the 0th link and joint lftlj[0]['name'] = 'link0' lftlj[0]['mother'] = -1 lftlj[0]['child'] = 1 lftlj[0]['linkpos'] = np.array([0, 0, 0]) lftlj[0]['linkvec'] = np.array([0, 237.50, 954.39]) + np.dot( rm.rodrigues([1, 0, 0], -135), np.array([0, 0, 89.159])) lftlj[0]['rotax'] = np.array([0, 0, 1]) lftlj[0]['rotangle'] = 0 lftlj[0]['rotmat'] = np.eye(3) lftlj[0]['linkend'] = np.dot(lftlj[0]['rotmat'], lftlj[0]['linkvec']) + lftlj[0]['linkpos'] # the 1st joint and link lftlj[1]['name'] = 'link1' lftlj[1]['mother'] = 0 lftlj[1]['child'] = 2 lftlj[1]['linkpos'] = lftlj[0]['linkend'] lftlj[1]['linkvec'] = np.array([0, 135.85, 0]) lftlj[1]['rotax'] = np.array([0, 0, 1]) lftlj[1]['rotangle'] = 0 lftlj[1]['inherentR'] = np.dot(rm.rodrigues([0, 0, 1], 180), rm.rodrigues([1, 0, 0], 135)) lftlj[1]['rotmat'] = np.dot(np.dot(lftlj[0]['rotmat'], lftlj[1]['inherentR']), \ rm.rodrigues(lftlj[1]['rotax'], lftlj[1]['rotangle'])) lftlj[1]['linkend'] = np.dot(lftlj[1]['rotmat'], lftlj[1]['linkvec']) + lftlj[1]['linkpos'] lftlj[1]['rngmin'] = -(360 - rngsafemargin) lftlj[1]['rngmax'] = +(360 - rngsafemargin) # the 2nd joint and link lftlj[2]['name'] = 'link2' lftlj[2]['mother'] = 1 lftlj[2]['child'] = 3 lftlj[2]['linkpos'] = lftlj[1]['linkend'] lftlj[2]['linkvec'] = np.array([0, -119.70, 425.00]) lftlj[2]['rotax'] = np.array([0, 1, 0]) lftlj[2]['rotangle'] = 0 lftlj[2]['inherentR'] = rm.rodrigues([0, 1, 0], 90) lftlj[2]['rotmat'] = np.dot(np.dot(lftlj[1]['rotmat'], lftlj[2]['inherentR']), \ rm.rodrigues(lftlj[2]['rotax'], lftlj[2]['rotangle'])) lftlj[2]['linkend'] = np.dot(lftlj[2]['rotmat'], lftlj[2]['linkvec']) + lftlj[2]['linkpos'] lftlj[2]['rngmin'] = -(360 - rngsafemargin) lftlj[2]['rngmax'] = +(360 - rngsafemargin) # the 3rd joint and link lftlj[3]['name'] = 'link3' lftlj[3]['mother'] = 2 lftlj[3]['child'] = 4 lftlj[3]['linkpos'] = lftlj[2]['linkend'] lftlj[3]['linkvec'] = np.array([0, 0, 392.25]) lftlj[3]['rotax'] = np.array([0, 1, 0]) lftlj[3]['rotangle'] = 0 lftlj[3]['rotmat'] = np.dot( lftlj[2]['rotmat'], rm.rodrigues(lftlj[3]['rotax'], lftlj[3]['rotangle'])) lftlj[3]['linkend'] = np.dot(lftlj[3]['rotmat'], lftlj[3]['linkvec']) + lftlj[3]['linkpos'] lftlj[3]['rngmin'] = -(360 - rngsafemargin) lftlj[3]['rngmax'] = +(360 - rngsafemargin) # the 4th joint and link lftlj[4]['name'] = 'link4' lftlj[4]['mother'] = 3 lftlj[4]['child'] = 5 lftlj[4]['linkpos'] = lftlj[3]['linkend'] lftlj[4]['linkvec'] = np.array([0, 93.00, 0]) lftlj[4]['rotax'] = np.array([0, 1, 0]) lftlj[4]['rotangle'] = 0 lftlj[4]['inherentR'] = rm.rodrigues([0, 1, 0], 90) lftlj[4]['rotmat'] = np.dot(np.dot(lftlj[3]['rotmat'], lftlj[4]['inherentR']), \ rm.rodrigues(lftlj[4]['rotax'], lftlj[4]['rotangle'])) lftlj[4]['linkend'] = np.dot(lftlj[4]['rotmat'], lftlj[4]['linkvec']) + lftlj[4]['linkpos'] lftlj[4]['rngmin'] = -(360 - rngsafemargin) lftlj[4]['rngmax'] = +(360 - rngsafemargin) # the 5th joint and link lftlj[5]['name'] = 'link5' lftlj[5]['mother'] = 4 lftlj[5]['child'] = 6 lftlj[5]['linkpos'] = lftlj[4]['linkend'] lftlj[5]['linkvec'] = np.array([0, 82.30, 94.65]) lftlj[5]['rotax'] = np.array([0, 0, 1]) lftlj[5]['rotangle'] = 0 lftlj[5]['rotmat'] = np.dot( lftlj[4]['rotmat'], rm.rodrigues(lftlj[5]['rotax'], lftlj[5]['rotangle'])) lftlj[5]['linkend'] = np.dot(lftlj[5]['rotmat'], lftlj[5]['linkvec']) + lftlj[5]['linkpos'] lftlj[5]['rngmin'] = -(360 - rngsafemargin) lftlj[5]['rngmax'] = +(360 - rngsafemargin) # the 6th joint and link lftlj[6]['name'] = 'link6' lftlj[6]['mother'] = 5 lftlj[6]['child'] = -1 lftlj[6]['linkpos'] = lftlj[5]['linkend'] # for hrp5three # lftlj[6]['linkvec'] = np.array([-172.7,0,0]) # for rtq85 lftlj[6]['linkvec'] = np.array([-13.5 - 145.0, 0, 0]) lftlj[6]['rotax'] = np.array([1, 0, 0]) lftlj[6]['rotangle'] = 0 lftlj[6]['inherentR'] = rm.rodrigues([0, 0, 1], -90) lftlj[6]['rotmat'] = np.dot(np.dot(lftlj[5]['rotmat'], lftlj[6]['inherentR']), \ rm.rodrigues(lftlj[6]['rotax'], lftlj[6]['rotangle'])) lftlj[6]['linkend'] = np.dot(lftlj[6]['rotmat'], lftlj[6]['linkvec']) + lftlj[6]['linkpos'] lftlj[6]['rngmin'] = -(360 - rngsafemargin) lftlj[6]['rngmax'] = +(360 - rngsafemargin) return lftlj
def __initlftarmj(self): """ Init hrp5's lft arm links and joints ## note x is facing forward, y is facing left, z is facing upward each element of lftlj is a dictionary lftlj[i]['linkpos'] indicates the position of a link lftlj[i]['linkvec'] indicates the vector of a link that points from start to end lftlj[i]['rotmat'] indicates the frame of this link lftlj[i]['rotax'] indicates the rotation axis of the link lftlj[i]['rotangle'] indicates the rotation angle of the link around the rotax lftlj[i]['linkend'] indicates the end position of the link (passively computed) lftlj[i]['inherentR'] is the inherent rotation of the joint lftlj[i]['refcs'] is the reference coordinate system of the joint ## more note: lftlj[1]['linkpos'] is the position of the first joint lftlj[i]['linkend'] is the same as lftlj[i+1]['linkpos'], I am keeping this value for the eef (end-effector) ## even more note: joint is attached to the linkpos of a link for the first link, the joint is fixed and the rotax = 0,0,0 :return: lftlj: a list of dictionaries with each dictionary holding name, mother, child, linkpos (link position), linkvec (link vector), rotmat (orientation), rotax (rotation axis of a joint), rotangle (rotation angle of the joint around rotax) linkend (the end position of a link) is computed using rotmat, rotax, linkpos, and linklen lj means link and joint, the joint attached to the link is at the linkend author: weiwei date: 20161202, tsukuba, 20190328, toyonaka """ # create a arm with 6 joints lftlj = [dict() for i in range(7)] rngsafemargin = 0 # the 0th link and joint lftlj[0]['name'] = 'link0' lftlj[0]['mother'] = -1 lftlj[0]['child'] = 1 lftlj[0]['linkpos'] = self.__pos lftlj[0]['linkvec'] = np.array([0, 258.485281374, 1610.51471863])+\ np.dot(rm.rodrigues([1,0,0], -135), np.array([0,0,0])) #89.159 lftlj[0]['rotax'] = np.array([0, 0, 1]) lftlj[0]['rotangle'] = 0 lftlj[0]['inherentR'] = self.__rotmat lftlj[0]['refcs'] = lftlj[0]['inherentR'] lftlj[0]['rotmat'] = np.dot( lftlj[0]['refcs'], rm.rodrigues(lftlj[0]['rotax'], lftlj[0]['rotangle'])) lftlj[0]['linkend'] = np.dot(lftlj[0]['rotmat'], lftlj[0]['linkvec']) + lftlj[0]['linkpos'] lftlj[0]['rngmin'] = -(0 - rngsafemargin) lftlj[0]['rngmax'] = +(0 - rngsafemargin) # the 1st joint and link lftlj[1]['name'] = 'link1' lftlj[1]['mother'] = 0 lftlj[1]['child'] = 2 lftlj[1]['linkpos'] = lftlj[0]['linkend'] lftlj[1]['linkvec'] = np.array([0, -119.8, 151.9]) lftlj[1]['rotax'] = np.array([0, 0, 1]) lftlj[1]['rotangle'] = 0 lftlj[1]['inherentR'] = rm.rodrigues([1, 0, 0], -135) lftlj[1]['refcs'] = np.dot(lftlj[0]['rotmat'], lftlj[1]['inherentR']) lftlj[1]['rotmat'] = np.dot( lftlj[1]['refcs'], rm.rodrigues(lftlj[1]['rotax'], lftlj[1]['rotangle'])) lftlj[1]['linkend'] = np.dot(lftlj[1]['rotmat'], lftlj[1]['linkvec']) + lftlj[1]['linkpos'] lftlj[1]['rngmin'] = -(360 - rngsafemargin) lftlj[1]['rngmax'] = +(360 - rngsafemargin) # the 2nd joint and link lftlj[2]['name'] = 'link2' lftlj[2]['mother'] = 1 lftlj[2]['child'] = 3 lftlj[2]['linkpos'] = lftlj[1]['linkend'] lftlj[2]['linkvec'] = np.array([-243.65, 92.5, 0]) lftlj[2]['rotax'] = np.array([0, -1, 0]) lftlj[2]['rotangle'] = 0 lftlj[2]['inherentR'] = np.eye(3) lftlj[2]['refcs'] = np.dot(lftlj[1]['rotmat'], lftlj[2]['inherentR']) lftlj[2]['rotmat'] = np.dot( lftlj[2]['refcs'], rm.rodrigues(lftlj[2]['rotax'], lftlj[2]['rotangle'])) lftlj[2]['linkend'] = np.dot(lftlj[2]['rotmat'], lftlj[2]['linkvec']) + lftlj[2]['linkpos'] lftlj[2]['rngmin'] = -(360 - rngsafemargin) lftlj[2]['rngmax'] = +(360 - rngsafemargin) # the 3rd joint and link lftlj[3]['name'] = 'link3' lftlj[3]['mother'] = 2 lftlj[3]['child'] = 4 lftlj[3]['linkpos'] = lftlj[2]['linkend'] lftlj[3]['linkvec'] = np.array([-213.25, 0, 0]) lftlj[3]['rotax'] = np.array([0, -1, 0]) lftlj[3]['rotangle'] = 0 lftlj[3]['inherentR'] = np.eye(3) lftlj[3]['refcs'] = np.dot(lftlj[2]['rotmat'], lftlj[3]['inherentR']) lftlj[3]['rotmat'] = np.dot( lftlj[3]['refcs'], rm.rodrigues(lftlj[3]['rotax'], lftlj[3]['rotangle'])) lftlj[3]['linkend'] = np.dot(lftlj[3]['rotmat'], lftlj[3]['linkvec']) + lftlj[3]['linkpos'] lftlj[3]['rngmin'] = -(360 - rngsafemargin) lftlj[3]['rngmax'] = +(360 - rngsafemargin) # the 4th joint and link lftlj[4]['name'] = 'link4' lftlj[4]['mother'] = 3 lftlj[4]['child'] = 5 lftlj[4]['linkpos'] = lftlj[3]['linkend'] lftlj[4]['linkvec'] = np.array([0, -85.35, 0]) lftlj[4]['rotax'] = np.array([0, -1, 0]) lftlj[4]['rotangle'] = 0 lftlj[4]['inherentR'] = np.eye(3) lftlj[4]['refcs'] = np.dot(lftlj[3]['rotmat'], lftlj[4]['inherentR']) lftlj[4]['rotmat'] = np.dot( lftlj[4]['refcs'], rm.rodrigues(lftlj[4]['rotax'], lftlj[4]['rotangle'])) lftlj[4]['linkend'] = np.dot(lftlj[4]['rotmat'], lftlj[4]['linkvec']) + lftlj[4]['linkpos'] lftlj[4]['rngmin'] = -(360 - rngsafemargin) lftlj[4]['rngmax'] = +(360 - rngsafemargin) # the 5th joint and link lftlj[5]['name'] = 'link5' lftlj[5]['mother'] = 4 lftlj[5]['child'] = 6 lftlj[5]['linkpos'] = lftlj[4]['linkend'] lftlj[5]['linkvec'] = np.array([0, -81.9, -85]) lftlj[5]['rotax'] = np.array([0, 0, -1]) lftlj[5]['rotangle'] = 0 lftlj[5]['inherentR'] = np.eye(3) lftlj[5]['refcs'] = np.dot(lftlj[4]['rotmat'], lftlj[5]['inherentR']) lftlj[5]['rotmat'] = np.dot( lftlj[5]['refcs'], rm.rodrigues(lftlj[5]['rotax'], lftlj[5]['rotangle'])) lftlj[5]['linkend'] = np.dot(lftlj[5]['rotmat'], lftlj[5]['linkvec']) + lftlj[5]['linkpos'] lftlj[5]['rngmin'] = -(360 - rngsafemargin) lftlj[5]['rngmax'] = +(360 - rngsafemargin) # the 6th joint and link lftlj[6]['name'] = 'link6' lftlj[6]['mother'] = 5 lftlj[6]['child'] = -1 lftlj[6]['linkpos'] = lftlj[5]['linkend'] lftlj[6]['linkvec'] = np.array([0, 0, 195.0]) lftlj[6]['rotax'] = np.array([0, 0, 1]) lftlj[6]['rotangle'] = 0 lftlj[6]['inherentR'] = rm.rodrigues([1, 0, 0], 90) lftlj[6]['refcs'] = np.dot(lftlj[5]['rotmat'], lftlj[6]['inherentR']) lftlj[6]['rotmat'] = np.dot( lftlj[6]['refcs'], rm.rodrigues(lftlj[6]['rotax'], lftlj[6]['rotangle'])) lftlj[6]['linkend'] = np.dot(lftlj[6]['rotmat'], lftlj[6]['linkvec']) + lftlj[6]['linkpos'] lftlj[6]['rngmin'] = -(360 - rngsafemargin) lftlj[6]['rngmax'] = +(360 - rngsafemargin) return lftlj
return bulletboxesnode def genBulletCDBox(obstaclecm, name='autogen'): """ generate a bullet cd obj using a list of AABB box generated from obstaclenp :param obstaclenp: the AABB box of the obstaclenp will be computed and used for cd :return: bulletrigidbody author: weiwei date: 20190126, osaka """ bulletboxesnode = BulletRigidBodyNode(name) bulletboxshape = BulletBoxShape.makeFromSolid(obstaclecm.cdcn.getSolid(0)) bulletboxesnode.addShape(bulletboxshape, TransformState.makeMat(obstaclecm.getMat())) return bulletboxesnode if __name__ == "main": import numpy as np import pandaplotutils.pandageom as pg import trimesh.primitives as tp import utiltools.robotmath as rm tribox = pg.trimeshToPanda( tp.Box(box_center=np.array([0, 0, 0]), box_transform=rm.rodrigues([0, 0, 1], 30)))
if __name__ == '__main__': import robothelper import utiltools.misc.p3dhtils as p3dh import environment.collisionmodel as cm import manipulation.grip.freegrip as fg import manipulation.grip.yumiintegrated.yumiintegrated as yi import trimesh pg = PcdGrab() rhx = robothelper.RobotHelperX(usereal=True, startworld=True) eepos = np.array([400, 0, 200]) eerot = np.array([[1, 0, 0], [0, 0, -1], [0, 1, 0]]).T armjnts = rhx.movetoposrot(eepos=eepos, eerot=eerot, armname="rgt") rhx.movetox(armjnts, "rgt") nppcd1 = pg.capturecorrectedpcd(pxc=rhx.pxc, id=1) - eepos eerot2 = np.dot(rm.rodrigues([0, 1, 0], 90), eerot) armjnts = rhx.movetoposrot(eepos=eepos, eerot=eerot2, armname="rgt") rhx.movetox(armjnts, "rgt") nppcd2 = pg.capturecorrectedpcd(pxc=rhx.pxc, id=2) - eepos eerot3 = np.dot(rm.rodrigues([0, 1, 0], 180), eerot) armjnts = rhx.movetoposrot(eepos=eepos, eerot=eerot3, armname="rgt") rhx.movetox(armjnts, "rgt") nppcd3 = pg.capturecorrectedpcd(pxc=rhx.pxc, id=3) - eepos eerot4 = np.dot(rm.rodrigues([0, 1, 0], 270), eerot) armjnts = rhx.movetoposrot(eepos=eepos, eerot=eerot4, armname="rgt") rhx.movetox(armjnts, "rgt") nppcd4 = pg.capturecorrectedpcd(pxc=rhx.pxc, id=4) - eepos mergednppcd = nppcd4 mergedlistnrmls = [[0, 0, 1]] * len(mergednppcd) mergednppcd = o3dh.merge_pcd(mergednppcd,
def __init__(self, jawwidth=50, hndcolor=None, ftsensoroffset=52): """ load the robotiq85 model, set jawwidth and return a nodepath ## input pandabase: the showbase() object jawwidth: the distance between fingertips ftsensoroffset: the offset for ftsensor ## output sck918np: the nodepath of this rtq85 hand author: wangyan date: 20190413 """ self.sck918np = NodePath("sck918hnd") self.handnp = self.sck918np self.__ftsensoroffset = ftsensoroffset self.jawwidth = jawwidth this_dir, this_filename = os.path.split(__file__) sck918basepath = Filename.fromOsSpecific( os.path.join(this_dir, "schunk918egg", "sck918_base.egg")) sck918sliderpath = Filename.fromOsSpecific( os.path.join(this_dir, "schunk918egg", "sck918_slider.egg")) sck918gripperpath = Filename.fromOsSpecific( os.path.join(this_dir, "schunk918egg", "sck918_gripper.egg")) sck918base = NodePath("sck918base") sck918lslider = NodePath("sck918lslider") sck918rslider = NodePath("sck918rslider") sck918lgripper = NodePath("sck918lgripper") sck918rgripper = NodePath("sck918rgripper") # loader is a global variable defined by panda3d sck918_basel = loader.loadModel(sck918basepath) sck918_sliderl = loader.loadModel(sck918sliderpath) sck918_gripperl = loader.loadModel(sck918gripperpath) # base sck918_basel.instanceTo(sck918base) if hndcolor is None: # rtq85base.setColor(.2,.2,.2,1) pass else: sck918base.setColor(hndcolor[0], hndcolor[1], hndcolor[2], hndcolor[3]) sck918base.setTransparency(TransparencyAttrib.MAlpha) # sck918base.setColor(.2, .2, .2, 1.0) sck918base.setHpr(90, -90, 0) # left and right outer knuckle sck918_sliderl.instanceTo(sck918lslider) sck918lslider.setColor(.1, .1, .1, 1.0) sck918lslider.setPos(-10, 40, 73) sck918lslider.setHpr(-90, 0, 0) sck918lslider.reparentTo(sck918base) sck918_sliderl.instanceTo(sck918rslider) sck918rslider.setColor(.1, .1, .1, 1.0) sck918rslider.setPos(10, -40, 73) sck918rslider.setHpr(90, 0, 0) sck918rslider.reparentTo(sck918base) # left and right finger sck918_gripperl.instanceTo(sck918lgripper) sck918lgripper.setColor(.7, .7, .7, 1.0) sck918lgripper.setPos(-8, 20, 0) sck918lgripper.setHpr(0, 180, 0) sck918lgripper.reparentTo(sck918lslider) sck918_gripperl.instanceTo(sck918rgripper) sck918rgripper.setColor(.7, .7, .7, 1.0) sck918rgripper.setPos(-8, 20, 0) sck918rgripper.setHpr(0, 180, 0) sck918rgripper.reparentTo(sck918rslider) # rotate to x, y, z coordinates (this one rotates the base, not the self.rtq85np) # the default x direction is facing the ee, the default z direction is facing downward # execute this file to see the default pose sck918base.setTransparency(TransparencyAttrib.MAlpha) sck918base.setMat( pandageom.npToMat4(rm.rodrigues([0, 0, 1], 90)) * pandageom.npToMat4(rm.rodrigues([1, 0, 0], 90)) * sck918base.getMat()) sck918base.setPos(0, 0, self.__ftsensoroffset) sck918base.reparentTo(self.sck918np) self.setJawwidth(jawwidth) self.__jawwidthopen = 50.0 self.__jawwidthclosed = 0.0
def __init__(self, jawwidth=85, ftsensoroffset=52): ''' load the robotiq85 model, set jawwidth and return a nodepath the rtq85 gripper is composed of a parallelism and a fixed triangle, the parallelism: 1.905-1.905; 5.715-5.715; 70/110 degree the triangle: 4.75 (finger) 5.715 (inner knuckle) 3.175 (outer knuckle) ## input pandabase: the showbase() object jawwidth: the distance between fingertips ftsensoroffset: the offset for ftsensor ## output rtq85np: the nodepath of this rtq85 hand author: weiwei date: 20160627 ''' self.rtq85np = NodePath("rtq85hnd") self.handnp = self.rtq85np self.__ftsensoroffset = ftsensoroffset self.jawwidth = jawwidth this_dir, this_filename = os.path.split(__file__) rtq85basepath = Filename.fromOsSpecific( os.path.join(this_dir, "rtq85egg", "robotiq_85_base_link.egg")) rtq85fingerpath = Filename.fromOsSpecific( os.path.join(this_dir, "rtq85egg", "robotiq_85_finger_link.egg")) rtq85fingertippath = Filename.fromOsSpecific( os.path.join(this_dir, "rtq85egg", "robotiq_85_finger_tip_link.egg")) rtq85innerknucklepath = Filename.fromOsSpecific( os.path.join(this_dir, "rtq85egg", "robotiq_85_inner_knuckle_link.egg")) rtq85knucklepath = Filename.fromOsSpecific( os.path.join(this_dir, "rtq85egg", "robotiq_85_knuckle_link.egg")) rtq85base = NodePath("rtq85base") rtq85lknuckle = NodePath("rtq85lknuckle") rtq85rknuckle = NodePath("rtq85rknuckle") rtq85lfgr = NodePath("rtq85lfgr") rtq85rfgr = NodePath("rtq85rfgr") rtq85ilknuckle = NodePath("rtq85ilknuckle") rtq85irknuckle = NodePath("rtq85irknuckle") rtq85lfgrtip = NodePath("rtq85lfgrtip") rtq85rfgrtip = NodePath("rtq85rfgrtip") # loader is a global variable defined by panda3d rtq85_basel = loader.loadModel(rtq85basepath) rtq85_fingerl = loader.loadModel(rtq85fingerpath) rtq85_fingertipl = loader.loadModel(rtq85fingertippath) rtq85_innerknucklel = loader.loadModel(rtq85innerknucklepath) rtq85_knucklel = loader.loadModel(rtq85knucklepath) # base rtq85_basel.instanceTo(rtq85base) # left and right outer knuckle rtq85_knucklel.instanceTo(rtq85lknuckle) rtq85lknuckle.setPos(-30.60114443, 54.90451627, 0) rtq85lknuckle.setHpr(0, 0, 180) rtq85lknuckle.reparentTo(rtq85base) rtq85_knucklel.instanceTo(rtq85rknuckle) rtq85rknuckle.setPos(30.60114443, 54.90451627, 0) rtq85rknuckle.setHpr(0, 0, 0) rtq85rknuckle.reparentTo(rtq85base) # left and right finger rtq85_fingerl.instanceTo(rtq85lfgr) rtq85lfgr.setPos(31.48504435, -4.08552455, 0) rtq85lfgr.reparentTo(rtq85lknuckle) rtq85_fingerl.instanceTo(rtq85rfgr) rtq85rfgr.setPos(31.48504435, -4.08552455, 0) rtq85rfgr.reparentTo(rtq85rknuckle) # left and right inner knuckle rtq85_innerknucklel.instanceTo(rtq85ilknuckle) rtq85ilknuckle.setPos(-12.7, 61.42, 0) rtq85ilknuckle.setHpr(0, 0, 180) rtq85ilknuckle.reparentTo(rtq85base) rtq85_innerknucklel.instanceTo(rtq85irknuckle) rtq85irknuckle.setPos(12.7, 61.42, 0) rtq85irknuckle.setHpr(0, 0, 0) rtq85irknuckle.reparentTo(rtq85base) # left and right fgr tip rtq85_fingertipl.instanceTo(rtq85lfgrtip) rtq85lfgrtip.setPos(37.59940821, 43.03959807, 0) rtq85lfgrtip.reparentTo(rtq85ilknuckle) rtq85_fingertipl.instanceTo(rtq85rfgrtip) rtq85rfgrtip.setPos(37.59940821, 43.03959807, 0) rtq85rfgrtip.setHpr(0, 0, 0) rtq85rfgrtip.reparentTo(rtq85irknuckle) # rotate to x, y, z coordinates (this one rotates the base, not the self.rtq85np) # the default x direction is facing the ee, the default z direction is facing downward # execute this file to see the default pose rtq85base.setMat( base.pg.npToMat4(rm.rodrigues([1, 0, 0], 90)) * rtq85base.getMat()) rtq85base.setPos(0, 0, self.__ftsensoroffset) rtq85base.reparentTo(self.rtq85np) self.setJawwidth(jawwidth) self.__jawwidthopen = 85.0 self.__jawwidthclosed = 0.0
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 __init__(self, jawwidth=85, hndcolor=None, ftsensoroffset = 52): ''' load the robotiq85 model, set jawwidth and return a nodepath the rtq85 gripper is composed of a parallelism and a fixed triangle, the parallelism: 1.905-1.905; 5.715-5.715; 70/110 degree the triangle: 4.75 (finger) 5.715 (inner knuckle) 3.175 (outer knuckle) NOTE: the setColor function is only useful when the models dont have any materials ## input pandabase: the showbase() object jawwidth: the distance between fingertips ftsensoroffset: the offset for ftsensor ## output rtq85np: the nodepath of this rtq85 hand author: weiwei date: 20160627 ''' self.rtq85np = NodePath("rtq85hnd") self.jawwidth = jawwidth self.__ftsensoroffset = ftsensoroffset this_dir, this_filename = os.path.split(__file__) rtq85basepath = Filename.fromOsSpecific(os.path.join(this_dir, "rtq85egg/nomat", "robotiq_85_base_link_nm.egg")) rtq85fingerpath = Filename.fromOsSpecific(os.path.join(this_dir, "rtq85egg/nomat", "robotiq_85_finger_link_nm.egg")) rtq85fingertippath = Filename.fromOsSpecific(os.path.join(this_dir, "rtq85egg/nomat", "robotiq_85_finger_tip_link_nm.egg")) rtq85innerknucklepath = Filename.fromOsSpecific(os.path.join(this_dir, "rtq85egg/nomat", "robotiq_85_inner_knuckle_link_nm.egg")) rtq85knucklepath = Filename.fromOsSpecific(os.path.join(this_dir, "rtq85egg/nomat", "robotiq_85_knuckle_link_nm.egg")) rtq85base = NodePath("rtq85base") rtq85lknuckle = NodePath("rtq85lknuckle") rtq85rknuckle = NodePath("rtq85rknuckle") rtq85lfgr = NodePath("rtq85lfgr") rtq85rfgr = NodePath("rtq85rfgr") rtq85ilknuckle = NodePath("rtq85ilknuckle") rtq85irknuckle = NodePath("rtq85irknuckle") rtq85lfgrtip = NodePath("rtq85lfgrtip") rtq85rfgrtip = NodePath("rtq85rfgrtip") # loader is a global variable defined by panda3d rtq85_basel = loader.loadModel(rtq85basepath) rtq85_fingerl = loader.loadModel(rtq85fingerpath) rtq85_fingertipl = loader.loadModel(rtq85fingertippath) rtq85_innerknucklel = loader.loadModel(rtq85innerknucklepath) rtq85_knucklel = loader.loadModel(rtq85knucklepath) # base rtq85_basel.instanceTo(rtq85base) if hndcolor is None: # rtq85base.setColor(.2,.2,.2,1) pass else: rtq85base.setColor(hndcolor[0],hndcolor[1],hndcolor[2],hndcolor[3]) rtq85base.setTransparency(TransparencyAttrib.MAlpha) # left and right outer knuckle rtq85_knucklel.instanceTo(rtq85lknuckle) rtq85lknuckle.setPos(-30.60114443, 54.90451627, 0) rtq85lknuckle.setHpr(0, 0, 180) if hndcolor is None: # rtq85lknuckle.setColor(.5,.5,.5,1) pass else: rtq85lknuckle.setColor(hndcolor[0],hndcolor[1],hndcolor[2],hndcolor[3]) rtq85lknuckle.setTransparency(TransparencyAttrib.MAlpha) rtq85lknuckle.reparentTo(rtq85base) rtq85_knucklel.instanceTo(rtq85rknuckle) rtq85rknuckle.setPos(30.60114443, 54.90451627, 0) rtq85rknuckle.setHpr(0, 0, 0) if hndcolor is None: # rtq85rknuckle.setColor(.5,.5,.5,1) pass else: rtq85rknuckle.setColor(hndcolor[0],hndcolor[1],hndcolor[2],hndcolor[3]) rtq85rknuckle.setTransparency(TransparencyAttrib.MAlpha) rtq85rknuckle.reparentTo(rtq85base) # left and right finger rtq85_fingerl.instanceTo(rtq85lfgr) rtq85lfgr.setPos(31.48504435, -4.08552455, 0) if hndcolor is None: # rtq85lfgr.setColor(0.2,0.2,0.2,1) pass else: rtq85lfgr.setColor(hndcolor[0],hndcolor[1],hndcolor[2],hndcolor[3]) rtq85lfgr.setTransparency(TransparencyAttrib.MAlpha) rtq85lfgr.reparentTo(rtq85lknuckle) rtq85_fingerl.instanceTo(rtq85rfgr) rtq85rfgr.setPos(31.48504435, -4.08552455, 0) if hndcolor is None: # rtq85rfgr.setColor(0.2,0.2,0.2,1) pass else: rtq85rfgr.setColor(hndcolor[0],hndcolor[1],hndcolor[2],hndcolor[3]) rtq85rfgr.setTransparency(TransparencyAttrib.MAlpha) rtq85rfgr.reparentTo(rtq85rknuckle) # left and right inner knuckle rtq85_innerknucklel.instanceTo(rtq85ilknuckle) rtq85ilknuckle.setPos(-12.7, 61.42, 0) rtq85ilknuckle.setHpr(0, 0, 180) if hndcolor is None: # rtq85ilknuckle.setColor(.5,.5,.5,1) pass else: rtq85ilknuckle.setColor(hndcolor[0],hndcolor[1],hndcolor[2],hndcolor[3]) rtq85ilknuckle.setTransparency(TransparencyAttrib.MAlpha) rtq85ilknuckle.reparentTo(rtq85base) rtq85_innerknucklel.instanceTo(rtq85irknuckle) rtq85irknuckle.setPos(12.7, 61.42, 0) rtq85irknuckle.setHpr(0, 0, 0) if hndcolor is None: # rtq85irknuckle.setColor(.5,.5,.5,1) pass else: rtq85irknuckle.setColor(hndcolor[0],hndcolor[1],hndcolor[2],hndcolor[3]) rtq85irknuckle.setTransparency(TransparencyAttrib.MAlpha) rtq85irknuckle.reparentTo(rtq85base) # left and right fgr tip rtq85_fingertipl.instanceTo(rtq85lfgrtip) rtq85lfgrtip.setPos(37.59940821, 43.03959807, 0) if hndcolor is None: # rtq85lfgrtip.setColor(.5,.5,.5,1) pass else: rtq85lfgrtip.setColor(hndcolor[0],hndcolor[1],hndcolor[2],hndcolor[3]) rtq85lfgrtip.setTransparency(TransparencyAttrib.MAlpha) rtq85lfgrtip.reparentTo(rtq85ilknuckle) rtq85_fingertipl.instanceTo(rtq85rfgrtip) rtq85rfgrtip.setPos(37.59940821, 43.03959807, 0) rtq85rfgrtip.setHpr(0, 0, 0) if hndcolor is None: # rtq85rfgrtip.setColor(.5,.5,.5,1) pass else: rtq85rfgrtip.setColor(hndcolor[0],hndcolor[1],hndcolor[2],hndcolor[3]) rtq85rfgrtip.setTransparency(TransparencyAttrib.MAlpha) rtq85rfgrtip.reparentTo(rtq85irknuckle) # rotate to x, y, z coordinates (this one rotates the base, not the self.rtq85np) rtq85base.setMat(pandageom.npToMat4(rm.rodrigues([1,0,0], 90))*rtq85base.getMat()) rtq85base.setPos(0,0,self.__ftsensoroffset) rtq85base.reparentTo(self.rtq85np) self.setJawwidth(jawwidth) self.__jawwidthopen = 85.0 self.__jawwidthclosed = 0.0
base = pandactrl.World(camp=[2700, 300, 2700], lookatp=[0, 0, 1000]) env = Env() env.reparentTo(base.render) objcm = env.loadobj("bunnysim.stl") objcm.setColor(.2, .5, 0, 1) objcm.setPos(400, -200, 1200) objcm.reparentTo(base.render) objcm.showcn() obscmlist = env.getstationaryobslist() for obscm in obscmlist: obscm.showcn() objpos = np.array([400, -300, 1200]) 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) robotsim = robotsim.NxtRobot() rgthnd = rtq85nm.newHand(hndid="rgt", ftsensoroffset=0) lfthnd = rtq85nm.newHand(hndid="lft", ftsensoroffset=0) robotmeshgen = robotmesh.NxtMesh(rgthand=rgthnd, lfthand=lfthnd) robotmesh = robotmeshgen.genmnp(robotsim, toggleendcoord=False) robotmesh.reparentTo(base.render) base.pggen.plotAxis(base.render, spos=Vec3(400, -300, 900)) base.pggen.plotAxis(base.render, spos=Vec3(300, 0, 1300)) base.run()
base.pggen.plotAxis(base.render, length=30, thickness=2) objpath = "./objects/tubelarge.stl" obj = cm.CollisionModel(objinit=objpath) obj.setColor(.8, .6, .3, .5) obj.reparentTo(base.render) hndfa = yi.YumiIntegratedFactory() pregrasps = [] yihnd = hndfa.genHand() c0nvec = np.array([0, -1, 0]) approachvec = np.array([1, 0, 0]) for z in [60, 90]: for anglei in [0, 22.5, 45, 67.5, 90, 112.5, 135, 157.5]: newcv = np.dot(rm.rodrigues((0, 0, 1), anglei), c0nvec) tempav = np.dot(rm.rodrigues((0, 0, 1), anglei), approachvec) for anglej in [ 0, 22.5, 45, 67.5, 90, 112.5, 135, 157.5, 180, 202.5, 225, 247.5, 270, 292.5, 315, 337.5 ]: newyihand = yihnd.copy() newav = np.dot(rm.rodrigues(newcv, anglej), tempav) base.pggen.plotAxis(newyihand.hndnp, length=15, thickness=2) pregrasps.append( newyihand.approachAt(0, 0, z, newcv[0], newcv[1], newcv[2],
def phoxi_computeeeinphx(yhx, pxc, armname, actionpos, actionrot, parameters, aruco_dict, criteriaradius=None): """ :param yhx: :param pxc: :param armname: :param actionpos: :param actionrot: :param parameters: :param aruco_dict: :param criteriaradius: rough radius used to determine if the newly estimated center is correct or not :return: author: weiwei date: 20190110 """ def fitfunc(p, coords): x0, y0, z0, R = p x, y, z = coords.T return np.sqrt((x - x0)**2 + (y - y0)**2 + (z - z0)**2) errfunc = lambda p, x: fitfunc(p, x) - p[3] coords = [] rangex = [np.array([1, 0, 0]), [-30, -15, 0, 15, 30]] rangey = [np.array([0, 1, 0]), [-30, -15, 15, 30]] rangez = [np.array([0, 0, 1]), [-90, -60, -30, 30, 60]] rangeaxis = [rangex, rangey, rangez] lastarmjnts = yhx.robot_s.initrgtjnts for axisid in range(3): axis = rangeaxis[axisid][0] for angle in rangeaxis[axisid][1]: goalpos = actionpos goalrot = np.dot(rm.rodrigues(axis, angle), actionrot) armjnts = yhx.movetoposrotmsc(eepos=goalpos, eerot=goalrot, msc=lastarmjnts, armname=armname) if armjnts is not None and not yhx.pcdchecker.isSelfCollided( yhx.robot_s): lastarmjnts = armjnts yhx.movetox(armjnts, armname=armname) pxc.triggerframe() img = pxc.gettextureimg() pcd = pxc.getpcd() phxpos = getcenter(img, pcd, aruco_dict, parameters) if phxpos is not None: coords.append(phxpos) print(coords) if len(coords) < 3: return [None, None] # for coord in coords: # yhx.p3dh.gensphere(coord, radius=5).reparentTo(base.render) coords = np.asarray(coords) # try: initialguess = np.ones(4) initialguess[:3] = np.mean(coords, axis=0) finalestimate, flag = leastsq(errfunc, initialguess, args=(coords, )) if len(finalestimate) == 0: return [None, None] print(finalestimate) print(np.linalg.norm(coords - finalestimate[:3], axis=1)) # yhx.p3dh.gensphere(finalestimate[:3], rgba=np.array([0,1,0,1]), radius=5).reparentTo(base.render) # yhx.base.run() # except: # return [None, None] if criteriaradius is not None: if abs(finalestimate[3] - criteriaradius) > 5: return [None, None] return np.array(finalestimate[:3]), finalestimate[3]
objstpos = np.array([354.5617667638, -256.0889005661, 1090.5]) objstrot = np.array([[1.0, 0.0, 0.0], [0.0, 1.0, 0.0], [0.0, 0.0, 1.0]]).T objgpos = np.array([304.5617667638, -277.1026725769, 1101.0]) objgrot = np.array([[1.0, 0.0, 0.0], [0.0, 6.12323426293e-17, -1.0], [0.0, 1.0, 6.12323426293e-17]]).T objcmcopys = copy.deepcopy(objcm) objcmcopys.setColor(0.0, 0.0, 1.0, .3) objcmcopys.setMat(base.pg.npToMat4(npmat3=objstrot, npvec3=objstpos)) objcmcopys.reparentTo(base.render) startpos = np.array([354.5617667638, -256.0889005661, 1060.5]) startrot = np.array([[1, 0, 0], [0, -1, 0], [0, 0, -1]]).T goalpos3 = np.array([354.5617667638, -256.0889005661, 1150.5]) goalrot3 = np.array([[1, 0, 0], [0, -0.92388, -0.382683], [0, 0.382683, -0.92388]]).T goalrot3 = np.dot(rm.rodrigues([1, 0, 0], 20), goalrot3) goalpos3 = goalpos3 - 70.0 * goalrot3[:, 2] goalpos4 = np.array([154.5617667638, -500, 1050.5]) goalrot4 = np.dot(rm.rodrigues([1, 0, 0], -90), goalrot3) goalpos4 = goalpos4 - 150.0 * goalrot4[:, 2] # goalpos2 = goalpos2 - 150.0*goalrot2[:,2] start = robot.numik(startpos, startrot, armname) goal3 = robot.numikmsc(goalpos3, goalrot3, msc=start, armname=armname) print(goal3) goal4 = robot.numikmsc(goalpos4, goalrot4, msc=start, armname=armname) print(goal4) planner = rrtc.RRTConnect(start=goal3, goal=goal4, ctcallback=ctcallback, starttreesamplerate=starttreesamplerate, endtreesamplerate=endtreesamplerate,
reduceradius=30, discretesize=8, torqueresist=100) # freesuctst.showfacets(togglesamples=True, togglenormals=False, # togglesamples_ref=True, togglenormals_ref=False, # togglesamples_refcls=True, togglenormals_refcls=False, specificfacet=True) p3dh.gensphere(pos=np.mean(freesuctst.objcm.objtrm.vertices, axis=0), radius=5, rgba=[1, 1, 1, 1]).reparentTo(rhx.base.render) print(len(freesuctst.sucrotmats_planned)) print(len(freesuctst.facets)) for i, homomat in enumerate(freesuctst.sucrotmats_planned): # homomat[:3,3] = homomat[:3,3]-homomat[:3,2]*120 homomatnew = np.copy(homomat) homomatnew[:3, 3] = homomat[:3, 3] - homomat[:3, 2] * 3 homomatnew[:3, :3] = np.dot(rm.rodrigues(homomat[:3, 0], 45), homomat[:3, :3]) # tmpef = effa.genendeffector() # tmpef.sethomomat(homomat) # tmpef.reparentTo(rhx.base.render) # tmpef.setcolor(1, 1, 1, .3) armjnts = rhx.movetopose(homomatnew, "lft") if armjnts is not None: # if i == 1: tmpef = effa.genendeffector() tmpef.set_homomat(homomat) tmpef.reparentTo(rhx.base.render) tmpef.set_rgba(1, 1, 1, .3) pos = homomat[:3, 3] rot = homomat[:3, :3] rhx.opengripperx("lft")
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