parameters: (self, T0links, T1links, transvars,rotvars,solveRotationFirst,endbranchtree)
docstring: Solve 6D equations using fact that 3 axes are intersecting. The 3 intersecting axes are all part of T0links and will be used to compute the rotation of the robot. The other 3 axes are part of T1links and will be used to first compute the position.
docstring_summary: Solve 6D equations using fact that 3 axes are intersecting. The 3 intersecting axes are all part of T0links and will be used to compute the rotation of the robot. The other 3 axes are part of T1links and will be used to first compute the position.
docstring_tokens: Solve 6D equations using fact that 3 axes are intersecting . The 3 intersecting axes are all part of T0links and will be used to compute the rotation of the robot . The other 3 axes are part of T1links and will be used to first compute the position .
function: def solve6DIntersectingAxes(self, T0links, T1links, transvars,rotvars,solveRotationFirst,endbranchtree):
          """Solve 6D equations using fact that 3 axes are intersecting. The 3 intersecting axes are all part of T0links and will be used to compute the rotation of the robot. The other 3 axes are part of T1links and will be used to first compute the position.
          """
          self._iktype = 'transform6d'
          assert(len(transvars)==3) # and len(rotvars) == 3)  # ok if rotation is 2
          T0 = self.multiplyMatrix(T0links)
          T0posoffset = eye(4)
          T0posoffset[0:3,3] = -T0[0:3,3]
          T0links = [T0posoffset] + T0links
          T1links = [T0posoffset] + T1links
          T1 = self.multiplyMatrix(T1links)
          othersolvedvars = rotvars+self.freejointvars if solveRotationFirst else self.freejointvars[:]
          T1linksinv = [self.affineInverse(T) for T in T1links]
          AllEquations = self.buildEquationsFromPositions(T1links,T1linksinv,transvars,othersolvedvars,uselength=True)
          self.checkSolvability(AllEquations,transvars,self.freejointvars)
          rottree = []
          if solveRotationFirst:
              newendbranchtree = endbranchtree
          else:
              newendbranchtree = [AST.SolverSequence([rottree])]
          curvars = transvars[:]
          solsubs=self.freevarsubs[:]
          transtree = self.SolveAllEquations(AllEquations,curvars=curvars,othersolvedvars=othersolvedvars[:],solsubs=solsubs,endbranchtree=newendbranchtree)
          transtree = self.verifyAllEquations(AllEquations,rotvars if solveRotationFirst else transvars+rotvars,self.freevarsubs[:],transtree)
          solvertree= []
          solvedvarsubs = self.freevarsubs[:]
          if solveRotationFirst:
              storesolutiontree = transtree
          else:
              solvertree += transtree
              storesolutiontree = endbranchtree
              for tvar in transvars:
                  solvedvarsubs += self.Variable(tvar).subs
  
          Ree = zeros((3,3))
          for i in range(3):
              for j in range(3):
                  Ree[i,j] = Symbol('new_r%d%d'%(i,j))
          try:
              T1sub = T1.subs(solvedvarsubs)
              othersolvedvars = self.freejointvars if solveRotationFirst else transvars+self.freejointvars
              AllEquations = self.buildEquationsFromRotation(T0links,Ree,rotvars,othersolvedvars)
              self.checkSolvability(AllEquations,rotvars,othersolvedvars)
              currotvars = rotvars[:]
              
              rottree += self.SolveAllEquations(AllEquations,curvars=currotvars,othersolvedvars=othersolvedvars,solsubs=self.freevarsubs[:],endbranchtree=storesolutiontree)
  
              if len(rotvars) < 3:
                  # since rotation variables are not enough for 3D rotation, do not check equations used
                  for ss in rottree:
                      ss.checkEquationsUsed = False
              
              # has to be after SolveAllEquations...?
              for i in range(3):
                  for j in range(3):
                      self.globalsymbols.append((Ree[i,j],T1sub[i,j]))
  
              if len(rottree) == 0:
                  raise self.CannotSolveError('could not solve for all rotation variables: %s:%s'%(str(freevar),str(freevalue)))
  
              if solveRotationFirst:
                  solvertree.append(AST.SolverRotation(T1sub, rottree))
              else:
                  rottree[:] = [AST.SolverRotation(T1sub, rottree[:])]
              return solvertree
          finally:
              # remove the Ree global symbols
              removesymbols = set()
              for i in range(3):
                  for j in range(3):
                      removesymbols.add(Ree[i,j])
              self.globalsymbols = [g for g in self.globalsymbols if not g[0] in removesymbols]
function_tokens: def solve6DIntersectingAxes ( self , T0links , T1links , transvars , rotvars , solveRotationFirst , endbranchtree ) : self . _iktype = 'transform6d' assert ( len ( transvars ) == 3 ) # and len(rotvars) == 3)  # ok if rotation is 2 T0 = self . multiplyMatrix ( T0links ) T0posoffset = eye ( 4 ) T0posoffset [ 0 : 3 , 3 ] = - T0 [ 0 : 3 , 3 ] T0links = [ T0posoffset ] + T0links T1links = [ T0posoffset ] + T1links T1 = self . multiplyMatrix ( T1links ) othersolvedvars = rotvars + self . freejointvars if solveRotationFirst else self . freejointvars [ : ] T1linksinv = [ self . affineInverse ( T ) for T in T1links ] AllEquations = self . buildEquationsFromPositions ( T1links , T1linksinv , transvars , othersolvedvars , uselength = True ) self . checkSolvability ( AllEquations , transvars , self . freejointvars ) rottree = [ ] if solveRotationFirst : newendbranchtree = endbranchtree else : newendbranchtree = [ AST . SolverSequence ( [ rottree ] ) ] curvars = transvars [ : ] solsubs = self . freevarsubs [ : ] transtree = self . SolveAllEquations ( AllEquations , curvars = curvars , othersolvedvars = othersolvedvars [ : ] , solsubs = solsubs , endbranchtree = newendbranchtree ) transtree = self . verifyAllEquations ( AllEquations , rotvars if solveRotationFirst else transvars + rotvars , self . freevarsubs [ : ] , transtree ) solvertree = [ ] solvedvarsubs = self . freevarsubs [ : ] if solveRotationFirst : storesolutiontree = transtree else : solvertree += transtree storesolutiontree = endbranchtree for tvar in transvars : solvedvarsubs += self . Variable ( tvar ) . subs Ree = zeros ( ( 3 , 3 ) ) for i in range ( 3 ) : for j in range ( 3 ) : Ree [ i , j ] = Symbol ( 'new_r%d%d' % ( i , j ) ) try : T1sub = T1 . subs ( solvedvarsubs ) othersolvedvars = self . freejointvars if solveRotationFirst else transvars + self . freejointvars AllEquations = self . buildEquationsFromRotation ( T0links , Ree , rotvars , othersolvedvars ) self . checkSolvability ( AllEquations , rotvars , othersolvedvars ) currotvars = rotvars [ : ] rottree += self . SolveAllEquations ( AllEquations , curvars = currotvars , othersolvedvars = othersolvedvars , solsubs = self . freevarsubs [ : ] , endbranchtree = storesolutiontree ) if len ( rotvars ) < 3 : # since rotation variables are not enough for 3D rotation, do not check equations used for ss in rottree : ss . checkEquationsUsed = False # has to be after SolveAllEquations...? for i in range ( 3 ) : for j in range ( 3 ) : self . globalsymbols . append ( ( Ree [ i , j ] , T1sub [ i , j ] ) ) if len ( rottree ) == 0 : raise self . CannotSolveError ( 'could not solve for all rotation variables: %s:%s' % ( str ( freevar ) , str ( freevalue ) ) ) if solveRotationFirst : solvertree . append ( AST . SolverRotation ( T1sub , rottree ) ) else : rottree [ : ] = [ AST . SolverRotation ( T1sub , rottree [ : ] ) ] return solvertree finally : # remove the Ree global symbols removesymbols = set ( ) for i in range ( 3 ) : for j in range ( 3 ) : removesymbols . add ( Ree [ i , j ] ) self . globalsymbols = [ g for g in self . globalsymbols if not g [ 0 ] in removesymbols ]