def movejntsin360(self):
"""
the function move all joints back to -360,360
due to multi R problem, the last joint could be out of 360
this function moves the last joint back
:return:
author: weiwei
date: 20180202
"""
rgtarmjnts = ur3u.getjnts('rgt')
lftarmjnts = ur3u.getjnts('lft')
rgtarmjnts = rm.cvtRngPM360(rgtarmjnts)
lftarmjnts = rm.cvtRngPM360(lftarmjnts)
ur3u.movejntssgl(rgtarmjnts, armname='rgt')
ur3u.movejntssgl(lftarmjnts, armname='lft')
def numik(ur5dualrobot, tgtpos, tgtrot, armname="rgt"):
"""
solve the ik numerically for the specified armname
:param ur5dualrobot: see the ur5.ur5dual 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 armname: 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 armname != "rgt" and armname != "lft":
raise ep.ValueError
# stablizer
steplength = 30
armjntssave = ur5dualrobot.getarmjnts(armname)
armjntsiter = armjntssave.copy()
for i in range(500):
armjac = jacobian(ur5dualrobot, armname)
if np.linalg.matrix_rank(armjac) == 6:
err = tcperror(ur5dualrobot, tgtpos, tgtrot, armname)
dq = steplength * (np.linalg.lstsq(armjac, err))[0]
else:
print("The Jacobian Matrix of the specified arm is at singularity")
ur5dualrobot.movearmfk(armjntssave, armname)
return None
if np.linalg.norm(err) < 1e-4:
if ur5dualrobot.chkrng(armjntsiter, armname):
armjntsreturn = ur5dualrobot.getarmjnts(armname)
ur5dualrobot.movearmfk(armjntssave, armname)
return armjntsreturn
else:
ur5dualrobot.movearmfk(armjntssave, armname)
return None
else:
# todo dq definition
armjntsiter += dq
armjntsiter = rm.cvtRngPM360(armjntsiter)
# print dq
# print armjntsiter
if ur5dualrobot.chkrng(armjntsiter, armname) or i < 150:
# print armjntsiter
ur5dualrobot.movearmfk(armjntsiter, armname)
# import ur5dualplot
# import manipulation.grip.hrp5three.hrp5three as handpkg
# ur5dualplot.plotstick(base.render, ur5dualrobot)
# ur5mnp = ur5dualplot.genUr5dualmnp(ur5dualrobot, handpkg)
# ur5mnp.reparentTo(base.render)
else:
ur5dualrobot.movearmfk(armjntssave, armname)
return None
return None
def movejntsin360(self):
"""
the function move all joints back to -360,360
due to multi R problem, the last joint could be out of 360
this function moves the last joint back
:return:
author: weiwei
date: 20180202
"""
armjnts = self.getjnts()
armjnts = rm.cvtRngPM360(armjnts)
self.movejntssgl(armjnts)
def anaik(robot, tgtpos, tgtrot, armid="rgt"):
"""
solve the ik analytically for the specified armid
:param robot: see the ur3dual.Ur3SglRobot 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: 20180203
"""
if armid!="rgt" and armid!="lft":
raise ep.ValueError
# stablizer
steplength = 5
steplengthinc = 10
armjntssave = robot.getarmjnts(armid)
armjntsiter = robot.getinitarmjnts(armid)
robot.movearmfk(armjntsiter, armid)
errnormlast = 0.0
nlocalencountered = 0
for i in range(100):
armjac = jacobian(robot, armid)
if np.linalg.matrix_rank(armjac) == 6:
err = tcperror(robot, tgtpos, tgtrot, armid)
dq = steplength * (np.linalg.lstsq(armjac, err))[0]
else:
print( "The Jacobian Matrix of the specified arm is at singularity")
break
# print(np.linalg.norm(err))
# if np.linalg.norm(err)<1e-4:
errnorm = np.linalg.norm(err)
if errnorm < 1e-6:
# print('goal reached', armjntsiter)
# print("number of iteration ", i)
# if hrp5nrobot.chkrng(armjntsiter, armid):
armjntsreturn = robot.getarmjnts(armid)
robot.movearmfk(armjntssave, armid)
return armjntsreturn
else:
# todo dq definition
# judge local minima
if abs(errnorm - errnormlast) < 1e-6:
nlocalencountered += 1
print( "local minima at iteration", i)
print( "n local encountered", nlocalencountered)
steplength = 3
steplengthinc = 7
if nlocalencountered > 2:
break
else:
if steplengthinc == 10 and steplength < 50:
steplength = steplength + steplengthinc
armjntsiter += dq
# armjntsiter = rm.cvtRngPM180(armjntsiter)
armjntsiter = rm.cvtRngPM360(armjntsiter)
bdragged, jntangles = robot.chkrngdrag(armjntsiter, armid)
armjntsiter[:] = jntangles[:]
robot.movearmfk(jntangles, armid)
# print(jntangles)
# ur5sglstick = ur5sglmeshgen.gensnp(ur5sglrobot)
# ur5sglstick.reparentTo(base.render)
errnormlast = errnorm
# print(errnorm)
# ur5sglmnp = ur5sglmeshgen.genmnp(ur5sglrobot)
# ur5sglmnp.reparentTo(base.render)
robot.movearmfk(armjntssave, armid)
return None
def numikmsc(robot, tgtpos, tgtrot, msc, armname="rgt"):
"""
solve the ik numerically for the specified armname with manually specified starting configuration (msc)
:param robot: see the robot 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 armname: a string "rgt" or "lft" indicating the arm tht will be solved
:return: armjnts: a 1-by-6 numpy ndarray
author: weiwei
date: 20180808, osaka
"""
if armname != "rgt" and armname != "lft":
raise ValueError
# stablizer
steplength = 5
steplengthinc = 10
armjntssave = robot.getarmjnts(armname)
armjntsiter = copy.deepcopy(msc)
robot.movearmfk(armjntsiter, armname)
errnormlast = 0.0
nlocalencountered = 0
for i in range(100):
armjac = jacobian(robot, armname)
if np.linalg.matrix_rank(armjac) == 6:
err = tcperror(robot, tgtpos, tgtrot, armname)
dq = steplength * (np.linalg.lstsq(armjac, err, rcond=None))[0]
else:
print("The Jacobian Matrix of the specified arm is at singularity")
break
# print np.linalg.norm(err)
# if np.linalg.norm(err)<1e-4:
errnorm = np.linalg.norm(err)
if errnorm < 1e-6:
# print 'goal reached', armjntsiter
# print "number of iteration ", i
# if hrp5nrobot.chkrng(armjntsiter, armname):
armjntsreturn = robot.getarmjnts(armname)
robot.movearmfk(armjntssave, armname)
return armjntsreturn
else:
# todo dq definition
# judge local minima
if abs(errnorm - errnormlast) < 1e-6:
nlocalencountered += 1
print("local minima at iteration", i)
print("n local encountered", nlocalencountered)
steplength = 3
steplengthinc = 7
if nlocalencountered > 0:
break
else:
if steplengthinc == 10 and steplength < 50:
steplength = steplength + steplengthinc
armjntsiter += dq
# armjntsiter = rm.cvtRngPM180(armjntsiter)
armjntsiter = rm.cvtRngPM360(armjntsiter)
bdragged, jntangles = robot.chkrngdrag(armjntsiter, armname)
armjntsiter[:] = jntangles[:]
robot.movearmfk(jntangles, armname)
# print jntangles
# ur5sglstick = ur5sglmeshgen.gensnp(ur5sglrobot)
# ur5sglstick.reparentTo(base.render)
errnormlast = errnorm
# print errnorm
# ur5sglmnp = ur5sglmeshgen.genmnp(ur5sglrobot)
# ur5sglmnp.reparentTo(base.render)
robot.movearmfk(armjntssave, armname)
return None