def numik(hrp2krobot, tgtpos, tgtrot, armid="rgt"):
"""
solve the ik numerically for the specified armid
:param hrp2krobot: see hrp2krobot.Hrp2KRobot 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 ValueError
# stablizer
steplength = 30
armjntssave = hrp2krobot.getarmjnts(armid)
armjntsiter = armjntssave.copy()
for i in range(500):
armjac = jacobian(hrp2krobot, armid)
if np.linalg.matrix_rank(armjac) == 6:
err = tcperror(hrp2krobot, tgtpos, tgtrot, armid)
dq = steplength*(np.linalg.lstsq(armjac, err))[0]
else:
print("The Jacobian Matrix of the specified arm is at singularity")
hrp2krobot.movearmfk(armjntssave, armid)
return None
if np.linalg.norm(err)<1e-4:
if hrp2krobot.chkrng(armjntsiter, armid):
armjntsreturn = hrp2krobot.getarmjnts(armid)
hrp2krobot.movearmfk(armjntssave, armid)
return armjntsreturn
else:
hrp2krobot.movearmfk(armjntssave, armid)
return None
else:
# todo dq definition
armjntsiter += dq
armjntsiter = rm.cvtRngPM180(armjntsiter)
if hrp2krobot.chkrng(armjntsiter, armid) or i < 30:
hrp2krobot.movearmfk(armjntsiter, armid)
# import hrp2kplot
# from manipulation.grip.robotiq85 import rtq85nm
# handpkg = rtq85nm
# hrp2kplot.plotstick(base.render, hrp2krobot)
# print(hrp2krobot.rgtarm[hrp2krobot.targetjoints[-1]]['linkend'])
# hrp2kmnp_nm = hrp2kplot.genmnp_nm(hrp2krobot, handpkg)
# hrp2kmnp_nm.setColor(.7,.2,0.7,.1)
# hrp2kmnp_nm.reparentTo(base.render)
else:
hrp2krobot.movearmfk(armjntssave, armid)
return None
hrp2krobot.movearmfk(armjntssave, armid)
return None
def numik(hrp2krobot, tgtpos, tgtrot, armid="rgt"):
"""
solve the ik numerically for the specified armid
:param hrp2krobot: see hrp2krobot.Hrp2KRobot 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 = 30
armjntssave = hrp2krobot.getarmjnts(armid)
armjntsiter = armjntssave.copy()
for i in range(500):
armjac = jacobian(hrp2krobot, armid)
if np.linalg.matrix_rank(armjac) == 6:
err = tcperror(hrp2krobot, tgtpos, tgtrot, armid)
dq = steplength*(np.linalg.lstsq(armjac, err))[0]
else:
print "The Jacobian Matrix of the specified arm is at singularity"
hrp2krobot.movearmfk(armjntssave, armid)
return None
if np.linalg.norm(err)<1e-4:
if hrp2krobot.chkrng(armjntsiter, armid):
armjntsreturn = hrp2krobot.getarmjnts(armid)
hrp2krobot.movearmfk(armjntssave, armid)
return armjntsreturn
else:
hrp2krobot.movearmfk(armjntssave, armid)
return None
else:
# todo dq definition
armjntsiter += dq
armjntsiter = rm.cvtRngPM180(armjntsiter)
if hrp2krobot.chkrng(armjntsiter, armid) or i < 30:
hrp2krobot.movearmfk(armjntsiter, armid)
# import hrp2kplot
# from manipulation.grip.robotiq85 import rtq85nm
# handpkg = rtq85nm
# hrp2kplot.plotstick(base.render, hrp2krobot)
# print hrp2krobot.rgtarm[hrp2krobot.targetjoints[-1]]['linkend']
# hrp2kmnp_nm = hrp2kplot.genmnp_nm(hrp2krobot, handpkg)
# hrp2kmnp_nm.setColor(.7,.2,0.7,.1)
# hrp2kmnp_nm.reparentTo(base.render)
else:
hrp2krobot.movearmfk(armjntssave, armid)
return None
hrp2krobot.movearmfk(armjntssave, armid)
return None
def numik(nxtrobot, tgtpos, tgtrot, armid="rgt"):
"""
solve the ik numerically for the specified armid
:param nxtrobot: see nextage.NxtRobot 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: 20161111
"""
if armid != "rgt" and armid != "lft":
raise ep.ValueError
# armlj = nxtrobot.rgtarm
# if armid == "lft":
# armlj = nxtrobot.lftarm
# stablizer
steplength = 30
armjntssave = nxtrobot.getarmjnts(armid)
armjntsiter = armjntssave.copy()
for i in range(500):
armjac = jacobian(nxtrobot, armid)
if abs(np.linalg.det(armjac)) > 1e-6:
err = tcperror(nxtrobot, tgtpos, tgtrot, armid)
dq = steplength * (np.linalg.solve(armjac, err))
else:
print "The Jacobian Matrix of the specified arm is at singularity"
nxtrobot.movearmfk6(armjntssave, armid)
return None
if np.linalg.norm(err) < 1e-4:
armjntsreturn = nxtrobot.getarmjnts(armid)
nxtrobot.movearmfk(armjntssave, armid)
return armjntsreturn
else:
armjntsiter += dq
armjntsiter = rm.cvtRngPM180(armjntsiter)
if nxtrobot.chkrng(armjntsiter) or i < 10:
nxtrobot.movearmfk(armjntsiter, armid)
import nxtplot
# nxtplot.plotstick(base.render, nxtrobot)
# nxtmnp = nxtplot.genNxtmnp_nm(nxtrobot,plotcolor=[.5,.5,0.1,.2])
# nxtmnp.reparentTo(base.render)
# nxtmnp = nxtplot.genNxtmnp(nxtrobot)
# nxtmnp.reparentTo(base.render)
else:
nxtrobot.movearmfk(armjntssave, armid)
# print "No feasible IK"
return None
nxtrobot.movearmfk(armjntssave, armid)
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(nxtrobot, tgtpos, tgtrot, armid="rgt"):
"""
solve the ik numerically for the specified armid
:param nxtrobot: see nextage.NxtRobot 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: 20161111
"""
if armid != "rgt" and armid != "lft":
raise ValueError
# armlj = nxtrobot.rgtarm
# if armid == "lft":
# armlj = nxtrobot.lftarm
# stablizer
steplength = 5
steplengthinc = 10
armjntssave = nxtrobot.getarmjnts(armid)
armjntsiter = nxtrobot.getinitarmjnts(armid)
nxtrobot.movearmfk(armjntsiter)
errnormlast = 0.0
nlocalencountered = 0
for i in range(100):
armjac = jacobian(nxtrobot, armid)
if np.linalg.matrix_rank(armjac) == 6:
err = tcperror(nxtrobot, 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))
errnorm = np.linalg.norm(err)
if errnorm < 1:
# print('goal reached', armjntsiter)
# print("number of iteration ", i)
armjntsreturn = nxtrobot.getarmjnts(armid)
nxtrobot.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 steplength < 50:
steplength = steplength + steplengthinc
armjntsiter += dq
armjntsiter = rm.cvtRngPM180(armjntsiter)
bdragged, jntangles = nxtrobot.chkrngdrag(armjntsiter, armid)
armjntsiter[:] = jntangles[:]
nxtrobot.movearmfk(jntangles, armid)
# print(jntangles)
# import nxtplot
# nxtplot.plotstick(base.render, nxtrobot)
errnormlast = errnorm
# print(errnorm)
# print("out of max iteration")
nxtrobot.movearmfk(armjntssave, armid)
return None
def numik(nxtrobot, tgtpos, tgtrot, armid="rgt"):
"""
solve the ik numerically for the specified armid
:param nxtrobot: see nextage.NxtRobot 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: 20161111
"""
if armid!="rgt" and armid!="lft":
raise ep.ValueError
# armlj = nxtrobot.rgtarm
# if armid == "lft":
# armlj = nxtrobot.lftarm
# stablizer
steplength = 5
steplengthinc = 10
armjntssave = nxtrobot.getarmjnts(armid)
armjntsiter = armjntssave.copy()
errnormlast = 0.0
nlocalencountered = 0
for i in range(100):
armjac = jacobian(nxtrobot, armid)
if np.linalg.matrix_rank(armjac) == 6:
err = tcperror(nxtrobot, 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)
errnorm = np.linalg.norm(err)
if errnorm < 1:
# print 'goal reached', armjntsiter
# print "number of iteration ", i
armjntsreturn = nxtrobot.getarmjnts(armid)
nxtrobot.movearmfk(armjntssave, armid)
return armjntsreturn
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)
bdragged, jntangles = nxtrobot.chkrngdrag(armjntsiter, armid)
armjntsiter[:] = jntangles[:]
nxtrobot.movearmfk(jntangles, armid)
# print jntangles
# import nxtplot
# nxtplot.plotstick(base.render, nxtrobot)
errnormlast = errnorm
# print errnorm
# print "out of max iteration"
nxtrobot.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
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 = 30
armjntssave = hrp5nrobot.getarmjnts(armid)
armjntsiter = armjntssave.copy()
for i in range(500):
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
if np.linalg.norm(err) < 1e-4:
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.genHrp5Nmnp_nm(hrp5nrobot, handpkg)
# hrp5nmnp.reparentTo(base.render)
# print "out of range"
hrp5nrobot.movearmfk(armjntssave, armid)
return None
else:
# todo dq definition
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
hrp5nrobot.movearmfk(armjntssave, armid)
return None
