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
return task.done
# 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):
# this_dir, this_filename = os.path.split(__file__)
# f = open(os.path.join(this_dir, "document", "Planned_pegpath_deriv2_robot.txt"), "a")
jntslist_waist = []
jntslist_rgt = []
jntslist_lft = []
jawwidthlist_rgt = []
jawwidthlist_lft = []
new_jntslist_waist_rad = []
jntslist_rgt_rad = []
jntslist_lft_rad = []
if base.inputmgr.keyMap['space'] is True or (motionseccounter[0] > 5 and not taskMgr.hasTaskNamed("updatemotionsec")):
# 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 = []
# new_jntslist_waist_rad = []
# jntslist_rgt_rad = []
# jntslist_lft_rad = []
# angles = []
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)
angles = []
if new_jntslist_waist_rad:
length = len(new_jntslist_waist_rad)
else:
length = len(jntslist_rgt_rad)
for n in range(length):
angle = []
# if new_jntslist_waist_rad:
headwaistangles = [new_jntslist_waist_rad[n][0], 0, 0]
for headwaistangle in headwaistangles:
angle.append(headwaistangle)
# if jntslist_rgt_rad:
for jntsrgt in jntslist_rgt_rad[n]:
angle.append(jntsrgt)
# if jntslist_lft_rad:
for jntslft in jntslist_lft_rad[n]:
angle.append(jntslft)
print("Angle is", angle)
angles.append(angle)
print("Angles are", angles)
# for poses in angles:
# # print(poses)
# f.write(str(poses) + '\n')
# f.close()
# 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.done
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
def planRegrasp(robot,
regrip,
objcm,
cdchecker,
obstaclecmlist,
id=0,
switch="OC",
previous=[],
end=False,
togglemp=True):
"""
plan the regrasp sequences
:param objpath:
:param robot:
:param hand:
:param dbase:
:param obstaclecmlist:
:param id = 0
:param switch in "OC" open-close "CC" close-close
:param previous: set it to [] if the motion is not a continuing one, or else, set it to [lastobjmat4, lastikr, lastjawwidth]
:param end: set it to True if it is the last one
:param togglemp denotes whether the motion between the keyposes are planned or not, True by default
:return:
author: weiwei
date: 20180924
"""
while True:
print("new search")
regrip.updateshortestpath()
[objms, numikrms, jawwidth, pathnidlist, originalpathnidlist] = \
sqdes.getMotionSequence(regrip, id = id, choice = regrip.choice, type=switch, previous=previous)
if objms is None:
return [None, None, None, None]
bcdfree = True
for i in range(len(numikrms)):
rgtarmjnts = numikrms[i][1].tolist()
lftarmjnts = numikrms[i][2].tolist()
robot.movealljnts([numikrms[i][0], 0, 0] + rgtarmjnts + lftarmjnts)
# skip the exact handover pose and only detect the cd between armhnd and body
if pathnidlist[i].startswith('ho') and pathnidlist[
i + 1].startswith('ho'):
abcd = cdchecker.isCollidedHO(robot, obstaclecmlist)
if abcd:
regrip.removeBadNodes([pathnidlist[i][:-1]])
print("Abcd collided at ho pose")
bcdfree = False
# robotmesh.genmnp(robot).reparentTo(base.render)
# robotball.showbcn(base, robotball.genbcndict(robot))
# base.run()
break
else:
# NOTE: we ignore both arms here for conciseness
# This might be a potential bug
if cdchecker.isCollided(robot,
obstaclecmlist,
holdarmname="all"):
regrip.removeBadNodes([pathnidlist[i][:-1]])
print("Robot collided at non-ho pose")
bcdfree = False
# robotmesh.genmnp(robot).reparentTo(base.render)
# robotball.showbcn(base, robotball.genbcndict(robot))
# base.run()
break
robot.goinitpose()
if bcdfree:
objmsmp = []
numikrmsmp = []
jawwidthmp = []
print(pathnidlist)
if not togglemp:
for i, numikrm in enumerate(numikrms):
if i > 0:
startid = pathnidlist[i - 1]
endid = pathnidlist[i]
if (not end) and (endid is 'end'):
continue
if (len(previous) > 0) and (startid is 'begin'):
continue
numikrmsmp.append([numikrms[i - 1], numikrms[i]])
objmsmp.append([objms[i - 1], objms[i]])
jawwidthmp.append([jawwidth[i - 1], jawwidth[i]])
return objmsmp, numikrmsmp, jawwidthmp, originalpathnidlist
# INNERLOOP motion planning
smoother = sm.Smoother()
ctcallback = ctcb.CtCallback(base, robot, cdchecker)
breakflag = False
for i, numikrm in enumerate(numikrms):
if i > 0:
# determine which arm to plan
# assume right
# assume redundant planning
robot.goinitpose()
startid = pathnidlist[i - 1]
endid = pathnidlist[i]
objmat = base.pg.mat4ToNp(objms[i - 1])
objrot = objmat[:3, :3]
objpos = objmat[:3, 3]
if (not end) and (endid is 'end'):
continue
if (len(previous) > 0) and (startid is 'begin'):
continue
if (startid[-1] == "o"
and endid[-1] == "c") or (startid[-1] == "c"
and endid[-1] == "o"):
# open and close gripper
print("O/C hands, simply include ", pathnidlist[i - 1],
" and ", pathnidlist[i])
numikrmsmp.append([numikrms[i - 1], numikrms[i]])
objmsmp.append([objms[i - 1], objms[i]])
jawwidthmp.append([jawwidth[i - 1], jawwidth[i]])
continue
if (startid[:-1] == endid[:-1]):
if (startid[-1] != "i") and (endid[-1] != "i"):
print("Motion primitives, interplate ",
pathnidlist[i - 1], " and ", pathnidlist[i])
# linear interpolation
tempnumikrmsmp = []
tempjawwidthmp = []
tempobjmsmp = []
temparmname = "rgt"
startjntags = [
numikrms[i - 1][0], numikrms[i - 1][1]
]
goaljntags = [numikrms[i][0], numikrms[i][1]]
if "lft" in startid:
temparmname = "lft"
startjntags = [
numikrms[i - 1][0], numikrms[i - 1][2]
]
goaljntags = [numikrms[i][0], numikrms[i][2]]
ctcallback.setarmname(temparmname, usewaist=True)
[interplatedjnts, interplatedobjposes] = \
ctcallback.isLMAvailableJNTwithObj(startjntags, goaljntags,
[objpos, objrot], armname = temparmname, type = startid[-1])
if len(interplatedjnts) == 0:
print(
"Failed to interpolate motion primitive! restarting..."
)
regrip.removeBadNodes(
[pathnidlist[i - 1][:-1]])
breakflag = True
break
for eachitem in interplatedjnts:
if temparmname == "rgt":
tempnumikrmsmp.append([
eachitem[0], eachitem[1],
numikrms[i - 1][2]
])
else:
tempnumikrmsmp.append([
eachitem[0], numikrms[i - 1][1],
eachitem[1]
])
tempjawwidthmp.append(jawwidth[i - 1])
for eachitem in interplatedobjposes:
tempobjmsmp.append(
base.pg.npToMat4(eachitem[1], eachitem[0]))
numikrmsmp.append(tempnumikrmsmp)
jawwidthmp.append(tempjawwidthmp)
objmsmp.append(tempobjmsmp)
continue
# numikrmsmp.append([numikrms[i-1], numikrms[i]])
# objmsmp.append([objms[i-1], objms[i]])
# jawwidthmp.append([jawwidth[i-1], jawwidth[i]])
# continue
if (startid[0] not in [
'b', 's', 'g'
]) and (endid[0] not in ['b', 's', 'g']):
print("Planning ho motion between ",
pathnidlist[i - 1], " and ", pathnidlist[i])
rgtarmjnts = numikrms[i - 1][1].tolist()
lftarmjnts = numikrms[i - 1][2].tolist()
robot.movealljnts([numikrms[i - 1][0], 0, 0] +
rgtarmjnts + lftarmjnts)
# assume rgt
armname = 'rgt'
start = numikrms[i - 1][1].tolist()
goal = numikrms[i][1].tolist()
startjawwidth = jawwidth[i - 1][0]
if "lft" in endid:
armname = 'lft'
start = numikrms[i - 1][2].tolist()
goal = numikrms[i][2].tolist()
startjawwidth = jawwidth[i - 1][1]
starttreesamplerate = 25
endtreesamplerate = 30
print(armname)
print(startjawwidth)
ctcallback.setarmname(armname, usewaist=False)
planner = ddrrtc.DDRRTConnect(
start=start,
goal=goal,
ctcallback=ctcallback,
starttreesamplerate=starttreesamplerate,
endtreesamplerate=endtreesamplerate,
expanddis=5,
maxiter=200,
maxtime=10.0)
tempnumikrmsmp = []
tempjawwidthmp = []
tempobjmsmp = []
if (endid[-1] == "c") or (endid[-1] == "w"):
# if the arm is holding an object
print("Planning hold motion between ",
pathnidlist[i - 1], " and ", pathnidlist[i])
relpos, relrot = robot.getinhandpose(
objpos, objrot, armname)
path, sampledpoints = planner.planninghold(
objcm, [relpos, relrot], obstaclecmlist)
if path is False:
print(
"Motion planning with hold failed! restarting..."
)
# regrip.removeBadNodes([pathnidlist[i-1][:-1]])
regrip.removeBadNodes([pathnidlist[i][:-1]])
breakflag = True
break
path = smoother.pathsmoothinghold(
path, planner, objcm, [relpos, relrot], 30)
npath = len(path)
for j in range(npath):
if armname == 'rgt':
tempnumikrmsmp.append([
0.0,
np.array(path[j]), numikrms[i - 1][2]
])
else:
tempnumikrmsmp.append([
0.0, numikrms[i - 1][1],
np.array(path[j])
])
robot.movearmfk(np.array(path[j]),
armname=armname)
tempjawwidthmp.append(jawwidth[i - 1])
objpos, objrot = robot.getworldpose(
relpos, relrot, armname)
tempobjmsmp.append(
base.pg.npToMat4(objrot, objpos))
else:
# if the arm is not holding an object, the object will be treated as an obstacle
print("Planning motion ", pathnidlist[i - 1],
" and ", pathnidlist[i])
objcmcopy = copy.deepcopy(objcm)
objcmcopy.setMat(objms[i - 1])
obstaclecmlistnew = obstaclecmlist + [objcmcopy]
path, sampledpoints = planner.planning(
obstaclecmlistnew)
if path is False:
print("Motion planning failed! restarting...")
# regrip.removeBadNodes([pathnidlist[i-1][:-1]])
# regrip.removeBadNodes([pathnidlist[i][:-1]])
node0 = pathnidlist[i - 1][:-1]
node1 = pathnidlist[i][:-1]
regrip.removeBadEdge(node0, node1)
breakflag = True
break
path = smoother.pathsmoothing(path, planner, 30)
npath = len(path)
for j in range(npath):
if armname == 'rgt':
tempnumikrmsmp.append([
0.0,
np.array(path[j]), numikrms[i - 1][2]
])
else:
tempnumikrmsmp.append([
0.0, numikrms[i - 1][1],
np.array(path[j])
])
tempjawwidthmp.append(jawwidth[i - 1])
tempobjmsmp.append(objms[i - 1])
numikrmsmp.append(tempnumikrmsmp)
jawwidthmp.append(tempjawwidthmp)
objmsmp.append(tempobjmsmp)
continue
# init robot pose
rgtarmjnts = numikrms[i - 1][1].tolist()
lftarmjnts = numikrms[i - 1][2].tolist()
robot.movealljnts([numikrms[i - 1][0], 0, 0] + rgtarmjnts +
lftarmjnts)
# assume rgt
armname = 'rgt'
start = [numikrms[i - 1][0]] + numikrms[i - 1][1].tolist()
goal = [numikrms[i][0]] + numikrms[i][1].tolist()
startjawwidth = jawwidth[i - 1][0]
if "lft" in endid:
armname = 'lft'
start = [numikrms[i - 1][0]
] + numikrms[i - 1][2].tolist()
goal = [numikrms[i][0]] + numikrms[i][2].tolist()
startjawwidth = jawwidth[i - 1][1]
starttreesamplerate = 25
endtreesamplerate = 30
print(armname)
print(startjawwidth)
ctcallback.setarmname(armname, usewaist=True)
planner = ddrrtc.DDRRTConnect(
start=start,
goal=goal,
ctcallback=ctcallback,
starttreesamplerate=starttreesamplerate,
endtreesamplerate=endtreesamplerate,
expanddis=5,
maxiter=200,
maxtime=10.0)
tempnumikrmsmp = []
tempjawwidthmp = []
tempobjmsmp = []
if (endid[-1] == "c") or (endid[-1] == "w"):
# if the arm is holding an object
print("Planning hold motion between ",
pathnidlist[i - 1], " and ", pathnidlist[i])
relpos, relrot = robot.getinhandpose(
objpos, objrot, armname)
# try:
# [path, sampledpoints] = planner.planninghold(objcm, [relpos, relrot], obstaclecmlist)
# except Exception as e:
# print(e)
# print("Motion planning with hold failed! restarting...")
# regrip.removeBadNodes([pathnidlist[i-1][:-1]])
# regrip.removeBadNodes([pathnidlist[i][:-1]])
# breakflag = True
# break
path, sampledpoints = planner.planninghold(
objcm, [relpos, relrot], obstaclecmlist)
if path is False:
print(
"Motion planning with hold failed! restarting..."
)
# regrip.removeBadNodes([pathnidlist[i-1][:-1]])
regrip.removeBadNodes([pathnidlist[i][:-1]])
breakflag = True
break
path = smoother.pathsmoothinghold(
path, planner, objcm, [relpos, relrot], 30)
npath = len(path)
for j in range(npath):
if armname == 'rgt':
tempnumikrmsmp.append([
path[j][0],
np.array(path[j][1:]), numikrms[i - 1][2]
])
else:
tempnumikrmsmp.append([
path[j][0], numikrms[i - 1][1],
np.array(path[j][1:])
])
robot.movearmfkr(
[path[j][0], np.array(path[j][1:])],
armname=armname)
tempjawwidthmp.append(jawwidth[i - 1])
objpos, objrot = robot.getworldpose(
relpos, relrot, armname)
tempobjmsmp.append(base.pg.npToMat4(
objrot, objpos))
else:
# if the arm is not holding an object, the object will be treated as an obstacle
print("Planning motion ", pathnidlist[i - 1], " and ",
pathnidlist[i])
objcmcopy = copy.deepcopy(objcm)
objcmcopy.setMat(objms[i - 1])
obstaclecmlistnew = obstaclecmlist + [objcmcopy]
path, sampledpoints = planner.planning(
obstaclecmlistnew)
if path is False:
print("Motion planning failed! restarting...")
# regrip.removeBadNodes([pathnidlist[i-1][:-1]])
# regrip.removeBadNodes([pathnidlist[i][:-1]])
if pathnidlist[i - 1] == "begin":
regrip.removeBadNodes([pathnidlist[i][:-1]])
breakflag = True
break
if pathnidlist[1] == "end":
regrip.removeBadNodes(
[pathnidlist[i - 1][:-1]])
breakflag = True
break
node0 = pathnidlist[i - 1][:-1]
node1 = pathnidlist[i][:-1]
regrip.removeBadEdge(node0, node1)
breakflag = True
break
path = smoother.pathsmoothing(path, planner, 30)
npath = len(path)
for j in range(npath):
if armname == 'rgt':
tempnumikrmsmp.append([
path[j][0],
np.array(path[j][1:]), numikrms[i - 1][2]
])
else:
tempnumikrmsmp.append([
path[j][0], numikrms[i - 1][1],
np.array(path[j][1:])
])
tempjawwidthmp.append(jawwidth[i - 1])
tempobjmsmp.append(objms[i - 1])
numikrmsmp.append(tempnumikrmsmp)
jawwidthmp.append(tempjawwidthmp)
objmsmp.append(tempobjmsmp)
print(i, len(numikrms) - 1)
if breakflag is False:
# successfully finished!
return [objmsmp, numikrmsmp, jawwidthmp, originalpathnidlist]
else:
# remov node and start new search
continue
objrot_lft = []
objpos_rgt = []
objrot_rgt = []
nxtrobot = nxt.NxtRobot()
robotball = nxtball.NxtBall()
lfthand = rtq85.Rtq85(jawwidth=85, ftsensoroffset=0)
rgthand = rtq85.Rtq85(jawwidth=85, ftsensoroffset=0)
robotmesh = nxtmesh.NxtMesh(lfthand=lfthand, rgthand=rgthand)
nxtmesh = robotmesh.genmnp(nxtrobot, jawwidthrgt=85.0, jawwidthlft=0.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] = \
# another example -- load obj collision model independently
# this_dir, this_filename = os.path.split(__file__)
# objname = "tool_motordriver.stl"
# objpath = os.path.join(this_dir, "objects", objname)
# objcm = cm.CollisionModel(objpath)
robot = ur3dual.Ur3DualRobot()
robotball = ur3dualball.Ur3DualBall()
rgthnd = rtq85.Rtq85(jawwidth=85)
lfthnd = rtq85.Rtq85(jawwidth=85)
robotmesh = ur3dualmesh.Ur3DualMesh(rgthand=rgthnd, lfthand=lfthnd)
robotnp = robotmesh.genmnp(robot, jawwidthrgt=85.0, jawwidthlft=0.0)
# robotnp.reparentTo(base.render)
armname = 'rgt'
cdchecker = cdck.CollisionCheckerBall(robotball)
ctcallback = ctcb.CtCallback(base, robot, cdchecker, armname=armname)
smoother = sm.Smoother()
robot.goinitpose()
# robotnp = robotmesh.genmnp(robot, jawwidthrgt = 85.0, jawwidthlft=0.0)
# robotnp.reparentTo(base.render)
# base.run()
starttreesamplerate = 25
endtreesamplerate = 30
objstpos = np.array([354.5617667638, -400.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)
#start and goal joints and move the robot
goalpos1 = np.array([600, 400, 1550]) # 1350 #500
goalrot1 = np.array([[0.0, 0.0, 1.0], [-1.0, 0.0, 0.0], [0.0, -1.0, 0.0]])
goalrot1 = rot_transform(goalrot1, 0)
start_armjnts = rbt.numik(startpos, startrot, "lft")
goal1_armjnts = rbt.numik(goalpos1, goalrot1, "lft")
rbt.movearmfk(start_armjnts, "lft")
rbt.movearmfk(rbt.initrgtjnts, "rgt")
# prepare the planner
rbtball = ur3dualball.Ur3DualBall()
cdchecker = cdck.CollisionCheckerBall(rbtball)
ctcallback = ctcb.CtCallback(base,
rbt,
cdchecker=cdchecker,
ctchecker=ctchecker,
ctangle=ctangle,
armname="lft")
planner = rrtc.RRTConnect(start=start_armjnts,
goal=goal1_armjnts,
ctcallback=ctcallback,
starttreesamplerate=30.0,
endtreesamplerate=30.0,
expanddis=10,
maxiter=2000.0,
maxtime=100.0)
smoother = sm.Smoother()
#calculate the object pose relative to the end effector pose
rbtmesh = rbtmg.genmnp(rbt, jawwidthlft=obj.height)
leepos = rbt.lftarm[-1]['linkend']
