OpenRAVE Documentation

Source code for openravepy.ikfast

#!/usr/bin/env python
# -*- coding: utf-8 -*-
# Software License Agreement (Lesser GPL)
#
# Copyright (C) 2009-2012 Rosen Diankov <rosen.diankov@gmail.com>
#
# ikfast is free software: you can redistribute it and/or modify
# it under the terms of the GNU Lesser General Public License as published by
# the Free Software Foundation, either version 3 of the License, or
# at your option) any later version.
#
# ikfast is distributed in the hope that it will be useful,
# but WITHOUT ANY WARRANTY; without even the implied warranty of
# MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
# GNU Lesser General Public License for more details.
#
# You should have received a copy of the GNU Lesser General Public License
# along with this program.  If not, see <http://www.gnu.org/licenses/>.
"""
.. _ikfast_compiler:

IKFast: The Robot Kinematics Compiler
-------------------------------------

.. image:: ../../images/ikfast_robots.jpg
  :width: 640

IKFast analytically solves robot inverse kinematics equations and generates optimized C++ files.

The inverse kinematics equations arise from attemping to place the robot end effector coordinate
system in the world while maintaining joint and user-specified constraints. User-specified constraints make up many different `IK Types`_, each of them having advantages depending on the task.

IKFast will work with any number of joints arranged in a chain; this is defined by the `Robot.Manipulator`. For chains containing more degrees of freedom (DOF) than the IK type requires, the user can set arbitrary values of a subset of the joints until the number of unknown joints matches the degrees of freedom of the IK type.

It is not trivial to create hand-optimized inverse kinematics solutions for arms that can capture all degenerate cases, having closed-form IK speeds up many tasks including planning algorithms, so it really is a must for most robotics researchers.

Closed-form solutions are necessary for motion planning due to two reasons:

- Numerical inverse kinematics solvers will always be much slower than closed form solutions. Planners require being able to process thousands of configurations per second. The closed-form code generated by ikfast can produce solutions on the order of **~4 microseconds**! As a comparison, most numerical solutions are on the order of 10 milliseconds (assuming good convergence).
- The null space of the solution set can be explored because all solutions are computed.

Features
========

- Can handle robots with arbitrary joint complexity like non-intersecting axes.
- All possible discrete solutions calculated (can be up to 16).
- Generated C++ code **independent** of OpenRAVE or any other library.
- Automatically detects degenerate cases where 2 or more axes align and cause infinite solutions.
- Invalid solutions are detected by checking if square roots are given negative values or arc sines and arc cosines are given inputs exceeding the [-1,1] range.
- All divide by zero conditions are automatically checked and handled.

.. _ikfast_types:

IK Types
--------

The following inverse kinematics types are supported:

* **Transform6D** - end effector reaches desired 6D transformation
* **Rotation3D** - end effector reaches desired 3D rotation
* **Translation3D** - end effector origin reaches desired 3D translation
* **Direction3D** - direction on end effector coordinate system reaches desired direction
* **Ray4D** - ray on end effector coordinate system reaches desired global ray
* **Lookat3D** - direction on end effector coordinate system points to desired 3D position
* **TranslationDirection5D** - end effector origin and direction reaches desired 3D translation and direction. Can be thought of as Ray IK where the origin of the ray must coincide.
* **TranslationXY2D** - end effector origin reaches desired XY translation position, Z is ignored. The coordinate system with relative to the base link.
* **TranslationLocalGlobal6D** - local point on end effector origin reaches desired 3D global point. Because both local point and global point can be specified, there are 6 values.
* **TranslationXAxisAngle4D**, **TranslationYAxisAngle4D**, **TranslationZAxisAngle4D** - end effector origin reaches desired 3D translation, manipulator direction makes a specific angle with x/y/z-axis (defined in the manipulator base link's coordinate system)
The possible solve methods are defined by `ikfast.IKFastSolver.GetSolvers()`

Usage
-----

The main file ikfast.py can be used both as a library and as an executable program. For advanced users, it is also possible to use run ikfast.py as a stand-alone program, which makes it mostly independent of the OpenRAVE run-time. 

**However, the recommended way of using IKFast** is through the OpenRAVE :mod:`.databases.inversekinematics` database generator which directly loads the IK into OpenRAVE as an interface. 

Stand-alone Executable
======================

To get help and a description of the ikfast arguments type

.. code-block:: bash

  python `openrave-config --python-dir`/openravepy/_openravepy_/ikfast.py --help

A simple example to generate IK for setting the 3rd joint free of the Barrett WAM is

.. code-block:: bash

  python `openrave-config --python-dir`/openravepy/_openravepy_/ikfast.py --robot=robots/barrettwam.robot.xml --baselink=0 --eelink=7 --savefile=ik.cpp --freeindex=2

Through Python
==============

IKFast can also be used as a library in python. Generating 6D IK for the Barrett WAM while setting the 3rd joint free can be achieved with:

.. code-block:: python

  kinbody = env.ReadRobotXMLFile('robots/barrettwam.robot.xml')
  env.Add(kinbody)
  solver = ikfast.IKFastSolver(kinbody=kinbody)
  chaintree = solver.generateIkSolver(baselink=0,eelink=7,freeindices=[2],solvefn=ikfast.IKFastSolver.solveFullIK_6D)
  code = solver.writeIkSolver(chaintree)
  open('ik.cpp','w').write(code)

.. _ikfast_generatedcpp:

Using Generated IK Files
========================

The common usage is to generate a C++ file that can be compiled into a stand-alone shared object/DLL, an executable program, or linked in statically to a bigger project. For more complex kinematics, LAPACK_ is needed. Here is the header file, which can be found in `share/openrave-X.Y/python/ikfast.h <../../coreapihtml/ikfast_8h.html>`_.

Compiling with GCC
~~~~~~~~~~~~~~~~~~

The most basic command is:

.. code-block:: bash

  gcc -lstdc++ -o ik ik.cpp

This will generate a small program that outputs all solutions given the end effector with respect to the robot base.

Using gcc, this requires "-llapack" to be added. For MSVC++, users will have to compile lapack and link it themselves.

Compiling with MSVC
~~~~~~~~~~~~~~~~~~~

`LAPACK For Windows`_ should be installed in order to get complex kinematics linking correctly.

Details
-------

Terminology:

- **solve joints** - the joints to solve for using inverse kinematics

- **free joints** - the joints that are specified before the IK is run, these values are known at runtime, but not known at IK generation time.


The top level class is `ikfast.IKFastSolver` and generates an Abstract Syntax Tree (AST) using definitions from `ikfast.AST`. The AST is then passed to the language-specific generators defined in `ikfast.CodeGenerators`.

Internal symbolic math uses sympy_. Infinite precision fractions are used in order to keep track of linearly independent equations and when they evaluate to 0. The infinite precision fractions are converted to decimals in the generators.

.. _LAPACK: http://www.netlib.org/lapack/

.. _`LAPACK For Windows`: http://icl.cs.utk.edu/lapack-for-windows/

.. _sympy: http://code.google.com/p/sympy/

Open Issues
-----------

1. currently ikfast does not handle big decimal numbers well. for example defining the axes or anchors as 1.032513241 will produce very big fractions and make things slow.

2. there are cases when axes align and there are infinite solutions. although ikfast can detect such cases, we need a lot more work in this area.

3. for 6D ik, there are still mechanisms it cannot solve, please send the kinematics model if such a situation is encountered.

4. there are 10 different types of IK, currently ray4d IK needs a lot of work.

FAQ
---

Q. **ikfast has been running for more than an hour, will it ever finish?**

A. Most likely not, usually an iksolver finishes within 10 minutes.

----


"""
from __future__ import with_statement # for python 2.5

from sympy import __version__ as sympy_version
if sympy_version < '0.7.0':
    raise ImportError('ikfast needs sympy 0.7.x or greater')

__author__ = 'Rosen Diankov'
__copyright__ = 'Copyright (C) 2009-2012 Rosen Diankov <rosen.diankov@gmail.com>'
__license__ = 'Lesser GPL, Version 3'
__version__ = '60' # also in ikfast.h

import sys, copy, time, math, datetime
import __builtin__
from optparse import OptionParser
try:
    from openravepy.metaclass import AutoReloader
    from openravepy import axisAngleFromRotationMatrix
except:
    axisAngleFromRotationMatrix = None
    class AutoReloader:
        pass

import numpy # required for fast eigenvalue computation
from sympy import *
try:
    import mpmath # on some distributions, sympy does not have mpmath in its scope
except ImportError:
    pass

try:
    import re # for latex cleanup
except ImportError:
    pass

try:
    from math import isinf, isnan
except ImportError:
    # python 2.5 
    from numpy import isinf as _isinf
    from numpy import isnan as _isnan
    def isinf(x): return _isinf(float(x))
    def isnan(x): return _isnan(float(x))

from operator import itemgetter
from itertools import izip
try:
    from itertools import combinations, permutations
except ImportError:
    def combinations(items,n):
        if n == 0: yield[]
        else:
            _internal_items=list(items)
            for  i in xrange(len(_internal_items)):
                for cc in combinations(_internal_items[i+1:],n-1):
                    yield [_internal_items[i]]+cc
                    
    def permutations(iterable, r=None):
        # permutations('ABCD', 2) --> AB AC AD BA BC BD CA CB CD DA DB DC
        # permutations(range(3)) --> 012 021 102 120 201 210
        pool = tuple(iterable)
        n = len(pool)
        r = n if r is None else r
        if r > n:
            return
        indices = range(n)
        cycles = range(n, n-r, -1)
        yield tuple(pool[i] for i in indices[:r])
        while n:
            for i in reversed(range(r)):
                cycles[i] -= 1
                if cycles[i] == 0:
                    indices[i:] = indices[i+1:] + indices[i:i+1]
                    cycles[i] = n - i
                else:
                    j = cycles[i]
                    indices[i], indices[-j] = indices[-j], indices[i]
                    yield tuple(pool[i] for i in indices[:r])
                    break
            else:
                return


import logging
log = logging.getLogger('openravepy.ikfast')

CodeGenerators = {}
# try:
#     import ikfast_generator_vb
#     CodeGenerators['vb'] = ikfast_generator_vb.CodeGenerator
#     CodeGenerators['vb6'] = ikfast_generator_vb.CodeGeneratorVB6
#     CodeGenerators['vb6special'] = ikfast_generator_vb.CodeGeneratorVB6Special
# except ImportError:
#     pass
try:
    import ikfast_generator_cpp
    CodeGenerators['cpp'] = ikfast_generator_cpp.CodeGenerator
    IkType = ikfast_generator_cpp.IkType
except ImportError:
    pass

# changes to sympy:

# core/power.py Pow
[docs]def Pow_eval_subs(self, old, new): if self == old: return new if old.func is self.func and self.base == old.base: coeff1, terms1 = self.exp.as_coeff_mul() coeff2, terms2 = old.exp.as_coeff_mul() if terms1==terms2: # pow = coeff1/coeff2 # if pow.is_Integer or self.base.is_commutative: # return Pow(new, pow) # (x**(2*y)).subs(x**(3*y),z) -> z**(2/3) # only divide if coeff2 is a divisor of coeff1 if coeff1.is_integer and coeff2.is_integer and (coeff1/coeff2).is_integer: return new ** (coeff1/coeff2) # (x**(2*y)).subs(x**(3*y),z) -> z**(2/3*y) if old.func is C.exp: coeff1, terms1 = old.args[0].as_coeff_mul() coeff2, terms2 = (self.exp*C.log(self.base)).as_coeff_mul() if terms1==terms2: # only divide if coeff2 is a divisor of coeff1 if coeff1.is_integer and coeff2.is_integer and (coeff1/coeff2).is_integer: return new ** (coeff1/coeff2) # (x**(2*y)).subs(exp(3*y*log(x)),z) -> z**(2/3*y) return Pow(self.base._eval_subs(old, new), self.exp._eval_subs(old, new))
power.Pow._eval_subs = Pow_eval_subs # simplify/simplify.py # def custom_trigsimp_nonrecursive(expr, deep=False): # """ # A nonrecursive trig simplifier, used from trigsimp. # # == Usage == # trigsimp_nonrecursive(expr) -> reduces expression by using known trig # identities # # == Notes == # # deep ........ apply trigsimp inside functions # # == Examples == # >>> from sympy import cos, sin, log # >>> from sympy.simplify.simplify import trigsimp, trigsimp_nonrecursive # >>> from sympy.abc import x, y # >>> e = 2*sin(x)**2 + 2*cos(x)**2 # >>> trigsimp(e) # 2 # >>> trigsimp_nonrecursive(log(e)) # log(2*cos(x)**2 + 2*sin(x)**2) # >>> trigsimp_nonrecursive(log(e), deep=True) # log(2) # # """ # from sympy.core.basic import S # sin, cos, tan, cot = C.sin, C.cos, C.tan, C.cot # # if expr.is_Function: # if deep: # return expr.func(trigsimp_nonrecursive(expr.args[0], deep)) # elif expr.is_Mul: # ret = S.One # for x in expr.args: # ret *= trigsimp_nonrecursive(x, deep) # # return ret # elif expr.is_Pow: # return Pow(trigsimp_nonrecursive(expr.base, deep), # trigsimp_nonrecursive(expr.exp, deep)) # elif expr.is_Add: # # TODO this needs to be faster # # # The types of trig functions we are looking for # a,b,c = map(Wild, 'abc') # matchers = ( # (a*sin(b)**2, a - a*cos(b)**2), # (a*tan(b)**2, a*(1/cos(b))**2 - a), # (a*cot(b)**2, a*(1/sin(b))**2 - a) # ) # # # Scan for the terms we need # ret = S.Zero # for term in expr.args: # term = trigsimp_nonrecursive(term, deep) # res = None # for pattern, result in matchers: # res = term.match(pattern) # if res is not None: # ret += result.subs(res) # break # if res is None: # ret += term # # # Reduce any lingering artifacts, such as sin(x)**2 changing # # to 1-cos(x)**2 when sin(x)**2 was "simpler" # artifacts = ( # (a - a*cos(b)**2 + c, a*sin(b)**2 + c, cos), # (a - a*(1/cos(b))**2 + c, -a*tan(b)**2 + c, cos), # (a - a*(1/sin(b))**2 + c, -a*cot(b)**2 + c, sin) # ) # # expr = ret # for pattern, result, ex in artifacts: # # Substitute a new wild that excludes some function(s) # # to help influence a better match. This is because # # sometimes, for example, 'a' would match sec(x)**2 # a_t = Wild('a', exclude=[ex]) # pattern = pattern.subs(a, a_t) # result = result.subs(a, a_t) # if expr.is_number: # continue # try: # m = expr.match(pattern) # except (TypeError): # break # # while m is not None: # if m[a_t] == 0 or -m[a_t] in m[c].args or m[a_t] + m[c] == 0: # break # expr = result.subs(m) # if expr.is_number: # continue # try: # m = expr.match(pattern) # except (TypeError): # break # # # return expr # return expr # # simplify.simplify.trigsimp_nonrecursive = custom_trigsimp_nonrecursive
[docs]class AST: """Abstarct Syntax Tree class definitions specific for evaluating complex math equations."""
[docs] class SolverSolution: """Contains equations for evaluating one unknown variable. The variable can have multiple solutions, and the solution is only valid if every equation in checkforzeros is non-zero """ jointname = None jointeval = None jointevalcos = None jointevalsin = None AddPiIfNegativeEq = None isHinge = True checkforzeros = None thresh = None AddHalfTanValue = False dictequations = None presetcheckforzeros = None equationsused = None """Meaning of FeasibleIsZeros: If set to false, then solution is feasible only if all of these equations evalute to non-zero. If set to true, solution is feasible only if all these equations evaluate to zero. """ FeasibleIsZeros = False score = None def __init__(self, jointname, jointeval=None,jointevalcos=None,jointevalsin=None,AddPiIfNegativeEq=None,isHinge=True,thresh=0.000001): self.jointname = jointname self.jointeval = jointeval self.jointevalcos = jointevalcos self.jointevalsin = jointevalsin self.AddPiIfNegativeEq = AddPiIfNegativeEq self.isHinge=isHinge self.thresh = thresh self.presetcheckforzeros = [] self.dictequations = [] self.equationsused = [] assert self.checkValidSolution()
[docs] def subs(self,solsubs): if self.jointeval is not None: self.jointeval = [e.subs(solsubs) for e in self.jointeval] if self.jointevalcos is not None: self.jointevalcos = [e.subs(solsubs) for e in self.jointevalcos] if self.jointevalsin is not None: self.jointevalsin = [e.subs(solsubs) for e in self.jointevalsin] if self.checkforzeros is not None: self.checkforzeros = [e.subs(solsubs) for e in self.checkforzeros] self.dictequations = [(s,v.subs(solsubs)) for s,v in self.dictequations] self.presetcheckforzeros = [e.subs(solsubs) for e in self.presetcheckforzeros] self.equationsused = [e.subs(solsubs) for e in self.equationsused] if not self.checkValidSolution(): raise IKFastSolver.CannotSolveError('substitution produced invalid results') return self
[docs] def generate(self, generator): assert self.checkValidSolution() return generator.generateSolution(self)
[docs] def end(self, generator): return generator.endSolution(self)
[docs] def numsolutions(self): n=0 if self.jointeval is not None: n += len(self.jointeval) if self.jointevalcos is not None: n += 2*len(self.jointevalcos) if self.jointevalsin is not None: n += 2*len(self.jointevalsin) return n
[docs] def checkValidSolution(self): valid=True if self.jointeval is not None: valid &= all([IKFastSolver.isValidSolution(e) for e in self.jointeval]) if self.jointevalsin is not None: valid &= all([IKFastSolver.isValidSolution(e) for e in self.jointevalsin]) if self.jointevalcos is not None: valid &= all([IKFastSolver.isValidSolution(e) for e in self.jointevalcos]) return valid
[docs] def getPresetCheckForZeros(self): return self.presetcheckforzeros
[docs] def getEquationsUsed(self): return self.equationsused
[docs] class SolverPolynomialRoots: """find all roots of the polynomial and plug it into jointeval. poly should be Poly """ jointname = None poly = None jointeval = None jointevalcos = None # not used jointevalsin = None # not used checkforzeros = None postcheckforzeros = None # fail if zero postcheckfornonzeros = None # fail if nonzero postcheckforrange = None # checks that value is within [-1,1] dictequations = None thresh = 1e-7 isHinge = True FeasibleIsZeros = False AddHalfTanValue = False score = None equationsused = None def __init__(self, jointname, poly=None, jointeval=None,isHinge=True): self.poly = poly self.jointname=jointname self.jointeval = jointeval self.isHinge = isHinge self.dictequations = [] self.equationsused = []
[docs] def numsolutions(self): return self.poly.degree(0)
[docs] def subs(self,solsubs): if self.jointeval is not None: self.jointeval = [e.subs(solsubs) for e in self.jointeval] if self.checkforzeros is not None: self.checkforzeros = [e.subs(solsubs) for e in self.checkforzeros] if self.postcheckforzeros is not None: self.postcheckforzeros = [e.subs(solsubs) for e in self.postcheckforzeros] if self.postcheckfornonzeros is not None: self.postcheckfornonzeros = [e.subs(solsubs) for e in self.postcheckfornonzeros] if self.postcheckforrange is not None: self.postcheckforrange = [e.subs(solsubs) for e in self.postcheckforrange] self.dictequations = [(s,v.subs(solsubs)) for s,v in self.dictequations] self.equationsused = [e.subs(solsubs) for e in self.equationsused] if self.poly is not None: self.poly = Poly(self.poly.subs(solsubs),*self.poly.gens) assert self.checkValidSolution() return self
[docs] def generate(self, generator): return generator.generatePolynomialRoots(self)
[docs] def end(self, generator): return generator.endPolynomialRoots(self)
[docs] def checkValidSolution(self): if self.poly is not None: valid = IKFastSolver.isValidSolution(self.poly.as_expr()) if self.jointeval is not None: valid &= all([IKFastSolver.isValidSolution(e) for e in self.jointeval]) return valid
[docs] def getPresetCheckForZeros(self): return [self.poly.LC()] # make sure highest coefficient is not 0!
[docs] def getEquationsUsed(self): return self.equationsused
[docs] class SolverCoeffFunction: """Evaluate a set of coefficients and pass them to a custom function which will then return all possible values of the specified variables in jointnames. """ jointnames = None jointeval = None isHinges = True exportvar = None exportcoeffeqs = None rootmaxdim = None exportfnname = None jointevalcos = None # used for half angles jointevalsin = None # used for half angles checkforzeros = None FeasibleIsZeros = False score = None presetcheckforzeros = None dictequations = None equationsused = None def __init__(self, jointnames, jointeval=None, exportvar=None, exportcoeffeqs=None,exportfnname=None,isHinges=None,rootmaxdim=16,jointevalcos=None,jointevalsin=None): self.jointnames=jointnames self.jointeval = jointeval self.isHinges = isHinges self.exportvar=exportvar self.exportcoeffeqs=exportcoeffeqs self.exportfnname=exportfnname self.rootmaxdim=rootmaxdim self.jointevalsin=jointevalsin self.jointevalcos=jointevalcos self.presetcheckforzeros = [] self.dictequations = [] self.equationsused = []
[docs] def numsolutions(self): return self.rootmaxdim
[docs] def subs(self,solsubs): if self.jointeval is not None: self.jointeval = [e.subs(solsubs) for e in self.jointeval] if self.jointevalcos is not None: self.jointevalcos = [e.subs(solsubs) for e in self.jointevalcos] if self.jointevalsin is not None: self.jointevalsin = [e.subs(solsubs) for e in self.jointevalsin] if self.checkforzeros is not None: self.checkforzeros = [e.subs(solsubs) for e in self.checkforzeros] self.dictequations = [(s,v.subs(solsubs)) for s,v in self.dictequations] self.presetcheckforzeros = [e.subs(solsubs) for e in self.presetcheckforzeros] self.equationsused = [e.subs(solsubs) for e in self.equationsused] #if self.poly is not None: # self.poly = Poly(self.poly.subs(solsubs)...) assert self.checkValidSolution() return self
[docs] def generate(self, generator): return generator.generateCoeffFunction(self)
[docs] def end(self, generator): return generator.endCoeffFunction(self)
[docs] def checkValidSolution(self): #if self.poly is not None: # valid = IKFastSolver.isValidSolution(self.poly.as_expr()) if self.jointeval is not None: valid &= all([IKFastSolver.isValidSolution(e) for e in self.jointeval]) if self.jointevalcos is not None: valid &= all([IKFastSolver.isValidSolution(e) for e in self.jointevalcos]) if self.jointevalsin is not None: valid &= all([IKFastSolver.isValidSolution(e) for e in self.jointevalsin]) return valid
[docs] def getPresetCheckForZeros(self): return self.presetcheckforzeros
[docs] def getEquationsUsed(self): return self.equationsused
[docs] class SolverMatrixInverse: """Take the inverse of a large matirx and set the coefficients of the inverse to the symbols in Asymbols. """ A = None Asymbols = None # has to be same size as B checkforzeros = None def __init__(self, A, Asymbols): self.A = A self.Asymbols = Asymbols
[docs] def subs(self,solsubs): return self
[docs] def generate(self, generator): return generator.generateMatrixInverse(self)
[docs] def end(self, generator): return generator.endMatrixInverse(self)
[docs] def checkValidSolution(self): return True
[docs] def getsubs(self,psubs): Anew = self.A.subs(psubs).inv() subs = [] for i in range(self.A.shape[0]): for j in range(self.A.shape[1]): if self.Asymbols[i][j] is not None: subs.append((self.Asymbols[i][j],Anew[i,j])) return subs
[docs] class SolverConditionedSolution: dictequations = None solversolutions = None thresh=0.000001 def __init__(self, solversolutions): self.solversolutions = solversolutions self.dictequations = []
[docs] def subs(self,solsubs): for s in self.solversolutions: s.subs(solsubs) return self
[docs] def generate(self, generator): return generator.generateConditionedSolution(self)
[docs] def end(self, generator): return generator.endConditionedSolution(self)
[docs] class SolverBranchConds: jointbranches = None thresh = 0.000001 def __init__(self, jointbranches): self.jointbranches = jointbranches
[docs] def generate(self, generator): return generator.generateBranchConds(self)
[docs] def end(self, generator): return generator.endBranchConds(self)
[docs] class SolverCheckZeros: jointname = None jointcheckeqs = None # only used for evaluation zerobranch = None nonzerobranch = None anycondition=None dictequations=None thresh=None # a threshold of 1e-6 breaks hiro ik equationsused = None def __init__(self, jointname, jointcheckeqs, zerobranch, nonzerobranch,thresh=0.000001,anycondition=True): self.jointname = jointname self.jointcheckeqs = jointcheckeqs self.zerobranch = zerobranch self.nonzerobranch = nonzerobranch self.thresh = thresh self.anycondition = anycondition self.dictequations = []
[docs] def generate(self, generator): return generator.generateCheckZeros(self)
[docs] def end(self, generator): return generator.endCheckZeros(self)
[docs] def getPresetCheckForZeros(self): return []
[docs] def checkValidSolution(self): for branch in self.nonzerobranch: if not branch.checkValidSolution(): return False for branch in self.zerobranch: if not branch.checkValidSolution(): return False return True
[docs] def numsolutions(self): return 1
[docs] def subs(self,solsubs): for branch in self.nonzerobranch: if hasattr(branch,'subs'): branch.subs(solsubs) for branch in self.zerobranch: if hasattr(branch,'subs'): branch.subs(solsubs) return self
[docs] def getEquationsUsed(self): return self.equationsused
[docs] class SolverFreeParameter: jointname = None jointtree = None def __init__(self, jointname, jointtree): self.jointname = jointname self.jointtree = jointtree
[docs] def generate(self, generator): return generator.generateFreeParameter(self)
[docs] def end(self, generator): return generator.endFreeParameter(self)
[docs] class SolverRotation: T = None jointtree = None functionid=0 def __init__(self, T, jointtree): self.T = T self.jointtree = jointtree self.dictequations = []
[docs] def generate(self, generator): return generator.generateRotation(self)
[docs] def end(self, generator): return generator.endRotation(self)
[docs] class SolverStoreSolution: """Called when all the unknowns have been solved to add a solution. """ alljointvars = None checkgreaterzero = None # used for final sanity checks to ensure IK solution is consistent thresh = 0 offsetvalues = None isHinge = None def __init__(self, alljointvars,checkgreaterzero=None,isHinge=None): self.alljointvars = alljointvars self.checkgreaterzero = checkgreaterzero self.isHinge=isHinge if isHinge is None: log.warn('SolverStoreSolution.isHinge is not initialized') self.isHinge = [True]*len(self.alljointvars)
[docs] def generate(self, generator): return generator.generateStoreSolution(self)
[docs] def end(self, generator): return generator.endStoreSolution(self)
[docs] class SolverSequence: jointtrees = None def __init__(self, jointtrees): self.jointtrees = jointtrees
[docs] def generate(self, generator): return generator.generateSequence(self)
[docs] def end(self, generator): return generator.endSequence(self)
[docs] class SolverBreak: """Terminates this scope"""
[docs] def generate(self,generator): return generator.generateBreak(self)
[docs] def end(self,generator): return generator.endBreak(self)
[docs] def checkValidSolution(self): return True
[docs] class SolverIKChainTransform6D: solvejointvars = None freejointvars = None jointtree = None Tfk = None Tee = None dictequations = None def __init__(self, solvejointvars, freejointvars, Tee, jointtree,Tfk=None): self.solvejointvars = solvejointvars self.freejointvars = freejointvars self.Tee = Tee self.jointtree = jointtree self.Tfk = Tfk self.dictequations = []
[docs] def generate(self, generator): return generator.generateChain(self)
[docs] def end(self, generator): return generator.endChain(self)
[docs] def leftmultiply(self,Tleft,Tleftinv): self.Tfk = Tleft*self.Tfk self.Tee = Tleftinv*self.Tee
[docs] class SolverIKChainRotation3D: solvejointvars = None freejointvars = None Rfk = None Ree = None jointtree = None dictequations = None def __init__(self, solvejointvars, freejointvars, Ree, jointtree,Rfk=None): self.solvejointvars = solvejointvars self.freejointvars = freejointvars self.Ree = Ree self.Rfk=Rfk self.jointtree = jointtree self.dictequations = []
[docs] def generate(self, generator): return generator.generateIKChainRotation3D(self)
[docs] def end(self, generator): return generator.endIKChainRotation3D(self)
[docs] def leftmultiply(self,Tleft,Tleftinv): self.Rfk = Tleft[0:3,0:3]*self.Rfk self.Ree = Tleftinv[0:3,0:3]*self.Ree
[docs] class SolverIKChainTranslation3D: solvejointvars = None freejointvars = None jointtree = None Pfk = None Pee = None dictequations = None uselocaltrans = False def __init__(self, solvejointvars, freejointvars, Pee, jointtree,Pfk=None): self.solvejointvars = solvejointvars self.freejointvars = freejointvars self.Pee = Pee self.jointtree = jointtree self.Pfk=Pfk self.dictequations = []
[docs] def generate(self, generator): return generator.generateIKChainTranslation3D(self)
[docs] def end(self, generator): return generator.endIKChainTranslation3D(self)
[docs] def leftmultiply(self,Tleft,Tleftinv): self.Pfk = Tleft[0:3,0:3]*self.Pfk+Tleft[0:3,3] self.Pee = Tleftinv[0:3,0:3]*self.Pee+Tleftinv[0:3,3]
[docs] class SolverIKChainTranslationXY2D: solvejointvars = None freejointvars = None jointtree = None Pfk = None Pee = None dictequations = None def __init__(self, solvejointvars, freejointvars, Pee, jointtree,Pfk=None): self.solvejointvars = solvejointvars self.freejointvars = freejointvars self.Pee = Pee self.jointtree = jointtree self.Pfk=Pfk self.dictequations = []
[docs] def generate(self, generator): return generator.generateIKChainTranslationXY2D(self)
[docs] def end(self, generator): return generator.endIKChainTranslationXY2D(self)
[docs] def leftmultiply(self,Tleft,Tleftinv): self.Pfk = Tleft[0:2,0:2]*self.Pfk+Tleft[0:2,3] self.Pee = Tleftinv[0:2,0:2]*self.Pee+Tleftinv[0:2,3]
[docs] class SolverIKChainDirection3D: solvejointvars = None freejointvars = None jointtree = None Dfk = None Dee = None dictequations = None def __init__(self, solvejointvars, freejointvars, Dee, jointtree,Dfk=None): self.solvejointvars = solvejointvars self.freejointvars = freejointvars self.Dee = Dee self.jointtree = jointtree self.Dfk=Dfk self.dictequations = []
[docs] def generate(self, generator): return generator.generateIKChainDirection3D(self)
[docs] def end(self, generator): return generator.endIKChainDirection3D(self)
[docs] def leftmultiply(self,Tleft,Tleftinv): self.Dfk = Tleft[0:3,0:3]*self.Dfk self.Dee = Tleftinv[0:3,0:3]*self.Dee
[docs] class SolverIKChainRay: solvejointvars = None freejointvars = None jointtree = None Pfk = None Dfk = None Pee = None Dee = None dictequations = None is5dray = False # if True, then full 3D position becomes important and things shouldn't be normalized def __init__(self, solvejointvars, freejointvars, Pee, Dee, jointtree,Pfk=None,Dfk=None,is5dray=False): self.solvejointvars = solvejointvars self.freejointvars = freejointvars self.Pee = Pee self.Dee = Dee self.jointtree = jointtree self.Pfk = Pfk self.Dfk = Dfk self.dictequations = [] self.is5dray=is5dray
[docs] def generate(self, generator): return generator.generateIKChainRay(self)
[docs] def end(self, generator): return generator.endIKChainRay(self)
[docs] def leftmultiply(self,Tleft,Tleftinv): self.Pfk = Tleft[0:3,0:3]*self.Pfk+Tleft[0:3,3] self.Dfk = Tleft[0:3,0:3]*self.Dfk self.Pee = Tleftinv[0:3,0:3]*self.Pee+Tleftinv[0:3,3] self.Dee = Tleftinv[0:3,0:3]*self.Dee
[docs] class SolverIKChainLookat3D: solvejointvars = None freejointvars = None jointtree = None Pfk = None Dfk = None Pee = None dictequations = None def __init__(self, solvejointvars, freejointvars, Pee, jointtree,Pfk=None,Dfk=None): self.solvejointvars = solvejointvars self.freejointvars = freejointvars self.Pee = Pee self.jointtree = jointtree self.Pfk=Pfk self.Dfk=Dfk self.dictequations = []
[docs] def generate(self, generator): return generator.generateIKChainLookat3D(self)
[docs] def end(self, generator): return generator.endIKChainLookat3D(self)
[docs] def leftmultiply(self,Tleft,Tleftinv): self.Pfk = Tleft[0:3,0:3]*self.Pfk+Tleft[0:3,3] self.Dfk = Tleft[0:3,0:3]*self.Dfk self.Pee = Tleftinv[0:3,0:3]*self.Pee+Tleftinv[0:3,3]
[docs] class SolverIKChainAxisAngle: solvejointvars = None freejointvars = None jointtree = None Pfk = None Pee = None dictequations = None angleee=None anglefk=None iktype=None def __init__(self, solvejointvars, freejointvars, Pee, angleee,jointtree,Pfk=None,anglefk=None,iktype=None): self.solvejointvars = solvejointvars self.freejointvars = freejointvars self.Pee = Pee self.anglefk=anglefk self.jointtree = jointtree self.Pfk=Pfk self.angleee=angleee self.dictequations = [] self.iktype=iktype
[docs] def generate(self, generator): return generator.generateSolverIKChainAxisAngle(self)
[docs] def end(self, generator): return generator.endSolverIKChainAxisAngle(self)
[docs] def leftmultiply(self,Tleft,Tleftinv): self.Pfk = Tleft[0:2,0:2]*self.Pfk+Tleft[0:2,3] self.Pee = Tleftinv[0:2,0:2]*self.Pee+Tleftinv[0:2,3] assert(0) # need to change angle
from sympy.core import function # for sympy 0.7.1+
[docs]class fmod(function.Function): """defines floating-point mod""" nargs = 2 is_real = True is_Function = True
[docs]class atan2check(atan2): """defines floating-point mod""" nargs = 2 is_real = True is_Function = True
[docs]class IKFastSolver(AutoReloader): """Solves the analytical inverse kinematics equations. The symbol naming conventions are as follows: cjX - cos joint angle constX - temporary constant used to simplify computations dummyX - dummy intermediate variables to solve for gconstX - global constant that is also used during ik generation phase htjX - half tan of joint angle jX - joint angle pX - end effector position information rX - end effector rotation information sjX - sin joint angle tconstX - second-level temporary constant tjX - tan of joint angle """
[docs] class CannotSolveError(Exception): """thrown when ikfast fails to solve a particular set of equations with the given knowns and unknowns """ def __init__(self,value): self.value=value def __str__(self): return repr(self.value)
[docs] class IKFeasibilityError(Exception): """thrown when it is not possible to solve the IK due to robot not having enough degrees of freedom. For example, a robot with 5 joints does not have 6D IK """ def __init__(self,equations,checkvars): self.equations=equations self.checkvars=checkvars def __str__(self): s = "Not enough equations to solve variables %s!\nThis means one of several things: not enough constraints to solve all variables, or the manipulator does not span the target IK space. This is not an ikfast failure, it just means the robot kinematics are invalid for this type of IK. Equations that are not uniquely solvable are:\n"%str(self.checkvars) for eq in self.equations: s += str(eq) + '\n' return s
[docs] class JointAxis: __slots__ = ['joint','iaxis']
[docs] class Variable: __slots__ = ['var','svar','cvar','tvar','htvar'] def __init__(self, var): self.name = var.name self.var = var self.svar = Symbol("s%s"%var.name) self.cvar = Symbol("c%s"%var.name) self.tvar = Symbol("t%s"%var.name) self.htvar = Symbol("ht%s"%var.name) self.vars = [self.var,self.svar,self.cvar,self.tvar,self.htvar] self.subs = [(cos(self.var),self.cvar),(sin(self.var),self.svar),(tan(self.var),self.tvar),(tan(self.var/2),self.htvar)] self.subsinv = [(self.cvar,cos(self.var)),(self.svar, sin(self.var)),(self.tvar,tan(self.tvar))]
[docs] def getsubs(self,value): return [(self.var,value)]+[(s,v.subs(self.var,value).evalf()) for v,s in self.subs]
[docs] class DegenerateCases: def __init__(self): self.handleddegeneratecases = []
[docs] def clone(self): clone=IKFastSolver.DegenerateCases() clone.handleddegeneratecases = self.handleddegeneratecases[:] return clone
[docs] def addcasesconds(self,newconds,currentcases): for case in newconds: newcases = set(currentcases) newcases.add(case) assert not self.hascases(newcases) self.handleddegeneratecases.append(newcases)
[docs] def addcases(self,currentcases): assert not self.hascases(currentcases) self.handleddegeneratecases.append(currentcases)
[docs] def gethandledconds(self,currentcases): handledconds = [] for handledcases in self.handleddegeneratecases: if len(currentcases)+1==len(handledcases) and currentcases < handledcases: handledconds.append((handledcases - currentcases).pop()) return handledconds
[docs] def hascases(self,currentcases): for handledcases in self.handleddegeneratecases: if handledcases == currentcases: return True return False
def __init__(self, kinbody=None,kinematicshash='',precision=None): self.usinglapack = False self.useleftmultiply = True self.freevarsubs = [] self.degeneratecases = None self.kinematicshash = kinematicshash self.testconsistentvalues = None if precision is None: self.precision=8 else: self.precision=precision self.kinbody = kinbody self.axismap = {} self.axismapinv = {} with self.kinbody: for idof in range(self.kinbody.GetDOF()): axis = IKFastSolver.JointAxis() axis.joint = self.kinbody.GetJointFromDOFIndex(idof) axis.iaxis = idof-axis.joint.GetDOFIndex() name = str('j%d')%idof self.axismap[name] = axis self.axismapinv[idof] = name
[docs] def convertRealToRational(self, x,precision=None): if precision is None: precision=self.precision if Abs(x) < 10**-precision: return S.Zero r0 = Rational(str(round(Float(float(x),30),precision))) if x == 0: return r0 r1 = 1/Rational(str(round(Float(1/float(x),30),precision))) return r0 if len(str(r0)) < len(str(r1)) else r1
[docs] def normalizeRotation(self,M): """error from openrave can be on the order of 1e-6 (especially if they are defined diagonal to some axis) """ right = Matrix(3,1,[self.convertRealToRational(x,self.precision-3) for x in M[0,0:3]]) right = right/right.norm() up = Matrix(3,1,[self.convertRealToRational(x,self.precision-3) for x in M[1,0:3]]) up = up - right*right.dot(up) up = up/up.norm() d = right.cross(up) for i in range(3): # don't round the rotational part anymore since it could lead to unnormalized rotations! M[0,i] = right[i] M[1,i] = up[i] M[2,i] = d[i] M[i,3] = self.convertRealToRational(M[i,3]) M[3,i] = S.Zero M[3,3] = S.One return M
[docs] def numpyMatrixToSympy(self,T): if axisAngleFromRotationMatrix is not None: axisangle = axisAngleFromRotationMatrix(T) angle = sqrt(axisangle[0]**2+axisangle[1]**2+axisangle[2]**2) axisangle /= angle log.debug('rotation angle: %f, axis=[%f,%f,%f]', (angle*180/pi).evalf(),axisangle[0],axisangle[1],axisangle[2]) return self.normalizeRotation(Matrix(4,4,[x for x in T.flat]))
[docs] def numpyVectorToSympy(self,v,precision=None): return Matrix(len(v),1,[self.convertRealToRational(x,precision) for x in v])
@staticmethod
[docs] def rodrigues(axis, angle): return IKFastSolver.rodrigues2(axis,cos(angle),sin(angle))
@staticmethod
[docs] def matrixFromQuat(quat): M = eye(3) qq1 = 2*quat[1]*quat[1] qq2 = 2*quat[2]*quat[2] qq3 = 2*quat[3]*quat[3] M[0,0] = 1 - qq2 - qq3 M[0,1] = 2*(quat[1]*quat[2] - quat[0]*quat[3]) M[0,2] = 2*(quat[1]*quat[3] + quat[0]*quat[2]) M[1,0] = 2*(quat[1]*quat[2] + quat[0]*quat[3]) M[1,1]= 1 - qq1 - qq3 M[1,2]= 2*(quat[2]*quat[3] - quat[0]*quat[1]) M[2,0] = 2*(quat[1]*quat[3] - quat[0]*quat[2]) M[2,1] = 2*(quat[2]*quat[3] + quat[0]*quat[1]) M[2,2] = 1 - qq1 - qq2 return M
@staticmethod
[docs] def rodrigues2(axis, cosangle, sinangle): skewsymmetric = Matrix(3, 3, [S.Zero,-axis[2],axis[1],axis[2],S.Zero,-axis[0],-axis[1],axis[0],S.Zero]) return eye(3) + sinangle * skewsymmetric + (S.One-cosangle)*skewsymmetric*skewsymmetric
@staticmethod
[docs] def affineInverse(affinematrix): T = eye(4) T[0:3,0:3] = affinematrix[0:3,0:3].transpose() T[0:3,3] = -affinematrix[0:3,0:3].transpose() * affinematrix[0:3,3] return T
@staticmethod
[docs] def affineSimplify(T): return Matrix(T.shape[0],T.shape[1],[trigsimp(x.expand()) for x in T])
@staticmethod
[docs] def multiplyMatrix(Ts): Tfinal = eye(4) for T in Ts: Tfinal = Tfinal*T return Tfinal
@staticmethod
[docs] def equal(eq0,eq1): return expand(eq0-eq1) == S.Zero
[docs] def chop(self,expr,precision=None): return expr
[docs] def isHinge(self,axisname): if axisname[0]!='j' or not axisname in self.axismap: log.info('isHinge returning false for variable %s'%axisname) return False # dummy joint most likely for angles return self.axismap[axisname].joint.IsRevolute(self.axismap[axisname].iaxis)
[docs] 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): 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.numpyMatrixToSympy(joint.GetInternalHierarchyLeftTransform()) TRightjoint = self.numpyMatrixToSympy(joint.GetInternalHierarchyRightTransform()) axissign = S.One else: TLeftjoint = self.affineInverse(self.numpyMatrixToSympy(joint.GetInternalHierarchyRightTransform())) TRightjoint = self.affineInverse(self.numpyMatrixToSympy(joint.GetInternalHierarchyLeftTransform())) axissign = -S.One if joint.IsStatic(): Tright = self.affineSimplify(Tright * TLeftjoint * TRightjoint) else: Tjoints = [] for iaxis in range(joint.GetDOF()): if joint.GetDOFIndex() >= 0: var = Symbol(self.axismapinv[joint.GetDOFIndex()]) cosvar = cos(var) sinvar = sin(var) jointvars.append(var) elif joint.IsMimic(iaxis): # get the mimic equation var = joint.GetMimicEquation(iaxis) # this needs to be reduced! cosvar = cos(var) sinvar = sin(var) else: raise ValueError('cannot solve for mechanism when a non-mimic passive joint %s is in chain'%str(joint)) Tj = eye(4) 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 > 0: 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(Tright * TLeftjoint) for Tj in Tjoints: jointinds.append(len(Links)) Links.append(Tj) Tright = TRightjoint Links.append(Tright) # 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, jointvars
[docs] def countVariables(self,expr,var): """Counts number of terms variable appears in""" if not expr.is_Add: if expr.has(var): return 1 return 0 num = 0 for term in expr.args: if term.has(var): num += 1 return num
@staticmethod
[docs] def isValidPowers(expr): if expr.is_Pow: if not expr.exp.is_number or expr.exp < 0: return False return IKFastSolver.isValidPowers(expr.base) elif expr.is_Add or expr.is_Mul or expr.is_Function: return all([IKFastSolver.isValidPowers(arg) for arg in expr.args]) else: return True
@staticmethod
[docs] def rotateDirection(sourcedir,targetdir): sourcedir /= sqrt(sourcedir.dot(sourcedir)) targetdir /= sqrt(targetdir.dot(targetdir)) rottodirection = sourcedir.cross(targetdir) fsin = sqrt(rottodirection.dot(rottodirection)) fcos = sourcedir.dot(targetdir) M = eye(4) if fsin > 1e-6: M[0:3,0:3] = IKFastSolver.rodrigues(rottodirection*(1/fsin),atan2(fsin,fcos)) elif fcos < 0: # hand is flipped 180, rotate around x axis rottodirection = Matrix(3,1,[S.One,S.Zero,S.Zero]) rottodirection -= sourcedir * sourcedir.dot(rottodirection) M[0:3,0:3] = IKFastSolver.rodrigues(rottodirection.normalized(), atan2(fsin, fcos)) return M
@staticmethod
[docs] def has(eqs,*sym): return any([eq.has(*sym) for eq in eqs]) if len(sym) > 0 else False
[docs] def trigsimp(self, eq,trigvars): trigsubs = [(sin(v)**2,1-cos(v)**2) for v in trigvars if self.isHinge(v.name)] eq=expand(eq) curcount = eq.count_ops() while True: eq=eq.subs(trigsubs).expand() newcount = eq.count_ops() if IKFastSolver.equal(curcount,newcount): break curcount=newcount return eq
[docs] def codeComplexity(self,expr): complexity = 1 if expr.is_Add: for term in expr.args: complexity += self.codeComplexity(term) elif expr.is_Mul: for term in expr.args: complexity += self.codeComplexity(term) elif expr.is_Pow: complexity += self.codeComplexity(expr.base)+self.codeComplexity(expr.exp) elif expr.is_Function: complexity += 1 for term in expr.args: complexity += self.codeComplexity(term) return complexity
[docs] def sortComplexity(self,exprs): exprs.sort(lambda x, y: self.codeComplexity(x)-self.codeComplexity(y))
[docs] def checkForDivideByZero(self,eq): """returns the equations to check for zero """ checkforzeros = [] try: if eq.is_Function: for arg in eq.args: checkforzeros += self.checkForDivideByZero(arg) elif eq.is_Add: for arg in eq.args: checkforzeros += self.checkForDivideByZero(arg) elif eq.is_Mul: for arg in eq.args: checkforzeros += self.checkForDivideByZero(arg) elif eq.is_Pow: for arg in eq.args: checkforzeros += self.checkForDivideByZero(arg) if eq.exp.is_number and eq.exp < 0: checkforzeros.append(eq.base) except AssertionError,e: log.warn('%s',e) if len(checkforzeros) > 0: newcheckforzeros = [] for eqtemp in checkforzeros: # check for abs(x**y), in that case choose x if eqtemp.is_Function and eqtemp.func == Abs: eqtemp = eqtemp.args[0] while eqtemp.is_Pow: eqtemp = eqtemp.base checkeq = self.removecommonexprs(eqtemp,onlygcd=False,onlynumbers=True) if self.isExpressionUnique(newcheckforzeros,checkeq) and self.isExpressionUnique(newcheckforzeros,-checkeq): newcheckforzeros.append(checkeq) return newcheckforzeros return checkforzeros
[docs] def solutionComplexity(self,sol,solvedvars,unsolvedvars): # for all solutions, check if there is a divide by zero sol.checkforzeros = sol.getPresetCheckForZeros() sol.score = 20000*sol.numsolutions() try: # multiby by 400 in order to prioritize equations with less solutions if hasattr(sol,'jointeval') and sol.jointeval is not None: for s in sol.jointeval: sol.score += self.codeComplexity(s) sol.checkforzeros += self.checkForDivideByZero(s) subexprs = sol.jointeval elif hasattr(sol,'jointevalsin') and sol.jointevalsin is not None: for s in sol.jointevalsin: sol.score += self.codeComplexity(s) sol.checkforzeros += self.checkForDivideByZero(s) subexprs = sol.jointevalsin elif hasattr(sol,'jointevalcos') and sol.jointevalcos is not None: for s in sol.jointevalcos: sol.score += self.codeComplexity(s) sol.checkforzeros += self.checkForDivideByZero(s) subexprs = sol.jointevalcos else: return sol.score # have to also check solution dictionary for s,v in sol.dictequations: sol.score += self.codeComplexity(v) sol.checkforzeros += self.checkForDivideByZero(v) def checkpow(expr,sexprs): score = 0 if expr.is_Pow: sexprs.append(expr.base) if expr.base.is_finite is not None and not expr.base.is_finite: return oo # infinity if expr.exp.is_number and expr.exp < 0: # check if exprbase contains any variables that have already been solved containsjointvar = expr.base.has(*solvedvars) cancheckexpr = not expr.base.has(*unsolvedvars) score += 10000 if not cancheckexpr: score += 100000 elif not self.isValidSolution(expr): return oo # infinity return score sexprs = subexprs[:] while len(sexprs) > 0: sexpr = sexprs.pop(0) if sexpr.is_Add: for arg in sexpr.args: if arg.is_Mul: for arg2 in arg.args: sol.score += checkpow(arg2,sexprs) else: sol.score += checkpow(arg,sexprs) elif sexpr.is_Mul: for arg in sexpr.args: sol.score += checkpow(arg,sexprs) elif sexpr.is_Function: sexprs += sexpr.args elif not self.isValidSolution(sexpr): log.warn('not valid: %s',sexpr) sol.score = oo # infinity else: sol.score += checkpow(sexpr,sexprs) except AssertionError, e: log.warn('%s',e) sol.score=1e10 newcheckforzeros = [] for eqtemp in sol.checkforzeros: checkeq = self.removecommonexprs(eqtemp,onlygcd=False,onlynumbers=True) if self.isExpressionUnique(newcheckforzeros,checkeq) and self.isExpressionUnique(newcheckforzeros,-checkeq): newcheckforzeros.append(checkeq) sol.checkforzeros = newcheckforzeros return sol.score
[docs] def checkSolvability(self,AllEquations,checkvars,othervars): pass
[docs] def checkSolvabilityReal(self,AllEquations,checkvars,othervars): """returns true if there are enough equations to solve for checkvars """ subs = [] checksymbols = [] allsymbols = [] for var in checkvars: subs += self.Variable(var).subs checksymbols += self.Variable(var).vars allsymbols = checksymbols[:] for var in othervars: subs += self.Variable(var).subs allsymbols += self.Variable(var).vars found = False for testconsistentvalue in self.testconsistentvalues: psubvalues = [(s,v) for s,v in testconsistentvalue if not s.has(*checksymbols)] eqs = [eq.subs(self.globalsymbols).subs(subs).subs(psubvalues) for eq in AllEquations] usedsymbols = [s for s in checksymbols if self.has(eqs,s)] eqs = [Poly(eq,*usedsymbols) for eq in eqs if eq != S.Zero] # check if any equations have monos of degree more than 1, if yes, then quit with success since 0.6.7 sympy solver will freeze numhigherpowers = 0 for eq in eqs: for monom in eq.monoms(): if any([m > 1 for m in monom]): numhigherpowers += 1 if numhigherpowers > 0: log.info('checkSolvability has %d higher powers, returning solvable if > 6'%numhigherpowers) if numhigherpowers > 6: found = True break for var in checkvars: varsym = self.Variable(var) if self.isHinge(var.name): if varsym.cvar in usedsymbols and varsym.svar in usedsymbols: eqs.append(Poly(varsym.cvar**2+varsym.svar**2-1,*usedsymbols)) # have to make sure there are representative symbols of all the checkvars, otherwise degenerate solution setusedsymbols = set(usedsymbols) if any([len(setusedsymbols.intersection(self.Variable(var).vars)) == 0 for var in checkvars]): continue try: sol=solve_poly_system(eqs) if sol is not None and len(sol) > 0 and len(sol[0]) == len(usedsymbols): found = True break except: pass if not found: raise self.IKFeasibilityError(AllEquations,checkvars)
[docs] def writeIkSolver(self,chaintree,lang=None): """write the ast into a specific langauge, prioritize c++ """ if lang is None: if CodeGenerators.has_key('cpp'): lang = 'cpp' else: lang = CodeGenerators.keys()[0] log.info('generating %s code...'%lang) return CodeGenerators[lang](kinematicshash=self.kinematicshash,version=__version__).generate(chaintree)
[docs] def generateIkSolver(self, baselink, eelink, freeindices=None,solvefn=None): 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) 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.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))] # 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)] self.pvars = self.Tee[0:12]+self.npxyz+[self.pp]+self.rxp[0]+self.rxp[1]+self.rxp[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 = [] # before passing to the solver, set big numbers to constant variables, this will greatly reduce computation times # numbersubs = [] # LinksRaw2 = [] # for Torig in LinksRaw: # T = Matrix(Torig) # #print axisAngleFromRotationMatrix(numpy.array(numpy.array(T[0:3,0:3]),numpy.float64)) # for i in range(12): # ti = T[i] # if ti.is_number and len(str(ti)) > 30: # matchnumber = self.MatchSimilarFraction(ti,numbersubs) # if matchnumber is None: # sym = self.gsymbolgen.next() # log.info('adding global symbol %s=%s'%(sym,ti)) # numbersubs.append((sym,ti)) # T[i] = sym # else: # T[i] = matchnumber # LinksRaw2.append(T) # if len(numbersubs) > 10: # log.info('substituting %d global symbols',len(numbersubs)) # LinksRaw = LinksRaw2 # self.globalsymbols += numbersubs 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
[docs] def MatchSimilarFraction(self,num,numbersubs,matchlimit = 40): """returns None if no appropriate match found """ for c,v in numbersubs: if self.equal(v,num): return c # nothing matched, so check gcd largestgcd = S.One retnum = None for c,v in numbersubs: curgcd = gcd(v,num) if len(str(curgcd)) > len(str(largestgcd)): newfraction = (num/v) if len(str(newfraction)) <= matchlimit: largestgcd = curgcd retnum = c * newfraction return retnum
[docs] def computeConsistentValues(self,jointvars,T,numsolutions=1,subs=None): possibleangles = [S.Zero, pi.evalf()/2, asin(3.0/5).evalf(), asin(4.0/5).evalf(), asin(5.0/13).evalf(), asin(12.0/13).evalf()] possibleanglescos = [S.One, S.Zero, Rational(4,5), Rational(3,5), Rational(12,13), Rational(5,13)] possibleanglessin = [S.Zero, S.One, Rational(3,5), Rational(4,5), Rational(5,13), Rational(12,13)] testconsistentvalues = [] varsubs = [] for jointvar in jointvars: varsubs += self.Variable(jointvar).subs for isol in range(numsolutions): inds = [0]*len(jointvars) if isol < numsolutions-1: for j in range(len(jointvars)): inds[j] = (isol+j)%len(possibleangles) valsubs = [] for i,ind in enumerate(inds): v,s,c = possibleangles[ind],possibleanglessin[ind],possibleanglescos[ind] var = self.Variable(jointvars[i]) valsubs += [(var.var,v),(var.cvar,c),(var.svar,s),(var.tvar,s/c),(var.htvar,s/(1+c))] psubs = [] for i in range(12): psubs.append((self.pvars[i],T[i].subs(varsubs).subs(self.globalsymbols+valsubs))) for s,v in self.ppsubs+self.npxyzsubs+self.rxpsubs: psubs.append((s,v.subs(psubs))) allsubs = valsubs+psubs if subs is not None: allsubs += [(dvar,var.subs(varsubs).subs(valsubs)) for dvar,var in subs] testconsistentvalues.append(allsubs) return testconsistentvalues
def solveFullIK_Direction3D(self,LinksRaw, jointvars, isolvejointvars, rawbasedir=Matrix(3,1,[S.Zero,S.Zero,S.One])): """basedir needs to be filled with a 3elemtn vector of the initial direction to control""" basedir = Matrix(3,1,[Float(x,30) for x in rawbasedir]) basedir /= sqrt(basedir[0]*basedir[0]+basedir[1]*basedir[1]+basedir[2]*basedir[2]) for i in range(3): basedir[i] = self.convertRealToRational(basedir[i]) Links = LinksRaw[:] LinksInv = [self.affineInverse(link) for link in Links] T = self.multiplyMatrix(Links) Tfinal = zeros((4,4)) Tfinal[0,0:3] = (T[0:3,0:3]*basedir).transpose() self.testconsistentvalues = self.computeConsistentValues(jointvars,Tfinal,numsolutions=4) endbranchtree = [AST.SolverStoreSolution(jointvars,isHinge=[self.isHinge(var.name) for var in jointvars])] solvejointvars = [jointvars[i] for i in isolvejointvars] if len(solvejointvars) != 2: raise self.CannotSolveError('need 2 joints') log.info('ikfast direction3d: %s',solvejointvars) Daccum = self.Tee[0,0:3].transpose() numvarsdone = 2 Ds = [] Dsee = [] for i in range(len(Links)-1): T = self.multiplyMatrix(Links[i:]) D = T[0:3,0:3]*basedir hasvars = [self.has(D,v) for v in solvejointvars] if __builtin__.sum(hasvars) == numvarsdone: Ds.append(D) Dsee.append(Daccum) numvarsdone -= 1 Tinv = self.affineInverse(Links[i]) Daccum = Tinv[0:3,0:3]*Daccum AllEquations = self.buildEquationsFromTwoSides(Ds,Dsee,jointvars,uselength=False) self.checkSolvability(AllEquations,solvejointvars,self.freejointvars) tree = self.solveAllEquations(AllEquations,curvars=solvejointvars,othersolvedvars = self.freejointvars[:],solsubs = self.freevarsubs[:],endbranchtree=endbranchtree) tree = self.verifyAllEquations(AllEquations,solvejointvars,self.freevarsubs,tree) return AST.SolverIKChainDirection3D([(jointvars[ijoint],ijoint) for ijoint in isolvejointvars], [(v,i) for v,i in izip(self.freejointvars,self.ifreejointvars)], Dee=self.Tee[0,0:3].transpose().subs(self.freevarsubs), jointtree=tree,Dfk=Tfinal[0,0:3].transpose()) def solveFullIK_Lookat3D(self,LinksRaw, jointvars, isolvejointvars,rawbasedir=Matrix(3,1,[S.Zero,S.Zero,S.One]),rawbasepos=Matrix(3,1,[S.Zero,S.Zero,S.Zero])): """basedir,basepos needs to be filled with a direction and position of the ray to control the lookat """ basedir = Matrix(3,1,[Float(x,30) for x in rawbasedir]) basepos = Matrix(3,1,[self.convertRealToRational(x) for x in rawbasepos]) basedir /= sqrt(basedir[0]*basedir[0]+basedir[1]*basedir[1]+basedir[2]*basedir[2]) for i in range(3): basedir[i] = self.convertRealToRational(basedir[i]) basepos = basepos-basedir*basedir.dot(basepos) Links = LinksRaw[:] LinksInv = [self.affineInverse(link) for link in Links] T = self.multiplyMatrix(Links) Tfinal = zeros((4,4)) Tfinal[0,0:3] = (T[0:3,0:3]*basedir).transpose() Tfinal[0:3,3] = T[0:3,0:3]*basepos+T[0:3,3] self.testconsistentvalues = self.computeConsistentValues(jointvars,Tfinal,numsolutions=4) solvejointvars = [jointvars[i] for i in isolvejointvars] if len(solvejointvars) != 2: raise self.CannotSolveError('need 2 joints') log.info('ikfast lookat3d: %s',solvejointvars) Paccum = self.Tee[0:3,3] numvarsdone = 2 Positions = [] Positionsee = [] for i in range(len(Links)-1): T = self.multiplyMatrix(Links[i:]) P = T[0:3,0:3]*basepos+T[0:3,3] D = T[0:3,0:3]*basedir hasvars = [self.has(P,v) or self.has(D,v) for v in solvejointvars] if __builtin__.sum(hasvars) == numvarsdone: Positions.append(P.cross(D)) Positionsee.append(Paccum.cross(D)) numvarsdone -= 1 Tinv = self.affineInverse(Links[i]) Paccum = Tinv[0:3,0:3]*Paccum+Tinv[0:3,3] frontcond = (Links[-1][0:3,0:3]*basedir).dot(Paccum-(Links[-1][0:3,0:3]*basepos+Links[-1][0:3,3])) for v in jointvars: frontcond = frontcond.subs(self.Variable(v).subs) endbranchtree = [AST.SolverStoreSolution (jointvars,checkgreaterzero=[frontcond],isHinge=[self.isHinge(var.name) for var in jointvars])] AllEquations = self.buildEquationsFromTwoSides(Positions,Positionsee,jointvars,uselength=True) self.checkSolvability(AllEquations,solvejointvars,self.freejointvars) tree = self.solveAllEquations(AllEquations,curvars=solvejointvars,othersolvedvars = self.freejointvars[:],solsubs = self.freevarsubs[:],endbranchtree=endbranchtree) tree = self.verifyAllEquations(AllEquations,solvejointvars,self.freevarsubs,tree) chaintree = AST.SolverIKChainLookat3D([(jointvars[ijoint],ijoint) for ijoint in isolvejointvars], [(v,i) for v,i in izip(self.freejointvars,self.ifreejointvars)], Pee=self.Tee[0:3,3].subs(self.freevarsubs), jointtree=tree,Dfk=Tfinal[0,0:3].transpose(),Pfk=Tfinal[0:3,3]) chaintree.dictequations += self.ppsubs return chaintree def solveFullIK_Rotation3D(self,LinksRaw, jointvars, isolvejointvars, Rbaseraw=eye(3)): Rbase = eye(4) for i in range(3): for j in range(3): Rbase[i,j] = self.convertRealToRational(Rbaseraw[i,j]) Tfirstright = LinksRaw[-1]*Rbase Links = LinksRaw[:-1] LinksInv = [self.affineInverse(link) for link in Links] Tfinal = self.multiplyMatrix(Links) self.testconsistentvalues = self.computeConsistentValues(jointvars,Tfinal,numsolutions=4) endbranchtree = [AST.SolverStoreSolution (jointvars,isHinge=[self.isHinge(var.name) for var in jointvars])] solvejointvars = [jointvars[i] for i in isolvejointvars] if len(solvejointvars) != 3: raise self.CannotSolveError('need 3 joints') log.info('ikfast rotation3d: %s',solvejointvars) AllEquations = self.buildEquationsFromRotation(Links,self.Tee[0:3,0:3],solvejointvars,self.freejointvars) self.checkSolvability(AllEquations,solvejointvars,self.freejointvars) tree = self.solveAllEquations(AllEquations,curvars=solvejointvars[:],othersolvedvars=self.freejointvars,solsubs = self.freevarsubs[:],endbranchtree=endbranchtree) tree = self.verifyAllEquations(AllEquations,solvejointvars,self.freevarsubs,tree) return AST.SolverIKChainRotation3D([(jointvars[ijoint],ijoint) for ijoint in isolvejointvars], [(v,i) for v,i in izip(self.freejointvars,self.ifreejointvars)], (self.Tee[0:3,0:3] * self.affineInverse(Tfirstright)[0:3,0:3]).subs(self.freevarsubs), tree, Rfk = Tfinal[0:3,0:3] * Tfirstright[0:3,0:3]) def solveFullIK_TranslationLocalGlobal6D(self,LinksRaw, jointvars, isolvejointvars, Tgripperraw=eye(4)): Tgripper = eye(4) for i in range(4): for j in range(4): Tgripper[i,j] = self.convertRealToRational(Tgripperraw[i,j]) localpos = Matrix(3,1,[self.Tee[0,0],self.Tee[1,1],self.Tee[2,2]]) chain = self._solveFullIK_Translation3D(LinksRaw,jointvars,isolvejointvars,Tgripper[0:3,3]+Tgripper[0:3,0:3]*localpos,False) chain.uselocaltrans = True return chain def solveFullIK_Translation3D(self,LinksRaw, jointvars, isolvejointvars, rawbasepos=Matrix(3,1,[S.Zero,S.Zero,S.Zero])): basepos = Matrix(3,1,[self.convertRealToRational(x) for x in rawbasepos]) return self._solveFullIK_Translation3D(LinksRaw,jointvars,isolvejointvars,basepos) def _solveFullIK_Translation3D(self,LinksRaw, jointvars, isolvejointvars, basepos,check=True): Links = LinksRaw[:] LinksInv = [self.affineInverse(link) for link in Links] Tfinal = self.multiplyMatrix(Links) Tfinal[0:3,3] = Tfinal[0:3,0:3]*basepos+Tfinal[0:3,3] self.testconsistentvalues = self.computeConsistentValues(jointvars,Tfinal,numsolutions=4) endbranchtree = [AST.SolverStoreSolution (jointvars,isHinge=[self.isHinge(var.name) for var in jointvars])] solvejointvars = [jointvars[i] for i in isolvejointvars] if len(solvejointvars) != 3: raise self.CannotSolveError('need 3 joints') log.info('ikfast translation3d: %s',solvejointvars) Tbaseposinv = eye(4) Tbaseposinv[0:3,3] = -basepos T1links = [Tbaseposinv]+LinksInv[::-1]+[self.Tee] T1linksinv = [self.affineInverse(Tbaseposinv)]+Links[::-1]+[self.Teeinv] AllEquations = self.buildEquationsFromPositions(T1links,T1linksinv,solvejointvars,self.freejointvars,uselength=True) if check: self.checkSolvability(AllEquations,solvejointvars,self.freejointvars) transtree = self.solveAllEquations(AllEquations,curvars=solvejointvars[:],othersolvedvars=self.freejointvars,solsubs = self.freevarsubs[:],endbranchtree=endbranchtree) transtree = self.verifyAllEquations(AllEquations,solvejointvars,self.freevarsubs,transtree) chaintree = AST.SolverIKChainTranslation3D([(jointvars[ijoint],ijoint) for ijoint in isolvejointvars], [(v,i) for v,i in izip(self.freejointvars,self.ifreejointvars)], Pee=self.Tee[0:3,3], jointtree=transtree, Pfk = Tfinal[0:3,3]) chaintree.dictequations += self.ppsubs return chaintree def solveFullIK_TranslationXY2D(self,LinksRaw, jointvars, isolvejointvars, rawbasepos=Matrix(2,1,[S.Zero,S.Zero])): self.ppsubs = [] # disable since pz is not valid self.pp = None basepos = Matrix(2,1,[self.convertRealToRational(x) for x in rawbasepos]) Links = LinksRaw[:] LinksInv = [self.affineInverse(link) for link in Links] Tfinal = self.multiplyMatrix(Links) Tfinal[0:2,3] = Tfinal[0:2,0:2]*basepos+Tfinal[0:2,3] self.testconsistentvalues = self.computeConsistentValues(jointvars,Tfinal,numsolutions=4) endbranchtree = [AST.SolverStoreSolution (jointvars,isHinge=[self.isHinge(var.name) for var in jointvars])] solvejointvars = [jointvars[i] for i in isolvejointvars] if len(solvejointvars) != 2: raise self.CannotSolveError('need 2 joints') log.info('ikfast translationxy2d: %s',solvejointvars) Tbaseposinv = eye(4) Tbaseposinv[2,2] = S.Zero Tbaseposinv[0:2,3] = -basepos Tbasepos = eye(4) Tbasepos[2,2] = S.Zero Tbasepos[0:2,3] = basepos T1links = [Tbaseposinv]+LinksInv[::-1]+[self.Tee] T1linksinv = [Tbasepos]+Links[::-1]+[self.Teeinv] Taccum = eye(4) numvarsdone = 1 Positions = [] Positionsee = [] for i in range(len(T1links)-1): Taccum = T1linksinv[i]*Taccum hasvars = [self.has(Taccum,v) for v in solvejointvars] if __builtin__.sum(hasvars) == numvarsdone: Positions.append(Taccum[0:2,3]) Positionsee.append(self.multiplyMatrix(T1links[(i+1):])[0:2,3]) numvarsdone += 1 if numvarsdone > 2: # more than 2 variables is almost always useless break if len(Positions) == 0: Positions.append(zeros((2,1))) Positionsee.append(self.multiplyMatrix(T1links)[0:2,3]) AllEquations = self.buildEquationsFromTwoSides(Positions,Positionsee,solvejointvars+self.freejointvars,uselength=True) self.checkSolvability(AllEquations,solvejointvars,self.freejointvars) transtree = self.solveAllEquations(AllEquations,curvars=solvejointvars[:],othersolvedvars=self.freejointvars,solsubs = self.freevarsubs[:],endbranchtree=endbranchtree) transtree = self.verifyAllEquations(AllEquations,solvejointvars,self.freevarsubs,transtree) chaintree = AST.SolverIKChainTranslationXY2D([(jointvars[ijoint],ijoint) for ijoint in isolvejointvars], [(v,i) for v,i in izip(self.freejointvars,self.ifreejointvars)], Pee=self.Tee[0:2,3], jointtree=transtree, Pfk = Tfinal[0:2,3]) chaintree.dictequations += self.ppsubs return chaintree def solveFullIK_TranslationXYOrientation3D(self,LinksRaw, jointvars, isolvejointvars, rawbasepos=Matrix(2,1,[S.Zero,S.Zero]), rawangle=S.Zero): raise self.CannotSolveError('TranslationXYOrientation3D not implemented yet') def solveFullIK_Ray4D(self,LinksRaw, jointvars, isolvejointvars, rawbasedir=Matrix(3,1,[S.Zero,S.Zero,S.One]),rawbasepos=Matrix(3,1,[S.Zero,S.Zero,S.Zero])): """basedir,basepos needs to be filled with a direction and position of the ray to control""" basedir = Matrix(3,1,[Float(x,30) for x in rawbasedir]) basepos = Matrix(3,1,[self.convertRealToRational(x) for x in rawbasepos]) basedir /= sqrt(basedir[0]*basedir[0]+basedir[1]*basedir[1]+basedir[2]*basedir[2]) for i in range(3): basedir[i] = self.convertRealToRational(basedir[i]) basepos = basepos-basedir*basedir.dot(basepos) Links = LinksRaw[:] LinksInv = [self.affineInverse(link) for link in Links] T = self.multiplyMatrix(Links) Tfinal = zeros((4,4)) Tfinal[0,0:3] = (T[0:3,0:3]*basedir).transpose() Tfinal[0:3,3] = T[0:3,0:3]*basepos+T[0:3,3] self.testconsistentvalues = self.computeConsistentValues(jointvars,Tfinal,numsolutions=4) endbranchtree = [AST.SolverStoreSolution (jointvars,isHinge=[self.isHinge(var.name) for var in jointvars])] solvejointvars = [jointvars[i] for i in isolvejointvars] if len(solvejointvars) != 4: raise self.CannotSolveError('need 4 joints') log.info('ikfast ray4d: %s',solvejointvars) Pee = self.Tee[0:3,3] Dee = self.Tee[0,0:3].transpose() numvarsdone = 2 Positions = [] Positionsee = [] for i in range(len(Links)-1): T = self.multiplyMatrix(Links[i:]) P = T[0:3,0:3]*basepos+T[0:3,3] D = T[0:3,0:3]*basedir hasvars = [self.has(P,v) or self.has(D,v) for v in solvejointvars] if __builtin__.sum(hasvars) == numvarsdone: Positions.append(P.cross(D)) Positionsee.append(Pee.cross(Dee)) Positions.append(D) Positionsee.append(Dee) break Tinv = self.affineInverse(Links[i]) Pee = Tinv[0:3,0:3]*Pee+Tinv[0:3,3] Dee = Tinv[0:3,0:3]*Dee AllEquations = self.buildEquationsFromTwoSides(Positions,Positionsee,jointvars,uselength=True) self.checkSolvability(AllEquations,solvejointvars,self.freejointvars) #try: tree = self.solveAllEquations(AllEquations,curvars=solvejointvars[:],othersolvedvars = self.freejointvars[:],solsubs = self.freevarsubs[:],endbranchtree=endbranchtree) #except self.CannotSolveError: # build the raghavan/roth equations and solve with higher power methods # pass tree = self.verifyAllEquations(AllEquations,solvejointvars,self.freevarsubs,tree) chaintree = AST.SolverIKChainRay([(jointvars[ijoint],ijoint) for ijoint in isolvejointvars], [(v,i) for v,i in izip(self.freejointvars,self.ifreejointvars)], Pee=self.Tee[0:3,3].subs(self.freevarsubs), Dee=self.Tee[0,0:3].transpose().subs(self.freevarsubs),jointtree=tree,Dfk=Tfinal[0,0:3].transpose(),Pfk=Tfinal[0:3,3]) chaintree.dictequations += self.ppsubs return chaintree def solveFullIK_TranslationDirection5D(self, LinksRaw, jointvars, isolvejointvars, rawbasedir=Matrix(3,1,[S.Zero,S.Zero,S.One]),rawbasepos=Matrix(3,1,[S.Zero,S.Zero,S.Zero])): """Solves 3D translation + 3D direction """ basepos = Matrix(3,1,[self.convertRealToRational(x) for x in rawbasepos]) basedir = Matrix(3,1,[Float(x,30) for x in rawbasedir]) basedir /= sqrt(basedir[0]*basedir[0]+basedir[1]*basedir[1]+basedir[2]*basedir[2]) for i in range(3): basedir[i] = self.convertRealToRational(basedir[i],5) basedir /= sqrt(basedir[0]*basedir[0]+basedir[1]*basedir[1]+basedir[2]*basedir[2]) # unfortunately have to do it again... offsetdist = basedir.dot(basepos) basepos = basepos-basedir*offsetdist Links = LinksRaw[:] endbranchtree = [AST.SolverStoreSolution (jointvars,isHinge=[self.isHinge(var.name) for var in jointvars])] numzeros = int(basedir[0]==S.Zero) + int(basedir[1]==S.Zero) + int(basedir[2]==S.Zero) # if numzeros < 2: # try: # log.info('try to rotate the last joint so that numzeros increases') # assert(not self.has(Links[-1],*solvejointvars)) # localdir = Links[-1][0:3,0:3]*basedir # localpos = Links[-1][0:3,0:3]*basepos+Links[-1][0:3,3] # AllEquations = Links[-2][0:3,0:3]*localdir # tree=self.solveAllEquations(AllEquations,curvars=solvejointvars[-1:],othersolvedvars = [],solsubs = [],endbranchtree=[]) # offset = tree[0].jointeval[0] # endbranchtree[0].offsetvalues = [S.Zero]*len(solvejointvars) # endbranchtree[0].offsetvalues[-1] = offset # Toffset = Links[-2].subs(solvejointvars[-1],offset).evalf() # localdir2 = Toffset[0:3,0:3]*localdir # localpos2 = Toffset[0:3,0:3]*localpos+Toffset[0:3,3] # Links[-1]=eye(4) # for i in range(3): # basedir[i] = self.convertRealToRational(localdir2[i]) # basedir /= sqrt(basedir[0]*basedir[0]+basedir[1]*basedir[1]+basedir[2]*basedir[2]) # unfortunately have to do it again... # basepos = Matrix(3,1,[self.convertRealToRational(x) for x in localpos2]) # except Exception, e: # print 'failed to rotate joint correctly',e LinksInv = [self.affineInverse(link) for link in Links] T = self.multiplyMatrix(Links) Tfinal = zeros((4,4)) Tfinal[0,0:3] = (T[0:3,0:3]*basedir).transpose() Tfinal[0:3,3] = T[0:3,0:3]*basepos+T[0:3,3] self.testconsistentvalues = self.computeConsistentValues(jointvars,Tfinal,numsolutions=4) solvejointvars = [jointvars[i] for i in isolvejointvars] if len(solvejointvars) != 5: raise self.CannotSolveError('need 5 joints') log.info('ikfast translation direction 5d: %s',solvejointvars) # if last two axes are intersecting, can divide computing position and direction ilinks = [i for i,Tlink in enumerate(Links) if self.has(Tlink,*solvejointvars)] T = self.multiplyMatrix(Links[ilinks[-2]:]) P = T[0:3,0:3]*basepos+T[0:3,3] D = T[0:3,0:3]*basedir tree = None if not self.has(P,*solvejointvars): Tposinv = eye(4) Tposinv[0:3,3] = -P T0links=[Tposinv]+Links[:ilinks[-2]] try: log.info('last 2 axes are intersecting') tree = self.solve5DIntersectingAxes(T0links,basepos,D,solvejointvars,endbranchtree) except self.CannotSolveError, e: log.warn('%s', e) if tree is None: rawpolyeqs2 = [None]*len(solvejointvars) coupledsolutions = None endbranchtree2 = [] for solvemethod in [self.solveLiWoernleHiller, self.solveKohliOsvatic, self.solveManochaCanny]: if coupledsolutions is not None: break for index in [2,3]: T0links=LinksInv[:ilinks[index]][::-1] T0 = self.multiplyMatrix(T0links) T1links=Links[ilinks[index]:] T1 = self.multiplyMatrix(T1links) p0 = T0[0:3,0:3]*self.Tee[0:3,3]+T0[0:3,3] p1 = T1[0:3,0:3]*basepos+T1[0:3,3] l0 = T0[0:3,0:3]*self.Tee[0,0:3].transpose() l1 = T1[0:3,0:3]*basedir if rawpolyeqs2[index] is None: rawpolyeqs2[index] = self.buildRaghavanRothEquations(p0,p1,l0,l1,solvejointvars) try: coupledsolutions,usedvars = solvemethod(rawpolyeqs2[index],solvejointvars,endbranchtree=[AST.SolverSequence([endbranchtree2])]) break except self.CannotSolveError, e: log.warn('%s', e) continue if coupledsolutions is None: raise self.CannotSolveError('raghavan roth equations too complex') log.info('solved coupled variables: %s',usedvars) if len(usedvars) < len(solvejointvars): AllEquations = [] for i in range(3): AllEquations.append(self.simplifyTransform(p0[i]-p1[i]).expand()) AllEquations.append(self.simplifyTransform(l0[i]-l1[i]).expand()) self.sortComplexity(AllEquations) curvars=solvejointvars[:] solsubs = self.freevarsubs[:] for var in usedvars: curvars.remove(var) solsubs += self.Variable(var).subs self.checkSolvability(AllEquations,curvars,self.freejointvars+usedvars) endbranchtree2 += self.solveAllEquations(AllEquations,curvars=curvars,othersolvedvars = self.freejointvars+usedvars,solsubs = solsubs,endbranchtree=endbranchtree) tree = self.verifyAllEquations(AllEquations,curvars,solsubs,coupledsolutions) else: endbranchtree2 += endbranchtree tree = coupledsolutions chaintree = AST.SolverIKChainRay([(jointvars[ijoint],ijoint) for ijoint in isolvejointvars], [(v,i) for v,i in izip(self.freejointvars,self.ifreejointvars)], Pee=(self.Tee[0:3,3]-self.Tee[0,0:3].transpose()*offsetdist).subs(self.freevarsubs), Dee=self.Tee[0,0:3].transpose().subs(self.freevarsubs),jointtree=tree,Dfk=Tfinal[0,0:3].transpose(),Pfk=Tfinal[0:3,3],is5dray=True) chaintree.dictequations += self.ppsubs return chaintree
[docs] def solve5DIntersectingAxes(self, T0links, basepos, D, solvejointvars, endbranchtree): LinksInv = [self.affineInverse(T) for T in T0links] T0 = self.multiplyMatrix(T0links) Tbaseposinv = eye(4) Tbaseposinv[0:3,3] = -basepos T1links = [Tbaseposinv]+LinksInv[::-1]+[self.Tee] T1linksinv = [self.affineInverse(Tbaseposinv)]+T0links[::-1]+[self.Teeinv] AllEquations = self.buildEquationsFromPositions(T1links,T1linksinv,solvejointvars,self.freejointvars,uselength=True) transvars = [v for v in solvejointvars if self.has(T0,v)] self.checkSolvability(AllEquations,transvars,self.freejointvars) dirtree = [] newendbranchtree = [AST.SolverSequence([dirtree])] transtree = self.solveAllEquations(AllEquations,curvars=transvars[:],othersolvedvars=self.freejointvars,solsubs = self.freevarsubs[:],endbranchtree=newendbranchtree) transtree = self.verifyAllEquations(AllEquations,solvejointvars,self.freevarsubs,transtree) rotvars = [v for v in solvejointvars if self.has(D,v)] solsubs = self.freevarsubs[:] for v in transvars: solsubs += self.Variable(v).subs AllEquations = self.buildEquationsFromTwoSides([D],[T0[0:3,0:3]*self.Tee[0,0:3].transpose()],solvejointvars,uselength=False) self.checkSolvability(AllEquations,rotvars,self.freejointvars+transvars) localdirtree = self.solveAllEquations(AllEquations,curvars=rotvars[:],othersolvedvars = self.freejointvars+transvars,solsubs=solsubs,endbranchtree=endbranchtree) dirtree += self.verifyAllEquations(AllEquations,rotvars,solsubs,localdirtree) return transtree
def solveFullIK_6D(self, LinksRaw, jointvars, isolvejointvars,Tgripperraw=eye(4)): """Solves the full 6D translatio + rotation IK """ Tgripper = eye(4) for i in range(4): for j in range(4): Tgripper[i,j] = self.convertRealToRational(Tgripperraw[i,j]) Tfirstright = LinksRaw[-1]*Tgripper Links = LinksRaw[:-1] LinksInv = [self.affineInverse(link) for link in Links] Tfinal = self.multiplyMatrix(Links) self.testconsistentvalues = self.computeConsistentValues(jointvars,Tfinal,numsolutions=4) endbranchtree = [AST.SolverStoreSolution (jointvars,isHinge=[self.isHinge(var.name) for var in jointvars])] solvejointvars = [jointvars[i] for i in isolvejointvars] if len(solvejointvars) != 6: raise self.CannotSolveError('need 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: for T0links, T1links in self.iterateThreeNonIntersectingAxes(solvejointvars,Links, LinksInv): try: tree = self.solveFullIK_6DGeneral(T0links, T1links, solvejointvars, endbranchtree) break except (self.CannotSolveError,self.IKFeasibilityError), 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=Tfinal*Tfirstright) chaintree.dictequations += self.ppsubs+self.npxyzsubs+self.rxpsubs return chaintree
[docs] def TestIntersectingAxes(self,solvejointvars,Links,LinksInv,endbranchtree): for T0links,T1links,transvars,rotvars,solveRotationFirst in self.iterateThreeIntersectingAxes(solvejointvars,Links, LinksInv): try: return self.solve6DIntersectingAxes(T0links,T1links,transvars,rotvars,solveRotationFirst=solveRotationFirst, endbranchtree=endbranchtree) except (self.CannotSolveError,self.IKFeasibilityError), e: log.warn('%s',e) return None
[docs] def iterateThreeIntersectingAxes(self, solvejointvars, Links, LinksInv): """Search for 3 consectuive intersecting axes. If a robot has this condition, it makes a lot of IK computations simpler. """ TestLinks=Links TestLinksInv=LinksInv ilinks = [i for i,Tlink in enumerate(TestLinks) if self.has(Tlink,*solvejointvars)] hingejointvars = [var for var in solvejointvars if self.isHinge(var.name)] polysymbols = [] for solvejointvar in solvejointvars: polysymbols += [s[0] for s in self.Variable(solvejointvar).subs] for i in range(len(ilinks)-2): startindex = ilinks[i] endindex = ilinks[i+2]+1 T0links = TestLinks[startindex:endindex] T0 = self.multiplyMatrix(T0links) # count number of variables in T0[0:3,0:3] numVariablesInRotation = sum([self.has(T0[0:3,0:3],solvejointvar) for solvejointvar in solvejointvars]) if numVariablesInRotation < 3: continue solveRotationFirst = None # sometimes the intersecting condition can be there, but is masked by small epsilon errors # so set any coefficients in T0[:3,3] below self.precision precision to zero translationeqs = [self.RoundEquationTerms(eq.expand()) for eq in T0[:3,3]] if not self.has(translationeqs,*hingejointvars): T1links = TestLinksInv[:startindex][::-1] T1links.append(self.Tee) T1links += TestLinksInv[endindex:][::-1] solveRotationFirst = False else: T0links = TestLinksInv[startindex:endindex][::-1] T0 = self.multiplyMatrix(T0links) translationeqs = [self.RoundEquationTerms(eq.expand()) for eq in T0[:3,3]] if not self.has(translationeqs,*hingejointvars): T1links = TestLinks[endindex:] T1links.append(self.Teeinv) T1links += TestLinks[:startindex] solveRotationFirst = False if solveRotationFirst is not None: rotvars = [] transvars = [] for svar in solvejointvars: if self.has(T0[0:3,0:3],svar): rotvars.append(svar) else: transvars.append(svar) if len(rotvars) == 3 and len(transvars) == 3: log.info('found 3 consecutive intersecting axes links[%d:%d], rotvars=%s, translationvars=%s',startindex, endindex, rotvars,transvars) yield T0links,T1links,transvars,rotvars,solveRotationFirst
[docs] def RoundEquationTerms(self,eq,epsilon=None): if eq.is_Add: neweq = S.Zero for subeq in eq.args: neweq += self.RoundEquationTerms(subeq,epsilon) elif eq.is_Mul: neweq = self.RoundEquationTerms(eq.args[0]) for subeq in eq.args[1:]: neweq *= self.RoundEquationTerms(subeq,epsilon) elif eq.is_Function: newargs = [self.RoundEquationTerms(subeq) for subeq in eq.args] neweq = eq.func(*newargs) elif eq.is_number: if epsilon is None: epsilon = 5*(10**-self.precision) if abs(eq.evalf()) <= epsilon: neweq = S.Zero else: neweq = eq else: neweq=eq return neweq
[docs] def RoundPolynomialTerms(self,peq,epsilon): terms = {} for monom, coeff in peq.terms(): if not coeff.is_number or abs(coeff) > epsilon: terms[monom] = coeff if len(terms) == 0: return Poly(S.Zero,peq.gens) return peq.from_dict(terms, *peq.gens)
[docs] def iterateThreeNonIntersectingAxes(self, solvejointvars, Links, LinksInv): """check for three consecutive non-intersecting axes. if several points exist, so have to choose one that is least complex? """ ilinks = [i for i,Tlink in enumerate(Links) if self.has(Tlink,*solvejointvars)] usedindices = [] for imode in range(2): for i in range(len(ilinks)-2): if i in usedindices: continue startindex = ilinks[i] endindex = ilinks[i+2]+1 p0 = self.multiplyMatrix(Links[ilinks[i]:ilinks[i+1]])[0:3,3] p1 = self.multiplyMatrix(Links[ilinks[i+1]:ilinks[i+2]])[0:3,3] has0 = self.has(p0,*solvejointvars) has1 = self.has(p1,*solvejointvars) if (imode == 0 and has0 and has1) or (imode == 1 and (has0 or has1)): T0links = Links[startindex:endindex] T1links = LinksInv[:startindex][::-1] T1links.append(self.Tee) T1links += LinksInv[endindex:][::-1] usedindices.append(i) usedvars = [var for var in solvejointvars if any([self.has(T0,var) for T0 in T0links])] log.info('found 3 consecutive non-intersecting axes links[%d:%d], vars=%s',startindex,endindex,str(usedvars)) yield T0links, T1links
[docs] 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. """ assert(len(transvars)==3 and len(rotvars) == 3) 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 oldglobalsymbols = self.globalsymbols[:] try: T1sub = T1.subs(solvedvarsubs) Ree = zeros((3,3)) for i in range(3): for j in range(3): Ree[i,j] = Symbol('new_r%d%d'%(i,j)) self.globalsymbols.append((Ree[i,j],T1sub[i,j])) 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(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: self.globalsymbols = oldglobalsymbols
[docs] def solveFullIK_6DGeneral(self, T0links, T1links, solvejointvars, endbranchtree): """Solve 6D equations of a general kinematics structure. This method only works if there exists 3 consecutive joints in that do not always intersect! """ rawpolyeqs2 = [None,None] coupledsolutions = None leftovervarstree = [] origendbranchtree = endbranchtree for solvemethod in [self.solveLiWoernleHiller, self.solveKohliOsvatic, self.solveManochaCanny]: if coupledsolutions is not None: break for j in range(2): if rawpolyeqs2[j] is None: if j == 0: # invert, this seems to always give simpler solutions, so prioritize it T0 = self.affineSimplify(self.multiplyMatrix([self.affineInverse(T) for T in T0links][::-1])) T1 = self.affineSimplify(self.multiplyMatrix([self.affineInverse(T) for T in T1links][::-1])) else: T0 = self.affineSimplify(self.multiplyMatrix(T0links)) T1 = self.affineSimplify(self.multiplyMatrix(T1links)) rawpolyeqs,numminvars = self.buildRaghavanRothEquationsFromMatrix(T0,T1,solvejointvars) if numminvars <= 5 or len(rawpolyeqs[0][1].gens) <= 6: rawpolyeqs2[j] = rawpolyeqs try: if rawpolyeqs2[j] is not None: rawpolyeqs=rawpolyeqs2[j] endbranchtree=[AST.SolverSequence([leftovervarstree])] coupledsolutions,usedvars = solvemethod(rawpolyeqs,solvejointvars,endbranchtree=endbranchtree) break except self.CannotSolveError, e: log.warn('%s',e) continue if coupledsolutions is None: raise self.CannotSolveError('6D general method failed, raghavan roth equations might be too complex') log.info('solved coupled variables: %s',usedvars) AllEquations = [] for i in range(3): for j in range(4): AllEquations.append(self.simplifyTransform(T0[i,j]-T1[i,j])) self.sortComplexity(AllEquations) curvars=solvejointvars[:] solsubs = self.freevarsubs[:] for var in usedvars: curvars.remove(var) solsubs += self.Variable(var).subs self.checkSolvability(AllEquations,curvars,self.freejointvars+usedvars) leftovervarstree += self.solveAllEquations(AllEquations,curvars=curvars,othersolvedvars = self.freejointvars+usedvars,solsubs = solsubs,endbranchtree=origendbranchtree) return coupledsolutions
def solveFullIK_TranslationAxisAngle4D(self, LinksRaw, jointvars, isolvejointvars, rawbasedir=Matrix(3,1,[S.One,S.Zero,S.Zero]),rawbasepos=Matrix(3,1,[S.Zero,S.Zero,S.Zero]),rawglobaldir=Matrix(3,1,[S.Zero,S.Zero,S.One]), rawnormaldir=None): """Solves 3D translation + Angle with respect to X-axis """ globaldir = Matrix(3,1,[Float(x,30) for x in rawglobaldir]) globaldir /= sqrt(globaldir[0]*globaldir[0]+globaldir[1]*globaldir[1]+globaldir[2]*globaldir[2]) for i in range(3): globaldir[i] = self.convertRealToRational(globaldir[i],5) iktype = None if rawnormaldir is not None: normaldir = Matrix(3,1,[Float(x,30) for x in rawnormaldir]) binormaldir = normaldir.cross(globaldir).transpose() if globaldir[0] == S.One and normaldir[2] == S.One: iktype = IkType.TranslationXAxisAngleZNorm4D elif globaldir[1] == S.One and normaldir[0] == S.One: iktype = IkType.TranslationYAxisAngleXNorm4D elif globaldir[2] == S.One and normaldir[1] == S.One: iktype = IkType.TranslationZAxisAngleYNorm4D else: normaldir = None if globaldir[0] == S.One: iktype = IkType.TranslationXAxisAngle4D elif globaldir[1] == S.One: iktype = IkType.TranslationYAxisAngle4D elif globaldir[2] == S.One: iktype = IkType.TranslationZAxisAngle4D if iktype is None: raise ValueError('currently globaldir can only by one of x,y,z axes') basepos = Matrix(3,1,[self.convertRealToRational(x) for x in rawbasepos]) basedir = Matrix(3,1,[Float(x,30) for x in rawbasedir]) L = sqrt(basedir[0]*basedir[0]+basedir[1]*basedir[1]+basedir[2]*basedir[2]) basedir /= L for i in range(3): basedir[i] = self.convertRealToRational(basedir[i],5) basedir /= sqrt(basedir[0]*basedir[0]+basedir[1]*basedir[1]+basedir[2]*basedir[2]) # unfortunately have to do it again... Links = LinksRaw[:] endbranchtree = [AST.SolverStoreSolution (jointvars,isHinge=[self.isHinge(var.name) for var in jointvars])] LinksInv = [self.affineInverse(link) for link in Links] Tallmult = self.multiplyMatrix(Links) Tfinal = zeros((4,4)) if normaldir is None: Tfinal[0,0] = acos(globaldir.dot(Tallmult[0:3,0:3]*basedir)) else: Tfinal[0,0] = atan2(binormaldir.dot(Tallmult[0:3,0:3]*basedir), globaldir.dot(Tallmult[0:3,0:3]*basedir)) Tfinal[0:3,3] = Tallmult[0:3,0:3]*basepos+Tallmult[0:3,3] self.testconsistentvalues = self.computeConsistentValues(jointvars,Tfinal,numsolutions=4) solvejointvars = [jointvars[i] for i in isolvejointvars] if len(solvejointvars) != 4: raise self.CannotSolveError('need 4 joints') log.info('ikfast translation axis 4d, globaldir=%s, basedir=%s: %s',globaldir, basedir, solvejointvars) # if last two axes are intersecting, can divide computing position and direction ilinks = [i for i,Tlink in enumerate(Links) if self.has(Tlink,*solvejointvars)] Tbaseposinv = eye(4) Tbaseposinv[0:3,3] = -basepos T1links = [Tbaseposinv]+LinksInv[::-1]+[self.Tee] T1linksinv = [self.affineInverse(Tbaseposinv)]+Links[::-1]+[self.Teeinv] AllEquations = self.buildEquationsFromPositions(T1links,T1linksinv,solvejointvars,self.freejointvars,uselength=True) for index in range(len(ilinks)): T0links=LinksInv[:ilinks[index]][::-1] T0 = self.multiplyMatrix(T0links) T1links=Links[ilinks[index]:] T1 = self.multiplyMatrix(T1links) globaldir2 = T0[0:3,0:3]*globaldir basedir2 = T1[0:3,0:3]*basedir eq = self.simplifyTransform(self.trigsimp(globaldir2.dot(basedir2),solvejointvars))-cos(self.Tee[0]) if self.isExpressionUnique(AllEquations,eq) and self.isExpressionUnique(AllEquations,-eq): AllEquations.append(eq) if normaldir is not None: binormaldir2 = T0[0:3,0:3]*binormaldir eq = self.simplifyTransform(self.trigsimp(binormaldir2.dot(basedir2),solvejointvars))-sin(self.Tee[0]) if self.isExpressionUnique(AllEquations,eq) and self.isExpressionUnique(AllEquations,-eq): AllEquations.append(eq) # check if planar with respect to normaldir extravar = None if normaldir is not None: if Tallmult[0:3,0:3]*normaldir == normaldir: Tnormaltest = self.rodrigues(normaldir,pi/2) # planar, so know that the sum of all hinge joints is equal to the final angle # can use this fact to substitute one angle with the other values angles = [] for solvejoint in solvejointvars: if self.isHinge(solvejoint.name): Tall0 = Tallmult[0:3,0:3].subs(solvejoint,S.Zero) Tall1 = Tallmult[0:3,0:3].subs(solvejoint,pi/2) if Tall0*Tnormaltest-Tall1: angles.append(solvejoint) else: angles.append(-solvejoint) Tzero = Tallmult.subs([(a,S.Zero) for a in angles]) zeroangle = atan2(binormaldir.dot(Tzero[0:3,0:3]*basedir), globaldir.dot(Tzero[0:3,0:3]*basedir)) eqangles = self.Tee[0]-zeroangle for a in angles[:-1]: eqangles -= a extravar = (angles[-1],eqangles) coseq = cos(eqangles).expand(trig=True) sineq = sin(eqangles).expand(trig=True) AllEquationsOld = AllEquations AllEquations = [self.trigsimp(eq.subs([(cos(angles[-1]),coseq),(sin(angles[-1]),sineq)]).expand(),solvejointvars) for eq in AllEquationsOld] solvejointvars.remove(angles[-1]) self.sortComplexity(AllEquations) endbranchtree = [AST.SolverStoreSolution (jointvars,isHinge=[self.isHinge(var.name) for var in jointvars])] if extravar is not None: solution=AST.SolverSolution(extravar[0].name, jointeval=[extravar[1]],isHinge=self.isHinge(extravar[0].name)) endbranchtree.insert(0,solution) try: tree = self.solveAllEquations(AllEquations,curvars=solvejointvars[:],othersolvedvars=self.freejointvars,solsubs=self.freevarsubs[:],endbranchtree=endbranchtree) tree = self.verifyAllEquations(AllEquations,solvejointvars,self.freevarsubs,tree) except self.CannotSolveError: othersolvedvars = self.freejointvars[:] solsubs = self.freevarsubs[:] freevarinvsubs = [(f[1],f[0]) for f in self.freevarsubs] solinvsubs = [(f[1],f[0]) for f in solsubs] # single variable solutions solutions = [] for curvar in solvejointvars: othervars = [var for var in solvejointvars if var != curvar] curvarsym = self.Variable(curvar) raweqns = [] for e in AllEquations: if (len(othervars) == 0 or not e.has(*othervars)) and e.has(curvar,curvarsym.htvar,curvarsym.cvar,curvarsym.svar): eq = e.subs(self.freevarsubs+solsubs) if self.isExpressionUnique(raweqns,eq) and self.isExpressionUnique(raweqns,-eq): raweqns.append(eq) if len(raweqns) > 0: try: rawsolutions=self.solveSingleVariable(raweqns,curvar,othersolvedvars,unknownvars=solvejointvars) for solution in rawsolutions: self.solutionComplexity(solution,othersolvedvars,solvejointvars) solutions.append((solution,curvar)) except self.CannotSolveError: pass firstsolution, firstvar = solutions[0] othersolvedvars.append(firstvar) solsubs += self.Variable(firstvar).subs curvars=solvejointvars[:] curvars.remove(firstvar) trigsubs = [] polysubs = [] polyvars = [] for v in curvars: if self.isHinge(v.name): var = self.Variable(v) polysubs += [(cos(v),var.cvar),(sin(v),var.svar)] polyvars += [var.cvar,var.svar] trigsubs.append((var.svar**2,1-var.cvar**2)) trigsubs.append((var.svar**3,var.svar*(1-var.cvar**2))) else: polyvars.append(v) polysubsinv = [(b,a) for a,b in polysubs] rawpolyeqs = [Poly(Poly(eq.subs(polysubs),*polyvars).subs(trigsubs),*polyvars) for eq in AllEquations if eq.has(*curvars)] dummys = [] dummysubs = [] dummysubs2 = [] dummyvars = [] for i in range(0,len(polyvars),2): dummy = Symbol('ht%s'%polyvars[i].name[1:]) # [0] - cos, [1] - sin dummys.append(dummy) dummysubs += [(polyvars[i],(1-dummy**2)/(1+dummy**2)),(polyvars[i+1],2*dummy/(1+dummy**2))] var = polyvars[i].subs(self.invsubs).args[0] dummysubs2.append((var,2*atan(dummy))) dummyvars.append((dummy,tan(0.5*var))) newreducedeqs = [] for peq in rawpolyeqs: maxdenom = [0]*(len(polyvars)/2) for monoms in peq.monoms(): for i in range(len(maxdenom)): maxdenom[i] = max(maxdenom[i],monoms[2*i]+monoms[2*i+1]) eqnew = S.Zero for monoms,c in peq.terms(): term = c for i in range(len(polyvars)): num,denom = fraction(dummysubs[i][1]) term *= num**monoms[i] # the denoms for 0,1 and 2,3 are the same for i in range(len(maxdenom)): denom = fraction(dummysubs[2*i][1])[1] term *= denom**(maxdenom[i]-monoms[2*i]-monoms[2*i+1]) eqnew += term newreducedeqs.append(Poly(eqnew,*dummys)) newreducedeqs.sort(cmp=lambda x,y: len(x.monoms()) - len(y.monoms())) ileftvar = 0 leftvar = dummys[ileftvar] exportcoeffeqs=None for ioffset in range(len(newreducedeqs)): try: exportcoeffeqs,exportmonoms = self.solveDialytically(newreducedeqs[ioffset:],ileftvar) log.info('ioffset %d'%ioffset) break except self.CannotSolveError, e: log.debug('solveDialytically errors: %s',e) if exportcoeffeqs is None: raise self.CannotSolveError('failed to solveDialytically') coupledsolution = AST.SolverCoeffFunction(jointnames=[v.name for v in curvars],jointeval=[v[1] for v in dummysubs2],jointevalcos=[dummysubs[2*i][1] for i in range(len(curvars))],jointevalsin=[dummysubs[2*i+1][1] for i in range(len(curvars))],isHinges=[self.isHinge(v.name) for v in curvars],exportvar=[v.name for v in dummys],exportcoeffeqs=exportcoeffeqs,exportfnname='solvedialyticpoly12qep',rootmaxdim=16) self.usinglapack = True tree = [firstsolution,coupledsolution]+ endbranchtree # package final solution chaintree = AST.SolverIKChainAxisAngle([(jointvars[ijoint],ijoint) for ijoint in isolvejointvars], [(v,i) for v,i in izip(self.freejointvars,self.ifreejointvars)], Pee=self.Tee[0:3,3].subs(self.freevarsubs), angleee=self.Tee[0,0].subs(self.freevarsubs),jointtree=tree,Pfk=Tfinal[0:3,3],anglefk=Tfinal[0,0],iktype=iktype) chaintree.dictequations += self.ppsubs return chaintree
[docs] def buildEquationsFromTwoSides(self,leftside, rightside, usedvars, uselength=True): # try to shift all the constants of each Position expression to one side for i in range(len(leftside)): for j in range(leftside[i].shape[0]): p = leftside[i][j] pee = rightside[i][j] pconstterm = None peeconstterm = None if p.is_Add: pconstterm = [term for term in p.args if term.is_number] elif p.is_number: pconstterm = [p] else: continue if pee.is_Add: peeconstterm = [term for term in pee.args if term.is_number] elif pee.is_number: peeconstterm = [pee] else: continue if len(pconstterm) > 0 and len(peeconstterm) > 0: # shift it to the one that has the least terms for term in peeconstterm if len(p.args) < len(pee.args) else pconstterm: leftside[i][j] -= term rightside[i][j] -= term AllEquations = [] for i in range(len(leftside)): for j in range(leftside[i].shape[0]): e = self.trigsimp(leftside[i][j] - rightside[i][j],usedvars) if self.codeComplexity(e) < 1500: e = self.simplifyTransform(e) if self.isExpressionUnique(AllEquations,e) and self.isExpressionUnique(AllEquations,-e): AllEquations.append(e) if uselength: p2 = S.Zero pe2 = S.Zero for j in range(leftside[i].shape[0]): p2 += leftside[i][j]**2 pe2 += rightside[i][j]**2 if self.codeComplexity(p2) < 1200 and self.codeComplexity(pe2) < 1200: # sympy's trigsimp/customtrigsimp give up too easily e = self.simplifyTransform(self.trigsimp(p2,usedvars)-self.trigsimp(pe2,usedvars)) if self.isExpressionUnique(AllEquations,e) and self.isExpressionUnique(AllEquations,-e): AllEquations.append(e.expand()) else: log.info('length equations too big, skipping %d,%d',self.codeComplexity(p2),self.codeComplexity(pe2)) self.sortComplexity(AllEquations) return AllEquations
[docs] def buildEquationsFromPositions(self,T1links,T1linksinv,transvars,othersolvedvars,uselength=True,removesmallnumbers=True): Taccum = eye(4) numvarsdone = 1 Positions = [] Positionsee = [] for i in range(len(T1links)-1): Taccum = T1linksinv[i]*Taccum hasvars = [self.has(Taccum,v) for v in transvars] if __builtin__.sum(hasvars) == numvarsdone: Positions.append(Taccum[0:3,3]) Positionsee.append(self.multiplyMatrix(T1links[(i+1):])[0:3,3]) numvarsdone += 1 if numvarsdone > 2: # more than 2 variables is almost always useless break if len(Positions) == 0: Positions.append(zeros((3,1))) Positionsee.append(self.multiplyMatrix(T1links)[0:3,3]) if removesmallnumbers: for i in range(len(Positions)): for j in range(3): Positions[i][j] = self.RoundEquationTerms(Positions[i][j]) Positionsee[i][j] = self.RoundEquationTerms(Positionsee[i][j]) return self.buildEquationsFromTwoSides(Positions,Positionsee,transvars+othersolvedvars,uselength=uselength)
[docs] def buildEquationsFromRotation(self,T0links,Ree,rotvars,othersolvedvars): """Ree is a 3x3 matrix """ Raccum = Ree numvarsdone = 1 AllEquations = [] for i in range(len(T0links)-1): Raccum = T0links[i][0:3,0:3].transpose()*Raccum # transpose is the inverse hasvars = [self.has(Raccum,v) for v in rotvars] if len(AllEquations) > 0 and __builtin__.sum(hasvars) >= len(rotvars): break if __builtin__.sum(hasvars) == numvarsdone: R = self.multiplyMatrix(T0links[(i+1):]) for i in range(3): for j in range(3): AllEquations.append(self.trigsimp(Raccum[i,j]-R[i,j],othersolvedvars+rotvars)) numvarsdone += 1 self.sortComplexity(AllEquations) return AllEquations
[docs] def buildRaghavanRothEquationsFromMatrix(self,T0,T1,solvejointvars): """Builds the 14 equations using only 5 unknowns. Method explained in [Raghavan1993]_. Basically take the position and one column/row so that the least number of variables are used. .. [Raghavan1993] M Raghavan and B Roth, "Inverse Kinematics of the General 6R Manipulator and related Linkages", Journal of Mechanical Design, Volume 115, Issue 3, 1993. """ p0 = T0[0:3,3] p1 = T1[0:3,3] p=p0-p1 T = T0-T1 numminvars = 100000 for irow in range(3): hasvar = [self.has(T[0:3,irow],var) or self.has(p,var) for var in solvejointvars] numcurvars = __builtin__.sum(hasvar) if numminvars > numcurvars and numcurvars > 0: numminvars = numcurvars l0 = T0[0:3,irow] l1 = T1[0:3,irow] hasvar = [self.has(T[irow,0:3],var) or self.has(p,var) for var in solvejointvars] numcurvars = __builtin__.sum(hasvar) if numminvars > numcurvars and numcurvars > 0: numminvars = numcurvars l0 = T0[irow,0:3].transpose() l1 = T1[irow,0:3].transpose() return self.buildRaghavanRothEquations(p0,p1,l0,l1,solvejointvars),numminvars
[docs] def buildRaghavanRothEquations(self,p0,p1,l0,l1,solvejointvars): eqs = [] for i in range(3): eqs.append([l0[i],l1[i]]) for i in range(3): eqs.append([p0[i],p1[i]]) l0xp0 = l0.cross(p0) l1xp1 = l1.cross(p1) for i in range(3): eqs.append([l0xp0[i],l1xp1[i]]) ppl0 = p0.dot(p0)*l0 - 2*l0.dot(p0)*p0 ppl1 = p1.dot(p1)*l1 - 2*l1.dot(p1)*p1 for i in range(3): eqs.append([ppl0[i],ppl1[i]]) eqs.append([p0.dot(p0),p1.dot(p1)]) eqs.append([l0.dot(p0),l1.dot(p1)]) trigsubs = [] polysubs = [] polyvars = [] for v in solvejointvars: polyvars.append(v) if self.isHinge(v.name): var = self.Variable(v) polysubs += [(cos(v),var.cvar),(sin(v),var.svar)] polyvars += [var.cvar,var.svar] trigsubs.append((var.svar**2,1-var.cvar**2)) trigsubs.append((var.svar**3,var.svar*(1-var.cvar**2))) for v in self.freejointvars: if self.isHinge(v.name): trigsubs.append((sin(v)**2,1-cos(v)**2)) trigsubs.append((sin(v)**3,sin(v)*(1-cos(v)**2))) polysubsinv = [(b,a) for a,b in polysubs] usedvars = [] for j in range(2): usedvars.append([var for var in polyvars if any([eq[j].subs(polysubs).has(var) for eq in eqs])]) polyeqs = [] for i in range(len(eqs)): polyeqs.append([None,None]) for j in range(2): for i in range(len(eqs)): poly0 = Poly(eqs[i][j].subs(polysubs),*usedvars[j]).subs(trigsubs) poly1 = Poly(poly0.expand().subs(trigsubs),*usedvars[j]) if poly1 == S.Zero: polyeqs[i][j] = Poly(poly1,*usedvars[j]) else: polyeqs[i][j] = Poly(poly1,*usedvars[j]).termwise(lambda m,c: self.simplifyTransform(c)) # remove all fractions? having big integers could blow things up... return polyeqs
[docs] def reduceBothSides(self,polyeqs): """Reduces a set of equations in 5 unknowns to a set of equations with 3 unknowns by solving for one side with respect to another. The input is usually the output of buildRaghavanRothEquations. """ usedvars = [polyeqs[0][0].gens, polyeqs[0][1].gens] reducedelayed = [] for j in range(2): if len(usedvars[j]) <= 4: leftsideeqs = [polyeq[j] for polyeq in polyeqs if sum(polyeq[j].degree_list()) > 0] rightsideeqs = [polyeq[1-j] for polyeq in polyeqs if sum(polyeq[j].degree_list()) > 0] if all([all(d <= 2 for d in eq.degree_list()) for eq in leftsideeqs]): try: numsymbolcoeffs, _computereducedequations = self.reduceBothSidesSymbolicallyDelayed(leftsideeqs,rightsideeqs) reducedelayed.append([j,leftsideeqs,rightsideeqs,__builtin__.sum(numsymbolcoeffs), _computereducedequations]) except self.CannotSolveError: continue # sort with respect to least number of symbols reducedelayed.sort(lambda x,y: x[3]-y[3]) reducedeqs = [] tree = [] for j,leftsideeqs,rightsideeqs,numsymbolcoeffs, _computereducedequations in reducedelayed: try: reducedeqs2 = _computereducedequations() if len(reducedeqs2) == 0: log.info('forcing matrix inverse (might take some time)') reducedeqs2,tree = self.reduceBothSidesInverseMatrix(leftsideeqs,rightsideeqs) if len(reducedeqs2) > 0: # success, add all the reduced equations reducedeqs += [[Poly(eq[0],*usedvars[j]),Poly(eq[1],*usedvars[1-j])] for eq in reducedeqs2] + [[Poly(S.Zero,*polyeq[j].gens),polyeq[1-j]-polyeq[j].as_expr()] for polyeq in polyeqs if sum(polyeq[j].degree_list()) == 0] if len(reducedeqs) > 0: break; except self.CannotSolveError,e: log.warn(e) continue if len(reducedeqs) > 0: # check if any substitutions are needed # for eq in reducedeqs: # for j in range(2): # eq[j] = Poly(eq[j].subs(trigsubs).as_expr().expand(),*eq[j].gens) polyeqs = reducedeqs return [eq for eq in polyeqs if eq[0] != S.Zero or eq[1] != S.Zero],tree
[docs] def reduceBothSidesInverseMatrix(self,leftsideeqs,rightsideeqs): """solve a linear system inside the program since the matrix cannot be reduced so easily """ allmonomsleft = set() for peq in leftsideeqs: allmonomsleft = allmonomsleft.union(set(peq.monoms())) allmonomsleft = list(allmonomsleft) allmonomsleft.sort() if __builtin__.sum(allmonomsleft[0]) == 0: allmonomsleft.pop(0) if len(leftsideeqs) < len(allmonomsleft): raise self.CannotSolveError('left side has too few equations for the number of variables %d<%d'%(len(leftsideeqs),len(allmonomsleft))) systemcoeffs = [] for ileft,left in enumerate(leftsideeqs): coeffs = [S.Zero]*len(allmonomsleft) rank = 0 for m,c in left.terms(): if __builtin__.sum(m) > 0: if c != S.Zero: rank += 1 coeffs[allmonomsleft.index(m)] = c systemcoeffs.append((rank,ileft,coeffs)) # ideally we want to try all combinations of simple equations first until we arrive to linearly independent ones. # However, in practice most of the first equations are linearly dependent and it takes a lot of time to prune all of them, # so start at the most complex systemcoeffs.sort(lambda x,y: -x[0]+y[0]) # sort left and right in the same way leftsideeqs = [leftsideeqs[ileft] for rank,ileft,coeffs in systemcoeffs] rightsideeqs = [rightsideeqs[ileft] for rank,ileft,coeffs in systemcoeffs] A = zeros((len(allmonomsleft),len(allmonomsleft))) Asymbols = [] for i in range(A.shape[0]): Asymbols.append([Symbol('gconst%d_%d'%(i,j)) for j in range(A.shape[1])]) solution = None for eqindices in combinations(range(len(leftsideeqs)),len(allmonomsleft)): for i,index in enumerate(eqindices): for k in range(len(allmonomsleft)): A[i,k] = systemcoeffs[index][2][k] det = self.det_bareis(A,*self.pvars) if det == S.Zero: continue solution = AST.SolverMatrixInverse(A=A,Asymbols=Asymbols) self.usinglapack = True solution.checkforzeros = [self.removecommonexprs(det,onlygcd=False,onlynumbers=True)] Aadj=A.adjugate() # too big to be useful for now, but can be used to see if any symbols are always 0 break if solution is None: raise self.CannotSolveError('failed to find %d linearly independent equations'%len(allmonomsleft)) reducedeqs = [] for i in range(len(allmonomsleft)): var=S.One for k,kpower in enumerate(allmonomsleft[i]): if kpower != 0: var *= leftsideeqs[0].gens[k]**kpower pright = S.Zero for k in range(len(allmonomsleft)): if Aadj[i,k] != S.Zero: pright += Asymbols[i][k] * (rightsideeqs[eqindices[k]].as_expr()-leftsideeqs[eqindices[k]].TC()) reducedeqs.append([var,pright.expand()]) othereqindices = set(range(len(leftsideeqs))).difference(set(eqindices)) for i in othereqindices: # have to multiply just the constant by the determinant neweq = rightsideeqs[i].as_expr() for m,c in leftsideeqs[i].terms(): if __builtin__.sum(m) > 0: neweq -= c*reducedeqs[allmonomsleft.index(m)][1] else: neweq -= c reducedeqs.append([S.Zero,neweq]) return reducedeqs, [solution] # Adj=M[:,:-1].adjugate() # #D=M[:,:-1].det() # D=M[:,:-1].det() # sols=-Adj*M[:,-1] # solsubs = [] # for i,v in enumerate(newunknowns): # newsol=sols[i].subs(localsymbols) # solsubs.append((v,newsol)) # reducedeqs.append([v.subs(localsymbols)*D,newsol]) # othereqindices = set(range(len(newleftsideeqs))).difference(set(eqindices)) # for i in othereqindices: # # have to multiply just the constant by the determinant # newpoly = S.Zero # for c,m in newleftsideeqs[i].terms(): # monomindices = [index for index in range(len(newunknowns)) if m[index]>0] # if len(monomindices) == 0: # newpoly += c.subs(localsymbols)*D # else: # assert(len(monomindices)==1) # newpoly += c.subs(localsymbols)*solsubs[monomindices[0]][1] # reducedeqs.append([S.Zero,newpoly]) # break # # there are too many symbols, so have to resolve to a little more involved method # P,L,DD,U= M[:,:-1].LUdecompositionFF(*self.pvars) # finalnums = S.One # finaldenoms = S.One # for i in range(len(newunknowns)): # n,d = self.recursiveFraction(L[i,i]*U[i,i]/DD[i,i]) # finalnums *= n # finaldenoms *= d # n,d = self.recursiveFraction(DD[i,i]) # q,r = div(n,d,*pvars) # DD[i,i] = q # assert(r==S.Zero) # det,r = div(finalnums,finaldenoms,*pvars) # assert(r==S.Zero) # b = -P*M[:,-1] # y = [[b[0],L[0,0]]] # for i in range(1,L.shape[0]): # commondenom=y[0][1] # for j in range(1,i): # commondenom=lcm(commondenom,y[j][1],*pvars) # accum = S.Zero # for j in range(i): # accum += L[i,j]*y[j][0]*(commondenom/y[j][1]) # res = (commondenom*b[i]-accum)/(commondenom*L[i,i]) # y.append(self.recursiveFraction(res)) # # ynew = [] # for i in range(L.shape[0]): # q,r=div(y[i][0]*DD[i,i],y[i][1],*pvars) # print 'remainder: ',r # ynew.append(q) # # x = [[ynew[-1],U[-1,-1]]] # for i in range(U.shape[0]-2,-1,-1): # commondenom=x[0][1] # for j in range(i+1,U.shape[0]): # commondenom=lcm(commondenom,x[j][1],*pvars) # accum = S.Zero # for j in range(i+1,U.shape[0]): # accum += U[i,j]*x[j][0]*(commondenom/x[j][1]) # res = (commondenom*b[i]-accum)/(commondenom*U[i,i]) # x.append(self.recursiveFraction(res)) # # print 'ignoring num symbols: ',numsymbols # continue
[docs] def reduceBothSidesSymbolically(self,*args,**kwargs): numsymbolcoeffs, _computereducedequations = self.reduceBothSidesSymbolicallyDelayed(*args,**kwargs) return _computereducedequations()
[docs] def reduceBothSidesSymbolicallyDelayed(self,leftsideeqs,rightsideeqs,maxsymbols=10,usesymbols=True): """the left and right side of the equations need to have different variables """ assert(len(leftsideeqs)==len(rightsideeqs)) # first count the number of different monomials, then try to solve for each of them symbolgen = cse_main.numbered_symbols('const') vargen = cse_main.numbered_symbols('tempvar') rightsidedummy = [] localsymbols = [] dividesymbols = [] allmonoms = dict() for left,right in izip(leftsideeqs,rightsideeqs): if right != S.Zero: rightsidedummy.append(symbolgen.next()) localsymbols.append((rightsidedummy[-1],right.as_expr().expand())) else: rightsidedummy.append(S.Zero) for m in left.monoms(): if __builtin__.sum(m) > 0 and not m in allmonoms: newvar = vargen.next() localsymbols.append((newvar,Poly.from_dict({m:S.One},*left.gens).as_expr())) allmonoms[m] = newvar if len(leftsideeqs) < len(allmonoms): raise self.CannotSolveError('left side has too few equations for the number of variables %d<%d'%(len(leftsideeqs),len(allmonoms))) if len(allmonoms) == 0: def _returnequations(): return [[left,right] for left,right in izip(leftsideeqs,rightsideeqs)] return 0, _returnequations unknownvars = leftsideeqs[0].gens newleftsideeqs = [] numsymbolcoeffs = [] for left,right in izip(leftsideeqs,rightsidedummy): left = left - right newleft = Poly(S.Zero,*allmonoms.values()) leftcoeffs = [c for m,c in left.terms() if __builtin__.sum(m) > 0] allnumbers = all([c.is_number for c in leftcoeffs]) if usesymbols and not allnumbers: # check if all the equations are within a constant from each other # This is neceesary since the current linear system solver cannot handle too many symbols. reducedeq0,common0 = self.removecommonexprs(leftcoeffs[0],returncommon=True) commonmults = [S.One] for c in leftcoeffs[1:]: reducedeq1,common1 = self.removecommonexprs(c,returncommon=True) if self.equal(reducedeq1,reducedeq0): commonmults.append(common1/common0) elif self.equal(reducedeq1,-reducedeq0): commonmults.append(-common1/common0) else: break if len(commonmults) == len(leftcoeffs): # divide everything by reducedeq0 index = 0 for m,c in left.terms(): if __builtin__.sum(m) > 0: newleft = newleft + commonmults[index]*allmonoms.get(m) index += 1 else: # look in the dividesymbols for something similar gmult = None for gsym,geq in dividesymbols: greducedeq,gcommon = self.removecommonexprs(S.One/geq,returncommon=True) if self.equal(greducedeq,reducedeq0): gmult = gsym*(gcommon/common0) break elif self.equal(greducedeq,-reducedeq0): gmult = gsym*(-gcommon/common0) break if gmult is None: gmult = symbolgen.next() dividesymbols.append((gmult,S.One/leftcoeffs[0])) newc = (c*gmult).subs(localsymbols).expand() sym = symbolgen.next() localsymbols.append((sym,newc)) newleft = newleft + sym numsymbolcoeffs.append(0) newleftsideeqs.append(newleft) continue numsymbols = 0 for m,c in left.terms(): polyvar = S.One if __builtin__.sum(m) > 0: polyvar = allmonoms.get(m) if not c.is_number: numsymbols += 1 newleft = newleft + c*polyvar numsymbolcoeffs.append(numsymbols) newleftsideeqs.append(newleft) def _computereducedequations(): reducedeqs = [] # order the equations based on the number of terms newleftsideeqs.sort(lambda x,y: len(x.monoms()) - len(y.monoms())) newunknowns = newleftsideeqs[0].gens log.info('solving for all pairwise variables in %s, number of symbol coeffs are %s',unknownvars,__builtin__.sum(numsymbolcoeffs)) systemcoeffs = [] for eq in newleftsideeqs: eqdict = eq.as_dict() coeffs = [] for i,var in enumerate(newunknowns): monom = [0]*len(newunknowns) monom[i] = 1 coeffs.append(eqdict.get(tuple(monom),S.Zero)) monom = [0]*len(newunknowns) coeffs.append(-eqdict.get(tuple(monom),S.Zero)) systemcoeffs.append(coeffs) detvars = [s for s,v in localsymbols] + self.pvars for eqindices in combinations(range(len(newleftsideeqs)),len(newunknowns)): # very quick rejection numsymbols = __builtin__.sum([numsymbolcoeffs[i] for i in eqindices]) if numsymbols > maxsymbols: continue M = Matrix([systemcoeffs[i] for i in eqindices]) det = self.det_bareis(M[:,:-1], *detvars) if det == S.Zero: continue try: eqused = [newleftsideeqs[i] for i in eqindices] solution=solve(eqused,newunknowns) except IndexError: # not enough equations? continue if solution is not None and all([self.isValidSolution(value.subs(localsymbols)) for key,value in solution.iteritems()]): # substitute solsubs = [] allvalid = True for key,value in solution.iteritems(): valuesub = value.subs(localsymbols) solsubs.append((key,valuesub)) reducedeqs.append([key.subs(localsymbols),valuesub]) othereqindices = set(range(len(newleftsideeqs))).difference(set(eqindices)) for i in othereqindices: reducedeqs.append([S.Zero,(newleftsideeqs[i].subs(solsubs).subs(localsymbols)).as_expr().expand()]) break # remove the dividesymbols from reducedeqs for sym,ivalue in dividesymbols: value=1/ivalue for i in range(len(reducedeqs)): eq = reducedeqs[i][1] if eq.has(sym): neweq = S.Zero peq = Poly(eq,sym) for m,c in peq.terms(): neweq += c*value**(peq.degree(0) - m[0]) reducedeqs[i][1] = neweq.expand() reducedeqs[i][0] = (reducedeqs[i][0]*value**peq.degree(0)).expand() if len(reducedeqs) > 0: log.info('finished with %d equations',len(reducedeqs)) return reducedeqs return numsymbolcoeffs, _computereducedequations
[docs] def solveManochaCanny(self,rawpolyeqs,solvejointvars,endbranchtree): """Solves the IK equations using eigenvalues/eigenvectors of a 12x12 quadratic eigenvalue problem. Method explained in Dinesh Manocha and J.F. Canny. "Efficient inverse kinematics for general 6R manipulators", IEEE Transactions on Robotics and Automation, Volume 10, Issue 5, Oct 1994. """ log.info('attempting manocha/canny general ik method') PolyEquations, raghavansolutiontree = self.reduceBothSides(rawpolyeqs) # find all equations with zeros on the left side RightEquations = [] for ipeq,peq in enumerate(PolyEquations): if peq[0] == S.Zero: if len(raghavansolutiontree) > 0 or peq[1] == S.Zero: # give up on optimization RightEquations.append(peq[1]) else: RightEquations.append(self.simplifyTransformPoly(peq[1])) if len(RightEquations) < 6: raise self.CannotSolveError('number of equations %d less than 6'%(len(RightEquations))) # sort with respect to the number of monomials RightEquations.sort(lambda x, y: len(x.monoms())-len(y.monoms())) # substitute with dummy=tan(half angle) symbols = RightEquations[0].gens symbolsubs = [(symbols[i].subs(self.invsubs),symbols[i]) for i in range(len(symbols))] unsolvedsymbols = [] for solvejointvar in solvejointvars: testvars = self.Variable(solvejointvar).vars if not any([v in symbols for v in testvars]): unsolvedsymbols += testvars # check that the coefficients of the reduced equations do not contain any unsolved variables for peq in RightEquations: if peq.has(*unsolvedsymbols): raise self.CannotSolveError('found unsolved symbol being used so ignoring: %s'%peq) log.info('solving simultaneously for symbols: %s',symbols) dummys = [] dummysubs = [] dummysubs2 = [] dummyvars = [] usedvars = [] singlevariables = [] i = 0 while i < len(symbols): dummy = Symbol('ht%s'%symbols[i].name[1:]) var = symbols[i].subs(self.invsubs) if not isinstance(var,Symbol): # [0] - cos, [1] - sin var = var.args[0] dummys.append(dummy) dummysubs += [(symbols[i],(1-dummy**2)/(1+dummy**2)),(symbols[i+1],2*dummy/(1+dummy**2))] dummysubs2.append((var,2*atan(dummy))) dummyvars.append((dummy,tan(0.5*var))) if not var in usedvars: usedvars.append(var) i += 2 else: singlevariables.append(var) # most likely a single variable dummys.append(var) dummysubs += [(var,var)] dummysubs2.append((var,var)) if not var in usedvars: usedvars.append(var) i += 1 newreducedeqs = [] for peq in RightEquations: maxdenom = dict() for monoms in peq.monoms(): i = 0 while i < len(monoms): if peq.gens[i].name[0] == 'j': # single variable maxdenom[peq.gens[i]] = max(maxdenom.get(peq.gens[i],0),monoms[i]) i += 1 else: maxdenom[peq.gens[i]] = max(maxdenom.get(peq.gens[i],0),monoms[i]+monoms[i+1]) i += 2 eqnew = S.Zero for monoms,c in peq.terms(): term = c for i in range(len(dummysubs)): num,denom = fraction(dummysubs[i][1]) term *= num**monoms[i] # the denoms for 0,1 and 2,3 are the same i = 0 while i < len(monoms): if peq.gens[i].name[0] == 'j': denom = fraction(dummysubs[i][1])[1] term *= denom**(maxdenom[peq.gens[i]]-monoms[i]) i += 1 else: denom = fraction(dummysubs[i][1])[1] term *= denom**(maxdenom[peq.gens[i]]-monoms[i]-monoms[i+1]) i += 2 eqnew += term newreducedeqs.append(Poly(eqnew,*dummys)) # from IPython.Shell import IPShellEmbed # ipshell = IPShellEmbed(argv='',banner = 'OpenRAVE Dropping into IPython, variables: env, robot',exit_msg = 'Leaving Interpreter and closing program.') # ipshell(local_ns=locals()) # check for equations with a single variable if len(singlevariables) > 0: try: AllEquations = [eq.subs(self.invsubs).as_expr() for eq in newreducedeqs] tree = self.solveAllEquations(AllEquations,curvars=dummys,othersolvedvars=[],solsubs=self.freevarsubs,endbranchtree=endbranchtree) return raghavansolutiontree+tree,usedvars except self.CannotSolveError: pass if 0: # try solving for the single variable and substituting for the rest of the equations in order to get a set of equations without the single variable var = singlevariables[0] monomindex = symbols.index(var) singledegreeeqs = [] AllEquations = [] for peq in newreducedeqs: if all([m[monomindex] <= 1 for m in peq.monoms()]): newpeq = Poly(peq,var) if sum(newpeq.degree_list()) > 0: singledegreeeqs.append(newpeq) else: AllEquations.append(peq.subs(self.invsubs).as_expr()) for peq0, peq1 in combinations(singledegreeeqs,2): AllEquations.append(simplify((peq0.TC()*peq1.LC() - peq0.LC()*peq1.TC()).subs(self.invsubs))) log.info(str(AllEquations)) #sol=self.solvePairVariablesHalfAngle(AllEquations,usedvars[1],usedvars[2],[]) # choose which leftvar can determine the singularity of the following equations! exportcoeffeqs = None getsubs = raghavansolutiontree[0].getsubs if len(raghavansolutiontree) > 0 else None for ileftvar in range(len(dummys)): leftvar = dummys[ileftvar] try: exportcoeffeqs,exportmonoms = self.solveDialytically(newreducedeqs,ileftvar,getsubs=getsubs) break except self.CannotSolveError,e: log.warn('failed with leftvar %s: %s',leftvar,e) if exportcoeffeqs is None: raise self.CannotSolveError('failed to solve dialytically') if ileftvar > 0: raise self.CannotSolveError('solving equations dialytically succeeded with var index %d, unfortunately code generation supports only index 0'%ileftvar) jointevalcos=[d[1] for d in dummysubs if d[0].name[0] == 'c'] jointevalsin=[d[1] for d in dummysubs if d[0].name[0] == 's'] #jointeval=[d[1] for d in dummysubs if d[0].name[0] == 'j'] coupledsolution = AST.SolverCoeffFunction(jointnames=[v.name for v in usedvars],jointeval=[v[1] for v in dummysubs2],jointevalcos=jointevalcos, jointevalsin=jointevalsin, isHinges=[self.isHinge(v.name) for v in usedvars],exportvar=[v.name for v in dummys],exportcoeffeqs=exportcoeffeqs,exportfnname='solvedialyticpoly12qep',rootmaxdim=16) self.usinglapack = True return raghavansolutiontree+[coupledsolution]+endbranchtree,usedvars
[docs] def solveLiWoernleHiller(self,rawpolyeqs,solvejointvars,endbranchtree): """Li-Woernle-Hiller procedure covered in Jorge Angeles, "Fundamentals of Robotics Mechanical Systems", Springer, 2007. """ log.info('attempting li/woernle/hiller general ik method') if len(rawpolyeqs[0][0].gens) < len(rawpolyeqs[0][1].gens): for peq in rawpolyeqs: peq[0],peq[1] = peq[1],peq[0] symbols = list(rawpolyeqs[0][0].gens) othersymbols = list(rawpolyeqs[0][1].gens) symbolsubs = [(symbols[i].subs(self.invsubs),symbols[i]) for i in range(len(symbols))] numsymbols = 0 for solvejointvar in solvejointvars: for var in self.Variable(solvejointvar).vars: if var in symbols: numsymbols += 1 break if numsymbols != 3: raise self.CannotSolveError('Kohli/Osvatic method requires 3 unknown variables, has %d'%numsymbols) # choose which leftvar can determine the singularity of the following equations! allowedindices = [] for i in range(len(symbols)): # if first symbol is cjX, then next should be sjX if symbols[i].name[0] == 'c': assert( symbols[i+1].name == 's'+symbols[i].name[1:]) if 8 == __builtin__.sum([int(peq[0].has(symbols[i],symbols[i+1])) for peq in rawpolyeqs]): allowedindices.append(i) if len(allowedindices) == 0: raise self.CannotSolveError('need exactly 8 equations of one variable') solutiontree = [] checkforzeros = [] getsubs = None cvar = symbols[allowedindices[0]] svar = symbols[allowedindices[0]+1] varname = cvar.name[1:] tvar = Symbol('ht'+varname) symbols.remove(cvar) symbols.remove(svar) symbols.append(tvar) othersymbols.append(tvar) polyeqs = [[peq[0].as_expr(),peq[1]] for peq in rawpolyeqs if peq[0].has(cvar,svar)] neweqs=[] for i in range(0,len(polyeqs),2): p0 = Poly(polyeqs[i][0],cvar,svar) p0dict=p0.as_dict() p1 = Poly(polyeqs[i+1][0],cvar,svar) p1dict=p1.as_dict() r0 = polyeqs[i][1].as_expr() r1 = polyeqs[i+1][1].as_expr() if self.equal(p0dict.get((1,0),S.Zero),-p1dict.get((0,1),S.Zero)) and self.equal(p0dict.get((0,1),S.Zero),p1dict.get((1,0),S.Zero)): p0,p1 = p1,p0 p0dict,p1dict=p1dict,p0dict r0,r1 = r1,r0 if self.equal(p0dict.get((1,0),S.Zero),p1dict.get((0,1),S.Zero)) and self.equal(p0dict.get((0,1),S.Zero),-p1dict.get((1,0),S.Zero)): # p0+tvar*p1, p1-tvar*p0 # subs: tvar*svar + cvar = 1, svar-tvar*cvar=tvar neweqs.append([Poly(p0dict.get((1,0),S.Zero) + p0dict.get((0,1),S.Zero)*tvar + p0.TC() + tvar*p1.TC(),*symbols), Poly(r0+tvar*r1,*othersymbols)]) neweqs.append([Poly(p0dict.get((1,0),S.Zero)*tvar - p0dict.get((0,1),S.Zero) - p0.TC()*tvar + p1.TC(),*symbols), Poly(r1-tvar*r0,*othersymbols)]) if len(neweqs) != 8: raise self.CannotSolveError('coefficients of equations need to match! only got %d reduced equations'%len(neweqs)) for peq in rawpolyeqs: if not peq[0].has(cvar,svar): neweqs.append([Poly(peq[0],*symbols),Poly(peq[1],*othersymbols)]) neweqs.append([Poly(peq[0].as_expr()*tvar,*symbols),Poly(peq[1].as_expr()*tvar,*othersymbols)]) # one side should have only numbers, this makes the following inverse operations trivial neweqs_full = [] reducedeqs = [] for peq in neweqs: peqdict = peq[0].as_dict() peq[1] = peq[1] - tvar*peqdict.get((0,0,0,0,1),S.Zero)-peq[0].TC() peq[0] = peq[0] - tvar*peqdict.get((0,0,0,0,1),S.Zero)-peq[0].TC() if peq[0] != S.Zero: neweqs_full.append(peq) else: reducedeqs.append(peq[1].as_expr().subs(self.freevarsubs)) haszeroequations = len(reducedeqs)>0 allmonoms = set() for peq in neweqs_full: allmonoms = allmonoms.union(set(peq[0].monoms())) allmonoms = list(allmonoms) allmonoms.sort() if len(allmonoms) > len(neweqs_full): raise self.CannotSolveError('new monoms is not %d!=16'%len(allmonoms)) if len(allmonoms) < len(neweqs_full): # order with respect to complexity of [0], this is to make the inverse of A faster complexity = [(self.codeComplexity(peq[0].as_expr()),peq) for peq in neweqs_full] complexity.sort(key=itemgetter(0)) neweqs_full = [peq for c,peq in complexity] A = zeros((len(neweqs_full),len(allmonoms))) B = zeros((len(neweqs_full),1)) for ipeq,peq in enumerate(neweqs_full): for m,c in peq[0].terms(): A[ipeq,allmonoms.index(m)] = c.subs(self.freevarsubs) B[ipeq] = peq[1].as_expr().subs(self.freevarsubs) AU = zeros((len(allmonoms),len(allmonoms))) AL = zeros((A.shape[0]-len(allmonoms),len(allmonoms))) BU = zeros((len(allmonoms),1)) BL = zeros((A.shape[0]-len(allmonoms),1)) AUadjugate = None AUinv = None AU = A[:A.shape[1],:] nummatrixsymbols = __builtin__.sum([1 for a in AU if not a.is_number]) if nummatrixsymbols > 80: raise self.CannotSolveError('matrix has too many symbols (%d), giving up since most likely will freeze'%nummatrixsymbols) AUdet = AU.det() if AUdet != S.Zero: rows = range(A.shape[1]) else: # prune the dependent vectors AU = A[0:1,:] rows = [0] for i in range(1,A.shape[0]): AU2 = AU.col_join(A[i:(i+1),:]) if AU2.shape[0] == AU2.shape[1]: AUdet = AU2.det() else: AUdet = (AU2*AU2.transpose()).det() if AUdet == S.Zero: continue AU = AU2 rows.append(i) if AU.shape[0] == AU.shape[1]: break if AU.shape[0] != AU.shape[1]: raise self.CannotSolveError('could not find non-singular matrix') AUinv = AU.inv() if self.has(A,*self.freevars): log.info('AU has symbols, so working with inverse might take some time') AUdet = self.trigsimp(AUdet.subs(self.freevarsubsinv),self.freejointvars).subs(self.freevarsubs) # find the adjugate by simplifying from the inverse AUadjugate = zeros(AUinv.shape) sinsubs = [] for freevar in self.freejointvars: var=self.Variable(freevar) sinsubs.append((var.cvar**2,1-var.svar**2)) sinsubs.append((var.cvar**3,var.cvar*(1-var.svar**2))) for i in range(AUinv.shape[0]): for j in range(AUinv.shape[1]): numerator,denominator = self.recursiveFraction(AUinv[i,j]) numerator = self.trigsimp(numerator.subs(self.freevarsubsinv),self.freejointvars).subs(self.freevarsubs) numerator, common = self.removecommonexprs(numerator,onlygcd=True,returncommon=True) denominator = self.trigsimp((denominator/common).subs(self.freevarsubsinv),self.freejointvars).subs(self.freevarsubs) try: q,r=div(numerator*AUdet,denominator,self.freevars) except PolynomialError, e: # 1/(-9000000*cj16 - 9000000) contains an element of the generators set raise self.CannotSolveError('cannot divide for matrix inversion: %s'%e) if r != S.Zero: # sines and cosines can mix things up a lot, try converting to all sines numerator2 = (numerator*AUdet).subs(sinsubs) denominator2 = denominator.subs(sinsubs) q,r=div(numerator2,denominator2,self.freevars) if r != S.Zero: log.warn('cannot get rid of denominator in (%s/%s)',numerator2,denominator2) q = (numerator/denominator)*AUdet AUadjugate[i,j] = self.trigsimp(q.subs(self.freevarsubsinv),self.freejointvars).subs(self.freevarsubs) checkforzeros.append(self.removecommonexprs(AUdet,onlygcd=False,onlynumbers=True)) log.info('found non-singular AU matrix') otherrows = range(A.shape[0]) for i,row in enumerate(rows): BU[i] = B[row] otherrows.remove(row) for i,row in enumerate(otherrows): BL[i] = B[row] AL[i,:] = A[row,:] if AUadjugate is not None: # reason we're multiplying by adjugate instead of inverse is to get rid of the potential divides by (free) parameters C = AL*(AUadjugate*BU)-BL*AUdet else: C = AL*(AUinv*BU)-BL for c in C: reducedeqs.append(c) # is now a (len(neweqs)-len(allmonoms))x1 matrix, usually this is 4x1 htvars = [] htvarsubs = [] htvarsubs2 = [] usedvars = [] for nextindex in [0,2]: name = othersymbols[nextindex].name[1:] htvar = Symbol('ht%s'%name) htvarsubs += [(othersymbols[nextindex],(1-htvar**2)/(1+htvar**2)),(othersymbols[nextindex+1],2*htvar/(1+htvar**2))] htvars.append(htvar) htvarsubs2.append((Symbol(name),2*atan(htvar))) usedvars.append(Symbol(name)) htvarsubs += [(cvar,(1-tvar**2)/(1+tvar**2)),(svar,2*tvar/(1+tvar**2))] htvars.append(tvar) htvarsubs2.append((Symbol(varname),2*atan(tvar))) usedvars.append(Symbol(varname)) if haszeroequations: log.info('special structure in equations detected, try to solve through elimination') AllEquations = [eq.subs(self.invsubs) for eq in reducedeqs] for curvar in usedvars[:-1]: try: unknownvars = usedvars[:] unknownvars.remove(curvar) jointtrees2=[] curvarsubs=self.Variable(curvar).subs treefirst = self.solveAllEquations(AllEquations,curvars=[curvar],othersolvedvars=self.freejointvars,solsubs=self.freevarsubs[:],endbranchtree=[AST.SolverSequence([jointtrees2])],unknownvars=unknownvars+[tvar]) # solvable, which means we now have len(AllEquations)-1 with two variables, solve with half angles halfanglesolution=self.solvePairVariablesHalfAngle(raweqns=[eq.subs(curvarsubs) for eq in AllEquations],var0=unknownvars[0],var1=unknownvars[1],othersolvedvars=self.freejointvars+[curvar])[0] # sometimes halfanglesolution can evaluate to all zeros (katana arm), need to catch this and go to a different branch halfanglesolution.AddHalfTanValue = True jointtrees2.append(halfanglesolution) halfanglevar = unknownvars[0] if halfanglesolution.jointname==unknownvars[0].name else unknownvars[1] unknownvars.remove(halfanglevar) try: # give that two variables are solved, can most likely solve the rest. Solving with the original # equations yields simpler solutions since reducedeqs hold half-tangents curvars = solvejointvars[:] curvars.remove(curvar) curvars.remove(halfanglevar) subsinv = [] for v in solvejointvars: subsinv += self.Variable(v).subsinv AllEquationsOrig = [(peq[0].as_expr()-peq[1].as_expr()).subs(subsinv) for peq in rawpolyeqs] self.sortComplexity(AllEquationsOrig) jointtrees2 += self.solveAllEquations(AllEquationsOrig,curvars=curvars,othersolvedvars=self.freejointvars+[curvar,halfanglevar],solsubs=self.freevarsubs+curvarsubs+self.Variable(halfanglevar).subs,endbranchtree=endbranchtree) return solutiontree+treefirst,solvejointvars except self.CannotSolveError,e: # try another strategy log.debug(e) # solve all the unknowns now jointtrees3=[] treesecond = self.solveAllEquations(AllEquations,curvars=unknownvars,othersolvedvars=self.freejointvars+[curvar,halfanglevar],solsubs=self.freevarsubs+curvarsubs+self.Variable(halfanglevar).subs,endbranchtree=[AST.SolverSequence([jointtrees3])]) for t in treesecond: # most likely t is a solution... t.AddHalfTanValue = True if isinstance(t,AST.SolverCheckZeros): for t2 in t.zerobranch: t2.AddHalfTanValue = True for t2 in t.nonzerobranch: t2.AddHalfTanValue = True if len(t.zerobranch) == 0 or isinstance(t.zerobranch[0],AST.SolverBreak): log.info('detected zerobranch with SolverBreak, trying to fix') jointtrees2 += treesecond # using these solutions, can evaluate all monoms and check for consistency, this step is crucial since # AllEquations might not constrain all degrees of freedom (check out katana) indices = [] for i in range(4): monom = [0]*len(symbols) monom[i] = 1 indices.append(allmonoms.index(tuple(monom))) X = AUinv*BU for i in [0,2]: jointname=symbols[i].name[1:] try: # atan2(0,0) produces an invalid solution jointtrees3.append(AST.SolverSolution(jointname,jointeval=[atan2(X[indices[i+1]],X[indices[i]])],isHinge=self.isHinge(jointname))) usedvars.append(Symbol(jointname)) except Exception, e: log.warn(e) jointcheckeqs = [] for i,monom in enumerate(allmonoms): if not i in indices: eq = S.One for isymbol,ipower in enumerate(monom): eq *= symbols[isymbol]**ipower jointcheckeqs.append(eq-X[i]) # threshold can be a little more loose since just a sanity check jointtrees3.append(AST.SolverCheckZeros('sanitycheck',jointcheckeqs,zerobranch=endbranchtree,nonzerobranch=[AST.SolverBreak()],anycondition=False,thresh=0.001)) return solutiontree+treefirst,usedvars except self.CannotSolveError,e: log.info(e) try: log.info('try to solve first two variables pairwise') #solution = self.solvePairVariables(AllEquations,usedvars[0],usedvars[1],self.freejointvars,maxcomplexity=50) jointtrees=[] raweqns=[eq for eq in AllEquations if not eq.has(tvar)] if len(raweqns) > 0: halfanglesolution = self.solvePairVariablesHalfAngle(raweqns=raweqns,var0=usedvars[0],var1=usedvars[1],othersolvedvars=self.freejointvars)[0] halfanglevar = usedvars[0] if halfanglesolution.jointname==usedvars[0].name else usedvars[1] unknownvar = usedvars[1] if halfanglesolution.jointname==usedvars[0].name else usedvars[0] nexttree = self.solveAllEquations(raweqns,curvars=[unknownvar],othersolvedvars=self.freejointvars+[halfanglevar],solsubs=self.freevarsubs+self.Variable(halfanglevar).subs,endbranchtree=[AST.SolverSequence([jointtrees])]) #finalsolution = self.solveSingleVariable(AllEquations,usedvars[2],othersolvedvars=self.freejointvars+usedvars[0:2],maxsolutions=4,maxdegree=4) try: finaltree = self.solveAllEquations(AllEquations,curvars=usedvars[2:],othersolvedvars=self.freejointvars+usedvars[0:2],solsubs=self.freevarsubs+self.Variable(usedvars[0]).subs+self.Variable(usedvars[1]).subs,endbranchtree=endbranchtree) jointtrees += finaltree return [halfanglesolution]+nexttree,usedvars except self.CannotSolveError,e: log.debug('failed to solve for final variable %s, so returning just two: %s'%(usedvars[2],str(usedvars[0:2]))) jointtrees += endbranchtree # sometimes the last variable cannot be solved, so returned the already solved variables and let the higher function take care of it return [halfanglesolution]+nexttree,usedvars[0:2] except self.CannotSolveError,e: log.debug(e) newreducedeqs = [] hassinglevariable = False for eq in reducedeqs: peq = Poly(eq,*othersymbols) maxdenom = [0,0] for monoms in peq.monoms(): for i in range(len(maxdenom)): maxdenom[i] = max(maxdenom[i],monoms[2*i]+monoms[2*i+1]) eqnew = S.Zero for monoms,c in peq.terms(): term = c for i in range(4): num,denom = fraction(htvarsubs[i][1]) term *= num**monoms[i] term *= tvar**monoms[4] # the denoms for 0,1 and 2,3 are the same for i in range(len(maxdenom)): denom = fraction(htvarsubs[2*i][1])[1] term *= denom**(maxdenom[i]-monoms[2*i]-monoms[2*i+1]) eqnew += term newpeq = Poly(eqnew,htvars[0],htvars[1],tvar) newreducedeqs.append(newpeq) hassinglevariable |= any([all([__builtin__.sum(monom)==monom[i] for monom in newpeq.monoms()]) for i in range(3)]) if hassinglevariable: log.info('hassinglevariable, trying with raw equations') AllEquations = [] for eq in reducedeqs: peq = Poly(eq,tvar) if sum(peq.degree_list()) == 0: AllEquations.append(peq.TC().subs(self.invsubs).expand()) elif sum(peq.degree_list()) == 1 and peq.TC() == S.Zero: AllEquations.append(peq.LC().subs(self.invsubs).expand()) else: # two substitutions: sin/(1+cos), (1-cos)/sin neweq0 = S.Zero neweq1 = S.Zero for monoms,c in peq.terms(): neweq0 += c*(svar**monoms[0])*((1+cvar)**(peq.degree(0)-monoms[0])) neweq1 += c*((1-cvar)**monoms[0])*(svar**(peq.degree(0)-monoms[0])) AllEquations.append(neweq0.subs(self.invsubs).expand()) AllEquations.append(neweq1.subs(self.invsubs).expand()) self.sortComplexity(AllEquations) for i in range(3): try: unknownvars = usedvars[:] unknownvars.pop(i) endbranchtree2 = [] solutiontree = self.solveAllEquations(AllEquations,curvars=[usedvars[i]],othersolvedvars=self.freejointvars[:],solsubs=self.freevarsubs[:],endbranchtree=[AST.SolverSequence([endbranchtree2])],unknownvars=unknownvars) endbranchtree2 += self.solveAllEquations(AllEquations,curvars=unknownvars[0:2],othersolvedvars=self.freejointvars[:]+[usedvars[i]],solsubs=self.freevarsubs[:]+self.Variable(usedvars[i]).subs,endbranchtree=endbranchtree) return solutiontree, usedvars except self.CannotSolveError: pass # try: # testvars = [Symbol(othersymbols[0].name[1:]),Symbol(othersymbols[2].name[1:]),Symbol(varname)] # AllEquations = [(peq[0].as_expr()-peq[1].as_expr()).expand() for peq in polyeqs if not peq[0].has(*symbols)] # coupledsolutions = self.solveAllEquations(AllEquations,curvars=testvars,othersolvedvars=self.freejointvars[:],solsubs=self.freevarsubs[:],endbranchtree=endbranchtree) # return coupledsolutions,testvars # except self.CannotSolveError: # pass # exportcoeffeqs = None for ileftvar in range(len(htvars)): try: exportcoeffeqs,exportmonoms = self.solveDialytically(newreducedeqs,ileftvar,getsubs=getsubs) break except self.CannotSolveError,e: log.warn('failed with leftvar %s: %s',newreducedeqs[0].gens[ileftvar],e) if exportcoeffeqs is None: raise self.CannotSolveError('failed to solve dialytically') if ileftvar > 0: raise self.CannotSolveError('solving equations dialytically succeeded with var index %d, unfortunately code generation supports only index 0'%ileftvar) exportvar = [htvars[ileftvar].name] exportvar += [v.name for i,v in enumerate(htvars) if i != ileftvar] coupledsolution = AST.SolverCoeffFunction(jointnames=[v.name for v in usedvars],jointeval=[v[1] for v in htvarsubs2],jointevalcos=[htvarsubs[2*i][1] for i in range(len(htvars))],jointevalsin=[htvarsubs[2*i+1][1] for i in range(len(htvars))],isHinges=[self.isHinge(v.name) for v in usedvars],exportvar=exportvar,exportcoeffeqs=exportcoeffeqs,exportfnname='solvedialyticpoly8qep',rootmaxdim=16) coupledsolution.presetcheckforzeros = checkforzeros solutiontree.append(coupledsolution) self.usinglapack = True return solutiontree+endbranchtree,usedvars
[docs] def solveKohliOsvatic(self,rawpolyeqs,solvejointvars,endbranchtree): """Find a 16x16 matrix where the entries are linear with respect to the tan half-angle of one of the variables [Kohli1993]_. Takes in the 14 raghavan/roth equations. .. [Kohli1993] Dilip Kohli and M. Osvatic, "Inverse Kinematics of General 6R and 5R,P Serial Manipulators", Journal of Mechanical Design, Volume 115, Issue 4, Dec 1993. """ log.info('attempting kohli/osvatic general ik method') if len(rawpolyeqs[0][0].gens) < len(rawpolyeqs[0][1].gens): for peq in rawpolyeqs: peq[0],peq[1] = peq[1],peq[0] symbols = list(rawpolyeqs[0][0].gens) othersymbols = list(rawpolyeqs[0][1].gens) othersymbolsnames = [] for s in othersymbols: testeq = s.subs(self.invsubs) for solvejointvar in solvejointvars: if testeq.has(solvejointvar): othersymbolsnames.append(solvejointvar) break assert(len(othersymbols)==len(othersymbolsnames)) symbolsubs = [(symbols[i].subs(self.invsubs),symbols[i]) for i in range(len(symbols))] if len(symbols) != 6: raise self.CannotSolveError('Kohli/Osvatic method requires 3 unknown variables') # choose which leftvar can determine the singularity of the following equations! for i in range(0,6,2): eqs = [peq for peq in rawpolyeqs if peq[0].has(symbols[i],symbols[i+1])] if len(eqs) <= 8: break if len(eqs) > 8: raise self.CannotSolveError('need 8 or less equations of one variable') cvar = symbols[i] svar = symbols[i+1] tvar = Symbol('t'+cvar.name[1:]) symbols.remove(cvar) symbols.remove(svar) othereqs = [peq for peq in rawpolyeqs if not peq[0].has(cvar,svar)] polyeqs = [[eq[0].as_expr(),eq[1]] for eq in eqs] if len(polyeqs) < 8: raise self.CannotSolveError('solveKohliOsvatic: need 8 or more polyeqs') # solve the othereqs for symbols without the standalone symbols[2] and symbols[3] reducedeqs = [] othersymbolsnamesunique = list(set(othersymbolsnames)) # get the unique names for jother in range(len(othersymbolsnamesunique)): if not self.isHinge(othersymbolsnamesunique[jother].name): continue othervar=self.Variable(othersymbolsnamesunique[jother]) cosmonom = [0]*len(othersymbols) cosmonom[othersymbols.index(othervar.cvar)] = 1 cosmonom = tuple(cosmonom) sinmonom = [0]*len(othersymbols) sinmonom[othersymbols.index(othervar.svar)] = 1 sinmonom = tuple(sinmonom) leftsideeqs = [] rightsideeqs = [] finaleqsymbols = symbols + [othervar.cvar,othervar.svar] for eq0,eq1 in othereqs: leftsideeq = Poly(eq1,*othersymbols) leftsideeqdict = leftsideeq.as_dict() rightsideeq = Poly(eq0,*finaleqsymbols) coscoeff = leftsideeqdict.get(cosmonom,S.Zero) if coscoeff != S.Zero: rightsideeq = rightsideeq - othervar.cvar*coscoeff leftsideeq = leftsideeq - othervar.cvar*coscoeff sincoeff = leftsideeqdict.get(sinmonom,S.Zero) if sincoeff != S.Zero: rightsideeq = rightsideeq - othervar.svar*sincoeff leftsideeq = leftsideeq - othervar.svar*sincoeff const = leftsideeq.TC() if const != S.Zero: rightsideeq = rightsideeq - const leftsideeq = leftsideeq - const # check that leftsideeq doesn't hold any terms with cosmonom and sinmonom? rightsideeqs.append(rightsideeq) leftsideeqs.append(leftsideeq) # number of symbols for kawada-hiro robot is 16 if len(othersymbols) > 2: reducedeqs = self.reduceBothSidesSymbolically(leftsideeqs,rightsideeqs,usesymbols=False,maxsymbols=18) for peq in reducedeqs: peq[0] = Poly(peq[0],*othersymbols) else: reducedeqs = [[left,right] for left,right in izip(leftsideeqs,rightsideeqs)] if len(reducedeqs) > 0: break if len(reducedeqs) == 0: raise self.CannotSolveError('KohliOsvatic method: could not reduce the equations') finaleqs = [] for peq0,eq1 in reducedeqs: if peq0 == S.Zero: finaleqs.append(Poly(eq1,*finaleqsymbols)) if len(finaleqs) >= 2: # perhaps can solve finaleqs as is? # transfer othersymbols[2*jother:(2+2*jother)] to the leftside try: leftsideeqs = [] rightsideeqs = [] for finaleq in finaleqs: peq=Poly(finaleq,*othersymbols[2*jother:(2+2*jother)]) leftsideeqs.append(peq.sub(peq.TC())) rightsideeqs.append(-peq.TC()) reducedeqs2 = self.reduceBothSidesSymbolically(leftsideeqs,rightsideeqs,usesymbols=False,maxsymbols=18) # find all the equations with left side = to zero usedvars = set() for symbol in symbols: usedvars.add(Symbol(symbol.name[1:])) AllEquations = [] for eq0, eq1 in reducedeqs2: if eq0 == S.Zero: AllEquations.append(eq1.subs(self.invsubs)) if len(AllEquations) > 0: otherjointtrees = [] tree = self.solveAllEquations(AllEquations,curvars=list(usedvars),othersolvedvars=[],solsubs=self.freevarsubs,endbranchtree=[AST.SolverSequence([otherjointtrees])]) log.info('first solveAllEquations successful: %s',usedvars) # try: # # although things can be solved at this point, it yields a less optimal solution than if all variables were considered... # solsubs=list(self.freevarsubs) # for usedvar in usedvars: # solsubs += self.Variable(usedvar).subs # # solved, so substitute back into reducedeqs and see if anything new can be solved # otherusedvars = set() # for symbol in othersymbols: # otherusedvars.add(Symbol(symbol.name[1:])) # OtherAllEquations = [] # for peq0,eq1 in reducedeqs: # OtherAllEquations.append((peq0.as_expr()-eq1).subs(self.invsubs).expand()) # otherjointtrees += self.solveAllEquations(OtherAllEquations,curvars=list(otherusedvars),othersolvedvars=list(usedvars),solsubs=solsubs,endbranchtree=endbranchtree) # return tree, list(usedvars)+list(otherusedvars) # except self.CannotSolveError: # still have the initial solution otherjointtrees += endbranchtree return tree, list(usedvars) except self.CannotSolveError,e: pass log.info('build final equations for symbols: %s',finaleqsymbols) neweqs=[] for i in range(0,8,2): p0 = Poly(polyeqs[i][0],cvar,svar) p0dict = p0.as_dict() p1 = Poly(polyeqs[i+1][0],cvar,svar) p1dict = p1.as_dict() r0 = polyeqs[i][1].as_expr() r1 = polyeqs[i+1][1].as_expr() if self.equal(p0dict.get((1,0),S.Zero),-p1dict.get((0,1),S.Zero)) and self.equal(p0dict.get((0,1),S.Zero),p1dict.get((1,0),S.Zero)): p0,p1 = p1,p0 p0dict,p1dict=p1dict,p0dict r0,r1 = r1,r0 if self.equal(p0dict.get((1,0),S.Zero),p1dict.get((0,1),S.Zero)) and self.equal(p0dict.get((0,1),S.Zero),-p1dict.get((1,0),S.Zero)): # p0+tvar*p1, p1-tvar*p0 # subs: tvar*svar + cvar = 1, svar-tvar*cvar=tvar neweqs.append([Poly(p0dict.get((1,0),S.Zero) + p0dict.get((0,1),S.Zero)*tvar + p0.TC() + tvar*p1.TC(),*symbols), Poly(r0+tvar*r1,*othersymbols)]) neweqs.append([Poly(p0dict.get((1,0),S.Zero)*tvar - p0dict.get((0,1),S.Zero) - p0.TC()*tvar + p1.TC(),*symbols), Poly(r1-tvar*r0,*othersymbols)]) if len(neweqs) != 8: raise self.CannotSolveError('coefficients of equations need to match! only got %d reduced equations'%len(neweqs)) for eq0,eq1 in neweqs: commondenom = Poly(S.One,*self.pvars) hasunknown = False for m,c in eq1.terms(): foundreq = [req[1] for req in reducedeqs if req[0].monoms()[0] == m] if len(foundreq) > 0: n,d = fraction(foundreq[0]) commondenom = Poly(lcm(commondenom,d),*self.pvars) else: if m[2*(1-jother)] > 0 or m[2*(1-jother)+1] > 0: # perhaps there's a way to combine what's in reducedeqs? log.warn('unknown %s',m) hasunknown = True if hasunknown: continue commondenom = self.removecommonexprs(commondenom.as_expr(),onlygcd=True,onlynumbers=True) finaleq = eq0.as_expr()*commondenom for m,c in eq1.terms(): foundreq = [req[1] for req in reducedeqs if req[0].monoms()[0] == m] if len(foundreq) > 0: finaleq = finaleq - c*simplify(foundreq[0]*commondenom) else: finaleq = finaleq - Poly.from_dict({m:c*commondenom},*eq1.gens).as_expr() finaleqs.append(Poly(finaleq.expand(),*finaleqsymbols)) # finally do the half angle substitution with symbols # set: # j=othersymbols[2]*(1+dummys[0]**2)*(1+dummys[1]**2) # k=othersymbols[3]*(1+dummys[0]**2)*(1+dummys[1]**2) dummys = [] dummysubs = [] dummysubs2 = [] dummyvars = [] usedvars = [] dummys.append(tvar) dummyvars.append((tvar,tan(0.5*Symbol(tvar.name[1:])))) usedvars.append(Symbol(cvar.name[1:])) dummysubs2.append((usedvars[-1],2*atan(tvar))) dummysubs += [(cvar,(1-tvar**2)/(1+tvar**2)),(svar,2*tvar/(1+tvar**2))] for i in range(0,len(symbols),2): dummy = Symbol('ht%s'%symbols[i].name[1:]) # [0] - cos, [1] - sin dummys.append(dummy) dummysubs += [(symbols[i],(1-dummy**2)/(1+dummy**2)),(symbols[i+1],2*dummy/(1+dummy**2))] var = symbols[i].subs(self.invsubs).args[0] dummyvars.append((dummy,tan(0.5*var))) dummysubs2.append((var,2*atan(dummy))) if not var in usedvars: usedvars.append(var) commonmult = (1+dummys[1]**2)*(1+dummys[2]**2) usedvars.append(Symbol(othersymbols[2*jother].name[1:])) dummyj = Symbol('dummyj') dummyk = Symbol('dummyk') dummyjk = Symbol('dummyjk') dummys.append(dummyj) dummyvars.append((dummyj,othersymbols[2*jother]*(1+dummyvars[1][1]**2)*(1+dummyvars[2][1]**2))) dummysubs.append((othersymbols[2*jother],cos(dummyjk))) dummys.append(dummyk) dummyvars.append((dummyk,othersymbols[1+2*jother]*(1+dummyvars[1][1]**2)*(1+dummyvars[2][1]**2))) dummysubs.append((othersymbols[1+2*jother],sin(dummyjk))) dummysubs2.append((usedvars[-1],dummyjk)) newreducedeqs = [] for peq in finaleqs: eqnew = S.Zero for monoms,c in peq.terms(): term = S.One for i in range(4): term *= dummysubs[i+2][1]**monoms[i] if monoms[4] == 1: eqnew += c * dummyj elif monoms[5] == 1: eqnew += c * dummyk else: eqnew += c*simplify(term*commonmult) newreducedeqs.append(Poly(eqnew,*dummys)) exportcoeffeqs = None for ileftvar in range(len(dummys)): leftvar = dummys[ileftvar] try: exportcoeffeqs,exportmonoms = self.solveDialytically(newreducedeqs,ileftvar,getsubs=None) break except self.CannotSolveError,e: log.warn('failed with leftvar %s: %s',leftvar,e) if exportcoeffeqs is None: raise self.CannotSolveError('failed to solve dialytically') if ileftvar > 0: raise self.CannotSolveError('solving equations dialytically succeeded with var index %d, unfortunately code generation supports only index 0'%ileftvar) coupledsolution = AST.SolverCoeffFunction(jointnames=[v.name for v in usedvars],jointeval=[v[1] for v in dummysubs2],jointevalcos=[dummysubs[2*i][1] for i in range(len(usedvars))],jointevalsin=[dummysubs[2*i+1][1] for i in range(len(usedvars))],isHinges=[self.isHinge(v.name) for v in usedvars],exportvar=dummys[0:3]+[dummyjk],exportcoeffeqs=exportcoeffeqs,exportfnname='solvedialyticpoly16lep',rootmaxdim=16) self.usinglapack = True return [coupledsolution]+endbranchtree,usedvars
[docs] def solveDialytically(self,newreducedeqs,ileftvar,returnmatrix=False,getsubs=None): """ Return the coefficients to solve equations dialytically (Salmon 1885) leaving out variable index ileftvar. Extract the coefficients of 1, leftvar**1, leftvar**2, ... of every equation every len(newreducedeqs)*len(monoms) coefficients specify one degree of all the equations (order of monoms is specified in exportmonomorder there should be len(newreducedeqs)*len(monoms)*maxdegree coefficients Method also checks if the equations are linearly dependent """ if len(newreducedeqs) == 0: raise self.CannotSolveError('solveDialytically given zero equations') allmonoms = set() origmonoms = set() maxdegree = 0 for peq in newreducedeqs: if sum(peq.degree_list()) == 0: log.warn('solveDialytically: polynomial %s degree is 0',peq) continue for m in peq.monoms(): mlist = list(m) maxdegree=max(maxdegree,mlist.pop(ileftvar)) allmonoms.add(tuple(mlist)) origmonoms.add(tuple(mlist)) mlist[0] += 1 allmonoms.add(tuple(mlist)) allmonoms = list(allmonoms) allmonoms.sort() origmonoms = list(origmonoms) origmonoms.sort() if len(allmonoms)<2*len(newreducedeqs): log.warn('solveDialytically equations %d > %d, should be equal...', 2*len(newreducedeqs),len(allmonoms)) newreducedeqs = newreducedeqs[0:(len(allmonoms)/2)] if len(allmonoms) == 0 or len(allmonoms)>2*len(newreducedeqs): raise self.CannotSolveError('solveDialytically: more unknowns than equations %d>%d'%(len(allmonoms), 2*len(newreducedeqs))) Mall = [zeros((2*len(newreducedeqs),len(allmonoms))) for i in range(maxdegree+1)] exportcoeffeqs = [S.Zero]*(len(newreducedeqs)*len(origmonoms)*(maxdegree+1)) for ipeq,peq in enumerate(newreducedeqs): for m,c in peq.terms(): mlist = list(m) degree=mlist.pop(ileftvar) exportindex = degree*len(origmonoms)*len(newreducedeqs) + len(origmonoms)*ipeq+origmonoms.index(tuple(mlist)) exportcoeffeqs[exportindex] = c Mall[degree][len(newreducedeqs)+ipeq,allmonoms.index(tuple(mlist))] = c mlist[0] += 1 Mall[degree][ipeq,allmonoms.index(tuple(mlist))] = c # have to check that the determinant is not zero for several values of ileftvar! It is very common that # some equations are linearly dependent and not solvable through this method. if self.testconsistentvalues is not None: linearlyindependent = False for itest,subs in enumerate(self.testconsistentvalues): if getsubs is not None: # have to explicitly evaluate since testsubs can be very complex subsvals = [(s,v.evalf()) for s,v in subs] subs = subsvals+getsubs(subsvals) A = Mall[maxdegree].subs(subs).subs(self.globalsymbols).evalf() eps = 10**-(self.precision-3) Anumpy = numpy.array(numpy.array(A), numpy.float64) if numpy.isnan(numpy.sum(Anumpy)): break eigenvals = numpy.linalg.eigvals(Anumpy) if all([Abs(f) > eps for f in eigenvals]): Ainv = A.inv(method='LU') B = Ainv*Mall[1].subs(subs).evalf() C = Ainv*Mall[0].subs(subs).evalf() A2 = zeros((B.shape[0],B.shape[0]*2)) for i in range(B.shape[0]): A2[i,B.shape[0]+i] = S.One A2=A2.col_join((-C).row_join(-B)) eigenvals2,eigenvecs2 = numpy.linalg.eig(numpy.array(numpy.array(A2),numpy.float64)) # check if solutions can actually be extracted # find all the zero eigenvalues roots = [] numrepeating = 0 for ieig,eigenvalue in enumerate(eigenvals2): if abs(numpy.imag(eigenvalue)) < 1e-12: if abs(numpy.real(eigenvalue)) > 1: ev = eigenvecs2[A.shape[0]:,ieig] else: ev = eigenvecs2[:A.shape[0],ieig] if abs(ev[0]) < 1e-14: continue br = ev[1:] / ev[0] dists = abs(numpy.array(roots) - numpy.real(eigenvalue)) if any(dists<1e-7): numrepeating += 1 roots.append(numpy.real(eigenvalue)) if numrepeating > 0: log.info('found %d repeating roots in solveDialytically matrix: %s',numrepeating,roots) continue linearlyindependent = True break if not linearlyindependent: raise self.CannotSolveError('equations are not linearly independent') if returnmatrix: return Mall,allmonoms return exportcoeffeqs,origmonoms
[docs] def simplifyTransformPoly(self,peq): """simplifies the coefficients of the polynomial with simplifyTransform and returns the new polynomial """ return peq.termwise(lambda m,c: self.simplifyTransform(c))
[docs] def simplifyTransform(self,eq,othervars=None): """Attemps to simplify an equation given that variables from a rotation matrix have been used. There are 12 constraints that are tested: - lengths of rows and colums are 1 - dot products of combinations of rows/columns are 0 - cross products of combinations of rows/columns yield the left over row/column """ if othervars is not None: peq = Poly(eq,*othervars) if peq == S.Zero: return S.Zero peqnew = peq.termwise(lambda m,c: self.simplifyTransform(c)) return peqnew.as_expr() # first simplify just rotations (since they don't add any new variables) allsymbols = list(self.Tee[0:3,0:3]) # check normals normgroups = [] for i in range(3): normgroups.append([self.Tee[i,0],self.Tee[i,1],self.Tee[i,2],S.One]) normgroups.append([self.Tee[0,i],self.Tee[1,i],self.Tee[2,i],S.One]) def _simplifynorm(eq): neweq = None for group in normgroups: p = Poly(eq,group[0],group[1],group[2]) changed = False if len(p.terms()) == 1: continue for (m0,c0),(m1,c1) in combinations(p.terms(),2): if self.equal(c0,c1): for i,j,k in [(0,1,2),(0,2,1),(1,2,0)]: if ((m0[i] == 2 and m1[j] == 2) or (m0[j]==2 and m1[i]==2)) and m0[k]==m1[k]: p = p + c0*(group[3]-group[0]**2-group[1]**2-group[2]**2)*group[k]**(m0[k]) neweq = p.as_expr() eq = neweq changed = True break if changed: break return neweq # check for dot products between rows and columns dotgroups = [] for i,j in combinations(range(3),2): # dot product of rotations is always 0 dotgroups.append([[i,j],[i+3,j+3],[i+6,j+6],S.Zero]) dotgroups.append([[3*i,3*j],[3*i+1,3*j+1],[3*i+2,3*j+2],S.Zero]) def _simplifydot(eq): p = Poly(eq,*allsymbols) changed = False for dg in dotgroups: for i,j,k in [(0,1,2),(0,2,1),(1,2,0)]: for comb in combinations(p.terms(),2): if self.equal(comb[0][1],comb[1][1]): for (m0,c0),(m1,c1) in [comb,comb[::-1]]: if m0[dg[i][0]] == 1 and m0[dg[i][1]] == 1 and m1[dg[j][0]] == 1 and m1[dg[j][1]] == 1: # make sure the left over terms are also the same m0l = list(m0); m0l[dg[i][0]] = 0; m0l[dg[i][1]] = 0 m1l = list(m1); m1l[dg[j][0]] = 0; m1l[dg[j][1]] = 0 if tuple(m0l) == tuple(m1l): m2 = list(m0l); m2[dg[k][0]] += 1; m2[dg[k][1]] += 1 # there is a bug in sympy v0.6.7 polynomial adding here! p = p.sub(Poly.from_dict({m0:c0},*p.gens)).sub(Poly.from_dict({m1:c1},*p.gens)).sub(Poly.from_dict({tuple(m2):c0},*p.gens)) if dg[3] != S.Zero: p = p.add(Poly(dg[3],*p.gens)*Poly.from_dict({tuple(m0l):c0},*p.gens)) changed = True break if changed: break return p.as_expr() if changed else None # add cross products crossgroups = [] for i,j,k in [(0,1,2),(0,2,1),(1,2,0)]: # column crossgroups.append([[i+3,j+6],[i+6,j+3],k]) crossgroups.append([[i+6,j],[i,j+6],k+3]) crossgroups.append([[i,j+3],[i+3,j],k+6]) # row crossgroups.append([[3*i+1,3*j+2],[3*i+2,3*j+1],3*k]) crossgroups.append([[3*i+2,3*j],[3*i,3*j+2],3*k+1]) crossgroups.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 crossgroups[-6:]: crossgroup[0],crossgroup[1] = crossgroup[1],crossgroup[0] def _simplifycross(eq): # check cross products changed = False p = Poly(eq,*allsymbols) for cg in crossgroups: for comb in combinations(p.terms(),2): if self.equal(comb[0][1],-comb[1][1]): for (m0,c0),(m1,c1) in [comb,comb[::-1]]: if m0[cg[0][0]] == 1 and m0[cg[0][1]] == 1 and m1[cg[1][0]] == 1 and m1[cg[1][1]] == 1: # make sure the left over terms are also the same m0l = list(m0); m0l[cg[0][0]] = 0; m0l[cg[0][1]] = 0 m1l = list(m1); m1l[cg[1][0]] = 0; m1l[cg[1][1]] = 0 if tuple(m0l) == tuple(m1l): m2 = m0l; m2[cg[2]] += 1 # there is a bug in sympy polynomial caching here! (0.6.7) p = p.sub(Poly.from_dict({m0:c0},*p.gens)).sub(Poly.from_dict({m1:c1},*p.gens)).add(Poly.from_dict({tuple(m2):c0},*p.gens)) changed = True break if changed: break return p.as_expr() if changed else None fns = [_simplifynorm,_simplifydot,_simplifycross] changed = True while changed and eq.has(*allsymbols): changed = False for fn in fns: neweq = fn(eq) if neweq is not None: eq = neweq changed = True # check if full 3D position is available if self.pp is not None: # add positions ip = 9 inp = 12 ipp = 15 irxp = 16 allsymbols += list(self.Tee[0:3,3])+self.npxyz+[self.pp]+self.rxp[0]+self.rxp[1]+self.rxp[2] normgroups.append([self.Tee[0,3],self.Tee[1,3],self.Tee[2,3],self.pp]) for i in range(3): dotgroups.append([[i,ip],[i+3,ip+1],[i+6,ip+2],self.npxyz[i]]) dotgroups.append([[3*i+0,inp],[3*i+1,inp+1],[3*i+2,inp+2],self.Tee[i,3]]) # column i cross position crossgroups.append([[i+3,ip+2],[i+6,ip+1],irxp+3*i+0]) crossgroups.append([[i+6,ip+0],[i,ip+2],irxp+3*i+1]) crossgroups.append([[i,ip+1],[i+3,ip+0],irxp+3*i+2]) changed = True while changed and eq.has(*allsymbols): changed = False for fn in fns: neweq = fn(eq) if neweq is not None: eq = neweq changed = True return eq
[docs] def isExpressionUnique(self, exprs, expr): for exprtest in exprs: if self.equal(expr,exprtest): return False return True
[docs] def getCommonExpression(self, exprs, expr): for i,exprtest in enumerate(exprs): if self.equal(expr,exprtest): return i return None
[docs] def verifyAllEquations(self,AllEquations,unsolvedvars, solsubs, tree=None): extrazerochecks=[] for i in range(len(AllEquations)): expr = AllEquations[i] if not self.isValidSolution(expr): raise self.CannotSolveError('verifyAllEquations: equation is not valid: %s'%(str(expr))) if not expr.has(*unsolvedvars) and (self.isExpressionUnique(extrazerochecks,expr) or self.isExpressionUnique(extrazerochecks,-expr)): extrazerochecks.append(self.removecommonexprs(expr.subs(solsubs).evalf(),onlygcd=False,onlynumbers=True)) if len(extrazerochecks) > 0: return [AST.SolverCheckZeros(None,extrazerochecks,tree,[AST.SolverBreak()],anycondition=False)] return tree
[docs] def solveAllEquations(self,AllEquations,curvars,othersolvedvars,solsubs,endbranchtree,currentcases=None,unknownvars=None): if len(curvars) == 0: return endbranchtree if unknownvars is None: unknownvars = [] log.info('%s %s',othersolvedvars,curvars) solsubs = solsubs[:] freevarinvsubs = [(f[1],f[0]) for f in self.freevarsubs] solinvsubs = [(f[1],f[0]) for f in solsubs] # single variable solutions solutions = [] for curvar in curvars: othervars = unknownvars+[var for var in curvars if var != curvar] curvarsym = self.Variable(curvar) raweqns = [] for e in AllEquations: if (len(othervars) == 0 or not e.has(*othervars)) and e.has(curvar,curvarsym.htvar,curvarsym.cvar,curvarsym.svar): eq = e.subs(self.freevarsubs+solsubs) if self.isExpressionUnique(raweqns,eq) and self.isExpressionUnique(raweqns,-eq): raweqns.append(eq) if len(raweqns) > 0: try: rawsolutions=self.solveSingleVariable(raweqns,curvar,othersolvedvars, unknownvars=curvars+unknownvars) for solution in rawsolutions: self.solutionComplexity(solution,othersolvedvars,curvars) solutions.append((solution,curvar)) except self.CannotSolveError: pass # only return here if a solution was found that perfectly determines the unknown # otherwise, the pairwise solver could come up with something.. # There is still a problem with this: (bertold robot) # Sometimes an equation like atan2(y,x) evaluates to atan2(0,0) during runtime. # This cannot be known at compile time, so the equation is selected and any other possibilities are rejected. # In the bertold robot case, the next possibility is a pair-wise solution involving two variables if any([s[0].numsolutions()==1 for s in solutions]): return self.addSolution(solutions,AllEquations,curvars,othersolvedvars,solsubs,endbranchtree,currentcases=currentcases) curvarsubssol = [] for var0,var1 in combinations(curvars,2): othervars = unknownvars+[var for var in curvars if var != var0 and var != var1] raweqns = [] complexity = 0 for e in AllEquations: if (len(othervars) == 0 or not e.has(*othervars)) and e.has(var0,var1): eq = e.subs(self.freevarsubs+solsubs) if self.isExpressionUnique(raweqns,eq) and self.isExpressionUnique(raweqns,-eq): raweqns.append(eq) complexity += self.codeComplexity(eq) if len(raweqns) > 1: curvarsubssol.append((var0,var1,raweqns,complexity)) curvarsubssol.sort(lambda x, y: x[3]-y[3]) for var0,var1,raweqns,complexity in curvarsubssol: try: rawsolutions=self.solvePairVariables(raweqns,var0,var1,othersolvedvars,unknownvars=curvars+unknownvars) for solution in rawsolutions: #solution.subs(freevarinvsubs) self.solutionComplexity(solution,othersolvedvars,curvars) solutions.append((solution,Symbol(solution.jointname))) if len(rawsolutions) > 0: # solving a pair is rare, so any solution will do break except self.CannotSolveError: pass # take the least complex solution and go on if