- 当--iktype=transform6d时，要求规划组中非固定关节数必须是4、5或6。
- 关节定义成fixed类型时，不要让出现<axis>、<limit>。否则会让joint.IsStatic()误判断为true，导致GetNumJoints()数目错误变大。
| iktype | solvefn | chaintree | outputnames | 说明 |
| transform6d | solveFullIK_6D | SolverIKChainTransform6D | 'eerot[0]','eerot[1]','eerot[2]','eetrans[0]', 'eerot[3]','eerot[4]','eerot[5]','eetrans[1]', 'eerot[6]','eerot[7]','eerot[8]','eetrans[2]' | moveit位姿规划时 |
| ray4d | solveFullIK_Ray4D | SolverIKChainRay | 'eetrans[0]','eetrans[1]','eetrans[2]','eerot[0]', 'eerot[1]','eerot[2]' | |
| lookat3d | solveFullIK_Lookat3D | SolverIKChainLookat3D | 'eetrans[0]','eetrans[1]','eetrans[2]','eerot[0]', 'eerot[1]','eerot[2]' |
总逻辑（可认为IKFastSolver是最顶类）
- 由参数iktype得到slovefn。
- 调用IKFastSolver.generateIkSolver，该函数依次执行。
1. forwardKinematicsChain，得到要计算的非固定关节，存到jointvars。
2. 以jointvars为参数，调用solvefn，得到chaintree。 - 以chaintree为参数，调用IKFastSolver.writeIkSolver，生成源码code。
- 将生成的源码code写入参数savefile指定的文件。
一、为方便调试，如何修改源码
- 确定可以改的源码：ikfast.py、ikfast_generator_cpp.py。
- 两个py都位在/usr/local/lib/python2.7/dist-packages/openravepy/_openravepy_目录。在这里目录不支持就地改。
- 1）_openravepy_，给这两个位件做改名备份。2）把这两文件复制到一个有权限修改的目录。3）改完后，执行复制：sudo cp ikfast.py /usr/local/lib/python2.7/dist-packages/openravepy/_openravepy_ /.
二、源码
示例：sudo python `openrave-config --python-dir`/openravepy/_openravepy_/ikfast.py --robot=ur5.dae --iktype=transform6d --baselink=0 --eelink=9 --savefile=$(pwd)/ikfast61.cpp
2.1 __main__
- --robot(type='string', default=None)。机器人文件，支持的格式：COLLADA、OpenRAVE XML。示例：ur5.dae。
- --savefile(type='string', default='ik.cpp')。生成代码想到存储到的文件。示例：$(pwd)/ikfast61.cpp。
- --baselink(type='int')。要逆求解的规划组开始link索引。可用“openrave-robot.py ur5.dae --info links”得到值。示例：0。
- --eelink(type='int')。要逆求解的规划组结束link索引。可用“openrave-robot.py ur5.dae --info links”得到值。示例：9。
- --freeindex(type='int', default=[])。help='Optional joint index specifying a free parameter of the manipulator. If not specified, assumes all joints not solving for are free parameters. Can be specified multiple times for multiple free parameters.')。示例：没设置，即使用默认值。
- --iktype(default='transform6d')。逆求解类型，对moveit要求的位姿规划，要没意外应该使用transform6d。示例transform6d。
- --maxcasedepth(type='int', default=3)。help='The max depth to go into degenerate cases. If ikfast file is too big, try reducing this, (default=%default).')。示例：没设置，即使用默认值。
- --lang(type='string', default='cpp')。要生成哪种语言的源码。示例：没设置，即使用默认值，生成cpp源码。
- --debug(type='int', default=logging.INFO)。debug level。示例：没设置，即使用默认值。
<openravepy>/_openravepy_/ikfast.py
------
if __name__ == '__main__':
import openravepy
parser = OptionParser(...)
...
(options, args) = parser.parse_args()
if options.robot is None or options.baselink is None or options.eelink is None:
print('Error: Not all arguments specified')
sys.exit(1)
format = logging.Formatter('%(levelname)s: %(message)s')
handler = logging.StreamHandler(sys.stdout)
handler.setFormatter(format)
log.addHandler(handler)
log.setLevel(options.debug)
solvefn=IKFastSolver.GetSolvers()[options.iktype.lower()]solvefn是函数指针，指向IKFastSolver.solveFullIK_6D。
if options.robot is not None: try: env=openravepy.Environment() kinbody=env.ReadRobotXMLFile(options.robot) env.Add(kinbody) solver = IKFastSolver(kinbody,kinbody) solver.maxcasedepth = options.maxcasedepth chaintree = solver.generateIkSolver(options.baselink,options.eelink,options.freeindices,solvefn=solvefn)
返回值chaintree是个对象，类型SolverIKChainTransform6D。chaintree = solvefn(self, LinksRaw, jointvars, isolvejointvars)[IKFastSolver.solveFullIK_6D]
参数jointvars=[j0,j1,j2,j3], isolvejointvars=[0,1,2,3]
chaintree = AST.SolverIKChainTransform6D(...)。AST(Abstarct Syntax Tree)是class，SolverIKChainTransform6D是AST内的一个class。
code=solver.writeIkSolver(chaintree,lang=options.lang)
生成c++文件。返回值code是字符串，就要要写入ikfast61_arm.cpp内容。
finally: openravepy.RaveDestroy() if len(code) > 0: with open(options.savefile,'w') as f:
把code内容写入ikfast61.cpp
f.write(code)
2.2 IKFastSolver.generateIkSolver
class IKFastSolver(AutoReloader): def generateIkSolver(self, baselink, eelink, freeindices=None, solvefn=None, ikfastoptions=0): """ :param ikfastoptions: options that control how ikfast. """ self._CheckPreemptFn(progress=0) if solvefn is None: solvefn = IKFastSolver.solveFullIK_6D chainlinks = self.kinbody.GetChain(baselink,eelink,returnjoints=False) chainjoints = self.kinbody.GetChain(baselink,eelink,returnjoints=True) LinksRaw, jointvars = self.forwardKinematicsChain(chainlinks,chainjoints)
print(jointvars): [j0, j1, j2]
for T in LinksRaw: log.info('[' + ','.join(['[%s, %s, %s, %s]'%(T[i,0],T[i,1],T[i,2],T[i,3]) for i in range(3)]) + ']') self.degeneratecases = None if freeindices is None: # need to iterate through all combinations of free joints assert(0) isolvejointvars = [] solvejointvars = [] self._ikfastoptions = ikfastoptions self.ifreejointvars = [] self.freevarsubs = [] self.freevarsubsinv = [] self.freevars = [] self.freejointvars = [] self.invsubs = [] for i,v in enumerate(jointvars): var = self.Variable(v) axis = self.axismap[v.name] dofindex = axis.joint.GetDOFIndex()+axis.iaxis if dofindex in freeindices: # convert all free variables to constants self.ifreejointvars.append(i) self.freevarsubs += [(cos(var.var), var.cvar), (sin(var.var), var.svar)] self.freevarsubsinv += [(var.cvar,cos(var.var)), (var.svar,sin(var.var))] self.freevars += [var.cvar,var.svar] self.freejointvars.append(var.var) else: solvejointvars.append(v) isolvejointvars.append(i) self.invsubs += [(var.cvar,cos(v)),(var.svar,sin(v))] self._solvejointvars = solvejointvars self._jointvars = jointvars # set up the destination symbols self.Tee = eye(4) for i in range(0,3): for j in range(0,3): self.Tee[i,j] = Symbol("r%d%d"%(i,j)) self.Tee[0,3] = Symbol("px") self.Tee[1,3] = Symbol("py") self.Tee[2,3] = Symbol("pz") r00,r01,r02,px,r10,r11,r12,py,r20,r21,r22,pz = self.Tee[0:12] self.pp = Symbol('pp') self.ppsubs = [(self.pp,px**2+py**2+pz**2)] self.npxyz = [Symbol('npx'),Symbol('npy'),Symbol('npz')] self.npxyzsubs = [(self.npxyz[i],px*self.Tee[0,i]+py*self.Tee[1,i]+pz*self.Tee[2,i]) for i in range(3)] # cross products between columns of self.Tee self.rxp = [] self.rxpsubs = [] for i in range(3): self.rxp.append([Symbol('rxp%d_%d'%(i,j)) for j in range(3)]) c = self.Tee[0:3,i].cross(self.Tee[0:3,3]) self.rxpsubs += [(self.rxp[-1][j],c[j]) for j in range(3)] # have to include new_rXX self.pvars = self.Tee[0:12]+self.npxyz+[self.pp]+self.rxp[0]+self.rxp[1]+self.rxp[2] + [Symbol('new_r00'), Symbol('new_r01'), Symbol('new_r02'), Symbol('new_r10'), Symbol('new_r11'), Symbol('new_r12'), Symbol('new_r20'), Symbol('new_r21'), Symbol('new_r22')] self._rotsymbols = list(self.Tee[0:3,0:3]) # add positions ip = 9 inp = 12 ipp = 15 irxp = 16 self._rotpossymbols = self._rotsymbols + list(self.Tee[0:3,3])+self.npxyz+[self.pp]+self.rxp[0]+self.rxp[1]+self.rxp[2] # groups of rotation variables that are unit vectors self._rotnormgroups = [] for i in range(3): self._rotnormgroups.append([self.Tee[i,0],self.Tee[i,1],self.Tee[i,2],S.One]) self._rotnormgroups.append([self.Tee[0,i],self.Tee[1,i],self.Tee[2,i],S.One]) self._rotposnormgroups = list(self._rotnormgroups) self._rotposnormgroups.append([self.Tee[0,3],self.Tee[1,3],self.Tee[2,3],self.pp]) # dot product of rotation rows and columns is always 0 self._rotdotgroups = [] for i,j in combinations(range(3),2): self._rotdotgroups.append([[i,j],[i+3,j+3],[i+6,j+6],S.Zero]) self._rotdotgroups.append([[3*i,3*j],[3*i+1,3*j+1],[3*i+2,3*j+2],S.Zero]) self._rotposdotgroups = list(self._rotdotgroups) for i in range(3): self._rotposdotgroups.append([[i,ip],[i+3,ip+1],[i+6,ip+2],self.npxyz[i]]) self._rotposdotgroups.append([[3*i+0,inp],[3*i+1,inp+1],[3*i+2,inp+2],self.Tee[i,3]]) self._rotcrossgroups = [] # cross products of rotation rows and columns always yield the left over vector for i,j,k in [(0,1,2),(0,2,1),(1,2,0)]: # column self._rotcrossgroups.append([[i+3,j+6],[i+6,j+3],k]) self._rotcrossgroups.append([[i+6,j],[i,j+6],k+3]) self._rotcrossgroups.append([[i,j+3],[i+3,j],k+6]) # row self._rotcrossgroups.append([[3*i+1,3*j+2],[3*i+2,3*j+1],3*k]) self._rotcrossgroups.append([[3*i+2,3*j],[3*i,3*j+2],3*k+1]) self._rotcrossgroups.append([[3*i,3*j+1],[3*i+1,3*j],3*k+2]) # swap if sign is negative: if j!=1+i if j!=1+i: for crossgroup in self._rotcrossgroups[-6:]: crossgroup[0],crossgroup[1] = crossgroup[1],crossgroup[0] # add positions self._rotposcrossgroups = list(self._rotcrossgroups) for i in range(3): # column i cross position self._rotposcrossgroups.append([[i+3,ip+2],[i+6,ip+1],irxp+3*i+0]) self._rotposcrossgroups.append([[i+6,ip+0],[i,ip+2],irxp+3*i+1]) self._rotposcrossgroups.append([[i,ip+1],[i+3,ip+0],irxp+3*i+2]) self.Teeinv = self.affineInverse(self.Tee) LinksLeft = [] if self.useleftmultiply: while not self.has(LinksRaw[0],*solvejointvars): LinksLeft.append(LinksRaw.pop(0)) LinksLeftInv = [self.affineInverse(T) for T in LinksLeft] self.testconsistentvalues = None self.gsymbolgen = cse_main.numbered_symbols('gconst') self.globalsymbols = [] self._scopecounter = 0 # before passing to the solver, set big numbers to constant variables, this will greatly reduce computation times self.Teeleftmult = self.multiplyMatrix(LinksLeft) # the raw ee passed to the ik solver function self._CheckPreemptFn(progress=0.01)
此时print(jointvars): [j0, j1, j2]
chaintree = solvefn(self, LinksRaw, jointvars, isolvejointvars) if self.useleftmultiply: chaintree.leftmultiply(Tleft=self.multiplyMatrix(LinksLeft), Tleftinv=self.multiplyMatrix(LinksLeftInv[::-1])) chaintree.dictequations += self.globalsymbols return chaintree
2.3 IKFastSolver.forwardKinematicsChain
class IKFastSolver(AutoReloader): def forwardKinematicsChain(self, chainlinks, chainjoints): """The first and last matrices returned are always non-symbolic """ with self.kinbody: assert(len(chainjoints)+1==len(chainlinks)) Links = [] Tright = eye(4) jointvars = [] jointinds = [] for i,joint in enumerate(chainjoints):
print(joint)：RaveGetEnvironment(1).GetKinBody('tank_arm').GetJoint('arm_Joint')。其它joint，就是GetJoint中的“arm_Joint”不一样。
if len(joint.GetName()) == 0: raise self.CannotSolveError('chain %s:%s contains a joint with no name!'%(chainlinks[0].GetName(),chainlinks[-1].GetName())) if chainjoints[i].GetHierarchyParentLink() == chainlinks[i]: TLeftjoint = self.GetMatrixFromNumpy(joint.GetInternalHierarchyLeftTransform()) TRightjoint = self.GetMatrixFromNumpy(joint.GetInternalHierarchyRightTransform()) axissign = S.One else: TLeftjoint = self.affineInverse(self.GetMatrixFromNumpy(joint.GetInternalHierarchyRightTransform())) TRightjoint = self.affineInverse(self.GetMatrixFromNumpy(joint.GetInternalHierarchyLeftTransform())) axissign = -S.One if joint.IsStatic(): Tright = self.affineSimplify(Tright * TLeftjoint * TRightjoint)
IsStatic()什么时候返回true？——当joint是fixed类型时，但光有它是不够的，不要求内中不存能在<axis>、<limit>，虽然fixed时，这两个标签已没意义。
else: Tjoints = [] for iaxis in range(joint.GetDOF()): var = None if joint.GetDOFIndex() >= 0:
join.GetDOFIndex()返回该关节自由度，对一个值的，GetDOFIndex()是1。
var = Symbol(self.axismapinv[joint.GetDOFIndex()])
用print(var)，得到的值是类似j0、j1、j2。
cosvar = cos(var) sinvar = sin(var) jointvars.append(var) elif joint.IsMimic(iaxis): # get the mimic equation var = joint.GetMimicEquation(iaxis) for itestjoint, testjoint in enumerate(chainjoints): var = var.replace(testjoint.GetName(), 'j%d'%testjoint.GetDOFIndex()) # this needs to be reduced! cosvar = expand_trig(cos(var)) # cos(j1+j2) -> cos(j1)*cos(j2) - sin(j1)*sin(j2) sinvar = expand_trig(sin(var)) elif joint.IsStatic(): # joint doesn't move so assume identity pass else: raise ValueError('cannot solve for mechanism when a non-mimic passive joint %s is in chain'%str(joint)) Tj = eye(4) if var is not None: jaxis = axissign*self.numpyVectorToSympy(joint.GetInternalHierarchyAxis(iaxis)) if joint.IsRevolute(iaxis): Tj[0:3,0:3] = self.rodrigues2(jaxis,cosvar,sinvar) elif joint.IsPrismatic(iaxis): Tj[0:3,3] = jaxis*(var) else: raise ValueError('failed to process joint %s'%joint.GetName()) Tjoints.append(Tj) if axisAngleFromRotationMatrix is not None: axisangle = axisAngleFromRotationMatrix(numpy.array(numpy.array(Tright * TLeftjoint),numpy.float64)) angle = sqrt(axisangle[0]**2+axisangle[1]**2+axisangle[2]**2) if angle > 1e-8: axisangle = axisangle/angle log.debug('rotation angle of Links[%d]: %f, axis=[%f,%f,%f]', len(Links), (angle*180/pi).evalf(),axisangle[0],axisangle[1],axisangle[2]) Links.append(self.RoundMatrix(Tright * TLeftjoint)) for Tj in Tjoints: jointinds.append(len(Links)) Links.append(Tj) Tright = TRightjoint Links.append(self.RoundMatrix(Tright))
至此：jointinds=[1, 3, 5]。jointvars=[j0, j1, j2]。
# before returning the final links, try to push as much translation components
# outwards to both ends. Sometimes these components can get in the way of detecting
# intersecting axes
if len(jointinds) > 0:
iright = jointinds[-1]
Ttrans = eye(4)
Ttrans[0:3,3] = Links[iright-1][0:3,0:3].transpose() * Links[iright-1][0:3,3]
Trot_with_trans = Ttrans * Links[iright]
separated_trans = Trot_with_trans[0:3,0:3].transpose() * Trot_with_trans[0:3,3]
for j in range(0,3):
if separated_trans[j].has(*jointvars):
Ttrans[j,3] = Rational(0)
else:
Ttrans[j,3] = separated_trans[j]
Links[iright+1] = Ttrans * Links[iright+1]
Links[iright-1] = Links[iright-1] * self.affineInverse(Ttrans)
log.info("moved translation %s to right end",Ttrans[0:3,3].transpose())
if len(jointinds) > 1:
ileft = jointinds[0]
separated_trans = Links[ileft][0:3,0:3] * Links[ileft+1][0:3,3]
Ttrans = eye(4)
for j in range(0,3):
if not separated_trans[j].has(*jointvars):
Ttrans[j,3] = separated_trans[j]
Links[ileft-1] = Links[ileft-1] * Ttrans
Links[ileft+1] = self.affineInverse(Ttrans) * Links[ileft+1]
log.info("moved translation %s to left end",Ttrans[0:3,3].transpose())
if len(jointinds) > 3: # last 3 axes always have to be intersecting, move the translation of the first axis to the left
ileft = jointinds[-3]
separated_trans = Links[ileft][0:3,0:3] * Links[ileft+1][0:3,3]
Ttrans = eye(4)
for j in range(0,3):
if not separated_trans[j].has(*jointvars):
Ttrans[j,3] = separated_trans[j]
Links[ileft-1] = Links[ileft-1] * Ttrans
Links[ileft+1] = self.affineInverse(Ttrans) * Links[ileft+1]
log.info("moved translation on intersecting axis %s to left",Ttrans[0:3,3].transpose())
return Links, jointvars2.4 IKFastSolver.solveFullIK_6D
class IKFastSolver(AutoReloader): def solveFullIK_6D(self, LinksRaw, jointvars, isolvejointvars,Tmanipraw=eye(4)): """Solves the full 6D translatio + rotation IK """ self._iktype = 'transform6d' Tgripper = eye(4) for i in range(4): for j in range(4): Tgripper[i,j] = self.convertRealToRational(Tmanipraw[i,j]) Tfirstright = LinksRaw[-1]*Tgripper Links = LinksRaw[:-1] LinksInv = [self.affineInverse(link) for link in Links] self.Tfinal = self.multiplyMatrix(Links) self.testconsistentvalues = self.ComputeConsistentValues(jointvars,self.Tfinal,numsolutions=4) endbranchtree = [AST.SolverStoreSolution (jointvars,isHinge=[self.IsHinge(var.name) for var in jointvars])] solvejointvars = [jointvars[i] for i in isolvejointvars]
isolvejointvars: [0, 1, 2]，jointvars: [j0, j1, j2]，solvejointvars: [j0, j1, j2]。
if len(solvejointvars) > 6 or len(solvejointvars) < 4:
raise self.CannotSolveError('need at most 6 joints')对tank_arm，由于solvejointvars是[j0, j1, j2]，长度是3，会抛出'need at most 6 joints'。
log.info('ikfast 6d: %s',solvejointvars) tree = self.TestIntersectingAxes(solvejointvars,Links, LinksInv,endbranchtree) if tree is None: sliderjointvars = [var for var in solvejointvars if not self.IsHinge(var.name)] if len(sliderjointvars) > 0: ZeroMatrix = zeros(4) for i,Tlink in enumerate(Links): if self.has(Tlink,*sliderjointvars): # try sliding left if i > 0: ileftsplit = None for isplit in range(i-1,-1,-1): M = self.multiplyMatrix(Links[isplit:i]) if M*Tlink-Tlink*M != ZeroMatrix: break if self.has(M,*solvejointvars): # surpassed a variable! ileftsplit = isplit if ileftsplit is not None: # try with the new order log.info('rearranging Links[%d] to Links[%d]',i,ileftsplit) NewLinks = list(Links) NewLinks[(ileftsplit+1):(i+1)] = Links[ileftsplit:i] NewLinks[ileftsplit] = Links[i] NewLinksInv = list(LinksInv) NewLinksInv[(ileftsplit+1):(i+1)] = Links[ileftsplit:i] NewLinksInv[ileftsplit] = LinksInv[i] tree = self.TestIntersectingAxes(solvejointvars,NewLinks, NewLinksInv,endbranchtree) if tree is not None: break # try sliding right if i+1 < len(Links): irightsplit = None for isplit in range(i+1,len(Links)): M = self.multiplyMatrix(Links[i+1:(isplit+1)]) if M*Tlink-Tlink*M != ZeroMatrix: break if self.has(M,*solvejointvars): # surpassed a variable! irightsplit = isplit if irightsplit is not None: log.info('rearranging Links[%d] to Links[%d]',i,irightsplit) # try with the new order NewLinks = list(Links) NewLinks[i:irightsplit] = Links[(i+1):(irightsplit+1)] NewLinks[irightsplit] = Links[i] NewLinksInv = list(LinksInv) NewLinksInv[i:irightsplit] = LinksInv[(i+1):(irightsplit+1)] NewLinksInv[irightsplit] = LinksInv[i] tree = self.TestIntersectingAxes(solvejointvars,NewLinks, NewLinksInv,endbranchtree) if tree is not None: break if tree is None: linklist = list(self.iterateThreeNonIntersectingAxes(solvejointvars,Links, LinksInv)) # first try LiWoernleHiller since it is most robust for ilinklist, (T0links, T1links) in enumerate(linklist): log.info('try first group %d/%d', ilinklist, len(linklist)) try: # if T1links[-1] doesn't have any symbols, put it over to T0links. Since T1links has the position unknowns, putting over the coefficients to T0links makes things simpler if not self.has(T1links[-1], *solvejointvars): T0links.append(self.affineInverse(T1links.pop(-1))) tree = self.solveFullIK_6DGeneral(T0links, T1links, solvejointvars, endbranchtree, usesolvers=1) break except (self.CannotSolveError,self.IKFeasibilityError) as e: log.warn('%s',e) if tree is None: log.info('trying the rest of the general ik solvers') for ilinklist, (T0links, T1links) in enumerate(linklist): log.info('try second group %d/%d', ilinklist, len(linklist)) try: # if T1links[-1] doesn't have any symbols, put it over to T0links. Since T1links has the position unknowns, putting over the coefficients to T0links makes things simpler if not self.has(T1links[-1], *solvejointvars): T0links.append(self.affineInverse(T1links.pop(-1))) tree = self.solveFullIK_6DGeneral(T0links, T1links, solvejointvars, endbranchtree, usesolvers=6) break except (self.CannotSolveError,self.IKFeasibilityError) as e: log.warn('%s',e) if tree is None: raise self.CannotSolveError('cannot solve 6D mechanism!') chaintree = AST.SolverIKChainTransform6D([(jointvars[ijoint],ijoint) for ijoint in isolvejointvars], [(v,i) for v,i in izip(self.freejointvars,self.ifreejointvars)], (self.Tee * self.affineInverse(Tfirstright)).subs(self.freevarsubs), tree,Tfk=self.Tfinal*Tfirstright) chaintree.dictequations += self.ppsubs+self.npxyzsubs+self.rxpsubs return chaintree