def update(rbtmnp, motioncounter, rbt, path, armname, rbtmg, objcm, task): global obj_ee_t if motioncounter[0] < len(path): if rbtmnp[0] is not None: rbtmnp[0].detachNode() pose = path[motioncounter[0]] rbt.movearmfk(pose, armname) rbtmnp[0] = rbtmg.genmnp(rbt, 0, obj.height) pos = rbt.lftarm[-1]['linkend'] rot = rbt.lftarm[-1]['rotmat'] ee_w_t[:3, :3] = rot ee_w_t[:3, 3] = pos obj_w_t = np.dot(ee_w_t, obj_ee_t) objcm.setMat(pg.npToMat4(obj_w_t[:3, :3])) objcm.setPos(obj_w_t[0, 3], obj_w_t[1, 3], obj_w_t[2, 3]) objcm.reparentTo(base.render) rbtmnp[0].reparentTo(base.render) motioncounter[0] += 1 else: motioncounter[0] = 0 return task.again
hrp5nplot.plotstick(base.render, hrp5nrobot) handpkg = hrp5threenm handpkg = rtq85nm hrp5nmeshgen = hrp5nmesh.Hrp5NMesh(handpkg) hrp5nmnp = hrp5nmeshgen.genmnp(hrp5nrobot) hrp5nmnp.reparentTo(base.render) objpos = np.array([500, -400, 305]) objrot = np.array([[-1, 0, 0], [0, 1, 0], [0, 0, -1]]) objrot = np.array([[0.125178158283, 0.00399381108582, 0.992126166821], [0.98617619276, -0.109927728772, -0.123984932899], [0.108567006886, 0.993931531906, -0.0176991540939]]).T objrot = np.array([[0, 1, 0], [0, 0, 1], [1, 0, 0]]) objrotmat4 = pg.npToMat4(objrot) objrotmat4 = objrotmat4 * Mat4.rotateMat(30, Vec3(1, 0, 0)) objrot = pg.mat3ToNp(objrotmat4.getUpper3()) objpos = np.array([401.67913818, -644.12841797, 0]) objrot = np.array([[1.93558640e-06, -8.36298645e-01, 5.48274219e-01], [1.93560686e-06, -5.48274219e-01, -8.36298645e-01], [1.00000000e+00, 2.67997166e-06, 5.57513317e-07]]) # lfthnd # objpos = np.array([180,130,100]) # objrot = np.array([[0,0,-1],[1,0,0],[0,-1,0]]) armid = "rgt" armjntsgoal = hrp5nrobot.numikr(objpos, objrot, armid) if armjntsgoal is not None: hrp5nrobot.movearmfkr(armjntsgoal, armid) hrp5nmnp = hrp5nmeshgen.genmnp(hrp5nrobot) # hrp5nmnp.reparentTo(base.render)
ur5mnp = ur5dualplot.genmnp(ur5dualrobot, handpkg) ur5mnp.reparentTo(base.render) # goal hand # from manipulation.grip.robotiq85 import rtq85nm # hrp5robotrgthnd = rtq85nm.Rtq85NM() # hrp5robotrgthnd.setColor([1,0,0,.3]) # hrp5robotrgtarmlj9_rotmat = pandageom.cvtMat4(objrot, objpos+objrot[:,0]*130) # pg.plotAxisSelf(base.render, objpos, hrp5robotrgtarmlj9_rotmat) # hrp5robotrgthnd.setMat(hrp5robotrgtarmlj9_rotmat) # hrp5robotrgthnd.reparentTo(base.render) # # angle = nxtik.eurgtbik(objpos) # nxtrobot.movewaist(angle) # armjntsgoal=nxtik.numik(nxtrobot, objpos, objrot) # # # nxtplot.plotstick(base.render, nxtrobot) # pandamat4=Mat4() # pandamat4.setRow(3,Vec3(0,0,250)) # # pg.plotAxis(base.render, pandamat4) # # nxtplot.plotmesh(base, nxtrobot) # # pandageom.plotAxis(base.render, pandageom.cvtMat4(nxtrobot.rgtarm[6]['rotmat'], nxtrobot.rgtarm[6]['linkpos'])) # pg.plotDumbbell(base.render, objpos, objpos, rgba = [1,0,0,1]) pg.plotAxisSelf(base.render, objpos, pg.npToMat4(objrot)) # pg.plotArrow(base.render, hrp5robot.rgtarm[8]['linkpos'], hrp5robot.rgtarm[8]['linkpos']+hrp5robot.rgtarm[8]['rotax']*1000) # # # nxtrobot.movearmfk6(armjntsgoal) # # nxtmnp = nxtplot.genNxtmnp_nm(nxtrobot, plotcolor=[1,0,0,1]) # # nxtmnp.reparentTo(base.render) base.run()
def __loadGripsToBuildGraph(self, armname = "rgt"): """ load tabletopgrips retraction distance are also loaded from database :param robot: an robot defined in robotsim.hrp5 or robotsim.nextage :param gdb: an object of the database.GraspDB class :param idarm: value = 1 "lft" or 2 "rgt", which arm to use :return: author: weiwei date: 20170112 """ # load idarm idarm = self.gdb.loadIdArm(armname) idhand = self.gdb.loadIdHand(self.handname) # get the global grip ids # and prepare the global edges # for each globalgripid, find all its tabletopids (pertaining to placements) globalidsedges = {} sql = "SELECT idfreeairgrip FROM freeairgrip,object WHERE freeairgrip.idobject=object.idobject AND \ object.name LIKE '%s' AND freeairgrip.idhand = %d" % (self.dbobjname, idhand) # sql = "SELECT dropfreegrip.iddropfreegrip FROM dropfreegrip,object WHERE dropfreegrip.idobject=object.idobject AND \ # object.name LIKE '%s' AND dropfreegrip.idhand = %d" % (self.dbobjname, idhand) # sql = "SELECT dropworkcellgrip.iddropworkcellgrip FROM dropworkcellgrip,object WHERE dropworkcellgrip.idobject=object.idobject AND \ # object.name LIKE '%s' AND dropworkcellgrip.idhand = %d" % (self.dbobjname, idhand) result = self.gdb.execute(sql) if len(result) == 0: raise ValueError("Plan freeairgrip first!") for ggid in result: globalidsedges[str(ggid[0])] = [] self.globalgripids.append(ggid[0]) sql = "SELECT dropstablepos.iddropstablepos,dropstablepos.rot,dropstablepos.pos,angle_drop.value FROM \ dropstablepos,object,angle_drop WHERE \ dropstablepos.idobject=object.idobject AND \ object.name LIKE '%s' " % self.dbobjname result = self.gdb.execute(sql) if len(result) != 0: tpsrows = np.array(result) #self.angles = list([0.0]) self.angles = list(set(map(float, tpsrows[:, 3]))) # for plotting self.fttpsids = list(set(map(int, tpsrows[:, 0]))) self.nfttps = len(self.fttpsids) idrobot = self.gdb.loadIdRobot(self.robot) for i, idtps in enumerate(tpsrows[:,0]): sql = "SELECT dropworkcellgrip.iddropworkcellgrip, dropworkcellgrip.contactpnt0, dropworkcellgrip.contactpnt1, \ dropworkcellgrip.rotmat, dropworkcellgrip.jawwidth ,dropworkcellgrip.idfreeairgrip\ FROM dropworkcellgrip,dropfreegrip,freeairgrip,ik_drop\ WHERE \ dropworkcellgrip.iddropstablepos = %d \ AND dropworkcellgrip.iddropworkcellgrip=ik_drop.iddropworkcellgrip AND ik_drop.idrobot=%d AND ik_drop.idarm = %d AND\ ik_drop.feasibility='True' AND ik_drop.feasibility_handx='True' AND ik_drop.feasibility_handxworldz='True' \ AND ik_drop.feasibility_worlda='True' AND ik_drop.feasibility_worldaworldz='True' \ AND dropworkcellgrip.idfreeairgrip = freeairgrip.idfreeairgrip \ AND dropworkcellgrip.idhand = % d AND dropworkcellgrip.iddropfreegrip = dropfreegrip.iddropfreegrip " \ % (int(idtps),idrobot, idarm, idhand) resultttgs = self.gdb.execute(sql) if len(resultttgs)==0: print "no result" continue localidedges = [] for ttgsrow in resultttgs: ttgsid = int(ttgsrow[0]) ttgscct0 = dc.strToV3(ttgsrow[1]) ttgscct1 = dc.strToV3(ttgsrow[2]) ttgsrotmat = dc.strToMat4(ttgsrow[3]) ttgsjawwidth = float(ttgsrow[4]) ttgsidfreeair = int(ttgsrow[5]) ttgsfgrcenter = (ttgscct0+ttgscct1)/2 handx = ttgsrotmat.getRow3(0) ttgsfgrcenterhandx = ttgsfgrcenter + handx*self.rethandx ttgsfgrcenterhandxworldz = ttgsfgrcenterhandx + self.worldz*self.retworldz ttgsfgrcenterworlda = ttgsfgrcenter + self.worlda*self.retworlda ttgsfgrcenterworldaworldz = ttgsfgrcenterworlda+ self.worldz*self.retworldz ttgsfgrcenternp = pg.v3ToNp(ttgsfgrcenter) ttgsfgrcenternp_handx = pg.v3ToNp(ttgsfgrcenterhandx) ttgsfgrcenternp_handxworldz = pg.v3ToNp(ttgsfgrcenterhandxworldz) ttgsfgrcenternp_worlda = pg.v3ToNp(ttgsfgrcenterworlda) ttgsfgrcenternp_worldaworldz = pg.v3ToNp(ttgsfgrcenterworldaworldz) ttgsrotmat3np = pg.mat3ToNp(ttgsrotmat.getUpper3()) objrotmat4 = pg.npToMat4(np.transpose(pg.mat3ToNp(dc.strToMat3(tpsrows[:, 1][i]))), pg.v3ToNp(dc.strToV3(tpsrows[:, 2][i]))) objrotmat4worlda = Mat4(objrotmat4) objrotmat4worlda.setRow(3, objrotmat4.getRow3(3)+self.worlda*self.retworlda) objrotmat4worldaworldz = Mat4(objrotmat4worlda) objrotmat4worldaworldz.setRow(3, objrotmat4worlda.getRow3(3)+self.worldz*self.retworldz) self.regg.add_node('mid' + str(ttgsid), fgrcenter=ttgsfgrcenternp, fgrcenterhandx = ttgsfgrcenternp_handx, fgrcenterhandxworldz = ttgsfgrcenternp_handxworldz, fgrcenterworlda = ttgsfgrcenternp_worlda, fgrcenterworldaworldz = ttgsfgrcenternp_worldaworldz, jawwidth=ttgsjawwidth, hndrotmat3np=ttgsrotmat3np, globalgripid = ttgsidfreeair, #freetabletopplacementid = int(tpsrows[:,2][i]), freetabletopplacementid=int(tpsrows[:, 0][i]), tabletopplacementrotmat = objrotmat4, tabletopplacementrotmathandx = objrotmat4, tabletopplacementrotmathandxworldz = objrotmat4, tabletopplacementrotmatworlda = objrotmat4worlda, tabletopplacementrotmatworldaworldz = objrotmat4worldaworldz, angle = float(tpsrows[:,3][i]), tabletopposition = dc.strToV3(tpsrows[:,2][i])) print "str(ttgsidfreeair),str(ttgsid)",str(ttgsidfreeair),str(ttgsid) globalidsedges[str(ttgsidfreeair)].append('mid' + str(ttgsid)) localidedges.append('mid' + str(ttgsid)) for edge in list(itertools.combinations(localidedges, 2)): self.regg.add_edge(*edge, weight=1, edgetype = 'transit') #toc = time.clock() #print "(toc - tic2)", (toc - tic) if len(globalidsedges) == 0: raise ValueError("Plan tabletopgrips first!") #tic = time.clock() for globalidedgesid in globalidsedges: for edge in list(itertools.combinations(globalidsedges[globalidedgesid], 2)): self.regg.add_edge(*edge, weight=1, edgetype = 'transfer') #toc = time.clock() #print "(toc - tic3)", (toc - tic) else: print ('No placements planned!') assert('No placements planned!')
def genmnp(self, robot, jawwidthrgt = 0, jawwidthlft = 0, toggleendcoord = True, togglejntscoord = False, name = 'robotmesh'): """ generate the mesh model of ur5 mnp means mesh nodepath :param robotjoints: the joint positions of ur5 :param toggleendcoord: whether to plot end coordinate systems :param togglejntscoord: whether to plot joints coordinate systems :return: a nodepath which is ready to be plotted using plotmesh author: weiwei date: 20180130 """ robotmesh_nodepath = NodePath(name) # robotmesh_nodepath.setMat(pg.npToMat4(robot.base['rotmat'], robot.base['linkpos'])) robotwaist_rotmat = pg.npToMat4(robot.base['rotmat'], npvec3=robot.base['linkpos']) self.__robotwaist_nodepath.setMat(robotwaist_rotmat) # rgt # base ur3rgtbase_rotmat = pg.npToMat4(robot.rgtarm[1]['rotmat'], robot.rgtarm[1]['linkpos']) self.__ur3rgtbase_nodepath.setMat(ur3rgtbase_rotmat) self.__ur3rgtbase_nodepath.setColor(.5,.5,.5,1) # shoulder ur3rgtshoulder_rotmat = pg.npToMat4(robot.rgtarm[1]['rotmat'], robot.rgtarm[1]['linkpos']) self.__ur3rgtshoulder_nodepath.setMat(ur3rgtshoulder_rotmat) self.__ur3rgtshoulder_nodepath.setColor(.1,.3,.5,1) # upperarm ur3rgtupperarm_rotmat = pg.npToMat4(robot.rgtarm[2]['rotmat'], robot.rgtarm[2]['linkpos']) self.__ur3rgtupperarm_nodepath.setMat(ur3rgtupperarm_rotmat) self.__ur3rgtupperarm_nodepath.setColor(.7,.7,.7,1) # forearm ur3rgtforearm_rotmat = pg.npToMat4(robot.rgtarm[3]['rotmat'], robot.rgtarm[3]['linkpos']) self.__ur3rgtforearm_nodepath.setMat(ur3rgtforearm_rotmat) self.__ur3rgtforearm_nodepath.setColor(.35,.35,.35,1) # wrist1 ur3rgtwrist1_rotmat = pg.npToMat4(robot.rgtarm[4]['rotmat'], robot.rgtarm[4]['linkpos']) self.__ur3rgtwrist1_nodepath.setMat(ur3rgtwrist1_rotmat) self.__ur3rgtwrist1_nodepath.setColor(.7,.7,.7,1) # wrist2 ur3rgtwrist2_rotmat = pg.npToMat4(robot.rgtarm[5]['rotmat'], robot.rgtarm[5]['linkpos']) self.__ur3rgtwrist2_nodepath.setMat(ur3rgtwrist2_rotmat) self.__ur3rgtwrist2_nodepath.setColor(.1,.3,.5,1) # wrist3 ur3rgtwrist3_rotmat = pg.npToMat4(robot.rgtarm[6]['rotmat'], robot.rgtarm[6]['linkpos']) self.__ur3rgtwrist3_nodepath.setMat(ur3rgtwrist3_rotmat) self.__ur3rgtwrist3_nodepath.setColor(.5,.5,.5,1) self.__robotwaist_nodepath.reparentTo(robotmesh_nodepath) self.__ur3rgtbase_nodepath.reparentTo(robotmesh_nodepath) self.__ur3rgtshoulder_nodepath.reparentTo(robotmesh_nodepath) self.__ur3rgtupperarm_nodepath.reparentTo(robotmesh_nodepath) self.__ur3rgtforearm_nodepath.reparentTo(robotmesh_nodepath) self.__ur3rgtwrist1_nodepath.reparentTo(robotmesh_nodepath) self.__ur3rgtwrist2_nodepath.reparentTo(robotmesh_nodepath) self.__ur3rgtwrist3_nodepath.reparentTo(robotmesh_nodepath) # endcoord if toggleendcoord: self.pggen.plotAxis(robotmesh_nodepath, spos = robot.rgtarm[-1]['linkend'], pandamat3 = pg.npToMat3(robot.rgtarm[-1]['rotmat'])) # toggle all coord if togglejntscoord: for i in range(1, 7): self.pggen.plotAxis(robotmesh_nodepath, spos=robot.rgtarm[i]['linkpos'], pandamat3=pg.npToMat3(robot.rgtarm[i]['refcs'])) # hand if self.rgthnd is not None: self.rgthnd.setMat(pg.npToMat4(robot.rgtarm[6]['rotmat'], robot.rgtarm[6]['linkpos'])) self.rgthnd.setJawwidth(jawwidthrgt) self.rgthnd.reparentTo(robotmesh_nodepath) #lft # base ur3lftbase_rotmat = pg.npToMat4(robot.lftarm[1]['rotmat'], robot.lftarm[1]['linkpos']) self.__ur3lftbase_nodepath.setMat(ur3lftbase_rotmat) self.__ur3lftbase_nodepath.setColor(.5,.5,.5,1) # shoulder ur3lftshoulder_rotmat = pg.npToMat4(robot.lftarm[1]['rotmat'], robot.lftarm[1]['linkpos']) self.__ur3lftshoulder_nodepath.setMat(ur3lftshoulder_rotmat) self.__ur3lftshoulder_nodepath.setColor(.1,.3,.5,1) # upperarm ur3lftupperarm_rotmat = pg.npToMat4(robot.lftarm[2]['rotmat'], robot.lftarm[2]['linkpos']) self.__ur3lftupperarm_nodepath.setMat(ur3lftupperarm_rotmat) self.__ur3lftupperarm_nodepath.setColor(.7,.7,.7,1) # forearm ur3lftforearm_rotmat = pg.npToMat4(robot.lftarm[3]['rotmat'], robot.lftarm[3]['linkpos']) self.__ur3lftforearm_nodepath.setMat(ur3lftforearm_rotmat) self.__ur3lftforearm_nodepath.setColor(.35,.35,.35,1) # wrist1 ur3lftwrist1_rotmat = pg.npToMat4(robot.lftarm[4]['rotmat'], robot.lftarm[4]['linkpos']) self.__ur3lftwrist1_nodepath.setMat(ur3lftwrist1_rotmat) self.__ur3lftwrist1_nodepath.setColor(.7,.7,.7,1) # wrist2 ur3lftwrist2_rotmat = pg.npToMat4(robot.lftarm[5]['rotmat'], robot.lftarm[5]['linkpos']) self.__ur3lftwrist2_nodepath.setMat(ur3lftwrist2_rotmat) self.__ur3lftwrist2_nodepath.setColor(.1,.3,.5,1) # wrist3 ur3lftwrist3_rotmat = pg.npToMat4(robot.lftarm[6]['rotmat'], robot.lftarm[6]['linkpos']) self.__ur3lftwrist3_nodepath.setMat(ur3lftwrist3_rotmat) self.__ur3lftwrist3_nodepath.setColor(.5,.5,.5,1) self.__ur3lftbase_nodepath.reparentTo(robotmesh_nodepath) self.__ur3lftshoulder_nodepath.reparentTo(robotmesh_nodepath) self.__ur3lftupperarm_nodepath.reparentTo(robotmesh_nodepath) self.__ur3lftforearm_nodepath.reparentTo(robotmesh_nodepath) self.__ur3lftwrist1_nodepath.reparentTo(robotmesh_nodepath) self.__ur3lftwrist2_nodepath.reparentTo(robotmesh_nodepath) self.__ur3lftwrist3_nodepath.reparentTo(robotmesh_nodepath) # endcoord if toggleendcoord: self.pggen.plotAxis(robotmesh_nodepath, spos = robot.lftarm[-1]['linkend'], pandamat3 = pg.npToMat3(robot.lftarm[-1]['rotmat'])) # toggle all coord if togglejntscoord: for i in range(1, 7): self.pggen.plotAxis(robotmesh_nodepath, spos = robot.lftarm[i]['linkpos'], pandamat3 = pg.npToMat3(robot.lftarm[i]['refcs'])) # hand if self.lfthnd is not None: self.lfthnd.setMat(pg.npToMat4(robot.lftarm[6]['rotmat'], robot.lftarm[6]['linkpos'])) self.lfthnd.setJawwidth(jawwidthlft) self.lfthnd.reparentTo(robotmesh_nodepath) return copy.deepcopy(robotmesh_nodepath)
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
def match(self, perceivedpnts, perceivednormals, ddist=5.0, dangle=30.0): """ do a match :return: rotmats of top matches author: weiwei date: 20170802 """ # save txt pverts = np.array([tuple(x) for x in perceivedpnts], dtype=[('x', 'f4'), ('y', 'f4'), ('z', 'f4')]) el = PlyElement.describe(pverts, 'vertex') PlyData([el], text=True).write('perceivedpnts.ply') # save txt vandnarray = [] for i in range(len(self.temppnts)): v = self.temppnts[i] n = self.tempnormals[i] vandn = (v[0], v[1], v[2], n[0], n[1], n[2]) vandnarray.append(vandn) pverts = np.array(vandnarray, dtype=[('x', 'f4'),('y', 'f4'),('z', 'f4'),\ ('nx','f4'),('ny','f4'),('nz','f4')]) el = PlyElement.describe(pverts, 'vertex') PlyData([el], text=True).write('ttube.ply') accspace = {} # get the preceived global model descriptor nperceivedpnts = perceivedpnts.shape[0] i = np.argmax(perceivedpnts, axis=0)[2] for j in range(0, nperceivedpnts): print j, nperceivedpnts m_0 = np.asarray(perceivedpnts[i]) m_1 = np.asarray(perceivedpnts[j]) v_m0m1 = m_0 - m_1 v_m1m0 = m_1 - m_0 n_m0 = perceivednormals[i] n_m1 = perceivednormals[j] # f1, namely ||d||2 f1 = np.linalg.norm(m_0 - m_1) # f2, namely angle between n_m0 and v_m1m0 f2 = rm.degree_between(n_m0, v_m1m0) # f3, namely angle between n_m1 and v_m0m1 f3 = rm.degree_between(n_m1, v_m0m1) # f4, namely angle between n_m0 and n_m1 f4 = rm.degree_between(n_m0, n_m1) # discretize the values try: f1d = int(math.floor(f1 / ddist) * ddist + ddist) f2d = int(math.floor(f2 / dangle) * dangle + dangle) f3d = int(math.floor(f3 / dangle) * dangle + dangle) f4d = int(math.floor(f4 / dangle) * dangle + dangle) except: continue key = (f1d, f2d, f3d, f4d) # angle between n_m0 and x+ xplus = np.asarray([1, 0, 0]) yplus = np.asarray([0, 1, 0]) nm0xangle = math.degrees(rm.radian_between(n_m0, xplus)) rotax = np.cross(xplus, n_m0) if np.isnan(rotax).any() or not rotax.any(): continue rotmat = rm.rodrigues(rotax, nm0xangle) v_m1m0onxplus = np.dot(v_m1m0, rotmat) v_m1m0onxplusyzproj = np.asarray( [0, v_m1m0onxplus[1], v_m1m0onxplus[2]]) alpha_m0 = rm.radian_between(v_m1m0onxplusyzproj, yplus) if v_m1m0onxplus[2] < 0: alpha_m0 = 2 * math.pi - alpha_m0 if key in self.gmd.keys(): malist = self.gmd[key] print len(malist) for maslot in malist: alpha = math.degrees(alpha_m0 - maslot[2]) try: alphadiscrete = int( math.floor(alpha / dangle) * dangle + dangle) except: continue acckey = (maslot[0], alphadiscrete) if acckey in accspace.keys(): accspace[acckey] += 1 else: accspace[acckey] = 1 if len(accspace.keys()) is 0: return (None, None) # find top matches and rot matrices maxn = sorted(accspace.iteritems(), key=operator.itemgetter(1), reverse=True)[:5] rotmat4list = [] silist = [] milist = [] for maxnele in maxn: mi, alpha = maxnele[0] # step1 move to temppnts[mi] displacement0 = -self.temppnts[mi] rotmat4_0 = Mat4.translateMat(displacement0[0], displacement0[1], displacement0[2]) # step2 rotate to goal normalangle = math.degrees( rm.radian_between(self.tempnormals[mi], perceivednormals[i])) normalrotax = np.cross(self.tempnormals[mi], perceivednormals[i]) normalrotmat = rm.rodrigues(normalrotax, normalangle) anglerotmat = rm.rodrigues(perceivednormals[i], -alpha) rotmat = np.dot(anglerotmat, normalrotmat) rotmat4_1 = pg.npToMat4(rotmat) # step3 move to perceivedpnts[i] displacement1 = perceivedpnts[i] rotmat4_2 = Mat4.translateMat(displacement1[0], displacement1[1], displacement1[2]) rotmat4 = rotmat4_0 * rotmat4_1 * rotmat4_2 rotmat4list.append(rotmat4) silist.append(i) milist.append(mi) return rotmat4list, silist, milist
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
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 genmnp(self, robot, jawwidthrgt=0, jawwidthlft=0, toggleendcoord=False, togglejntscoord=False, name='robotmesh'): """ generate a panda3d nodepath for the robot mnp indicates this function generates a mesh nodepath :param robot: the robot object, see robot.py :return: a nodepath which is ready to be plotted using plotmesh author: weiwei date: 20180109 """ robotmesh_nodepath = NodePath(name) # body nxtleg_rotmat = pg.npToMat4(robot.base['refcs'], npvec3=robot.base['linkpos']) self.__nxtleg_nodepath.setMat(nxtleg_rotmat) nxtwaist_rotmat = pg.npToMat4(robot.base['rotmat'], npvec3=robot.base['linkpos'] + np.array([0.0, 0.0, 973.0])) self.__nxtwaist_nodepath.setMat(base.render, nxtwaist_rotmat) # rgtarm nxtrgtarmlj0_rotmat = pg.npToMat4(robot.rgtarm[1]['rotmat'], robot.rgtarm[1]['linkpos']) self.__nxtrgtarmlj0_nodepath.setMat(nxtrgtarmlj0_rotmat) nxtrgtarmlj1_rotmat = pg.npToMat4(robot.rgtarm[2]['rotmat'], robot.rgtarm[2]['linkpos']) self.__nxtrgtarmlj1_nodepath.setMat(nxtrgtarmlj1_rotmat) nxtrgtarmlj2_rotmat = pg.npToMat4(robot.rgtarm[3]['rotmat'], robot.rgtarm[3]['linkpos']) self.__nxtrgtarmlj2_nodepath.setMat(nxtrgtarmlj2_rotmat) nxtrgtarmlj3_rotmat = pg.npToMat4(robot.rgtarm[4]['rotmat'], robot.rgtarm[4]['linkpos']) self.__nxtrgtarmlj3_nodepath.setMat(nxtrgtarmlj3_rotmat) nxtrgtarmlj4_rotmat = pg.npToMat4(robot.rgtarm[5]['rotmat'], robot.rgtarm[5]['linkpos']) self.__nxtrgtarmlj4_nodepath.setMat(nxtrgtarmlj4_rotmat) # lftarm nxtlftarmlj0_rotmat = pg.npToMat4(robot.lftarm[1]['rotmat'], robot.lftarm[1]['linkpos']) self.__nxtlftarmlj0_nodepath.setMat(nxtlftarmlj0_rotmat) nxtlftarmlj1_rotmat = pg.npToMat4(robot.lftarm[2]['rotmat'], robot.lftarm[2]['linkpos']) self.__nxtlftarmlj1_nodepath.setMat(nxtlftarmlj1_rotmat) nxtlftarmlj2_rotmat = pg.npToMat4(robot.lftarm[3]['rotmat'], robot.lftarm[3]['linkpos']) self.__nxtlftarmlj2_nodepath.setMat(nxtlftarmlj2_rotmat) nxtlftarmlj3_rotmat = pg.npToMat4(robot.lftarm[4]['rotmat'], robot.lftarm[4]['linkpos']) self.__nxtlftarmlj3_nodepath.setMat(nxtlftarmlj3_rotmat) nxtlftarmlj4_rotmat = pg.npToMat4(robot.lftarm[5]['rotmat'], robot.lftarm[5]['linkpos']) self.__nxtlftarmlj4_nodepath.setMat(nxtlftarmlj4_rotmat) self.__nxtleg_nodepath.reparentTo(robotmesh_nodepath) self.__nxtrgtarmlj0_nodepath.reparentTo(robotmesh_nodepath) self.__nxtrgtarmlj1_nodepath.reparentTo(robotmesh_nodepath) self.__nxtrgtarmlj2_nodepath.reparentTo(robotmesh_nodepath) self.__nxtrgtarmlj3_nodepath.reparentTo(robotmesh_nodepath) self.__nxtrgtarmlj4_nodepath.reparentTo(robotmesh_nodepath) self.__nxtlftarmlj0_nodepath.reparentTo(robotmesh_nodepath) self.__nxtlftarmlj1_nodepath.reparentTo(robotmesh_nodepath) self.__nxtlftarmlj2_nodepath.reparentTo(robotmesh_nodepath) self.__nxtlftarmlj3_nodepath.reparentTo(robotmesh_nodepath) self.__nxtlftarmlj4_nodepath.reparentTo(robotmesh_nodepath) ## right # endcoord if toggleendcoord: self.pggen.plotAxis(robotmesh_nodepath, spos=robot.rgtarm[-1]['linkend'], pandamat3=pg.npToMat3( robot.rgtarm[-1]['rotmat'])) # toggle all coord if togglejntscoord: for i in range(1, 7): self.pggen.plotAxis(robotmesh_nodepath, spos=robot.rgtarm[i]['linkpos'], pandamat3=pg.npToMat3( robot.rgtarm[i]['refcs'])) # hand if self.rgthnd is not None: self.rgthnd.setMat( pg.npToMat4(robot.rgtarm[-1]['rotmat'], robot.rgtarm[-1]['linkpos'])) self.rgthnd.setJawwidth(jawwidthrgt) self.rgthnd.reparentTo(robotmesh_nodepath) # left # endcoord if toggleendcoord: self.pggen.plotAxis(robotmesh_nodepath, spos=robot.lftarm[-1]['linkend'], pandamat3=pg.npToMat3( robot.lftarm[-1]['rotmat'])) # toggle all coord if togglejntscoord: for i in range(1, 7): self.pggen.plotAxis(robotmesh_nodepath, spos=robot.lftarm[i]['linkpos'], pandamat3=pg.npToMat3( robot.lftarm[i]['refcs'])) # hand if self.lfthnd is not None: self.lfthnd.setMat( pg.npToMat4(robot.lftarm[-1]['rotmat'], robot.lftarm[-1]['linkpos'])) self.lfthnd.setJawwidth(jawwidthlft) self.lfthnd.reparentTo(robotmesh_nodepath) return copy.deepcopy(robotmesh_nodepath)
def freegripRotMove(objname,handname): """ :param objname: :return: for each dropstablepos caculate its grips after rot and move and save to database dropfreegrip to remove the hand around workcell next mention that the dropstablepos rot,pos rotmat=(np.transpose(rot),pos) """ gdb = db.GraspDB() idhand = gdb.loadIdHand(handname) idobject=gdb.loadIdObject(objname) sql = "SELECT dropstablepos.iddropstablepos\ FROM dropstablepos, object \ WHERE dropstablepos.idobject = object.idobject AND object.name like '%s'" % (objname) result = gdb.execute(sql) print result if len(result) == 0: print "no DropStablePos select" return None for idfree in result: idfree = int(idfree[0]) sql = "SELECT dropstablepos.iddropstablepos, \ dropstablepos.pos, dropstablepos.rot,\ freeairgrip.contactpnt0, freeairgrip.contactpnt1, \ freeairgrip.contactnormal0, freeairgrip.contactnormal1, \ freeairgrip.rotmat, freeairgrip.jawwidth, freeairgrip.idfreeairgrip \ FROM dropstablepos,freeairgrip WHERE \ freeairgrip.idhand = %d AND \ dropstablepos.iddropstablepos = %d" % (idhand, idfree) result1 = gdb.execute(sql) if len(result1) == 0: print "no free air grasp availalbe" continue if len(result1) > 20000: result1 = result1[0::int(len(result1) / 20000.0)] result1 = np.asarray(result1) idtabletopplacementslist = [int(x) for x in result1[:, 0]] tabletoppositionlist = [dc.strToV3(x) for x in result1[:, 1]] #rotanglelist = [float(x) for x in result1[:, 2]] rotanglelist = [dc.strToMat3(x) for x in result1[:, 2]] freegripcontactpoint0list = [dc.strToV3(x) for x in result1[:, 3]] freegripcontactpoint1list = [dc.strToV3(x) for x in result1[:, 4]] freegripcontactnormal0list = [dc.strToV3(x) for x in result1[:, 5]] freegripcontactnormal1list = [dc.strToV3(x) for x in result1[:, 6]] freegriprotmatlist = [dc.strToMat4(x) for x in result1[:, 7]] freegripjawwidthlist = [float(x) for x in result1[:, 8]] freegripidlist = [int(x) for x in result1[:, 9]] for idtabletopplacements, ttoppos, rotangle, cct0, cct1, cctn0, cctn1, \ freegriprotmat, jawwidth, idfreegrip in zip(idtabletopplacementslist, \ tabletoppositionlist, rotanglelist, \ freegripcontactpoint0list, freegripcontactpoint1list, \ freegripcontactnormal0list, freegripcontactnormal1list, freegriprotmatlist, freegripjawwidthlist, \ freegripidlist): rotmat4=pg.npToMat4(np.transpose(pg.mat3ToNp(rotangle)), ttoppos) ttpcct0 = rotmat4.xformPoint(cct0) ttpcct1 = rotmat4.xformPoint(cct1) ttpcctn0 = rotmat4.xformVec(cctn0) ttpcctn1 = rotmat4.xformVec(cctn1) ttpgriprotmat = freegriprotmat * rotmat4 #ttpgriprotmat = rotmat4*freegriprotmat sql = "INSERT INTO dropfreegrip(contactpnt0, contactpnt1, contactnormal0, contactnormal1, \ rotmat, jawwidth, idfreeairgrip, iddropstablepos,idhand,idobject) VALUES \ ('%s', '%s', '%s', '%s', '%s', '%s', %d, %d,%d,%d) " % \ (dc.v3ToStr(ttpcct0), dc.v3ToStr(ttpcct1), dc.v3ToStr(ttpcctn0), dc.v3ToStr(ttpcctn1), \ dc.mat4ToStr(ttpgriprotmat), str(jawwidth), idfreegrip, idtabletopplacements, idhand, idobject) gdb.execute(sql)
ur5mnp = ur5dualplot.genmnp(ur5dualrobot, handpkg) ur5mnp.reparentTo(base.render) # goal hand # from manipulation.grip.robotiq85 import rtq85nm # hrp5robotrgthnd = rtq85nm.Rtq85NM() # hrp5robotrgthnd.setColor([1,0,0,.3]) # hrp5robotrgtarmlj9_rotmat = pandageom.cvtMat4(objrot, objpos+objrot[:,0]*130) # pg.plotAxisSelf(base.render, objpos, hrp5robotrgtarmlj9_rotmat) # hrp5robotrgthnd.setMat(hrp5robotrgtarmlj9_rotmat) # hrp5robotrgthnd.reparentTo(base.render) # # angle = nxtik.eurgtbik(objpos) # nxtrobot.movewaist(angle) # armjntsgoal=nxtik.numik(nxtrobot, objpos, objrot) # # # nxtplot.plotstick(base.render, nxtrobot) # pandamat4=Mat4() # pandamat4.setRow(3,Vec3(0,0,250)) # # pg.plotAxis(base.render, pandamat4) # # nxtplot.plotmesh(base, nxtrobot) # # pandageom.plotAxis(base.render, pandageom.cvtMat4(nxtrobot.rgtarm[6]['rotmat'], nxtrobot.rgtarm[6]['linkpos'])) # pg.plotDumbbell(base.render, objpos, objpos, rgba = [1,0,0,1]) pg.plotAxisSelf(base.render, objpos, pg.npToMat4(objrot)) # pg.plotArrow(base.render, hrp5robot.rgtarm[8]['linkpos'], hrp5robot.rgtarm[8]['linkpos']+hrp5robot.rgtarm[8]['rotax']*1000) # # # nxtrobot.movearmfk6(armjntsgoal) # # nxtmnp = nxtplot.genNxtmnp_nm(nxtrobot, plotcolor=[1,0,0,1]) # # nxtmnp.reparentTo(base.render) base.run()
def saveToDB(self, positionlist, gdb, discretesize=8): """ :param positionlist: a list of positions to place the object one the table :param discretesize: the discretization of rotation angles around z axis :return: author: weiwei date: 20161215, osaka """ # save discretiezed angle sql = "SELECT * FROM angle" result = gdb.execute(sql) if len(result) == 0: sql = "INSERT INTO angle(value) VALUES " for i in range(discretesize): sql += "("+str(360*i*1.0/discretesize)+"), " sql = sql[:-2]+";" gdb.execute(sql) else: print("Angles already set!") # 1) select the tabletopplacements sql = "SELECT freetabletopplacement.idfreetabletopplacement, freetabletopplacement.rotmat \ FROM freetabletopplacement,object WHERE freetabletopplacement.idobject = object.idobject \ AND object.name LIKE '%s'" % self.dbobjname result = gdb.execute(sql) if len(result) == 0: raise Exception("Plan the freetabletopplacement first!") return result = np.asarray(result) idfreettplist = [int(x) for x in result[:, 0]] rotmatfreettplist = [dc.strToMat4(x) for x in result[:, 1]] # 2) select the angle sql = "SELECT angle.idangle,angle.value FROM angle" result = np.asarray(gdb.execute(sql)) idanglelist = [int(x) for x in result[:, 0]] anglevaluelist = [float(x) for x in result[:, 1]] # 3) save tabletopplacements sql = "SELECT idtabletopplacements FROM tabletopplacements,freetabletopplacement,object WHERE \ tabletopplacements.idfreetabletopplacement=freetabletopplacement.idfreetabletopplacement AND \ freetabletopplacement.idobject=object.idobject AND object.name LIKE '%s'" % self.dbobjname result = gdb.execute(sql) # 3.1) check if already exist if len(result) != 0: print("Tabletopplacements already exist! ") isredo = input("Do you want to overwrite the database? (Y/N)") if isredo != "Y" and isredo != "y": print("Placement planning aborted.") return else: for idfreettp in idfreettplist: sql = "DELETE FROM tabletopplacements WHERE \ tabletopplacements.idfreetabletopplacement LIKE '%s'" % idfreettp gdb.execute(sql) print("Old tabletopplacements have been deleted!") # save to database sql = "INSERT INTO tabletopplacements(rotmat, tabletopposition, idangle, idfreetabletopplacement) VALUES " for ttoppos in positionlist: ttoppos = Point3(ttoppos[0], ttoppos[1], ttoppos[2]) for idfree, rotmatfree in zip(idfreettplist, rotmatfreettplist): for idangle, anglevalue in zip(idanglelist, anglevaluelist): rotangle = anglevalue rotmat = rm.rodrigues([0, 0, 1], rotangle) rotmat4 = pg.npToMat4(rotmat, ttoppos) varrotmat = rotmatfree * rotmat4 sql += "('%s', '%s', %d, %d), " % \ (dc.mat4ToStr(varrotmat), dc.v3ToStr(ttoppos), idangle, idfree) sql = sql[:-2]+";" gdb.execute(sql) print("Tabletop placements saved!") # save tabletopgrips isttg, ttgresult = self.__getTtg(gdb) if not isttg: idhand = gdb.loadIdHand(self.handname) for idfree in idfreettplist: sql = "SELECT tabletopplacements.idtabletopplacements, \ tabletopplacements.tabletopposition, angle.value,\ freetabletopgrip.contactpoint0, freetabletopgrip.contactpoint1, \ freetabletopgrip.contactnormal0, freetabletopgrip.contactnormal1, \ freetabletopgrip.rotmat, freetabletopgrip.jawwidth, freetabletopgrip.idfreeairgrip \ FROM tabletopplacements,freetabletopplacement,freetabletopgrip,freeairgrip,angle WHERE \ tabletopplacements.idfreetabletopplacement = freetabletopplacement.idfreetabletopplacement AND \ tabletopplacements.idangle = angle.idangle AND \ freetabletopgrip.idfreetabletopplacement = freetabletopplacement.idfreetabletopplacement AND \ freetabletopgrip.idfreeairgrip = freeairgrip.idfreeairgrip AND \ freeairgrip.idhand = %d AND \ freetabletopplacement.idfreetabletopplacement = %d" % (idhand, idfree) result1 = gdb.execute(sql) if len(result1) == 0: # no availalbe grasps continue if len(result1) > 20000: result1 = result1[0::int(len(result1)/20000.0)] result1 = np.asarray(result1) idtabletopplacementslist = [int(x) for x in result1[:,0]] tabletoppositionlist = [dc.strToV3(x) for x in result1[:,1]] rotanglelist = [float(x) for x in result1[:,2]] freegripcontactpoint0list = [dc.strToV3(x) for x in result1[:,3]] freegripcontactpoint1list = [dc.strToV3(x) for x in result1[:,4]] freegripcontactnormal0list = [dc.strToV3(x) for x in result1[:,5]] freegripcontactnormal1list = [dc.strToV3(x) for x in result1[:,6]] freegriprotmatlist = [dc.strToMat4(x) for x in result1[:,7]] freegripjawwidthlist = [float(x) for x in result1[:,8]] freegripidlist = [int(x) for x in result1[:,9]] for idtabletopplacements, ttoppos, rotangle, cct0, cct1, cctn0, cctn1, \ freegriprotmat, jawwidth, idfreegrip in zip(idtabletopplacementslist, \ tabletoppositionlist, rotanglelist, freegripcontactpoint0list, freegripcontactpoint1list, \ freegripcontactnormal0list, freegripcontactnormal1list, freegriprotmatlist, freegripjawwidthlist, \ freegripidlist): rotmat = rm.rodrigues([0, 0, 1], rotangle) rotmat4 = pg.npToMat4(rotmat, ttoppos) ttpcct0 = rotmat4.xformPoint(cct0) ttpcct1 = rotmat4.xformPoint(cct1) ttpcctn0 = rotmat4.xformVec(cctn0) ttpcctn1 = rotmat4.xformVec(cctn1) ttpgriprotmat = freegriprotmat*rotmat4 sql = "INSERT INTO tabletopgrips(contactpnt0, contactpnt1, contactnormal0, contactnormal1, \ rotmat, jawwidth, idfreeairgrip, idtabletopplacements) VALUES \ ('%s', '%s', '%s', '%s', '%s', '%s', %d, %d) " % \ (dc.v3ToStr(ttpcct0), dc.v3ToStr(ttpcct1), dc.v3ToStr(ttpcctn0), dc.v3ToStr(ttpcctn1), \ dc.mat4ToStr(ttpgriprotmat), str(jawwidth), idfreegrip, idtabletopplacements) gdb.execute(sql) else: # TODO delete this one, placements and grasps always exist together print("Tabletopgrips already exist!") raise Exception("Warning! Grasps are NOT saved!") return print("Grasps saved!")
def match(self, perceivedpnts, perceivednormals, ddist = 5.0, dangle = 30.0): """ do a match :return: rotmats of top matches author: weiwei date: 20170802 """ # save txt pverts = np.array([tuple(x) for x in perceivedpnts], dtype=[('x', 'f4'),('y', 'f4'),('z', 'f4')]) el = PlyElement.describe(pverts, 'vertex') PlyData([el], text = True).write('perceivedpnts.ply') # save txt vandnarray = [] for i in range(len(self.temppnts)): v = self.temppnts[i] n = self.tempnormals[i] vandn = (v[0],v[1],v[2],n[0],n[1],n[2]) vandnarray.append(vandn) pverts = np.array(vandnarray, dtype=[('x', 'f4'),('y', 'f4'),('z', 'f4'),\ ('nx','f4'),('ny','f4'),('nz','f4')]) el = PlyElement.describe(pverts, 'vertex') PlyData([el], text = True).write('ttube.ply') accspace = {} # get the preceived global model descriptor nperceivedpnts = perceivedpnts.shape[0] i = np.argmax(perceivedpnts, axis = 0)[2] for j in range(0,nperceivedpnts): print j, nperceivedpnts m_0 = np.asarray(perceivedpnts[i]) m_1 = np.asarray(perceivedpnts[j]) v_m0m1 = m_0-m_1 v_m1m0 = m_1-m_0 n_m0 = perceivednormals[i] n_m1 = perceivednormals[j] # f1, namely ||d||2 f1 = np.linalg.norm(m_0-m_1) # f2, namely angle between n_m0 and v_m1m0 f2 = rm.degree_between(n_m0, v_m1m0) # f3, namely angle between n_m1 and v_m0m1 f3 = rm.degree_between(n_m1, v_m0m1) # f4, namely angle between n_m0 and n_m1 f4 = rm.degree_between(n_m0, n_m1) # discretize the values try: f1d = int(math.floor(f1/ddist)*ddist+ddist) f2d = int(math.floor(f2/dangle)*dangle+dangle) f3d = int(math.floor(f3/dangle)*dangle+dangle) f4d = int(math.floor(f4/dangle)*dangle+dangle) except: continue key = (f1d, f2d, f3d, f4d) # angle between n_m0 and x+ xplus = np.asarray([1,0,0]) yplus = np.asarray([0,1,0]) nm0xangle = math.degrees(rm.radian_between(n_m0, xplus)) rotax = np.cross(xplus, n_m0) if np.isnan(rotax).any() or not rotax.any(): continue rotmat = rm.rodrigues(rotax, nm0xangle) v_m1m0onxplus = np.dot(v_m1m0, rotmat) v_m1m0onxplusyzproj = np.asarray([0, v_m1m0onxplus[1], v_m1m0onxplus[2]]) alpha_m0 = rm.radian_between(v_m1m0onxplusyzproj, yplus) if v_m1m0onxplus[2] < 0: alpha_m0 = 2*math.pi - alpha_m0 if key in self.gmd.keys(): malist = self.gmd[key] print len(malist) for maslot in malist: alpha = math.degrees(alpha_m0-maslot[2]) try: alphadiscrete = int(math.floor(alpha/dangle)*dangle+dangle) except: continue acckey = (maslot[0], alphadiscrete) if acckey in accspace.keys(): accspace[acckey] += 1 else: accspace[acckey] = 1 if len(accspace.keys()) is 0: return (None, None) # find top matches and rot matrices maxn = sorted(accspace.iteritems(), key=operator.itemgetter(1), reverse=True)[:5] rotmat4list = [] silist = [] milist = [] for maxnele in maxn: mi, alpha = maxnele[0] # step1 move to temppnts[mi] displacement0 = -self.temppnts[mi] rotmat4_0 = Mat4.translateMat(displacement0[0], displacement0[1], displacement0[2]) # step2 rotate to goal normalangle = math.degrees(rm.radian_between(self.tempnormals[mi], perceivednormals[i])) normalrotax = np.cross(self.tempnormals[mi], perceivednormals[i]) normalrotmat = rm.rodrigues(normalrotax, normalangle) anglerotmat = rm.rodrigues(perceivednormals[i], -alpha) rotmat = np.dot(anglerotmat, normalrotmat) rotmat4_1 = pg.npToMat4(rotmat) # step3 move to perceivedpnts[i] displacement1 = perceivedpnts[i] rotmat4_2 = Mat4.translateMat(displacement1[0], displacement1[1], displacement1[2]) rotmat4 = rotmat4_0*rotmat4_1*rotmat4_2 rotmat4list.append(rotmat4) silist.append(i) milist.append(mi) return rotmat4list, silist, milist