rgthand = sck918.Sck918(ftsensoroffset=0)
robotmesh = nxtmesh.NxtMesh(lfthand=lfthand, rgthand=rgthand)
nxtmesh = robotmesh.genmnp(nxtrobot,
jawwidthrgt=50.0,
jawwidthlft=50.0,
togglejntscoord=False)
# nxtmesh.reparentTo(base.render)
armname = 'rgt'
nxtball = nxtball.NxtBall()
cdchecker = cdck.CollisionCheckerBall(nxtball)
ctcallback = ctcb.CtCallback(base,
nxtrobot,
cdchecker,
ctchecker=None,
armname=armname)
smoother = sm.Smoother()
starttreesamplerate = 25
endtreesamplerate = 30
pandamat4 = Mat4()
pandamat4.setRow(3, Vec3(0, 0, 250))
nxtrobot.goinitpose()
armjntsgoal6 = []
# # this part can be packed as a function
filename = "document/action0.yaml"
num = readyaml.getlength(filename)
for ts in range(num):
[main_object, s_tran, s_rot, s_tran_sub, s_rot_sub] = \
readyaml.readyaml(filename, ts)
def plannplay(self, transitoption = "useboth", choice = "startrgtgoalrgt"):
"""
updated 20190314
:param transitoption: "useplacement", "usehandover", "useboth"
:return:
"""
# capture initial objpose
self.robot.goinitpose()
# start OC
print("Finding motion to goal 1...")
startrotmat4 = base.pg.np4ToMat4(self.goallist[0])
goalrotmat4 = base.pg.np4ToMat4(self.goallist[1])
regrip = regriptppfp.RegripTppFp(self.objcm.objpath, self.robot, sck918, self.gdb, base,
self.obstaclecmlist, transitoption=transitoption)
regrip.addStartGoal(startrotmat4 = startrotmat4, goalrotmat4 = goalrotmat4, choice = choice, goaltoolvec=(-1,0,0))
cdchecker = ccball.CollisionCheckerBall(self.robotball)
# OUTERLOOP collision detection
objmsmp, numikrmsmp, jawwidthmp, originalpathnidlist = rp.planRegrasp(self.robot, regrip, self.objcm, cdchecker,
self.obstaclecmlist+[self.groove], switch="OC",
previous = [], end = False, togglemp=True)
if objmsmp is None:
return False
nextid = regrip.regg.node[originalpathnidlist[-1]]['globalgripid']
# nextgrasp = regrip.regg.node[originalpathnidlist[-1]]['globalgripid']
for id in range(1, len(self.goallist)-1):
print("Finding motion to goal "+str(id+1)+"...")
print("Start and goal hand global ids are ", nextid)
startrotmat4 = base.pg.np4ToMat4(self.goallist[id])
goalrotmat4 = base.pg.np4ToMat4(self.goallist[id+1])
# regrip.reset()
#--------------
for j, rotmat in enumerate(regrip.freegriprotmats):
# vector between object and robot ee
if regrip.freegripids[j] == nextid:
ttgsrotmat = rotmat * goalrotmat4
ttgscct0 = goalrotmat4.xformPoint(regrip.freegripcontacts[j][0])
ttgscct1 = goalrotmat4.xformPoint(regrip.freegripcontacts[j][1])
ttgsfgrcenter = (ttgscct0 + ttgscct1) / 2
base.pggen.plotSphere(base.render,base.pg.v3ToNp(ttgsfgrcenter),radius=30)
base.pggen.plotAxis(base.render,spos=base.pg.v3ToNp(ttgsfgrcenter),pandamat3=ttgsrotmat.getUpper3())
goalrotmat4np = base.pg.mat4ToNp(ttgsrotmat)
armname = 'rgt'
start = [numikrmsmp[-1][-1][0]] + numikrmsmp[-1][-1][1].tolist()
goal = self.robot.numikr(base.pg.v3ToNp(ttgsfgrcenter),goalrotmat4np[:3,:3],armname="rgt")
startjawwidth = jawwidthmp[-1][-1][0]
# if "lft" in originalpathnidlist[-1]:
# armname = 'lft'
# start = [numikrmsmp[-1][-1][0]] + numikrmsmp[-1][-1][2].tolist()
# goal = self.robot.numikr(base.pg.v3ToNp(ttgsfgrcenter), goalrotmat4np[:3, :3], armname="lft")
startjawwidth = jawwidthmp[-1][-1][1]
if goal is None:
print("Goal is None!!!!!")
return False
goal = [goal[0]] + goal[1].tolist()
starttreesamplerate = 25
endtreesamplerate = 30
print(armname)
print(startjawwidth)
# ctcallback.setarmname(armname, usewaist=True)
import motionplanning.ctcallback as ctcb
import motionplanning.rrt.ddrrtconnect as ddrrtc
import motionplanning.smoother as sm
ctcallback = ctcb.CtCallback(base, self.robot, cdchecker)
planner = ddrrtc.DDRRTConnect(start=start, goal=goal, ctcallback=ctcallback,
starttreesamplerate=starttreesamplerate,
endtreesamplerate=endtreesamplerate, expanddis=5,
maxiter=200, maxtime=10.0)
tempnumikrmsmp = []
tempjawwidthmp = []
tempobjmsmp = []
objcopy = copy.deepcopy(self.objcm)
objmanp = base.pg.mat4ToNp(objmsmp[-1][-1])
self.robot.movearmfkr([start[0],np.array(start[1:])],armname)
relpos, relrot = self.robot.getinhandpose(objmanp[:3,3], objmanp[:3,:3], armname)
ctcallback.setarmname(armname, usewaist=True)
path, sampledpoints = planner.planninghold(objcopy, [relpos, relrot], [])
if path is False:
print("Motion planning with hold failed! restarting...")
# regrip.removeBadNodes([pathnidlist[i-1][:-1]])
return False
smoother = sm.Smoother()
path = smoother.pathsmoothinghold(path, planner, objcopy, [relpos, relrot], 30)
npath = len(path)
for j in range(npath):
if armname == 'rgt':
tempnumikrmsmp.append([path[j][0], np.array(path[j][1:]), numikrmsmp[-1][-1][2]])
else:
tempnumikrmsmp.append([path[j][0], numikrmsmp[-1][-1][1], np.array(path[j][1:])])
self.robot.movearmfkr([path[j][0], np.array(path[j][1:])], armname=armname)
tempjawwidthmp.append(jawwidthmp[-1][-1])
objpos, objrot = self.robot.getworldpose(relpos, relrot, armname)
tempobjmsmp.append(base.pg.npToMat4(objrot, objpos))
objmsmp = objmsmp+[None]+[tempobjmsmp]
numikrmsmp = numikrmsmp+[None]+[tempnumikrmsmp]
jawwidthmp = jawwidthmp+[None]+[tempjawwidthmp]
#-----------------
# regrip.addStartGoal(startrotmat4 = startrotmat4, goalrotmat4 = goalrotmat4, choice = "startrgtgoalrgt",
# startgraspgid=nextid, starttoolvec = [0,0,0], goaltoolvec = [-1,0,0])
#
#
# # cdchecker = ccball.CollisionCheckerBall(self.robotball)
# # OUTERLOOP collision detection
# objmsmp_t, numikrmsmp_t, jawwidthmp_t, originalpathnidlist_t = rp.planRegrasp(self.robot, regrip,
# self.objcm, cdchecker,
# self.obstaclecmlist,
# switch="CC",
# previous=[objmsmp[-1][-1], numikrmsmp[-1][-1], jawwidthmp[-1][-1]],
# end = False,
# togglemp=True)
# if objmsmp_t is None and id == len(self.goallist)-2:
# break
# elif objmsmp_t is None:
# return False
# objmsmp = objmsmp+[None]+objmsmp_t
# numikrmsmp = numikrmsmp+[None]+numikrmsmp_t
# jawwidthmp = jawwidthmp+[None]+jawwidthmp_t
# nextid = regrip.regg.node[originalpathnidlist_t[-1]]['globalgripid']
objmsmp = objmsmp + [None]
numikrmsmp = numikrmsmp + [None]
jawwidthmp = jawwidthmp + [None]
self.rbtmnp.removeNode()
self.rbtmnpani = [None, None]
self.objmnpani = [None]
motioncounter = [0]
def updatemotionsec(objms, numikrms, jawwidth, rbtmnp, objmnp, motioncounter, robot, objcm, task):
if motioncounter[0] < len(numikrms):
if rbtmnp[0] is not None:
rbtmnp[0].detachNode()
# rbtmnp[1].detachNode()
if objmnp[0] is not None:
objmnp[0].detachNode()
rgtarmjnts = numikrms[motioncounter[0]][1].tolist()
lftarmjnts = numikrms[motioncounter[0]][2].tolist()
robot.movealljnts([numikrms[motioncounter[0]][0], 0, 0] + rgtarmjnts + lftarmjnts)
rgtjawwidth = jawwidth[motioncounter[0]][0]
lftjawwidth = jawwidth[motioncounter[0]][1]
# print rgtjawwidth, lftjawwidth
rbtmnp[0] = self.robotmesh.genmnp(robot, rgtjawwidth, lftjawwidth)
bcndict = self.robotball.genfullactivebcndict(robot)
# rbtmnp[1] = self.robotball.showcn(base, bcndict)
robot.goinitpose()
rbtmnp[0].reparentTo(base.render)
objmnp[0] = copy.deepcopy(objcm)
objmnp[0].setMat(objms[motioncounter[0]])
objmnp[0].reparentTo(base.render)
objmnp[0].showLocalFrame()
# objmnp[0].showcn()
motioncounter[0] += 1
else:
motioncounter[0] = 0
# base.win.saveScreenshot(Filename(str(motioncounter[0]) + '.jpg'))
return task.again
objmsmpactive = [objmsmp[0]]
numikrmsmpactive = [numikrmsmp[0]]
jawwidthmpactive = [jawwidthmp[0]]
taskMgr.doMethodLater(0.1, updatemotionsec, "updatemotionsec",
extraArgs=[objmsmpactive[0], numikrmsmpactive[0], jawwidthmpactive[0], self.rbtmnpani,
self.objmnpani, motioncounter, self.robot, self.objcm],
appendTask=True)
motionseccounter = [0]
def updatesection(objmsmpactive, numikrmsmpactive, jawwidthmpactive, objmsmp, numikrmsmp, jawwidthmp, rbtmnp,
objmnp, motioncounter, motionseccounter, robot, objcm, task):
if base.inputmgr.keyMap['space'] is True:
# print(motionseccounter[0], len(objmsmp) - 1)
if motionseccounter[0] <= len(objmsmp) - 1:
motionseccounter[0] = motionseccounter[0] + 1
if not (motionseccounter[0] == len(objmsmp)):
if objmsmp[motionseccounter[0]] is not None:
objmsmpactive[0] = objmsmp[motionseccounter[0]]
numikrmsmpactive[0] = numikrmsmp[motionseccounter[0]]
jawwidthmpactive[0] = jawwidthmp[motionseccounter[0]]
base.inputmgr.keyMap['space'] = False
# print(motionseccounter[0])
# print(jawwidthmpactive[0])
taskMgr.remove('updatemotionsec')
motioncounter[0] = 0
taskMgr.doMethodLater(0.1, updatemotionsec, "updatemotionsec",
extraArgs=[objmsmpactive[0], numikrmsmpactive[0], jawwidthmpactive[0], rbtmnp,
objmnp, motioncounter, robot, objcm],
appendTask=True)
if objmsmp[motionseccounter[0]-1] is not None:
# execute the last sequence
# temp_objmsmpactive = objmsmp[motionseccounter[0]-1]
temp_numikrmsmpactive = numikrmsmp[motionseccounter[0] - 1]
# print "temp_numikrmsmpactive is %s" % temp_numikrmsmpactive
temp_jawwidthmpactive = jawwidthmp[motionseccounter[0] - 1] # rgt and left
#
jntslist_waist = []
jntslist_rgt = []
jntslist_lft = []
jawwidthlist_rgt = []
jawwidthlist_lft = []
for eachtemp in temp_numikrmsmpactive:
jntslist_waist.append(eachtemp[0])
# print ("Joint list waist:{} ".format(jntslist_waist))
jntslist_rgt.append(eachtemp[1].tolist())
# print ("Joint list right:{} ".format(jntslist_rgt))
jntslist_lft.append(eachtemp[2].tolist())
# print ("Joint list left:{} ".format(jntslist_lft))
# print ("Length of joint list is:{} ".format(len(jntslist_rgt)))
for eachtemp in temp_jawwidthmpactive:
jawwidthlist_rgt.append(eachtemp[0])
# print ("jaw width list right:{} ".format(jawwidthlist_rgt))
jawwidthlist_lft.append(eachtemp[1])
# print ("jaw width list left:{} ".format(jawwidthlist_lft))
# print jawwidthlist_rgt
# uncomment this part to actuate the Nextage
if len(jntslist_rgt) == 2:
# do nothing; We dont need to examine lft since they are symmetric
pass
else:
# check if the waist is moved
waistdiff = abs(np.array(jntslist_waist[0])-np.array(jntslist_waist[-1]))
if waistdiff.max() > 1e-6:
# print "jntslist_waist is %s" % jntslist_waist
jntslist_waist_rad = []
for i in range(len(jntslist_waist)):
# print "jntslist_waist[%d] is %s" % (i, jntslist_waist[i])
jntslist_waist_rad.append(math.radians(jntslist_waist[i]))
# print "jntslist_waist_rad is %s" % jntslist_waist_rad
new_jntslist_waist_rad = []
for j in range(len(jntslist_waist_rad)):
jnt_waist_rad = []
jnt_waist_rad.append(jntslist_waist_rad[j])
new_jntslist_waist_rad.append(jnt_waist_rad)
# print "new_jntslist_waist_rad is %s" % new_jntslist_waist_rad
# time = [0.2]
# para_waist = ['torso', new_jntslist_waist_rad, time*len(new_jntslist_waist_rad)]
# self.nxtu.playPatternOfGroup(para_waist)
# check if the arm is moved
rgtdiff = abs(np.array(jntslist_rgt[0])-np.array(jntslist_rgt[-1]))
if rgtdiff.max() > 1e-6:
# print "jntslist_rgt is %s" % jntslist_rgt
jntslist_rgt_rad = []
jnts_rgt_rad = []
for i in range(len(jntslist_rgt)):
# print "jntslist_rgt[%d] is %s" % (i, jntslist_rgt[i])
for j in range(len(jntslist_rgt[i])):
jnts_rgt_rad.append(math.radians(jntslist_rgt[i][j]))
# print "jntslist_rgt_rad[%d] is %s" % (i, jnts_rgt_rad[(len(jntslist_rgt[i]) * i):len(jntslist_rgt[i]) * (i + 1)])
jntslist_rgt_rad.append(jnts_rgt_rad[(len(jntslist_rgt[i])*i):len(jntslist_rgt[i])*(i+1)])
# print "jntslist_rgt_rad is %s" % jntslist_rgt_rad
# time = [0.2]
# para_rgt = ['rarm', jntslist_rgt_rad, time*len(jntslist_rgt_rad)]
# self.nxtu.playPatternOfGroup(para_rgt)
lftdiff = abs(np.array(jntslist_lft[0])-np.array(jntslist_lft[-1]))
if lftdiff.max() > 1e-6:
# print "jntslist_lft is %s" % jntslist_lft
jntslist_lft_rad = []
jnts_lft_rad = []
for i in range(len(jntslist_lft)):
# print "jntslist_rgt[%d] is %s" % (i, jntslist_lft[i])
for j in range(len(jntslist_lft[i])):
jnts_lft_rad.append(math.radians(jntslist_lft[i][j]))
# print "jntslist_lft_rad[%d] is %s" % (i, jnts_lft_rad[(len(jntslist_lft[i]) * i):len(jntslist_lft[i]) * (i + 1)])
jntslist_lft_rad.append(jnts_lft_rad[(len(jntslist_rgt[i]) * i):len(jntslist_lft[i]) * (i + 1)])
# print "jntslist_lft_rad is %s" % jntslist_lft_rad
# time = [0.2]
# para_lft = ['larm', jntslist_lft_rad, time*len(jntslist_lft_rad)]
# self.nxtu.playPatternOfGroup(para_lft)
# if jawwidthlist_rgt[-1] <85.0:
# self.ur3u.closegripper("rgt")
# else:
# self.ur3u.opengripper("rgt")
# if jawwidthlist_lft[-1] < 85.0:
# self.ur3u.closegripper("lft")
# else:
# self.ur3u.opengripper("lft")
else:
#TODO add directokdialog
print("One motion section is done!")
else:
motionseccounter[0] = 0
taskMgr.remove('updateshow')
return task.again
return task.again
taskMgr.doMethodLater(0.04, updatesection, "updatesection",
extraArgs=[objmsmpactive, numikrmsmpactive, jawwidthmpactive, objmsmp, numikrmsmp,
jawwidthmp, self.rbtmnpani, self.objmnpani, motioncounter, motionseccounter,
self.robot, self.objcm],
appendTask=True)
return True