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