def __genPandaRotmat4(self, icolevel=1, angles=[0, 45, 90, 135, 180, 225, 270, 315]):
"""
generate panda3d rotmat4 using icospheres and rotationaangle each origin-vertex vector of the icosphere
:param icolevel, the default value 1 = 42vertices
:param angles, 8 directions by default
:return: a list of [pandarotmat4, ...] size of the inner list is size of the angles
author: weiwei
date: 20170221, tsukuba (copied from regrasp/floatingposes.py)
"""
objnp = pg.genObjmnp(self.obj0path)
mat4list = []
self.icos = trimesh.creation.icosphere(icolevel)
initmat4 = objnp.getMat()
for vert in self.icos.vertices:
mat4list.append([])
objnp.lookAt(vert[0], vert[1], vert[2])
ytozmat4 = Mat4.rotateMat(-90, objnp.getMat().getRow3(0))
newobjmat4 = objnp.getMat() * ytozmat4
for angle in angles:
tmppandamat4 = Mat4.rotateMat(angle, newobjmat4.getRow3(2))
tmppandamat4 = newobjmat4 * tmppandamat4
mat4list[-1].append(tmppandamat4)
objnp.setMat(initmat4)
self.floatingposemat4 = [e for l in mat4list for e in l]
def __genPandaRotmat4(self, icolevel=1, angles=[0, 45, 90, 135, 180, 225, 270, 315]):
"""
generate panda3d rotmat4 using icospheres and rotationaangle each origin-vertex vector of the icosphere
:param icolevel, the default value 1 = 42vertices
:param angles, 8 directions by default
:return: a list of [pandarotmat4, ...] size of the inner list is size of the angles
author: weiwei
date: 20170221, tsukuba (copied from regrasp/floatingposes.py)
"""
objnp = pg.genObjmnp(self.obj0path)
mat4list = []
self.icos = trimesh.creation.icosphere(icolevel)
initmat4 = objnp.getMat()
for vert in self.icos.vertices:
mat4list.append([])
objnp.lookAt(vert[0], vert[1], vert[2])
ytozmat4 = Mat4.rotateMat(-90, objnp.getMat().getRow3(0))
newobjmat4 = objnp.getMat() * ytozmat4
for angle in angles:
tmppandamat4 = Mat4.rotateMat(angle, newobjmat4.getRow3(2))
tmppandamat4 = newobjmat4 * tmppandamat4
mat4list[-1].append(tmppandamat4)
objnp.setMat(initmat4)
self.floatingposemat4 = [e for l in mat4list for e in l]
def updateshow(objms, numikrms, jawwidth, nxtmnp, objmnp, counter,
nxtrobot, handpkg, objpath, task):
if counter[0] < len(numikrms):
if nxtmnp[0] is not None:
nxtmnp[0].detachNode()
if objmnp[0] is not None:
objmnp[0].detachNode()
print counter[0]
print numikrms[counter[0]]
nxtrobot.movearmfkr(numikrms[counter[0]])
#move robot
alljnts = [numikrms[counter[0]][0], 0, 0]
alljnts.extend([i for i in numikrms[counter[0]][1]])
alljnts.extend([i for i in nxtrobot.initjnts[9:]])
nxtrobot.movealljnts(alljnts)
nxtmnp[0] = nxtplot.genmnp(nxtrobot,
handpkg,
jawwidthrgt=jawwidth[counter[0]])
nxtrobot.goinitpose()
nxtmnp[0].reparentTo(base.render)
objmnp[0] = pg.genObjmnp(objpath, color=Vec4(.7, .7, 0, 1))
objmnp[0].setMat(objms[counter[0]])
objmnp[0].reparentTo(base.render)
if counter[0] >= 1:
if base.inputmgr.keyMap['space'] is False:
pass
else:
c2e = counter[0] - 1
alljnts = [numikrms[c2e][0], 0, 0]
alljnts.extend([i for i in numikrms[c2e][1]])
alljnts.extend([i for i in nxtrobot.initjnts[9:]])
nxts.movejnts15(alljnts)
if jawwidth[c2e] < 85:
rtq85rs.openhandto(0)
else:
rtq85rs.openhandto(85)
counter[0] += 1
base.inputmgr.keyMap['space'] = False
else:
counter[0] += 1
else:
if base.inputmgr.keyMap['space'] is True:
c2e = counter[0] - 1
alljnts = [numikrms[c2e][0], 0, 0]
alljnts.extend([i for i in numikrms[c2e][1]])
alljnts.extend([i for i in nxtrobot.initjnts[9:]])
nxts.movejnts15(alljnts)
if jawwidth[c2e] < 85:
rtq85rs.openhandto(0)
else:
rtq85rs.openhandto(85)
nxts.movejnts15(nxtrobot.initjnts)
return task.done
return task.again
def updateshow(objms0, objms1, numikrms, jawwidth, nxtmnp, obj0mnp,
obj1mnp, counter, nxtrobot, obj0path, obj1path, task):
base.win.saveScreenshot(Filename(str(counter[0]) + '.jpg'))
if counter[0] < len(numikrms):
if nxtmnp[0] is not None:
nxtmnp[0].detachNode()
if obj0mnp[0] is not None:
obj0mnp[0].detachNode()
if obj1mnp[0] is not None:
obj1mnp[0].detachNode()
print counter[0]
print numikrms[counter[0]]
alljnts = [numikrms[counter[0]][0], 0, 0]
alljnts.extend([i for i in numikrms[counter[0]][1]])
alljnts.extend([i for i in numikrms[counter[0]][2]])
nxtrobot.movealljnts(alljnts)
nxtmnp[0] = nxtplot.genmnp(nxtrobot,
handpkg,
jawwidthrgt=jawwidth[counter[0]][0],
jawwidthlft=jawwidth[counter[0]][1])
nxtrobot.goinitpose()
nxtmnp[0].reparentTo(base.render)
obj0mnp[0] = pg.genObjmnp(obj0path, color=Vec4(.7, .3, 0, 1))
obj0mnp[0].setMat(objms0[counter[0]])
# pg.plotAxisSelf(base.render,objms0[counter[0]].getRow3(3), objms[counter[0]])
obj0mnp[0].reparentTo(base.render)
obj1mnp[0] = pg.genObjmnp(obj1path, color=Vec4(0, .3, .7, 1))
obj1mnp[0].setMat(objms1[counter[0]])
# pg.plotAxisSelf(base.render,objms1[counter[0]].getRow3(3), objms[counter[0]])
obj1mnp[0].reparentTo(base.render)
counter[0] += 1
else:
if nxtmnp[0] is not None:
nxtmnp[0].detachNode()
nxtrobot.goinitpose()
nxtmnp[0] = nxtplot.genmnp(nxtrobot, handpkg)
nxtmnp[0].reparentTo(base.render)
# counter[0] = 0
return task.done
pass
return task.again
def updateshow(objms0, objms1, numikrms, jawwidth, nxtmnp, obj0mnp, obj1mnp, counter, nxtrobot, obj0path, obj1path, task):
base.win.saveScreenshot(Filename(str(counter[0])+'.jpg'))
if counter[0] < len(numikrms):
if nxtmnp[0] is not None:
nxtmnp[0].detachNode()
if obj0mnp[0] is not None:
obj0mnp[0].detachNode()
if obj1mnp[0] is not None:
obj1mnp[0].detachNode()
print counter[0]
print numikrms[counter[0]]
alljnts = [numikrms[counter[0]][0], 0, 0]
alljnts.extend([i for i in numikrms[counter[0]][1]])
alljnts.extend([i for i in numikrms[counter[0]][2]])
nxtrobot.movealljnts(alljnts)
nxtmnp[0] = nxtplot.genmnp(nxtrobot, handpkg, jawwidthrgt=jawwidth[counter[0]][0], jawwidthlft=jawwidth[counter[0]][1])
nxtrobot.goinitpose()
nxtmnp[0].reparentTo(base.render)
obj0mnp[0] = pg.genObjmnp(obj0path, color = Vec4(.7,.3,0,1))
obj0mnp[0].setMat(objms0[counter[0]])
# pg.plotAxisSelf(base.render,objms0[counter[0]].getRow3(3), objms[counter[0]])
obj0mnp[0].reparentTo(base.render)
obj1mnp[0] = pg.genObjmnp(obj1path, color = Vec4(0,.3,.7,1))
obj1mnp[0].setMat(objms1[counter[0]])
# pg.plotAxisSelf(base.render,objms1[counter[0]].getRow3(3), objms[counter[0]])
obj1mnp[0].reparentTo(base.render)
counter[0] += 1
else:
if nxtmnp[0] is not None:
nxtmnp[0].detachNode()
nxtrobot.goinitpose()
nxtmnp[0] = nxtplot.genmnp(nxtrobot, handpkg)
nxtmnp[0].reparentTo(base.render)
# counter[0] = 0
return task.done
pass
return task.again
def updateshow(objms, numikrms, jawwidth, hrp5mnp, objmnp, counter, hrp5robot, objpath, task):
if counter[0] < len(numikrms):
if hrp5mnp[0] is not None:
hrp5mnp[0].detachNode()
if objmnp[0] is not None:
objmnp[0].detachNode()
print counter[0]
print numikrms[counter[0]]
hrp5robot.movearmfkr(numikrms[counter[0]])
hrp5mnp[0] = hrp5plot.genHrp5mnp(hrp5robot, jawwidthrgt=jawwidth[counter[0]])
hrp5robot.goinitpose()
hrp5mnp[0].reparentTo(base.render)
objmnp[0] = pg.genObjmnp(objpath, color = Vec4(.7,.7,0,1))
objmnp[0].setMat(objms[counter[0]])
# pg.plotAxisSelf(base.render,objms[counter[0]].getRow3(3), objms[counter[0]])
objmnp[0].reparentTo(base.render)
counter[0] += 1
return task.again
def updateshow(objms, numikrms, jawwidth, hrp5mnp, objmnp, counter,
hrp5robot, objpath, task):
if counter[0] < len(numikrms):
if hrp5mnp[0] is not None:
hrp5mnp[0].detachNode()
if objmnp[0] is not None:
objmnp[0].detachNode()
print counter[0]
print numikrms[counter[0]]
hrp5robot.movearmfkr(numikrms[counter[0]])
hrp5mnp[0] = hrp5plot.genHrp5mnp(hrp5robot,
jawwidthrgt=jawwidth[counter[0]])
hrp5robot.goinitpose()
hrp5mnp[0].reparentTo(base.render)
objmnp[0] = pg.genObjmnp(objpath, color=Vec4(.7, .7, 0, 1))
objmnp[0].setMat(objms[counter[0]])
# pg.plotAxisSelf(base.render,objms[counter[0]].getRow3(3), objms[counter[0]])
objmnp[0].reparentTo(base.render)
counter[0] += 1
return task.again
def updateshow(rbtmnp, objmnp, counter, robot, objpath, task):
if counter[0] < len(path):
if rbtmnp[0] is not None:
# rbtmnp[0].removeNode()
pass
if objmnp[0] is not None:
objmnp[0].removeNode()
robot.movearmfk(path[counter[0]], armid='rgt')
rbtmnp[0] = robotmesh.gensnp(robot=robot)
rbtmnp[0].reparentTo(base.render)
objmnp[0] = pg.genObjmnp(objpath, color=Vec4(.7, .7, 0, 1))
objmnp[0].setMat(obsrotmat4)
objmnp[0].reparentTo(base.render)
counter[0] += 1
else:
if rbtmnp[0] is not None:
rbtmnp[0].removeNode()
robot.goinitpose()
rbtmnp[0] = robotmesh.genmnp(robot)
rbtmnp[0].reparentTo(base.render)
return task.again
result = np.asarray(result)
idsglist = [int(x) for x in result[:, 0]]
sgrotmatlist = [dc.strToMat4(x) for x in result[:, 1]]
idfreeplacementlist = [int(x) for x in result[:, 2]]
for idstart, start in zip(idsglist, sgrotmatlist):
for idgoal, goal in zip(idsglist, sgrotmatlist):
# print idstart, idgoal
if idstart == 2 and idgoal == 1:
print start, goal
startrotmat4 = start
goalrotmat4 = goal
regrip.findshortestpath(start, goal, base)
break
#
objstart = pg.genObjmnp(objpath, color=Vec4(.7, .7, 0, 1))
objstart.setMat(startrotmat4)
objend = pg.genObjmnp(objpath, color=Vec4(.3, .3, 0, .5))
objend.setMat(goalrotmat4)
objstart.reparentTo(base.render)
objend.reparentTo(base.render)
print "path result on table", len(regrip.directshortestpaths)
# # # # toc = time.clock()
# # # # print len(regrip.directshortestpaths[0])
# # # # print toc-tic
# # # # assert False
# # # #
pltfig = plt.figure()
regrip.plotgraph(pltfig)
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']],
[robot.rgtarm[7]['rngmin'], robot.rgtarm[7]['rngmax']],
[robot.rgtarm[8]['rngmin'], robot.rgtarm[8]['rngmax']],
[robot.rgtarm[9]['rngmin'], robot.rgtarm[9]['rngmax']]]
import os
from panda3d.core import *
import pandaplotutils.pandageom as pg
obsrotmat4 = Mat4(1.0,0.0,0.0,0.0,0.0,1.0,0.0,0.0,0.0,0.0,1.0,0.0,405.252044678,-300.073120117,300.0000038147,1.0)
this_dir, this_filename = os.path.split(__file__)
objpath = os.path.join(os.path.split(this_dir)[0], "manipulation/grip", "objects", "tool.stl")
objmnp = pg.genObjmnp(objpath, color=Vec4(.7, .7, 0, 1))
objmnp.setMat(obsrotmat4)
objmnp.reparentTo(base.render)
# planner = rrt.RRT(start=start, goal=goal, iscollidedfunc = iscollidedfunc,
# jointlimits = jointlimits, expanddis = 5,
# robot = robot, cdchecker = cdchecker)
# planner = rrtc.RRTConnect(start=start, goal=goal, iscollidedfunc = iscollidedfunc,
# jointlimits = jointlimits, expanddis = 10, robot = robot, cdchecker = cdchecker)
# planner = ddrrt.DDRRT(start=start, goal=goal, iscollidedfunc = iscollidedfunc,
# jointlimits = jointlimits, goalsamplerate=30, expanddis = 5, robot = robot,
# cdchecker = cdchecker)
#
planner = ddrrtc.DDRRTConnect(start=start, goal=goal, iscollidedfunc = iscollidedfunc,
jointlimits = jointlimits, starttreesamplerate=30, expanddis = 5, robot = robot,
cdchecker = cdchecker)
def updateshow(objms0, objms1, numikrms, jawwidth, nxtmnp, obj0mnp,
obj1mnp, counter, nxtrobot, obj0path, obj1path, task):
if counter[0] < len(numikrms):
if nxtmnp[0] is not None:
nxtmnp[0].detachNode()
if obj0mnp[0] is not None:
obj0mnp[0].detachNode()
if obj1mnp[0] is not None:
obj1mnp[0].detachNode()
alljnts = [numikrms[counter[0]][0], 0, 0]
alljnts.extend([i for i in numikrms[counter[0]][1]])
alljnts.extend([i for i in numikrms[counter[0]][2]])
nxtrobot.movealljnts(alljnts)
nxtmnp[0] = nxtplot.genmnp(nxtrobot,
handpkg,
jawwidthrgt=jawwidth[counter[0]][0],
jawwidthlft=jawwidth[counter[0]][1])
nxtrobot.goinitpose()
nxtmnp[0].reparentTo(base.render)
obj0mnp[0] = pg.genObjmnp(obj0path, color=Vec4(.7, .3, 0, 1))
obj0mnp[0].setMat(objms0[counter[0]])
# pg.plotAxisSelf(base.render,objms0[counter[0]].getRow3(3), objms[counter[0]])
obj0mnp[0].reparentTo(base.render)
obj1mnp[0] = pg.genObjmnp(obj1path, color=Vec4(0, .3, .7, 1))
obj1mnp[0].setMat(objms1[counter[0]])
# pg.plotAxisSelf(base.render,objms1[counter[0]].getRow3(3), objms[counter[0]])
obj1mnp[0].reparentTo(base.render)
if counter[0] >= 1:
print base.inputmgr.keyMap['space']
if base.inputmgr.keyMap['space'] is False:
pass
else:
c2e = counter[0] - 1
alljnts = [numikrms[c2e][0], 0, 0]
alljnts.extend([i for i in numikrms[c2e][1]])
alljnts.extend([i for i in numikrms[c2e][2]])
nxts.movejnts15(alljnts)
if jawwidth[c2e][0] < 85:
rtq85rs.openhandto(0)
else:
rtq85rs.openhandto(85)
if jawwidth[c2e][1] < 85:
rtq85ls.openhandto(0)
else:
rtq85ls.openhandto(85)
counter[0] += 1
base.inputmgr.keyMap['space'] = False
else:
counter[0] += 1
else:
if base.inputmgr.keyMap['space'] is True:
c2e = counter[0] - 1
alljnts = [numikrms[c2e][0], 0, 0]
alljnts.extend([i for i in numikrms[c2e][1]])
alljnts.extend([i for i in numikrms[c2e][2]])
# nxts.movejnts15(alljnts)
if jawwidth[c2e][0] < 85:
rtq85rs.openhandto(0)
else:
rtq85rs.openhandto(85)
if jawwidth[c2e][1] < 85:
rtq85ls.openhandto(0)
else:
rtq85ls.openhandto(85)
# if nxtmnp[0] is not None:
# nxtmnp[0].detachNode()
# nxtrobot.goinitpose()
# nxtmnp[0] = nxtplot.genNxtmnp(nxtrobot)
# nxtmnp[0].reparentTo(base.render)
# counter[0] = 0
return task.done
return task.again
-2.18294604779e-16, -0.98311150074, -0.183007523417,
0.0, 399.926879883, -196.8688812256, 79.7615509033,
1.0)
# startrotmat4 = Mat4(0.129405856133,0.129405856133,0.98311150074,0.0,
# 0.707106769085,-0.707106769085,0.0,0.0,
# 0.69516479969,0.69516479969,-0.183007523417,0.0,
# 327.126998901,-102.976409912,19.7615509033,1.0)
# startrotmat4 = Mat4(-0.707106769085, 0.707106769085, 0.0, 0.0, -0.707106769085, -0.707106769085, 0.0, 0.0, 0.0, 0.0,
# 1.0, 0.0, 396.234527588, -396.33795166, -55.0000038147, 1.0)
goalrotmat4 = Mat4(-0.707106769085, -0.707106769085, 0.0, 0.0,
0.707106769085, -0.707106769085, 0.0, 0.0, 0.0, 0.0,
1.0, 0.0, 406.33795166, 103.76546859741, -55.0000038147,
1.0)
# goalrotmat4 = Mat4(-1.0,1.22464685259e-16,0.0,0.0,-1.22464685259e-16,-1.0,0.0,0.0,0.0,0.0,1.0,0.0,294.747955322,-200.073135376,-55.0000038147,1.0)
# goalrotmat4 = Mat4(-0.707106769085,0.707106769085,0.0,0.0,-0.707106769085,-0.707106769085,0.0,0.0,0.0,0.0,1.0,0.0,396.234527588,-396.33795166,-55.0000038147,1.0)
objstart = pg.genObjmnp(objpath, color=Vec4(.3, .0, .0, .1))
objstart.setMat(startrotmat4)
objend = pg.genObjmnp(objpath, color=Vec4(.0, .3, .0, .1))
objend.setMat(goalrotmat4)
objstart.reparentTo(base.render)
objend.reparentTo(base.render)
#tool
# startrotmat4 = Mat4(-0.0176398064941,-0.0176398064941,-0.99968880415,0.0,-0.707106769085,0.707106769085,0.0,0.0,0.706886708736,0.706886708736,-0.0249464549124,0.0,225.010162354,100,44.9175643921,1.0)
# startrotmat4 = Mat4(0.129405856133,0.129405856133,0.98311150074,0.0,0.707106769085,-0.707106769085,0.0,0.0,0.69516479969,0.69516479969,-0.183007523417,0.0,227.126983643,-327.023590088,74.7615509033,1.0)
# goalrotmat4 = Mat4(-1.0,1.22464685259e-16,0.0,0.0,-1.22464685259e-16,-1.0,0.0,0.0,0.0,0.0,1.0,0.0,294.747955322,-300.0731293559074,-3.99246982852e-06,1.0)
# planewheel
# startrotmat4 = Mat4(0.707106769085,0.707106769085,0.0,0.0,-4.32978030171e-17,4.32978030171e-17,-1.0,0.0,-0.707106769085,0.707106769085,6.12323426293e-17,0.0,400.0,-400.0,29.9999980927,1.0)
# goalrotmat4 = Mat4(0.707106769085,-0.707106769085,0.0,0.0,4.32978030171e-17,4.32978030171e-17,-1.0,0.0,0.707106769085,0.707106769085,6.12323426293e-17,0.0,400.0,-1.7017070846e-15,29.9999980927,1.0)
# planelowerbody
# startrotmat4 = Mat4(1.35963113277e-32,6.12323426293e-17,-1.0,0.0,-1.0,2.22044604925e-16,0.0,0.0,2.22044604925e-16,1.0,6.12323426293e-17,0.0,399.997558594,-16.3771038055,74.2884140015,1.0)
import numpy as np import pandaplotutils.pandactrl as pc import pandaplotutils.pandageom as pg import utils.robotmath as rm import trimesh import os from panda3d.core import Filename, TransparencyAttrib base = pc.World(camp=[1000, -300, 700], lookatp=[0, 10, 0], w=1000, h=1000) pggen = pg.PandaGeomGen() pggen.plotAxis(base.render) this_dir, this_filename = os.path.split(__file__) objpath = os.path.join(this_dir, "sandpart.stl") # objtrimesh=trimesh.load_mesh(objpath) # # # objmnp = pg.trimesh2Panda(objtrimesh) objmnp = pg.genObjmnp(objpath, color=(0, 0, 0, 1)) # # objmnp.reparentTo(base.render) # # eggpath = Filename.fromOsSpecific(os.path.join(this_dir, "bzcube.egg")) # objmnp2 = loader.loadModel(eggpath) # objmnp2.setScale(5.0) # objmnp2.setColor(0,0,1, 0.7) # objmnp2.setTransparency(TransparencyAttrib.MAlpha) # objmnp2.reparentTo(base.render) base.run()
def updateshow(objms0, objms1, numikrms, jawwidth, nxtmnp, obj0mnp, obj1mnp, counter, nxtrobot, obj0path, obj1path, task):
if counter[0] < len(numikrms):
if nxtmnp[0] is not None:
nxtmnp[0].detachNode()
if obj0mnp[0] is not None:
obj0mnp[0].detachNode()
if obj1mnp[0] is not None:
obj1mnp[0].detachNode()
alljnts = [numikrms[counter[0]][0], 0, 0]
alljnts.extend([i for i in numikrms[counter[0]][1]])
alljnts.extend([i for i in numikrms[counter[0]][2]])
nxtrobot.movealljnts(alljnts)
nxtmnp[0] = nxtplot.genmnp(nxtrobot, handpkg, jawwidthrgt=jawwidth[counter[0]][0], jawwidthlft=jawwidth[counter[0]][1])
nxtrobot.goinitpose()
nxtmnp[0].reparentTo(base.render)
obj0mnp[0] = pg.genObjmnp(obj0path, color = Vec4(.7,.3,0,1))
obj0mnp[0].setMat(objms0[counter[0]])
# pg.plotAxisSelf(base.render,objms0[counter[0]].getRow3(3), objms[counter[0]])
obj0mnp[0].reparentTo(base.render)
obj1mnp[0] = pg.genObjmnp(obj1path, color = Vec4(0,.3,.7,1))
obj1mnp[0].setMat(objms1[counter[0]])
# pg.plotAxisSelf(base.render,objms1[counter[0]].getRow3(3), objms[counter[0]])
obj1mnp[0].reparentTo(base.render)
if counter[0] >= 1:
print base.inputmgr.keyMap['space']
if base.inputmgr.keyMap['space'] is False:
pass
else:
c2e = counter[0]-1
alljnts = [numikrms[c2e][0], 0, 0]
alljnts.extend([i for i in numikrms[c2e][1]])
alljnts.extend([i for i in numikrms[c2e][2]])
nxts.movejnts15(alljnts)
if jawwidth[c2e][0] < 85:
rtq85rs.openhandto(0)
else:
rtq85rs.openhandto(85)
if jawwidth[c2e][1] < 85:
rtq85ls.openhandto(0)
else:
rtq85ls.openhandto(85)
counter[0] += 1
base.inputmgr.keyMap['space'] = False
else:
counter[0] += 1
else:
if base.inputmgr.keyMap['space'] is True:
c2e = counter[0]-1
alljnts = [numikrms[c2e][0], 0, 0]
alljnts.extend([i for i in numikrms[c2e][1]])
alljnts.extend([i for i in numikrms[c2e][2]])
# nxts.movejnts15(alljnts)
if jawwidth[c2e][0] < 85:
rtq85rs.openhandto(0)
else:
rtq85rs.openhandto(85)
if jawwidth[c2e][1] < 85:
rtq85ls.openhandto(0)
else:
rtq85ls.openhandto(85)
# if nxtmnp[0] is not None:
# nxtmnp[0].detachNode()
# nxtrobot.goinitpose()
# nxtmnp[0] = nxtplot.genNxtmnp(nxtrobot)
# nxtmnp[0].reparentTo(base.render)
# counter[0] = 0
return task.done
return task.again
nxtrobot.movearmfkr(numikrms[counter[0]])
nxtmnp[0] = nxtplot.genmnp(nxtrobot, handpkg, jawwidthrgt=jawwidth[counter[0]])
nxtrobot.goinitpose()
nxtmnp[0].reparentTo(base.render)
objmnp[0] = pg.genObjmnp(objpath, color = Vec4(.7,.7,0,1))
objmnp[0].setMat(objms[counter[0]])
# pg.plotAxisSelf(base.render,objms[counter[0]].getRow3(3), objms[counter[0]])
objmnp[0].reparentTo(base.render)
counter[0] += 1
return task.again
taskMgr.doMethodLater(1, updateshow, "updateshow",
extraArgs = [objms, numikrms, jawwidth, nxtmnp, objmnp, counter, nxtrobot, handpkg, objpath],
appendTask = True)
# one time show for start and end
objstart = pg.genObjmnp(objpath, color=Vec4(.7, .7, 0, 1))
objstart.setMat(startrotmat4)
objend = pg.genObjmnp(objpath, color=Vec4(.3, .3, 0, .5))
objend.setMat(goalrotmat4)
objstart.reparentTo(base.render)
objend.reparentTo(base.render)
# this_dir, this_filename = os.path.split(__file__)
# ttpath = Filename.fromOsSpecific(
# os.path.join(os.path.split(this_dir)[0] + os.sep, "grip", "supports", "tabletop.egg"))
# ttnodepath = NodePath("tabletop")
# ttl = loader.loadModel(ttpath)
# ttl.instanceTo(ttnodepath)
# ttnodepath.reparentTo(base.render)
hrp5nmnp = hrp5nmeshgen.genmnp(hrp5nrobot)
# hrp5nmnp.reparentTo(base.render)
startrotmat4 = Mat4(1.0, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 1.0,
0.0, 305.252044678, -400.073120117, -55.0000038147,
1.0)
startrotmat4 = Mat4(1.0, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 1.0,
0.0, 405.252044678, -199.926864624, -55.0000038147,
1.0)
import os
this_dir, this_filename = os.path.split(__file__)
objpath = os.path.join(
os.path.split(this_dir)[0], "../manipulation/grip", "objects",
"tool.stl")
objmnp = pg.genObjmnp(objpath, color=Vec4(.7, .7, 0, 1))
objmnp.setMat(startrotmat4)
# objmnp.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)
#
# ttube
startrotmat4 = Mat4(-4.06358332355e-17,-0.183007523417,0.98311150074,0.0,
-1.0,2.22044604925e-16,0.0,0.0,
-2.18294604779e-16,-0.98311150074,-0.183007523417,0.0,
399.926879883,-196.8688812256,79.7615509033,1.0)
# startrotmat4 = Mat4(0.129405856133,0.129405856133,0.98311150074,0.0,
# 0.707106769085,-0.707106769085,0.0,0.0,
# 0.69516479969,0.69516479969,-0.183007523417,0.0,
# 327.126998901,-102.976409912,19.7615509033,1.0)
# startrotmat4 = Mat4(-0.707106769085, 0.707106769085, 0.0, 0.0, -0.707106769085, -0.707106769085, 0.0, 0.0, 0.0, 0.0,
# 1.0, 0.0, 396.234527588, -396.33795166, -55.0000038147, 1.0)
goalrotmat4 = Mat4(-0.707106769085,-0.707106769085,0.0,0.0,0.707106769085,-0.707106769085,0.0,0.0,0.0,0.0,1.0,0.0,406.33795166,103.76546859741,-55.0000038147,1.0)
# goalrotmat4 = Mat4(-1.0,1.22464685259e-16,0.0,0.0,-1.22464685259e-16,-1.0,0.0,0.0,0.0,0.0,1.0,0.0,294.747955322,-200.073135376,-55.0000038147,1.0)
# goalrotmat4 = Mat4(-0.707106769085,0.707106769085,0.0,0.0,-0.707106769085,-0.707106769085,0.0,0.0,0.0,0.0,1.0,0.0,396.234527588,-396.33795166,-55.0000038147,1.0)
objstart = pg.genObjmnp(objpath, color=Vec4(.3, .0, .0, .1))
objstart.setMat(startrotmat4)
objend = pg.genObjmnp(objpath, color=Vec4(.0, .3, .0, .1))
objend.setMat(goalrotmat4)
objstart.reparentTo(base.render)
objend.reparentTo(base.render)
#tool
# startrotmat4 = Mat4(-0.0176398064941,-0.0176398064941,-0.99968880415,0.0,-0.707106769085,0.707106769085,0.0,0.0,0.706886708736,0.706886708736,-0.0249464549124,0.0,225.010162354,100,44.9175643921,1.0)
# startrotmat4 = Mat4(0.129405856133,0.129405856133,0.98311150074,0.0,0.707106769085,-0.707106769085,0.0,0.0,0.69516479969,0.69516479969,-0.183007523417,0.0,227.126983643,-327.023590088,74.7615509033,1.0)
# goalrotmat4 = Mat4(-1.0,1.22464685259e-16,0.0,0.0,-1.22464685259e-16,-1.0,0.0,0.0,0.0,0.0,1.0,0.0,294.747955322,-300.0731293559074,-3.99246982852e-06,1.0)
# planewheel
# startrotmat4 = Mat4(0.707106769085,0.707106769085,0.0,0.0,-4.32978030171e-17,4.32978030171e-17,-1.0,0.0,-0.707106769085,0.707106769085,6.12323426293e-17,0.0,400.0,-400.0,29.9999980927,1.0)
# goalrotmat4 = Mat4(0.707106769085,-0.707106769085,0.0,0.0,4.32978030171e-17,4.32978030171e-17,-1.0,0.0,0.707106769085,0.707106769085,6.12323426293e-17,0.0,400.0,-1.7017070846e-15,29.9999980927,1.0)
# planelowerbody
# startrotmat4 = Mat4(1.35963113277e-32,6.12323426293e-17,-1.0,0.0,-1.0,2.22044604925e-16,0.0,0.0,2.22044604925e-16,1.0,6.12323426293e-17,0.0,399.997558594,-16.3771038055,74.2884140015,1.0)
