import pandaplotutils.pandactrl as pandactrl
from direct.filter.CommonFilters import CommonFilters
import hrp5nmesh
from manipulation.grip.hrp5three import hrp5threenm
from manipulation.grip.robotiq85 import rtq85nm
# loadPrcFileData("", "want-directtools #t")
# loadPrcFileData("", "want-tk #t")
base = pandactrl.World()
hrp5nrobot = Hrp5NRobot()
# hrp5nrobot.goinitpose()
import hrp5nplot
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)
import pandaplotutils.pandactrl as pandactrl
from direct.filter.CommonFilters import CommonFilters
import hrp5nmesh
from manipulation.grip.hrp5three import hrp5threenm
from manipulation.grip.robotiq85 import rtq85nm
# loadPrcFileData("", "want-directtools #t")
# loadPrcFileData("", "want-tk #t")
base = pandactrl.World()
hrp5nrobot = Hrp5NRobot()
# hrp5nrobot.goinitpose()
import hrp5nplot
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]])
# plot goal axis
pg.plotAxisSelf(nodepath = base.render, spos = objpos, pandamat4 = pg.cvtMat4(objrot))
# 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)
def numik(hrp5nrobot, tgtpos, tgtrot, armid="rgt"):
"""
solve the ik numerically for the specified armid
:param hrp5robot: see hrp5robot.Hrp5Robot class
:param tgtpos: the position of the goal, 1-by-3 numpy ndarray
:param tgtrot: the orientation of the goal, 3-by-3 numpyndarray
:param armid: a string "rgt" or "lft" indicating the arm that will be solved
:return: armjnts: a 1-by-6 numpy ndarray
author: weiwei
date: 20161205
"""
if armid != "rgt" and armid != "lft":
raise ep.ValueError
# stablizer
steplength = 5
steplengthinc = 10
armjntssave = hrp5nrobot.getarmjnts(armid)
armjntsiter = armjntssave.copy()
errnormlast = 0.0
nlocalencountered = 0
for i in range(100):
armjac = jacobian(hrp5nrobot, armid)
if np.linalg.matrix_rank(armjac) == 6:
err = tcperror(hrp5nrobot, tgtpos, tgtrot, armid)
dq = steplength * (np.linalg.lstsq(armjac, err))[0]
else:
print "The Jacobian Matrix of the specified arm is at singularity"
# hrp5nrobot.movearmfk(armjntssave, armid)
# return None
break
print np.linalg.norm(err)
# if np.linalg.norm(err)<1e-4:
errnorm = np.linalg.norm(err)
if errnorm < 1:
print 'goal reached', armjntsiter
print "number of iteration ", i
# if hrp5nrobot.chkrng(armjntsiter, armid):
armjntsreturn = hrp5nrobot.getarmjnts(armid)
hrp5nrobot.movearmfk(armjntssave, armid)
return armjntsreturn
# else:
# # import hrp5nplot
# # import manipulation.grip.hrp5three.hrp5three as handpkg
# # # hrp5nplot.plotstick(base.render, hrp5nrobot)
# # hrp5nmnp = hrp5nplot.genmnp(hrp5nrobot, handpkg)
# # hrp5nmnp.reparentTo(base.render)
# # print "out of range"
# # hrp5nrobot.movearmfk(armjntssave, armid)
# # return None
# break
else:
# todo dq definition
# judge local minima
if abs(errnorm - errnormlast) < 1e-3:
nlocalencountered += 1
print "local minima at iteration", i
print "n local encountered", nlocalencountered
steplength = 3
steplengthinc = 7
if nlocalencountered > 2:
break
else:
if steplength < 50:
steplength = steplength + steplengthinc
armjntsiter += dq
armjntsiter = rm.cvtRngPM180(armjntsiter)
# print armjntsiter
# for i in range(armjntsiter.shape[0]):
# armjntsiter[i] = armjntsiter[i]%360
# print armjntsiter
# the robot may encounter overrange errors in the first few iterations
# use i<50 to avoid these errors
# if hrp5nrobot.chkrng(armjntsiter, armid) or i < 30:
# # print armjntsiter
# hrp5nrobot.movearmfk(armjntsiter, armid)
# # import hrp5plot
# # hrp5plot.plotstick(base.render, hrp5robot)
# # hrp5mnp = hrp5plot.genHrp5mnp(hrp5robot)
# # hrp5mnp.reparentTo(base.render)
# # nxtmnp = nxtplot.genNxtmnp(nxtrobot)
# # nxtmnp.reparentTo(base.render)
# # import hrp5nplot
# # import manipulation.grip.hrp5three.hrp5three as handpkg
# # # hrp5nplot.plotstick(base.render, hrp5nrobot)
# # hrp5nmnp = hrp5nplot.genHrp5Nmnp_nm(hrp5nrobot, handpkg)
# # hrp5nmnp.reparentTo(base.render)
# else:
# # import hrp5plot
# # hrp5plot.plotstick(base.render, hrp5robot)
# hrp5nrobot.movearmfk(armjntssave, armid)
# return None
bdragged, jntangles = hrp5nrobot.chkrngdrag(armjntsiter, armid)
armjntsiter[:] = jntangles[:]
print jntangles
hrp5nrobot.movearmfk(jntangles, armid)
import hrp5nplot
hrp5nplot.plotstick(base.render, hrp5nrobot)
errnormlast = errnorm
print errnorm
import manipulation.grip.hrp5three.hrp5three as handpkg
hrp5nmnp = hrp5nplot.genmnp(hrp5nrobot, handpkg)
hrp5nmnp.reparentTo(base.render)
hrp5nrobot.movearmfk(armjntssave, armid)
return None
def numik(hrp5nrobot, tgtpos, tgtrot, armid="rgt"):
"""
solve the ik numerically for the specified armid
:param hrp5robot: see hrp5robot.Hrp5Robot class
:param tgtpos: the position of the goal, 1-by-3 numpy ndarray
:param tgtrot: the orientation of the goal, 3-by-3 numpyndarray
:param armid: a string "rgt" or "lft" indicating the arm that will be solved
:return: armjnts: a 1-by-6 numpy ndarray
author: weiwei
date: 20161205
"""
if armid!="rgt" and armid!="lft":
raise ep.ValueError
# stablizer
steplength = 5
steplengthinc = 10
armjntssave = hrp5nrobot.getarmjnts(armid)
armjntsiter = armjntssave.copy()
errnormlast = 0.0
nlocalencountered = 0
for i in range(100):
armjac = jacobian(hrp5nrobot, armid)
if np.linalg.matrix_rank(armjac) == 6:
err = tcperror(hrp5nrobot, tgtpos, tgtrot, armid)
dq = steplength * (np.linalg.lstsq(armjac, err))[0]
else:
print "The Jacobian Matrix of the specified arm is at singularity"
# hrp5nrobot.movearmfk(armjntssave, armid)
# return None
break
print np.linalg.norm(err)
# if np.linalg.norm(err)<1e-4:
errnorm = np.linalg.norm(err)
if errnorm < 1:
print 'goal reached', armjntsiter
print "number of iteration ", i
# if hrp5nrobot.chkrng(armjntsiter, armid):
armjntsreturn = hrp5nrobot.getarmjnts(armid)
hrp5nrobot.movearmfk(armjntssave, armid)
return armjntsreturn
# else:
# # import hrp5nplot
# # import manipulation.grip.hrp5three.hrp5three as handpkg
# # # hrp5nplot.plotstick(base.render, hrp5nrobot)
# # hrp5nmnp = hrp5nplot.genmnp(hrp5nrobot, handpkg)
# # hrp5nmnp.reparentTo(base.render)
# # print "out of range"
# # hrp5nrobot.movearmfk(armjntssave, armid)
# # return None
# break
else:
# todo dq definition
# judge local minima
if abs(errnorm - errnormlast) < 1e-3:
nlocalencountered += 1
print "local minima at iteration", i
print "n local encountered", nlocalencountered
steplength = 3
steplengthinc = 7
if nlocalencountered > 2:
break
else:
if steplength < 50:
steplength = steplength + steplengthinc
armjntsiter += dq
armjntsiter = rm.cvtRngPM180(armjntsiter)
# print armjntsiter
# for i in range(armjntsiter.shape[0]):
# armjntsiter[i] = armjntsiter[i]%360
# print armjntsiter
# the robot may encounter overrange errors in the first few iterations
# use i<50 to avoid these errors
# if hrp5nrobot.chkrng(armjntsiter, armid) or i < 30:
# # print armjntsiter
# hrp5nrobot.movearmfk(armjntsiter, armid)
# # import hrp5plot
# # hrp5plot.plotstick(base.render, hrp5robot)
# # hrp5mnp = hrp5plot.genHrp5mnp(hrp5robot)
# # hrp5mnp.reparentTo(base.render)
# # nxtmnp = nxtplot.genNxtmnp(nxtrobot)
# # nxtmnp.reparentTo(base.render)
# # import hrp5nplot
# # import manipulation.grip.hrp5three.hrp5three as handpkg
# # # hrp5nplot.plotstick(base.render, hrp5nrobot)
# # hrp5nmnp = hrp5nplot.genHrp5Nmnp_nm(hrp5nrobot, handpkg)
# # hrp5nmnp.reparentTo(base.render)
# else:
# # import hrp5plot
# # hrp5plot.plotstick(base.render, hrp5robot)
# hrp5nrobot.movearmfk(armjntssave, armid)
# return None
bdragged, jntangles = hrp5nrobot.chkrngdrag(armjntsiter, armid)
armjntsiter[:] = jntangles[:]
print jntangles
hrp5nrobot.movearmfk(jntangles, armid)
import hrp5nplot
hrp5nplot.plotstick(base.render, hrp5nrobot)
errnormlast = errnorm
print errnorm
import manipulation.grip.hrp5three.hrp5three as handpkg
hrp5nmnp = hrp5nplot.genmnp(hrp5nrobot, handpkg)
hrp5nmnp.reparentTo(base.render)
hrp5nrobot.movearmfk(armjntssave, armid)
return None