def plannplay(self, transitoption="useboth", choice="startrgtgoallft"):
"""
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,
rtq85nm,
self.gdb,
base,
self.obstaclecmlist,
transitoption=transitoption)
regrip.addStartGoal(startrotmat4=startrotmat4,
goalrotmat4=goalrotmat4,
choice=choice)
cdchecker = ccball.CollisionCheckerBall(self.robotball)
# OUTERLOOP collision detection
objmsmp, numikrmsmp, jawwidthmp, originalpathnidlist = rp.planRegrasp(
self.robot,
regrip,
self.objcm,
cdchecker,
self.obstaclecmlist,
switch="OC",
previous=[],
end=False,
togglemp=True)
if objmsmp is None:
return False
nextid = regrip.regg.node[originalpathnidlist[-1]]['globalgripid']
for id in range(1, len(self.goallist) - 1):
isend = False
if id == len(self.goallist) - 2:
isend = True
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()
regrip.addStartGoal(startrotmat4=startrotmat4,
goalrotmat4=goalrotmat4,
choice="startrgtgoallft",
startgraspgid=nextid,
starttoolvec=[0, 0, 0],
goaltoolvec=[0, 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=isend,
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.ur3u.movejntssgl(self.robot.initjnts[3:9], 'rgt')
# self.ur3u.movejntssgl(self.robot.initjnts[9:15], 'lft')
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]
temp_jawwidthmpactive = jawwidthmp[motionseccounter[0]
- 1] # rgt and left
#
jntslist_rgt = []
jntslist_lft = []
jawwidthlist_rgt = []
jawwidthlist_lft = []
for eachtemp in temp_numikrmsmpactive:
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
# if len(jntslist_rgt) == 2:
# # do nothing; We dont need to examine lft since they are symmetric
# pass
# else:
# # check if the arm is moved
# rgtdiff = abs(np.array(jntslist_rgt[0])-np.array(jntslist_rgt[-1]))
# if rgtdiff.max() > 1e-6:
# self.ur3u.movejntssgl_cont(jntslist_rgt,armname="rgt")
# lftdiff = abs(np.array(jntslist_lft[0])-np.array(jntslist_lft[-1]))
# if lftdiff.max() > 1e-6:
# self.ur3u.movejntssgl_cont(jntslist_lft, armname="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
return True
else:
return False
if __name__ == '__main__':
import robotsim.nextage.nxtplot as nxtplot
base = pandactrl.World()
robot = nxt.NxtRobot()
handpkg = rtq85nm
robotmesh = nxtmesh.NxtMesh(handpkg)
robotball = nxtball.NxtBall()
# cdchecker = cdck.CollisionChecker(robotmesh)
cdchecker = cdck.CollisionCheckerBall(robotball)
start = [50.0, 0.0, -143.0, 0.0, 0.0, 0.0]
goal = [-15.0, 0.0, -143.0, 0.0, 0.0, 0.0]
# plot init and goal
robot.movearmfk(armjnts=start, armid='rgt')
robotmesh.genmnp(robot).reparentTo(base.render)
robot.movearmfk(armjnts=goal, armid='rgt')
robotmesh.genmnp(robot).reparentTo(base.render)
jointlimits = [[robot.rgtarm[1]['rngmin'], robot.rgtarm[1]['rngmax']],
[robot.rgtarm[2]['rngmin'], robot.rgtarm[2]['rngmax']],
[robot.rgtarm[3]['rngmin'], robot.rgtarm[3]['rngmax']],
[robot.rgtarm[4]['rngmin'], robot.rgtarm[4]['rngmax']],
[robot.rgtarm[5]['rngmin'], robot.rgtarm[5]['rngmax']],
[robot.rgtarm[6]['rngmin'], robot.rgtarm[6]['rngmax']]]
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
objpos_lft = []
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):
