#!/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)
* **TranslationXAxisAngleZNorm4D**, **TranslationYAxisAngleXNorm4D**, **TranslationZAxisAngleYNorm4D** - end effector origin reaches desired 3D translation, manipulator direction needs to be orthogonal to z, x, or y axis and be rotated at a certain angle starting from the x, y, or 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
env = Environment()
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__ = '62' # 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, chain
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
[ドキュメント]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
[ドキュメント]class AST:
"""Abstarct Syntax Tree class definitions specific for evaluating complex math equations."""
[ドキュメント] 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())
[ドキュメント] 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
[ドキュメント] def generate(self, generator):
assert(self.checkValidSolution())
return generator.generateSolution(self)
[ドキュメント] def end(self, generator):
return generator.endSolution(self)
[ドキュメント] 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
[ドキュメント] 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
[ドキュメント] def getPresetCheckForZeros(self):
return self.presetcheckforzeros
[ドキュメント] def getEquationsUsed(self):
return self.equationsused
[ドキュメント] 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 = []
[ドキュメント] def numsolutions(self):
return self.poly.degree(0)
[ドキュメント] 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
[ドキュメント] def generate(self, generator):
return generator.generatePolynomialRoots(self)
[ドキュメント] def end(self, generator):
return generator.endPolynomialRoots(self)
[ドキュメント] 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
[ドキュメント] def getPresetCheckForZeros(self):
return [self.poly.LC()] # make sure highest coefficient is not 0!
[ドキュメント] def getEquationsUsed(self):
return self.equationsused
[ドキュメント] 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 = []
[ドキュメント] def numsolutions(self):
return self.rootmaxdim
[ドキュメント] 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
[ドキュメント] def generate(self, generator):
return generator.generateCoeffFunction(self)
[ドキュメント] def end(self, generator):
return generator.endCoeffFunction(self)
[ドキュメント] 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
[ドキュメント] def getPresetCheckForZeros(self):
return self.presetcheckforzeros
[ドキュメント] def getEquationsUsed(self):
return self.equationsused
[ドキュメント] 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
[ドキュメント] def subs(self,solsubs):
return self
[ドキュメント] def generate(self, generator):
return generator.generateMatrixInverse(self)
[ドキュメント] def end(self, generator):
return generator.endMatrixInverse(self)
[ドキュメント] def checkValidSolution(self):
return True
[ドキュメント] 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
[ドキュメント] class SolverConditionedSolution:
dictequations = None
solversolutions = None
thresh=0.000001
def __init__(self, solversolutions):
self.solversolutions = solversolutions
self.dictequations = []
[ドキュメント] def subs(self,solsubs):
for s in self.solversolutions:
s.subs(solsubs)
return self
[ドキュメント] def generate(self, generator):
return generator.generateConditionedSolution(self)
[ドキュメント] def end(self, generator):
return generator.endConditionedSolution(self)
[ドキュメント] class SolverBranchConds:
jointbranches = None
thresh = 0.000001
def __init__(self, jointbranches):
self.jointbranches = jointbranches
[ドキュメント] def generate(self, generator):
return generator.generateBranchConds(self)
[ドキュメント] def end(self, generator):
return generator.endBranchConds(self)
[ドキュメント] 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 = []
[ドキュメント] def generate(self, generator):
return generator.generateCheckZeros(self)
[ドキュメント] def end(self, generator):
return generator.endCheckZeros(self)
[ドキュメント] def getPresetCheckForZeros(self):
return []
[ドキュメント] 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
[ドキュメント] def numsolutions(self):
return 1
[ドキュメント] 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
[ドキュメント] def getEquationsUsed(self):
return self.equationsused
[ドキュメント] class SolverFreeParameter:
jointname = None
jointtree = None
def __init__(self, jointname, jointtree):
self.jointname = jointname
self.jointtree = jointtree
[ドキュメント] def generate(self, generator):
return generator.generateFreeParameter(self)
[ドキュメント] def end(self, generator):
return generator.endFreeParameter(self)
[ドキュメント] class SolverRotation:
T = None
jointtree = None
functionid=0
def __init__(self, T, jointtree):
self.T = T
self.jointtree = jointtree
self.dictequations = []
[ドキュメント] def generate(self, generator):
return generator.generateRotation(self)
[ドキュメント] def end(self, generator):
return generator.endRotation(self)
[ドキュメント] 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)
[ドキュメント] def generate(self, generator):
return generator.generateStoreSolution(self)
[ドキュメント] def end(self, generator):
return generator.endStoreSolution(self)
[ドキュメント] class SolverSequence:
jointtrees = None
def __init__(self, jointtrees):
self.jointtrees = jointtrees
[ドキュメント] def generate(self, generator):
return generator.generateSequence(self)
[ドキュメント] def end(self, generator):
return generator.endSequence(self)
[ドキュメント] class SolverBreak:
"""Terminates this scope"""
[ドキュメント] def generate(self,generator):
return generator.generateBreak(self)
[ドキュメント] def end(self,generator):
return generator.endBreak(self)
[ドキュメント] def checkValidSolution(self):
return True
[ドキュメント] 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 = []
[ドキュメント] def generate(self, generator):
return generator.generateIKChainRotation3D(self)
[ドキュメント] def end(self, generator):
return generator.endIKChainRotation3D(self)
[ドキュメント] def leftmultiply(self,Tleft,Tleftinv):
self.Rfk = Tleft[0:3,0:3]*self.Rfk
self.Ree = Tleftinv[0:3,0:3]*self.Ree
[ドキュメント] 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 = []
[ドキュメント] def generate(self, generator):
return generator.generateIKChainTranslation3D(self)
[ドキュメント] def end(self, generator):
return generator.endIKChainTranslation3D(self)
[ドキュメント] 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]
[ドキュメント] 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 = []
[ドキュメント] def generate(self, generator):
return generator.generateIKChainTranslationXY2D(self)
[ドキュメント] def end(self, generator):
return generator.endIKChainTranslationXY2D(self)
[ドキュメント] 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]
[ドキュメント] 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 = []
[ドキュメント] def generate(self, generator):
return generator.generateIKChainDirection3D(self)
[ドキュメント] def end(self, generator):
return generator.endIKChainDirection3D(self)
[ドキュメント] def leftmultiply(self,Tleft,Tleftinv):
self.Dfk = Tleft[0:3,0:3]*self.Dfk
self.Dee = Tleftinv[0:3,0:3]*self.Dee
[ドキュメント] 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
[ドキュメント] def generate(self, generator):
return generator.generateIKChainRay(self)
[ドキュメント] def end(self, generator):
return generator.endIKChainRay(self)
[ドキュメント] 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
[ドキュメント] 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 = []
[ドキュメント] def generate(self, generator):
return generator.generateIKChainLookat3D(self)
[ドキュメント] def end(self, generator):
return generator.endIKChainLookat3D(self)
[ドキュメント] 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]
[ドキュメント] 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
[ドキュメント] def generate(self, generator):
return generator.generateSolverIKChainAxisAngle(self)
[ドキュメント] def end(self, generator):
return generator.endSolverIKChainAxisAngle(self)
[ドキュメント] 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+
[ドキュメント]class fmod(function.Function):
"""defines floating-point mod"""
nargs = 2
is_real = True
is_Function = True
[ドキュメント]class atan2check(atan2):
"""defines floating-point mod"""
nargs = 2
is_real = True
is_Function = True
[ドキュメント]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
"""
[ドキュメント] 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)
[ドキュメント] 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
[ドキュメント] class JointAxis:
__slots__ = ['joint','iaxis']
[ドキュメント] 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))]
[ドキュメント] def getsubs(self,value):
return [(self.var,value)]+[(s,v.subs(self.var,value).evalf()) for v,s in self.subs]
[ドキュメント] class DegenerateCases:
def __init__(self):
self.handleddegeneratecases = []
[ドキュメント] def clone(self):
clone=IKFastSolver.DegenerateCases()
clone.handleddegeneratecases = self.handleddegeneratecases[:]
return clone
[ドキュメント] def addcasesconds(self,newconds,currentcases):
for case in newconds:
newcases = set(currentcases)
newcases.add(case)
assert(not self.hascases(newcases))
self.handleddegeneratecases.append(newcases)
[ドキュメント] def addcases(self,currentcases):
assert(not self.hascases(currentcases))
self.handleddegeneratecases.append(currentcases)
[ドキュメント] 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
[ドキュメント] 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
[ドキュメント] 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
[ドキュメント] 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
[ドキュメント] 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]))
[ドキュメント] def numpyVectorToSympy(self,v,precision=None):
return Matrix(len(v),1,[self.convertRealToRational(x,precision) for x in v])
@staticmethod
[ドキュメント] def rodrigues(axis, angle):
return IKFastSolver.rodrigues2(axis,cos(angle),sin(angle))
@staticmethod
[ドキュメント] 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
[ドキュメント] 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
[ドキュメント] 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
[ドキュメント] def affineSimplify(T):
return Matrix(T.shape[0],T.shape[1],[trigsimp(x.expand()) for x in T])
@staticmethod
[ドキュメント] def multiplyMatrix(Ts):
Tfinal = eye(4)
for T in Ts:
Tfinal = Tfinal*T
return Tfinal
@staticmethod
[ドキュメント] def equal(eq0,eq1):
return expand(eq0-eq1) == S.Zero
[ドキュメント] def chop(self,expr,precision=None):
return expr
[ドキュメント] 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)
[ドキュメント] 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
[ドキュメント] 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
[ドキュメント] 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
[ドキュメント] 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
[ドキュメント] def has(eqs,*sym):
return any([eq.has(*sym) for eq in eqs]) if len(sym) > 0 else False
[ドキュメント] 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
[ドキュメント] 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
[ドキュメント] def sortComplexity(self,exprs):
exprs.sort(lambda x, y: self.codeComplexity(x)-self.codeComplexity(y))
[ドキュメント] 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
[ドキュメント] 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
[ドキュメント] def checkSolvability(self,AllEquations,checkvars,othervars):
pass
[ドキュメント] 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)
[ドキュメント] 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)
[ドキュメント] 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
[ドキュメント] 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
[ドキュメント] 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
[ドキュメント] 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]
# if Links[0][0:3,0:3] == eye(3):
# # first axis is prismatic, so zero out self.Tee
# for i in range(3):
# if Links[0][i,3] != S.Zero:
# self.Tee[i,3] = S.Zero
# self.Teeinv = self.affineInverse(self.Tee)
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
[ドキュメント] 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
[ドキュメント] 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
[ドキュメント] 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
[ドキュメント] 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)
[ドキュメント] 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
[ドキュメント] 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
[ドキュメント] 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 [0,1]:
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
[ドキュメント] 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
[ドキュメント] 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].expand())
Positionsee[i][j] = self.RoundEquationTerms(Positionsee[i][j].expand())
return self.buildEquationsFromTwoSides(Positions,Positionsee,transvars+othersolvedvars,uselength=uselength)
[ドキュメント] def buildEquationsFromRotation(self,T0links,Ree,rotvars,othersolvedvars):
"""Ree is a 3x3 matrix
"""
Raccum = Ree
numvarsdone = 0
AllEquations = []
for i in range(len(T0links)):
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):])
Reqs = []
for i in range(3):
Reqs.append([self.trigsimp(Raccum[i,j]-R[i,j],othersolvedvars+rotvars) for j in range(3)])
for i in range(3):
for eq in Reqs[i]:
AllEquations.append(eq)
numvarsdone += 1
# take dot products (equations become unnecessarily complex)
# eqdots = [S.Zero, S.Zero, S.Zero]
# for i in range(3):
# eqdots[0] += Reqs[0][i] * Reqs[1][i]
# eqdots[1] += Reqs[1][i] * Reqs[2][i]
# eqdots[2] += Reqs[2][i] * Reqs[0][i]
# for i in range(3):
# AllEquations.append(self.trigsimp(eqdots[i].expand(),othersolvedvars+rotvars))
#AllEquations.append((eqs[0]*eqs[0]+eqs[1]*eqs[1]+eqs[2]*eqs[2]-S.One).expand())
self.sortComplexity(AllEquations)
return AllEquations
[ドキュメント] 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
[ドキュメント] def CheckEquationForVarying(self, eq):
return eq.has('vj0px') or eq.has('vj0py') or eq.has('vj0pz')
[ドキュメント] 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)])
# prune any that have varying symbols
eqs = [(eq0,eq1) for eq0,eq1 in eqs if not self.CheckEquationForVarying(eq0) and not self.CheckEquationForVarying(eq1)]
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
[ドキュメント] 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
[ドキュメント] 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
[ドキュメント] def reduceBothSidesSymbolically(self,*args,**kwargs):
numsymbolcoeffs, _computereducedequations = self.reduceBothSidesSymbolicallyDelayed(*args,**kwargs)
return _computereducedequations()
[ドキュメント] 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
[ドキュメント] 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))
# 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
[ドキュメント] 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 = []
htvarcossinoffsets = []
nonhtvars = []
for iothersymbol, othersymbol in enumerate(othersymbols):
if othersymbol.name[0] == 'c':
assert(othersymbols[iothersymbol+1].name[0] == 's')
htvarcossinoffsets.append(iothersymbol)
name = othersymbol.name[1:]
htvar = Symbol('ht%s'%name)
htvarsubs += [(othersymbol,(1-htvar**2)/(1+htvar**2)),(othersymbols[iothersymbol+1],2*htvar/(1+htvar**2))]
htvars.append(htvar)
htvarsubs2.append((Symbol(name),2*atan(htvar)))
usedvars.append(Symbol(name))
elif othersymbol.name[0] != 'h' and othersymbol.name[0] != 's':
# not half-tan, sin, or cos
nonhtvars.append(othersymbol)
usedvars.append(othersymbol)
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]*len(htvarcossinoffsets)
for monoms in peq.monoms():
for i,ioffset in enumerate(htvarcossinoffsets):
maxdenom[i] = max(maxdenom[i],monoms[ioffset]+monoms[ioffset+1])
eqnew = S.Zero
for monoms,c in peq.terms():
term = c
for i,ioffset in enumerate(htvarcossinoffsets):
# for cos
num, denom = fraction(htvarsubs[2*i][1])
term *= num**monoms[ioffset]
# for sin
num, denom = fraction(htvarsubs[2*i+1][1])
term *= num**monoms[ioffset+1]
# the denoms for sin/cos of the same joint variable are the same
for i,ioffset in enumerate(htvarcossinoffsets):
denom = fraction(htvarsubs[2*i][1])[1]
term *= denom**(maxdenom[i]-monoms[ioffset]-monoms[ioffset+1])
# multiply the rest of the monoms
for imonom, monom in enumerate(monoms):
if not imonom in htvarcossinoffsets and not imonom-1 in htvarcossinoffsets:
# handle non-sin/cos variables yet
term *= othersymbols[imonom]**monom
eqnew += term
newpeq = Poly(eqnew,htvars+nonhtvars)
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:
if len(nonhtvars) > 0:
# try to solve one variable in terms of the others
usedvar0solution = solve(newreducedeqs[0],nonhtvars[0])[0]
num,denom = fraction(usedvar0solution)
igenoffset = len(htvars)
# substitute all instances of the variable
processedequations = []
for peq in newreducedeqs[1:]:
maxdegree = peq.degree(igenoffset)
eqnew = S.Zero
for monoms,c in peq.terms():
term = c
term *= denom**(maxdegree-monoms[igenoffset])
term *= num**(monoms[igenoffset])
for imonom, monom in enumerate(monoms):
if imonom != igenoffset:
term *= htvars[imonom]**monom
eqnew += term
try:
newpeq = Poly(eqnew,htvars)
except PolynomialError, e:
# most likel uservar0solution was bad
raise self.CannotSolveError('equation %s cannot be represented as a polynomial')
if newpeq != S.Zero:
processedequations.append(newpeq)
# check if any variables have degree <= 1 for all equations
for ihtvar,htvar in enumerate(htvars):
leftoverhtvars = list(htvars)
leftoverhtvars.pop(ihtvar)
freeequations = []
linearequations = []
higherequations = []
for peq in processedequations:
if peq.degree(ihtvar) == 0:
freeequations.append(peq)
elif peq.degree(ihtvar) == 1:
linearequations.append(peq)
else:
higherequations.append(peq)
if len(freeequations) > 0:
log.info('found a way to solve this! still need to implement it though...')
elif len(linearequations) > 0 and len(leftoverhtvars) == 1:
# try substituting one into the other equations Ax = B
A = S.Zero
B = S.Zero
for monoms,c in linearequations[0].terms():
term = c
for imonom, monom in enumerate(monoms):
if imonom != ihtvar:
term *= htvars[imonom]**monom
if monoms[ihtvar] > 0:
A += term
else:
B -= term
Apoly = Poly(A,leftoverhtvars)
Bpoly = Poly(B,leftoverhtvars)
singlepolyequations = []
for peq in linearequations[1:]+higherequations:
peqnew = Poly(S.Zero,leftoverhtvars)
maxhtvardegree = peq.degree(ihtvar)
for monoms,c in peq.terms():
term = c
for imonom, monom in enumerate(monoms):
if imonom != ihtvar:
term *= htvars[imonom]**monom
termpoly = Poly(term,leftoverhtvars)
peqnew += termpoly * (Bpoly**(monoms[ihtvar]) * Apoly**(maxhtvardegree-monoms[ihtvar]))
singlepolyequations.append(peqnew)
if len(singlepolyequations) > 0:
jointsol = 2*atan(leftoverhtvars[0])
jointname = leftoverhtvars[0].name[2:]
firstsolution = AST.SolverPolynomialRoots(jointname=jointname,poly=singlepolyequations[0],jointeval=[jointsol],isHinge=self.isHinge(jointname))
firstsolution.checkforzeros = []
# fail if the denom evaluations to 0
firstsolution.postcheckforzeros = [A.as_expr()]
firstsolution.postcheckfornonzeros = []
firstsolution.postcheckforrange = []
firstsolution.AddHalfTanValue = True
secondsolution = AST.SolverSolution(htvar.name[2:], isHinge=self.isHinge(htvar.name[2:]))
secondsolution.jointeval = [2*atan2(B.as_expr(), A.as_expr())]
secondsolution.AddHalfTanValue = True
thirdsolution = AST.SolverSolution(nonhtvars[0].name, isHinge=self.isHinge(nonhtvars[0].name))
thirdsolution.jointeval = [usedvar0solution]
return [firstsolution, secondsolution, thirdsolution]+endbranchtree, usedvars
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
[ドキュメント] 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
[ドキュメント] 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
[ドキュメント] def isExpressionUnique(self, exprs, expr):
for exprtest in exprs:
if self.equal(expr,exprtest):
return False
return True
[ドキュメント] def getCommonExpression(self, exprs, expr):
for i,exprtest in enumerate(exprs):
if self.equal(expr,exprtest):
return i
return None
[ドキュメント] 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
[ドキュメント] 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])
if len(curvars) == 2 and self.isHinge(curvars[0].name) and self.isHinge(curvars[1].name) and len(curvarsubssol) > 0:
# there's only two variables left, it might be the case that the axes are aligning and the two variables are dependent on each other
var0,var1,raweqns,complexity = curvarsubssol[0]
dummyvar = Symbol('dummy') # curvars[0] + curvars[1]
NewEquations = []
dummyvalue = curvars[0] + curvars[1]
for eq in raweqns:
neweq = self.trigsimp(eq.subs(curvars[0],dummyvar-curvars[1]).expand(trig=True),curvars)
if not neweq.has(*(othervars+curvars)) and neweq.has(dummyvar):
eq = neweq.subs(self.freevarsubs+solsubs)
if self.isExpressionUnique(NewEquations,eq) and self.isExpressionUnique(NewEquations,-eq):
NewEquations.append(eq)
if len(NewEquations) == 0:
# try subtracting
dummyvalue = curvars[0] - curvars[1]
for eq in raweqns:
neweq = self.trigsimp(eq.subs(curvars[0],dummyvar-curvars[1]).expand(trig=True),curvars)
if not neweq.has(*(othervars+curvars)) and neweq.has(dummyvar):
eq = neweq.subs(self.freevarsubs+solsubs)
if self.isExpressionUnique(NewEquations,eq) and self.isExpressionUnique(NewEquations,-eq):
NewEquations.append(eq)
if len(NewEquations) > 0:
dummysolutions = []
try:
rawsolutions=self.solveSingleVariable(NewEquations,dummyvar,othersolvedvars, unknownvars=curvars+unknownvars)
for solution in rawsolutions:
self.solutionComplexity(solution,othersolvedvars,curvars)
dummysolutions.append(solution)
except self.CannotSolveError:
pass
if any([s.numsolutions()==1 for s in dummysolutions]):
# two axes are aligning, so modify the solutions to reflect the original variables and add a free variable
log.info('found two aligning axes %s: %r',dummyvalue, NewEquations)
solutions = []
for dummysolution in dummysolutions:
if dummysolution.jointevalsin is not None or dummysolution.jointevalcos is not None:
log.warn('dummy solution should not have sin/cos parts!')
solution=AST.SolverSolution(curvars[0].name, isHinge=self.isHinge(curvars[0].name))
solution.jointeval = [dummysolution.jointeval[0] - dummyvalue + curvars[0]]
self.solutionComplexity(solution,othersolvedvars,curvars)
solutions.append((solution,curvars[0]))
tree = self.addSolution(solutions,raweqns,curvars[0:1],othersolvedvars+curvars[1:2],solsubs+self.Variable(curvars[1]).subs,endbranchtree,currentcases=currentcases)
if tree is None:
return None
return [AST.SolverFreeParameter(curvars[1].name, tree)]
else:
log.warn('almost found two axes but num solutions was: %r', [s.numsolutions()==1 for s in dummysolutions])
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 len(solutions) > 0:
return self.addSolution(solutions,AllEquations,curvars,othersolvedvars,solsubs,endbranchtree,currentcases=currentcases)
# test with higher degrees, necessary?
for curvar in curvars:
othervars = unknownvars+[var for var in curvars if var != curvar]
raweqns = []
for e in AllEquations:
if (len(othervars) == 0 or not e.has(*othervars)) and e.has(curvar):
eq = e.subs(self.freevarsubs+solsubs)
if self.isExpressionUnique(raweqns,eq) and self.isExpressionUnique(raweqns,-eq):
raweqns.append(eq)
for raweqn in raweqns:
try:
log.info('testing with higher degrees')
solution=self.solveHighDegreeEquationsHalfAngle([raweqn],self.Variable(curvar))
self.solutionComplexity(solution,othersolvedvars,curvars)
solutions.append((solution,curvar))
except self.CannotSolveError:
pass
if len(solutions) > 0:
return self.addSolution(solutions,AllEquations,curvars,othersolvedvars,solsubs,endbranchtree,currentcases=currentcases)
# solve with all 3 variables together
# have got this far, so perhaps two axes are aligned?
raise self.CannotSolveError('solveAllEquations failed to find a variable to solve')
[ドキュメント] def addSolution(self,solutions,AllEquations,curvars,othersolvedvars,solsubs,endbranchtree,currentcases=None):
"""Take the least complex solution of a set of solutions and resume solving
"""
solutions = [s for s in solutions if s[0].score < oo and s[0].checkValidSolution()] # remove infinite scores
if len(solutions) == 0:
raise self.CannotSolveError('no valid solutions')
solutions.sort(lambda x, y: x[0].score-y[0].score)
hasonesolution = False
for solution in solutions:
checkforzeros = solution[0].checkforzeros
hasonesolution |= solution[0].numsolutions() == 1
if len(checkforzeros) == 0 and solution[0].numsolutions() == 1:
# did find a good solution, so take it. Make sure to check any zero branches
var = solution[1]
newvars=curvars[:]
newvars.remove(var)
return [solution[0].subs(solsubs)]+self.solveAllEquations(AllEquations,curvars=newvars,othersolvedvars=othersolvedvars+[var],solsubs=solsubs+self.Variable(var).subs,endbranchtree=endbranchtree,currentcases=currentcases)
if not hasonesolution:
# check again except without the number of solutions requirement
for solution in solutions:
checkforzeros = solution[0].checkforzeros
if len(checkforzeros) == 0:
# did find a good solution, so take it. Make sure to check any zero branches
var = solution[1]
newvars=curvars[:]
newvars.remove(var)
return [solution[0].subs(solsubs)]+self.solveAllEquations(AllEquations,curvars=newvars,othersolvedvars=othersolvedvars+[var],solsubs=solsubs+self.Variable(var).subs,endbranchtree=endbranchtree,currentcases=currentcases)
# all solutions have check for zero equations
# choose the variable with the shortest solution and compute (this is a conservative approach)
usedsolutions = []
# remove any solutions with similar checkforzero constraints (because they are essentially the same)
for solution,var in solutions:
solution.subs(solsubs)
if len(usedsolutions) == 0:
usedsolutions.append((solution,var))
else:
match = False
for usedsolution,usedvar in usedsolutions:
if len(solution.checkforzeros) == len(usedsolution.checkforzeros):
if not any([self.isExpressionUnique(usedsolution.checkforzeros,-eq) or self.isExpressionUnique(usedsolution.checkforzeros,eq) for eq in solution.checkforzeros]):
match = True
break
if not match:
usedsolutions.append((solution,var))
if len(usedsolutions) >= 3:
# don't need more than three alternatives (used to be two, but then lookat barrettwam4 proved that wrong)
break
nextsolutions = dict()
allvars = curvars+[Symbol('s%s'%v.name) for v in curvars]+[Symbol('c%s'%v.name) for v in curvars]
allothersolvedvars = othersolvedvars + [Symbol('s%s'%v.name) for v in othersolvedvars]+[Symbol('c%s'%v.name) for v in othersolvedvars]
lastbranch = []
prevbranch=lastbranch
if currentcases is None:
currentcases = set()
if self.degeneratecases is None:
self.degeneratecases = self.DegenerateCases()
handledconds = self.degeneratecases.gethandledconds(currentcases)
# one to one correspondence with usedsolutions and the SolverCheckZeros hierarchies (used for cross product of equations later on)
zerosubstitutioneqs = []
# zerosubstitutioneqs equations flattened for easier checking
flatzerosubstitutioneqs = []
hascheckzeros = False
# iterate in reverse order and put the most recently processed solution at the front.
# There is a problem with this algorithm transferring the degenerate cases correctly.
# Although the zeros of the first equation are checked, they are not added as conditions
# to the later equations, so that the later equations will also use variables as unknowns (even though they are determined to be specific constants). This is most apparent in rotations.
for solution,var in usedsolutions[::-1]:
# there are divide by zeros, so check if they can be explicitly solved for joint variables
checkforzeros = []
eqs = []
for checkzero in solution.checkforzeros:
if checkzero.has(*allvars):
log.info('ignoring special check for zero since it has symbols %s: %s',str(allvars),str(checkzero))
continue
# bother trying to extract something if too complex (takes a lot of computation time to check and most likely nothing will be extracted). 100 is an arbitrary value
if self.codeComplexity(checkzero) > 120:
log.warn('checkforzero too big (%d): %s',self.codeComplexity(checkzero),checkzero)
checkforzeros.append(checkzero)#self.removecommonexprs(checkzero.evalf(),onlygcd=False,onlynumbers=True))
else:
# fractions could get big, so evaluate directly
checkforzeros.append(checkzero.evalf())#self.removecommonexprs(checkzero.evalf(),onlygcd=False,onlynumbers=True)
checksimplezeroexprs = [checkzero]
if not checkzero.has(*allothersolvedvars):
sumsquaresexprs = self._GetSumSquares(checkzero)
if sumsquaresexprs is not None:
checksimplezeroexprs += sumsquaresexprs
sumsquaresexprstozero = []
for sumsquaresexpr in sumsquaresexprs:
if sumsquaresexpr.is_Symbol:
sumsquaresexprstozero.append(sumsquaresexpr)
elif sumsquaresexpr.is_Mul:
for arg in sumsquaresexpr.args:
if arg.is_Symbol:
sumsquaresexprstozero.append(arg)
if len(sumsquaresexprstozero) > 0:
eqs.append([sumsquaresexprstozero,checkzero,[(sumsquaresexpr,S.Zero) for sumsquaresexpr in sumsquaresexprstozero]])
for checksimplezeroexpr in checksimplezeroexprs:
#if checksimplezeroexpr.has(*othersolvedvars): # cannot do this check since sjX,cjX might be used
for othervar in othersolvedvars:
sothervar = self.Variable(othervar).svar
cothervar = self.Variable(othervar).cvar
if checksimplezeroexpr.has(othervar,sothervar,cothervar):
# the easiest thing to check first is if the equation evaluates to zero on boundaries 0,pi/2,pi,-pi/2
s = AST.SolverSolution(othervar.name,jointeval=[],isHinge=self.isHinge(othervar.name))
for value in [S.Zero,pi/2,pi,-pi/2]:
try:
checkzerosub=checksimplezeroexpr.subs([(othervar,value),(sothervar,sin(value).evalf()),(cothervar,cos(value).evalf())])
if self.isValidSolution(checkzerosub) and checkzerosub.evalf() == S.Zero:
if s.jointeval is None:
s.jointeval = []
s.jointeval.append(S.One*value)
except AssertionError,e:
log.warn('othervar %s=%f: %s',str(othervar),value,e)
if s.jointeval is not None and len(s.jointeval) > 0:
ss = [s]
else:
ss = []
try:
ss += self.solveSingleVariable([checksimplezeroexpr.subs([(sothervar,sin(othervar)),(cothervar,cos(othervar))])],othervar,othersolvedvars)
except PolynomialError:
# checksimplezeroexpr was too complex
pass
except self.CannotSolveError,e:
# this is actually a little tricky, sometimes really good solutions can have a divide that looks like:
# ((0.405 + 0.331*cj2)**2 + 0.109561*sj2**2 (manusarm_left)
# This will never be 0, but the solution cannot be solved. Instead of rejecting, add a condition to check if checksimplezeroexpr itself is 0 or not
pass
for s in ss:
# can actually simplify Positions and possibly get a new solution!
if s.jointeval is not None:
for eq in s.jointeval:
if eq.is_number:
cond=othervar-eq.evalf()
if self.isExpressionUnique(handledconds+list(chain.from_iterable([tempeq[0] for tempeq in flatzerosubstitutioneqs+eqs])),-cond) and self.isExpressionUnique(handledconds+list(chain.from_iterable([tempeq[0] for tempeq in flatzerosubstitutioneqs+eqs])),cond):
if self.isHinge(othervar.name):
evalcond=fmod(cond+pi,2*pi)-pi
else:
evalcond=cond
eqs.append([[cond],evalcond,[(sothervar,sin(eq).evalf()),(sin(othervar),sin(eq).evalf()),(cothervar,cos(eq).evalf()),(cos(othervar),cos(eq).evalf()),(othervar,eq)]])
elif s.jointevalsin is not None:
for eq in s.jointevalsin:
if eq.is_number:
cond=othervar-asin(eq).evalf()
if self.isExpressionUnique(handledconds+list(chain.from_iterable([tempeq[0] for tempeq in flatzerosubstitutioneqs+eqs])),-cond) and self.isExpressionUnique(handledconds+list(chain.from_iterable([tempeq[0] for tempeq in flatzerosubstitutioneqs+eqs])),cond):
if self.isHinge(othervar.name):
evalcond=fmod(cond+pi,2*pi)-pi
else:
evalcond=cond
eqs.append([[cond],evalcond,[(sothervar,eq),(sin(othervar),eq),(cothervar,sqrt(1-eq*eq).evalf()),(cos(othervar),sqrt(1-eq*eq).evalf()),(othervar,asin(eq).evalf())]])
cond=othervar-(pi-asin(eq).evalf())
if self.isExpressionUnique(handledconds+list(chain.from_iterable([tempeq[0] for tempeq in flatzerosubstitutioneqs+eqs])),-cond) and self.isExpressionUnique(handledconds+list(chain.from_iterable([tempeq[0] for tempeq in flatzerosubstitutioneqs+eqs])),cond):
if self.isHinge(othervar.name):
evalcond=fmod(cond+pi,2*pi)-pi
else:
evalcond=cond
eqs.append([[cond],evalcond,[(sothervar,eq),(sin(othervar),eq),(cothervar,-sqrt(1-eq*eq).evalf()),(cos(othervar),-sqrt(1-eq*eq).evalf()),(othervar,(pi-asin(eq)).evalf())]])
elif s.jointevalcos is not None:
for eq in s.jointevalcos:
if eq.is_number:
cond=othervar-acos(eq).evalf()
if self.isExpressionUnique(handledconds+list(chain.from_iterable([tempeq[0] for tempeq in flatzerosubstitutioneqs+eqs])),-cond) and self.isExpressionUnique(handledconds+list(chain.from_iterable([tempeq[0] for tempeq in flatzerosubstitutioneqs+eqs])),cond):
if self.isHinge(othervar.name):
evalcond=fmod(cond+pi,2*pi)-pi
else:
evalcond=cond
eqs.append([[cond],evalcond,[(sothervar,sqrt(1-eq*eq).evalf()),(sin(othervar),sqrt(1-eq*eq).evalf()),(cothervar,eq),(cos(othervar),eq),(othervar,acos(eq).evalf())]])
cond=othervar+acos(eq).evalf()
if self.isExpressionUnique(handledconds+list(chain.from_iterable([tempeq[0] for tempeq in flatzerosubstitutioneqs+eqs])),-cond) and self.isExpressionUnique(handledconds+list(chain.from_iterable([tempeq[0] for tempeq in flatzerosubstitutioneqs+eqs])),cond):
if self.isHinge(othervar.name):
evalcond=fmod(cond+pi,2*pi)-pi
else:
evalcond=cond
eqs.append([[cond],evalcond,[(sothervar,-sqrt(1-eq*eq).evalf()),(sin(othervar),-sqrt(1-eq*eq).evalf()),(cothervar,eq),(cos(othervar),eq),(othervar,-acos(eq).evalf())]])
flatzerosubstitutioneqs += eqs
zerosubstitutioneqs.append(eqs)
if not var in nextsolutions:
newvars=curvars[:]
newvars.remove(var)
olddegeneratecases = self.degeneratecases
self.degeneratecases = olddegeneratecases.clone()
nextsolutions[var] = self.solveAllEquations(AllEquations,curvars=newvars,othersolvedvars=othersolvedvars+[var],solsubs=solsubs+self.Variable(var).subs,endbranchtree=endbranchtree,currentcases=currentcases)
self.degeneratecases = olddegeneratecases
if len(checkforzeros) > 0:
hascheckzeros = True
solvercheckzeros = AST.SolverCheckZeros(jointname=var.name,jointcheckeqs=checkforzeros,nonzerobranch=[solution]+nextsolutions[var],zerobranch=prevbranch,anycondition=True,thresh=solution.thresh)
# have to transfer the dictionary!
solvercheckzeros.dictequations = solution.dictequations
solution.dictequations = []
prevbranch=[solvercheckzeros]
else:
prevbranch = [solution]+nextsolutions[var]
if len(prevbranch) == 0:
raise self.CannotSolveError('failed to add solution!')
if len(currentcases) >= 4:
log.warn('4 levels deep in checking degenerate cases, skipping...')
lastbranch.append(AST.SolverBreak())
return prevbranch
# fill the last branch with all the zero conditions
if hascheckzeros:
# if not equations found, try setting two variables at once
# also try setting px, py, or pz to 0 (barrettwam4 lookat)
for isolution,(solution,var) in enumerate(usedsolutions[::-1]):
for checkzero in solution.checkforzeros:
if checkzero.has(*allvars):
log.info('ignoring special check for zero 2 since it has symbols %s: %s',str(allvars), str(checkzero))
continue
# don't bother trying to extract something if too complex (takes a lot of computation time to check and most likely nothing will be extracted). 120 is an arbitrary value
if self.codeComplexity(checkzero) > 120:
continue
for preal in [Symbol('px'),Symbol('py'),Symbol('pz')]:
if checkzero.has(preal):
# first check if the position alone can yield a zero
eq = checkzero.subs([(preal,S.Zero)]).evalf()
if eq == S.Zero:
cond = Abs(preal)
evalcond = Abs(preal)
if self.isExpressionUnique(handledconds+list(chain.from_iterable([tempeq[0] for tempeq in flatzerosubstitutioneqs])),-cond) and self.isExpressionUnique(handledconds+list(chain.from_iterable([tempeq[0] for tempeq in flatzerosubstitutioneqs])),cond):
checkexpr = [[cond],evalcond,[(preal,S.Zero)]]
flatzerosubstitutioneqs.append(checkexpr)
zerosubstitutioneqs[isolution].append(checkexpr)
log.info('%s=0 in %s',preal,checkzero)
continue
for othervar in othersolvedvars:
if not self.isHinge(othervar.name):
continue
sothervar = Symbol('s%s'%othervar.name)
cothervar = Symbol('c%s'%othervar.name)
if checkzero.has(othervar,sothervar,cothervar):
for value in [S.Zero,pi/2,pi,-pi/2]:
eq = checkzero.subs([(othervar,value),(sothervar,sin(value).evalf()),(cothervar,cos(value).evalf()),(preal,S.Zero)]).evalf()
if eq == S.Zero:
cond = Abs(othervar-value)+Abs(preal)
evalcond = Abs(fmod(othervar-value+pi,2*pi)-pi)+Abs(preal)
if self.isExpressionUnique(handledconds+list(chain.from_iterable([tempeq[0] for tempeq in flatzerosubstitutioneqs])),-cond) and self.isExpressionUnique(handledconds+list(chain.from_iterable([tempeq[0] for tempeq in flatzerosubstitutioneqs])),cond):
checkexpr = [[cond],evalcond,[(sothervar,sin(value).evalf()),(sin(othervar),sin(value).evalf()),(cothervar,cos(value).evalf()),(cos(othervar),cos(value).evalf()),(preal,S.Zero),(othervar,value)]]
flatzerosubstitutioneqs.append(checkexpr)
zerosubstitutioneqs[isolution].append(checkexpr)
log.info('%s=%s,%s=0 in %s', othervar,value,preal,checkzero)
# test the solutions
# have to take the cross product of all the zerosubstitutioneqs in order to form stronger constraints on the equations because the following condition will be executed only if all SolverCheckZeros evalute to 0
zerobranches = []
accumequations = []
# since sequence_cross_product requires all lists to be non-empty, insert None for empty lists
for conditioneqs in zerosubstitutioneqs:
if len(conditioneqs) == 0:
conditioneqs.append(None)
for conditioneqs in self.sequence_cross_product(*zerosubstitutioneqs):
validconditioneqs = [c for c in conditioneqs if c is not None]
if len(validconditioneqs) == 0:
continue
elif len(validconditioneqs) == 1:
cond,evalcond,othervarsubs = validconditioneqs[0]
elif len(validconditioneqs) > 1:
# merge the equations
cond = []
evalcond = S.Zero
othervarsubs = []
for subcond, subevalcond, subothervarsubs in validconditioneqs:
cond += subcond
evalcond += abs(subevalcond)
othervarsubs += subothervarsubs
# have to convert to fractions before substituting!
if not all([self.isValidSolution(v) for s,v in othervarsubs]):
continue
othervarsubs = [(s,self.convertRealToRational(v)) for s,v in othervarsubs]
NewEquations = [eq.subs(othervarsubs) for eq in AllEquations]
try:
# forcing a value, so have to check if all equations in NewEquations that do not contain
# unknown variables are really 0
extrazerochecks=[]
for i in range(len(NewEquations)):
expr = NewEquations[i]
if not self.isValidSolution(expr):
log.warn('not valid: %s',expr)
extrazerochecks=None
break
if not expr.has(*allvars) and (self.isExpressionUnique(extrazerochecks,expr) or self.isExpressionUnique(extrazerochecks,-expr)):
extrazerochecks.append(expr.subs(solsubs).evalf())
if extrazerochecks is not None:
newcases = set(currentcases)
for singlecond in cond:
newcases.add(singlecond)
newtree = self.solveAllEquations(NewEquations,curvars,othersolvedvars,solsubs,endbranchtree,currentcases=newcases)
accumequations.append(NewEquations) # store the equations for debugging purposes
zerobranches.append(([evalcond]+extrazerochecks,newtree))
self.degeneratecases.addcases(newcases)
except self.CannotSolveError:
continue
if len(zerobranches) > 0:
branchconds = AST.SolverBranchConds(zerobranches+[(None,[AST.SolverBreak()])])
branchconds.accumequations = accumequations
lastbranch.append(branchconds)
else:
lastbranch.append(AST.SolverBreak())
return prevbranch
[ドキュメント] def solvePairVariablesHalfAngle(self,raweqns,var0,var1,othersolvedvars,subs=None):
"""solves equations of two variables in sin and cos
"""
varsym0 = self.Variable(var0)
varsym1 = self.Variable(var1)
varsyms = [varsym0,varsym1]
unknownvars=[varsym0.cvar,varsym0.svar,varsym1.cvar,varsym1.svar]
varsubs=varsym0.subs+varsym1.subs
varsubsinv = varsym0.subsinv+varsym1.subsinv
halftansubs = []
for varsym in varsyms:
halftansubs += [(varsym.cvar,(1-varsym.htvar**2)/(1+varsym.htvar**2)),(varsym.svar,2*varsym.htvar/(1+varsym.htvar**2))]
dummyvars = []
for othervar in othersolvedvars:
v = self.Variable(othervar)
dummyvars += [v.cvar,v.svar,v.var,v.htvar]
polyeqs = []
for eq in raweqns:
trigsubs = [(varsym0.svar**2,1-varsym0.cvar**2), (varsym0.svar**3,varsym0.svar*(1-varsym0.cvar**2)), (varsym1.svar**2,1-varsym1.cvar**2), (varsym1.svar**3,varsym1.svar*(1-varsym1.cvar**2))]
peq = Poly(eq.subs(varsubs).subs(trigsubs).expand().subs(trigsubs),*unknownvars)
if peq.has(varsym0.var) or peq.has(varsym1.var):
raise self.CannotSolveError('expecting only sin and cos! %s'%peq)
maxmonoms = [0,0,0,0]
maxdenom = [0,0]
for monoms in peq.monoms():
for i in range(4):
maxmonoms[i] = max(maxmonoms[i],monoms[i])
maxdenom[0] = max(maxdenom[0],monoms[0]+monoms[1])
maxdenom[1] = max(maxdenom[1],monoms[2]+monoms[3])
eqnew = S.Zero
for monoms,c in peq.terms():
term = c
for i in range(4):
num,denom = fraction(halftansubs[i][1])
term *= num**monoms[i]
# the denoms for 0,1 and 2,3 are the same
for i in [0,2]:
denom = fraction(halftansubs[i][1])[1]
term *= denom**(maxdenom[i/2]-monoms[i]-monoms[i+1])
eqnew += simplify(term)
polyeq = Poly(eqnew,varsym0.htvar,varsym1.htvar)
if polyeq.TC() == S.Zero:
# might be able to divide out variables?
minmonoms = None
for monom in polyeq.monoms():
if minmonoms is None:
minmonoms = list(monom)
else:
for i in range(len(minmonoms)):
minmonoms[i] = min(minmonoms[i],monom[i])
newpolyeq = Poly(S.Zero,*polyeq.gens)
for m,c in polyeq.terms():
newm = list(m)
for i in range(len(minmonoms)):
newm[i] -= minmonoms[i]
newpolyeq = newpolyeq.add(Poly.from_dict({tuple(newm):c},*newpolyeq.gens))
log.warn('converting polyeq "%s" to "%s"'%(polyeq,newpolyeq))
# check if any equations are only in one variable
polyeq = newpolyeq
polyeqs.append(polyeq)
try:
return self.solveSingleVariable([e.as_expr() for e in polyeqs if not e.has(varsym1.htvar)],varsym0.var,othersolvedvars,unknownvars=[])
except self.CannotSolveError:
pass
try:
return self.solveSingleVariable([e.as_expr() for e in polyeqs if not e.has(varsym0.htvar)],varsym1.var,othersolvedvars,unknownvars=[])
except self.CannotSolveError:
pass
complexity = [(self.codeComplexity(peq.as_expr()),peq) for peq in polyeqs]
complexity.sort(key=itemgetter(0))
polyeqs = [peq[1] for peq in complexity]
solutions = [None,None]
linearsolution = None
for ileftvar in range(2):
if linearsolution is not None:
break
leftvar = varsyms[ileftvar].htvar
newpolyeqs = [Poly(eq,varsyms[1-ileftvar].htvar) for eq in polyeqs]
mindegree = __builtin__.min([max(peq.degree_list()) for peq in newpolyeqs])
maxdegree = __builtin__.max([max(peq.degree_list()) for peq in newpolyeqs])
for peq in newpolyeqs:
if len(peq.monoms()) == 1:
possiblefinaleq = self.checkFinalEquation(Poly(peq.LC(),leftvar),subs)
if possiblefinaleq is not None:
solutions[ileftvar] = [possiblefinaleq]
break
for degree in range(mindegree,maxdegree+1):
if solutions[ileftvar] is not None or linearsolution is not None:
break
newpolyeqs2 = [peq for peq in newpolyeqs if max(peq.degree_list()) <= degree]
if degree+1 <= len(newpolyeqs2):
# in order to avoid wrong solutions, have to get resultants for all equations
possibilities = []
unusedindices = range(len(newpolyeqs2))
for eqsindices in combinations(range(len(newpolyeqs2)),degree+1):
Mall = zeros((degree+1,degree+1))
for i,eqindex in enumerate(eqsindices):
eq = newpolyeqs2[eqindex]
for j,c in eq.terms():
Mall[i,j[0]] = c
# det_bareis freezes when there are huge fractions
#det=self.det_bareis(Mall,*(self.pvars+dummyvars+[leftvar]))
possiblefinaleq = self.checkFinalEquation(Poly(Mall.berkowitz_det(),leftvar),subs)
if possiblefinaleq is not None:
# sometimes +- I are solutions, so remove them
q,r = div(possiblefinaleq,leftvar+I)
if r == S.Zero:
possiblefinaleq = Poly(q,leftvar)
q,r = div(possiblefinaleq,leftvar-I)
if r == S.Zero:
possiblefinaleq = Poly(q,leftvar)
possibilities.append(possiblefinaleq)
for eqindex in eqsindices:
if eqindex in unusedindices:
unusedindices.remove(eqindex)
if len(unusedindices) == 0:
break
if len(possibilities) > 0:
if len(possibilities) > 1:
try:
linearsolutions = self.solveVariablesLinearly(possibilities,othersolvedvars)
# if can solve for a unique solution linearly, then prioritize this over anything
prevsolution = AST.SolverBreak()
for divisor,linearsolution in linearsolutions:
assert(len(linearsolution)==1)
divisorsymbol = self.gsymbolgen.next()
solversolution = AST.SolverSolution(varsyms[ileftvar].name,jointeval=[2*atan(linearsolution[0]/divisorsymbol)],isHinge=self.isHinge(varsyms[ileftvar].name))
prevsolution = AST.SolverCheckZeros(varsyms[ileftvar].name,[divisorsymbol],zerobranch=[prevsolution],nonzerobranch=[solversolution],thresh=1e-6)
prevsolution.dictequations = [(divisorsymbol,divisor)]
linearsolution = prevsolution
break
except self.CannotSolveError:
pass
# sort with respect to degree
equationdegrees = [(max(peq.degree_list())*100000+self.codeComplexity(peq.as_expr()),peq) for peq in possibilities]
equationdegrees.sort(key=itemgetter(0))
solutions[ileftvar] = [peq[1] for peq in equationdegrees]
break
if linearsolution is not None:
return [linearsolution]
# take the solution with the smallest degree
pfinals = None
ileftvar = None
if solutions[0] is not None:
if solutions[1] is not None:
if max(solutions[1][0].degree_list()) < max(solutions[0][0].degree_list()):
pfinals = solutions[1]
ileftvar = 1
elif max(solutions[1][0].degree_list()) == max(solutions[0][0].degree_list()) and self.codeComplexity(solutions[1][0].as_expr()) < self.codeComplexity(solutions[0][0].as_expr()):
pfinals = solutions[1]
ileftvar = 1
else:
pfinals = solutions[0]
ileftvar = 0
else:
pfinals = solutions[0]
ileftvar = 0
elif solutions[1] is not None:
pfinals = solutions[1]
ileftvar = 1
dictequations = []
if pfinals is None:
#simplifyfn = self._createSimplifyFn(self.freejointvars,self.freevarsubs,self.freevarsubsinv)
for newreducedeqs in combinations(polyeqs,2):
try:
Mall = None
for ileftvar in range(2):
# TODO, sometimes this works and sometimes this doesn't
try:
Mall, allmonoms = self.solveDialytically(newreducedeqs,ileftvar,returnmatrix=True)
if Mall is not None:
leftvar=polyeqs[0].gens[ileftvar]
break
except self.CannotSolveError, e:
log.debug(e)
if Mall is None:
continue
shape=Mall[0].shape
Malltemp = [None]*len(Mall)
M = zeros(shape)
for idegree in range(len(Mall)):
Malltemp[idegree] = zeros(shape)
for i in range(shape[0]):
for j in range(shape[1]):
if Mall[idegree][i,j] != S.Zero:
sym = self.gsymbolgen.next()
Malltemp[idegree][i,j] = sym
dictequations.append((sym,Mall[idegree][i,j]))
M += Malltemp[idegree]*leftvar**idegree
tempsymbols = [self.gsymbolgen.next() for i in range(16)]
tempsubs = []
for i in range(16):
if M[i] != S.Zero:
tempsubs.append((tempsymbols[i],Poly(M[i],leftvar)))
else:
tempsymbols[i] = S.Zero
Mtemp = Matrix(4,4,tempsymbols)
dettemp=Mtemp.det()
log.info('multiplying all determinant coefficients for solving %s',leftvar)
eqadds = []
for arg in dettemp.args:
eqmuls = [Poly(arg2.subs(tempsubs),leftvar) for arg2 in arg.args]
if sum(eqmuls[0].degree_list()) == 0:
eq = eqmuls.pop(0)
eqmuls[0] = eqmuls[0]*eq
while len(eqmuls) > 1:
ioffset = 0
eqmuls2 = []
while ioffset < len(eqmuls)-1:
eqmuls2.append(eqmuls[ioffset]*eqmuls[ioffset+1])
ioffset += 2
eqmuls = eqmuls2
eqadds.append(eqmuls[0])
det = Poly(S.Zero,leftvar)
for eq in eqadds:
det += eq
# if len(Mall) == 2:
# log.info('attempting to simplify determinant...')
# newdet = Poly(S.Zero,leftvar)
# for m,c in det.terms():
# newc = self.trigsimp(c.subs(dictequations),othersolvedvars)
# newdet += newc*leftvar**m[0]
# dictequations = []
# det = newdet
pfinals = [det]
break
except self.CannotSolveError,e:
log.debug(e)
if pfinals is None:
raise self.CannotSolveError('solvePairVariablesHalfAngle: solve dialytically with %d equations'%(len(polyeqs)))
jointsol = 2*atan(varsyms[ileftvar].htvar)
solution = AST.SolverPolynomialRoots(jointname=varsyms[ileftvar].name,poly=pfinals[0],jointeval=[jointsol],isHinge=self.isHinge(varsyms[ileftvar].name))
solution.checkforzeros = []
solution.postcheckforzeros = []
solution.postcheckfornonzeros = [peq.as_expr() for peq in pfinals[1:]]
solution.postcheckforrange = []
solution.dictequations = dictequations
solution.AddHalfTanValue = True
return [solution]
def _createSimplifyFn(self,vars,varsubs,varsubsinv):
return lambda eq: self.trigsimp(eq.subs(varsubsinv),vars).subs(varsubs)
[ドキュメント] def solveVariablesLinearly(self,polyeqs,othersolvedvars,maxsolvabledegree=4):
log.debug('solveVariablesLinearly for %s: othersolvedvars=%s',polyeqs[0].gens,othersolvedvars)
nummonoms = [len(peq.monoms())-int(peq.TC()!=S.Zero) for peq in polyeqs]
mindegree = __builtin__.min(nummonoms)
maxdegree = min(__builtin__.max(nummonoms),len(polyeqs))
complexity = [(self.codeComplexity(peq.as_expr()),peq) for peq in polyeqs]
complexity.sort(key=itemgetter(0))
polyeqs = [peq[1] for peq in complexity]
trigsubs = []
trigsubsinv = []
for othervar in othersolvedvars:
v = self.Variable(othervar)
trigsubs += v.subs
trigsubsinv += v.subsinv
symbolscheck = []
for i,solvevar in enumerate(polyeqs[0].gens):
monom = [0]*len(polyeqs[0].gens)
monom[i] = 1
symbolscheck.append(tuple(monom))
solutions = []
for degree in range(mindegree,maxdegree+1):
allindices = [i for i,n in enumerate(nummonoms) if n <= degree]
if len(allindices) >= degree:
allmonoms = set()
for index in allindices:
allmonoms = allmonoms.union(set(polyeqs[index].monoms()))
allmonoms = list(allmonoms)
allmonoms.sort()
if __builtin__.sum(allmonoms[0]) == 0:
allmonoms.pop(0)
# allmonoms has to have symbols as a single variable
if not all([check in allmonoms for check in symbolscheck]):
continue
if len(allmonoms) == degree:
if degree > maxsolvabledegree:
log.warn('cannot handle linear solving for more than 4 equations')
continue
systemequations = []
consts = []
for index in allindices:
pdict = polyeqs[index].as_dict()
systemequations.append([pdict.get(monom,S.Zero) for monom in allmonoms])
consts.append(-polyeqs[index].TC())
# generate at least two solutions in case first's determinant is 0
solutions = []
for startrow in range(len(systemequations)):
rows = [startrow]
M = Matrix(1,len(allmonoms),systemequations[rows[0]])
for i in range(startrow+1,len(systemequations)):
numequationsneeded = M.shape[1] - M.shape[0]
if i+numequationsneeded > len(systemequations):
# cannot do anything
break
mergedsystemequations = list(systemequations[i])
for j in range(1,numequationsneeded):
mergedsystemequations += systemequations[i+j]
M2 = M.col_join(Matrix(numequationsneeded,len(allmonoms),mergedsystemequations))
Mdet = M2.det()
if Mdet != S.Zero:
M = M2
for j in range(numequationsneeded):
rows.append(i+j)
break
if M.shape[0] == M.shape[1]:
Mdet = self.trigsimp(Mdet.subs(trigsubsinv),othersolvedvars).subs(trigsubs)
#Minv = M.inv()
B = Matrix(M.shape[0],1,[consts[i] for i in rows])
Madjugate = M.adjugate()
solution = []
for check in symbolscheck:
value = Madjugate[allmonoms.index(check),:]*B
solution.append(self.trigsimp(value[0].subs(trigsubsinv),othersolvedvars).subs(trigsubs))
solutions.append([Mdet,solution])
if len(solutions) >= 2:
break
if len(solutions) > 0:
break
if len(solutions) == 0:
raise self.CannotSolveError('solveVariablesLinearly failed')
return solutions
[ドキュメント] def solveSingleVariableLinearly(self,raweqns,solvevar,othervars,maxnumeqs=2,douniquecheck=True):
"""tries to linearly solve for one variable treating everything else as constant.
need at least 3 equations
"""
cvar = Symbol('c%s'%solvevar.name)
svar = Symbol('s%s'%solvevar.name)
varsubs = [(cos(solvevar),cvar),(sin(solvevar),svar)]
othervarsubs = [(sin(v)**2,1-cos(v)**2) for v in othervars]
eqpolys = [Poly(eq.subs(varsubs),cvar,svar) for eq in raweqns]
eqpolys = [eq for eq in eqpolys if sum(eq.degree_list()) == 1 and not eq.TC().has(solvevar)]
#eqpolys.sort(lambda x,y: iksolver.codeComplexity(x) - iksolver.codeComplexity(y))
partialsolutions = []
neweqs = []
for p0,p1 in combinations(eqpolys,2):
p0dict = p0.as_dict()
p1dict = p1.as_dict()
M = Matrix(2,3,[p0dict.get((1,0),S.Zero),p0dict.get((0,1),S.Zero),p0.TC(),p1dict.get((1,0),S.Zero),p1dict.get((0,1),S.Zero),p1.TC()])
M = M.subs(othervarsubs).expand()
partialsolution = [-M[1,1]*M[0,2]+M[0,1]*M[1,2],M[1,0]*M[0,2]-M[0,0]*M[1,2],M[0,0]*M[1,1]-M[0,1]*M[1,0]]
partialsolution = [eq.expand().subs(othervarsubs).expand() for eq in partialsolution]
rank = [self.codeComplexity(eq) for eq in partialsolution]
partialsolutions.append([rank,partialsolution])
# cos(A)**2 + sin(A)**2 - 1 = 0, useful equation but the squares introduce wrong solutions
#neweqs.append(partialsolution[0]**2+partialsolution[1]**2-partialsolution[2]**2)
# try to cross
partialsolutions.sort(lambda x, y: int(min(x[0])-min(y[0])))
for (rank0,ps0),(rank1,ps1) in combinations(partialsolutions,2):
if self.equal(ps0[0]*ps1[2]-ps1[0]*ps0[2],S.Zero):
continue
neweqs.append(ps0[0]*ps1[2]-ps1[0]*ps0[2])
neweqs.append(ps0[1]*ps1[2]-ps1[1]*ps0[2])
# probably a linear combination of the first two
#neweqs.append(ps0[0]*ps1[1]-ps1[0]*ps0[1])
# too long
#neweqs.append(ps0[0]*ps1[0]+ps0[1]*ps1[1]-ps0[2]*ps1[2])
if len(neweqs) >= maxnumeqs:
break;
neweqs2 = [eq.expand().subs(othervarsubs).expand() for eq in neweqs]
if douniquecheck:
reducedeqs = []
i = 0
while i < len(neweqs2):
reducedeq = self.removecommonexprs(neweqs2[i])
if neweqs2[i] != S.Zero and self.isExpressionUnique(reducedeqs,reducedeq) and self.isExpressionUnique(reducedeqs,-reducedeq):
reducedeqs.append(reducedeq)
i += 1
else:
eq=neweqs2.pop(i)
return neweqs2
[ドキュメント] def solveHighDegreeEquationsHalfAngle(self,lineareqs,varsym,subs=None):
"""solve a set of equations in one variable with half-angle substitution
"""
dummysubs = [(varsym.cvar,(1-varsym.htvar**2)/(1+varsym.htvar**2)),(varsym.svar,2*varsym.htvar/(1+varsym.htvar**2))]
polyeqs = []
for eq in lineareqs:
trigsubs = [(varsym.svar**2,1-varsym.cvar**2), (varsym.svar**3,varsym.svar*(1-varsym.cvar**2))]
peq = Poly(eq.subs(varsym.subs).subs(trigsubs),varsym.cvar,varsym.svar)
if peq.has(varsym.var):
raise self.CannotSolveError('expecting only sin and cos! %s'%peq)
if sum(peq.degree_list()) == 0:
continue
# check if all terms are multiples of cos/sin
maxmonoms = [0,0]
maxdenom = 0
for monoms in peq.monoms():
for i in range(2):
maxmonoms[i] = max(maxmonoms[i],monoms[i])
maxdenom = max(maxdenom,monoms[0]+monoms[1])
eqnew = S.Zero
for monoms,c in peq.terms():
if c.evalf() != S.Zero: # big fractions might make this difficult to reduce to 0
term = c
for i in range(2):
num,denom = fraction(dummysubs[i][1])
term *= num**monoms[i]
# the denoms for 0,1 and 2,3 are the same
denom = fraction(dummysubs[0][1])[1]
term *= denom**(maxdenom-monoms[0]-monoms[1])
eqnew += simplify(term)
polyeqs.append(Poly(eqnew,varsym.htvar))
for peq in polyeqs:
# do some type of resultants, for now just choose first polynomial
finaleq = simplify(peq.as_expr()).expand()
pfinal = Poly(self.removecommonexprs(finaleq,onlygcd=False,onlynumbers=True),varsym.htvar)
pfinal = self.checkFinalEquation(pfinal,subs)
if pfinal is not None:
jointsol = 2*atan(varsym.htvar)
solution = AST.SolverPolynomialRoots(jointname=varsym.name,poly=pfinal,jointeval=[jointsol],isHinge=self.isHinge(varsym.name))
solution.AddHalfTanValue = True
solution.checkforzeros = []
solution.postcheckforzeros = []
solution.postcheckfornonzeros = []
solution.postcheckforrange = []
return solution
raise self.CannotSolveError('half-angle substitution for joint %s failed, %d equations examined'%(varsym.var,len(polyeqs)))
[ドキュメント] def checkFinalEquation(self,pfinal,subs=None):
"""check an equation in one variable for validity
"""
assert(len(pfinal.gens)==1)
if subs is None:
subs = []
htvar = pfinal.gens[0]
# remove all trivial 0s
while sum(pfinal.degree_list()) > 0 and pfinal.TC() == S.Zero:
pfinalnew = Poly(S.Zero,htvar)
for m,c in pfinal.terms():
if m[0] > 0:
pfinalnew += c*htvar**(m[0]-1)
pfinal = pfinalnew
# check to see that LC is non-zero for at least one solution
if pfinal.LC().evalf() == S.Zero or all([pfinal.LC().subs(subs).subs(self.globalsymbols).subs(testconsistentvalue).evalf()==S.Zero for testconsistentvalue in self.testconsistentvalues]):
return None
# sanity check that polynomial can produce a solution and is not actually very small values
found = False
LCnormalized, common = self.removecommonexprs(pfinal.LC(),returncommon=True,onlygcd=False,onlynumbers=True)
pfinaldict = pfinal.as_dict()
for testconsistentvalue in self.testconsistentvalues:
coeffs = []
globalsymbols = [(s,v.subs(self.globalsymbols).subs(testconsistentvalue).evalf()) for s,v in self.globalsymbols]
for degree in range(pfinal.degree(0),-1,-1):
coeffs.append(pfinaldict.get((degree,),S.Zero).subs(subs).subs(globalsymbols+testconsistentvalue).evalf()/common.evalf())
# since coeffs[0] is normalized with the LC constant, can compare for precision
if len(coeffs) == 1 and Abs(coeffs[0]) < 2*(10.0**-self.precision):
coeffs = None
break
if coeffs is None:
continue
if not all([c.is_number for c in coeffs]):
# cannot evalute
log.warn('cannot evalute: %s',coeffs)
found = True
break
realsolution = pfinal.gens[0].subs(subs).subs(self.globalsymbols).subs(testconsistentvalue).evalf()
# need to convert to float64 first, X.evalf() is still a sympy object
roots = mpmath.polyroots(numpy.array(numpy.array(coeffs),numpy.float64))
for root in roots:
if Abs(float(root.imag)) < 10.0**-self.precision and Abs(float(root.real)-realsolution) < 10.0**-(self.precision-2):
found = True
break
if found:
break
return pfinal if found else None
[ドキュメント] def solveSingleVariable(self,raweqns,var,othersolvedvars,maxsolutions=4,maxdegree=2,subs=None, unknownvars=None):
varsym = self.Variable(var)
vars = [varsym.cvar,varsym.svar,varsym.htvar,var]
othersubs = []
for othersolvedvar in othersolvedvars:
othersubs += self.Variable(othersolvedvar).subs
# eqns = []
# for eq in raweqns:
# if eq.has(*vars):
# # for equations that are very complex, make sure at least one set of values yields a non zero equation
# testeq = eq.subs(varsym.subs+othersubs)
# if any([testeq.subs(testconsistentvalue).evalf()!=S.Zero for testconsistentvalue in self.testconsistentvalues]):
# eqns.append(eq)
eqns = [eq.expand() for eq in raweqns if eq.has(*vars)]
if len(eqns) == 0:
raise self.CannotSolveError('not enough equations')
# prioritize finding a solution when var is alone
for eq in eqns:
symbolgen = cse_main.numbered_symbols('const')
eqnew, symbols = self.groupTerms(eq.subs(varsym.subs), vars, symbolgen)
try:
ps = Poly(eqnew,varsym.svar)
pc = Poly(eqnew,varsym.cvar)
if sum(ps.degree_list()) > 0 or sum(pc.degree_list()) > 0 or ps.TC() == S.Zero or pc.TC() == S.Zero:
continue
except PolynomialError:
continue
numvar = self.countVariables(eqnew,var)
if numvar >= 1 and numvar <= 2:
tempsolutions = solve(eqnew,var)
jointsolutions = [self.trigsimp(s.subs(symbols),othersolvedvars) for s in tempsolutions]
if all([self.isValidSolution(s) and s != S.Zero for s in jointsolutions]) and len(jointsolutions)>0:
return [AST.SolverSolution(var.name,jointeval=jointsolutions,isHinge=self.isHinge(var.name))]
numvar = self.countVariables(eqnew,varsym.htvar)
if Poly(eqnew,varsym.htvar).TC() != S.Zero and numvar >= 1 and numvar <= 2:
tempsolutions = solve(eqnew,varsym.htvar)
jointsolutions = [2*atan(self.trigsimp(s.subs(symbols),othersolvedvars)) for s in tempsolutions]
if all([self.isValidSolution(s) and s != S.Zero for s in jointsolutions]) and len(jointsolutions)>0:
return [AST.SolverSolution(var.name,jointeval=jointsolutions,isHinge=self.isHinge(var.name))]
solutions = []
if len(eqns) > 1:
neweqns = []
listsymbols = []
symbolgen = cse_main.numbered_symbols('const')
for e in eqns:
enew, symbols = self.groupTerms(e.subs(varsym.subs),[varsym.cvar,varsym.svar,var], symbolgen)
# remove coupled equations
if any([(m[0]>0)+(m[1]>0)+(m[2]>0)>1 for m in Poly(enew,varsym.cvar,varsym.svar,var).monoms()]):
continue
# ignore any equations with degree 3 or more
if max(Poly(enew,varsym.svar).degree_list()) > maxdegree or max(Poly(enew,varsym.cvar).degree_list()) > maxdegree:
log.debug('ignoring equation: ',enew)
continue
if Poly(enew,varsym.svar).TC() == S.Zero or Poly(enew,varsym.cvar) == S.Zero or Poly(enew,varsym.var) == S.Zero:
log.debug('equation %s is allowing trivial solution for variable %s, ignoring ',e,varsym.name)
continue
rank = self.codeComplexity(enew)
for s in symbols:
rank += self.codeComplexity(s[1])
neweqns.append((rank,enew))
listsymbols += symbols
# since we're solving for two variables, we only want to use two equations, so
# start trying all the equations starting from the least complicated ones to the most until a solution is found
eqcombinations = []
for eqs in combinations(neweqns,2):
eqcombinations.append((eqs[0][0]+eqs[1][0],[Eq(e[1],0) for e in eqs]))
eqcombinations.sort(lambda x, y: x[0]-y[0])
hasgoodsolution = False
for icomb,comb in enumerate(eqcombinations):
# skip if too complex
if len(solutions) > 0 and comb[0] > 200:
break
# try to solve for both sin and cos terms
if not self.has(comb[1],varsym.svar) or not self.has(comb[1], varsym.cvar):
continue
try:
s = solve(comb[1],[varsym.svar,varsym.cvar])
except (PolynomialError,CoercionFailed), e:
log.debug('solveSingleVariable: failed: %s',e)
continue
if s is not None:
sollist = None
if hasattr(s,'has_key'):
if s.has_key(varsym.svar) and s.has_key(varsym.cvar):
sollist = [(s[varsym.svar],s[varsym.cvar])]
else:
sollist = []
else:
sollist = s
solversolution = AST.SolverSolution(var.name,jointeval=[],isHinge=self.isHinge(var.name))
goodsolution = 0
for svarsol,cvarsol in sollist:
# solutions cannot be trivial
if (svarsol-cvarsol).subs(listsymbols).expand() == S.Zero:
break
if svarsol.subs(listsymbols).expand() == S.Zero and Abs(cvarsol.subs(listsymbols).expand()) - S.One != S.Zero:
break
if cvarsol.subs(listsymbols).expand() == S.Zero and Abs(svarsol.subs(listsymbols).expand()) - S.One != S.Zero:
break
# check the numerator and denominator if solutions are the same or for possible divide by zeros
svarfrac=fraction(svarsol)
svarfrac = [svarfrac[0].subs(listsymbols), svarfrac[1].subs(listsymbols)]
cvarfrac=fraction(cvarsol)
cvarfrac = [cvarfrac[0].subs(listsymbols), cvarfrac[1].subs(listsymbols)]
if self.equal(svarfrac[0],cvarfrac[0]) and self.equal(svarfrac[1],cvarfrac[1]):
break
if not self.isValidSolution(svarfrac[0]) or not self.isValidSolution(svarfrac[1]) or not self.isValidSolution(cvarfrac[0]) or not self.isValidSolution(cvarfrac[1]):
continue
# check if there exists at least one test solution with non-zero denominators
if subs is None:
testeqs = [svarfrac[1].subs(othersubs),cvarfrac[1].subs(othersubs)]
else:
testeqs = [svarfrac[1].subs(subs).subs(othersubs),cvarfrac[1].subs(subs).subs(othersubs)]
testsuccess = False
for testconsistentvalue in self.testconsistentvalues:
if all([testeq.subs(self.globalsymbols).subs(testconsistentvalue).evalf()!=S.Zero for testeq in testeqs]):
testsuccess = True
break
if not testsuccess:
continue
scomplexity = self.codeComplexity(svarfrac[0])+self.codeComplexity(svarfrac[1])
ccomplexity = self.codeComplexity(cvarfrac[0])+self.codeComplexity(cvarfrac[1])
if scomplexity > 1200 or ccomplexity > 1200:
log.debug('equation too complex for single variable solution (%d,%d).... (probably wrong?)',scomplexity,ccomplexity)
break
if scomplexity < 500:
svarfrac[1] = simplify(svarfrac[1])
if self.chop(svarfrac[1])== 0:
break
if ccomplexity < 500:
cvarfrac[1] = simplify(cvarfrac[1])
if self.chop(cvarfrac[1])== 0:
break
# sometimes the returned simplest solution makes really gross approximations
svarfracsimp_denom = self.trigsimp(svarfrac[1],othersolvedvars)
cvarfracsimp_denom = self.trigsimp(cvarfrac[1],othersolvedvars)
# self.simplifyTransform could help in reducing denoms further...
denomsequal = False
if self.equal(svarfracsimp_denom,cvarfracsimp_denom):
denomsequal = True
elif self.equal(svarfracsimp_denom,-cvarfracsimp_denom):
cvarfrac[0] = -cvarfrac[0]
cvarfracsimp_denom = -cvarfracsimp_denom
if self.equal(svarfracsimp_denom,cvarfracsimp_denom) and not svarfracsimp_denom.is_number:
log.debug('%s solution: denominator is equal %s, doing a global substitution',var.name,svarfracsimp_denom)
denom = self.gsymbolgen.next()
solversolution.dictequations.append((denom,sign(svarfracsimp_denom)))
svarsolsimp = self.trigsimp(svarfrac[0],othersolvedvars)*denom
cvarsolsimp = self.trigsimp(cvarfrac[0],othersolvedvars)*denom
solversolution.FeasibleIsZeros = False
solversolution.presetcheckforzeros.append(svarfracsimp_denom)
expandedsol = atan2(svarsolsimp,cvarsolsimp)
else:
svarfracsimp_num = self.trigsimp(svarfrac[0],othersolvedvars)
cvarfracsimp_num = self.trigsimp(cvarfrac[0],othersolvedvars)
svarsolsimp = svarfracsimp_num/svarfracsimp_denom
cvarsolsimp = cvarfracsimp_num/cvarfracsimp_denom
if svarsolsimp.is_number and cvarsolsimp.is_number:
if Abs(svarsolsimp**2+cvarsolsimp**2-S.One).evalf() > 1e-10:
log.debug('%s solution: atan2(%s,%s), sin/cos not on circle so ignoring',var.name,svarsolsimp,cvarsolsimp)
continue
expandedsol = atan2check(svarsolsimp,cvarsolsimp)
solversolution.FeasibleIsZeros = False
log.debug('%s solution: atan2 check for joint',var.name)
solversolution.jointeval.append(expandedsol)
if unknownvars is not None:
unsolvedsymbols = []
for unknownvar in unknownvars:
if unknownvar != var:
unsolvedsymbols += self.Variable(unknownvar).vars
if len(unsolvedsymbols) > 0:
solversolution.equationsused = [eq for eq in eqns if not eq.has(*unsolvedsymbols)]
else:
solversolution.equationsused = eqns
if len(solversolution.equationsused) > 0:
log.info('%s solution: equations used for atan2: %s',var.name, str(solversolution.equationsused))
if len(self.checkForDivideByZero(expandedsol)) == 0:
goodsolution += 1
if len(solversolution.jointeval) == len(sollist) and len(sollist) > 0:
solutions.append(solversolution)
if goodsolution > 0:
hasgoodsolution = True
if len(sollist) == goodsolution and goodsolution == 1 and len(solutions) >= 2:
break
if len(solutions) >= maxsolutions:
# probably more than enough already?
break
if len(solutions) > 0 or hasgoodsolution: # found a solution without any divides, necessary for pr2 head_torso lookat3d ik
return solutions
# solve one equation
for ieq,eq in enumerate(eqns):
symbolgen = cse_main.numbered_symbols('const')
eqnew, symbols = self.groupTerms(eq.subs(varsym.subs), [varsym.cvar,varsym.svar,varsym.var], symbolgen)
try:
# ignore any equations with degree 3 or more
ps = Poly(eqnew,varsym.svar)
pc = Poly(eqnew,varsym.cvar)
if max(ps.degree_list()) > maxdegree or max(pc.degree_list()) > maxdegree:
log.debug('cannot solve equation with high degree: %s',str(eqnew))
continue
if ps.TC() == S.Zero and len(ps.monoms()) > 0:
log.debug('equation %s has trivial solution, ignoring...', ps)
continue
if pc.TC() == S.Zero and len(pc.monoms()) > 0:
log.debug('equation %s has trivial solution, ignoring...', pc)
continue
except PolynomialError:
# might not be a polynomial, so ignore
continue
equationsused = None
if unknownvars is not None:
unsolvedsymbols = []
for unknownvar in unknownvars:
if unknownvar != var:
unsolvedsymbols += self.Variable(unknownvar).vars
if len(unsolvedsymbols) > 0:
equationsused = [eq2 for ieq2,eq2 in enumerate(eqns) if ieq2!=ieq and not eq2.has(*unsolvedsymbols)]
else:
equationsused = eqns[:]
equationsused.pop(ieq)
numcvar = self.countVariables(eqnew,varsym.cvar)
numsvar = self.countVariables(eqnew,varsym.svar)
if numcvar == 1 and numsvar == 1:
a = Wild('a',exclude=[varsym.svar,varsym.cvar])
b = Wild('b',exclude=[varsym.svar,varsym.cvar])
c = Wild('c',exclude=[varsym.svar,varsym.cvar])
m = eqnew.match(a*varsym.cvar+b*varsym.svar+c)
if m is not None:
symbols += [(varsym.svar,sin(var)),(varsym.cvar,cos(var))]
asinsol = trigsimp(asin(-m[c]/Abs(sqrt(m[a]*m[a]+m[b]*m[b]))).subs(symbols),deep=True)
constsol = -atan2(m[a],m[b]).subs(symbols).evalf()
jointsolutions = [constsol+asinsol,constsol+pi.evalf()-asinsol]
if all([self.isValidSolution(s) and self.isValidSolution(s) for s in jointsolutions]):
solutions.append(AST.SolverSolution(var.name,jointeval=jointsolutions,isHinge=self.isHinge(var.name)))
solutions[-1].equationsused = equationsused
continue
if numcvar > 0:
try:
# substitute cos
if self.countVariables(eqnew,varsym.svar) <= 1 or (self.countVariables(eqnew,varsym.cvar) <= 2 and self.countVariables(eqnew,varsym.svar) == 0): # anything more than 1 implies quartic equation
tempsolutions = solve(eqnew.subs(varsym.svar,sqrt(1-varsym.cvar**2)),varsym.cvar)
jointsolutions = [self.trigsimp(s.subs(symbols+varsym.subsinv),othersolvedvars) for s in tempsolutions]
if all([self.isValidSolution(s) and self.isValidSolution(s) for s in jointsolutions]):
solutions.append(AST.SolverSolution(var.name,jointevalcos=jointsolutions,isHinge=self.isHinge(var.name)))
solutions[-1].equationsused = equationsused
continue
except self.CannotSolveError,e:
log.debug(e)
except NotImplementedError:
pass
if numsvar > 0:
# substitute sin
try:
if self.countVariables(eqnew,varsym.svar) <= 1 or (self.countVariables(eqnew,varsym.svar) <= 2 and self.countVariables(eqnew,varsym.cvar) == 0): # anything more than 1 implies quartic equation
tempsolutions = solve(eqnew.subs(varsym.cvar,sqrt(1-varsym.svar**2)),varsym.svar)
jointsolutions = [self.trigsimp(s.subs(symbols+varsym.subsinv),othersolvedvars) for s in tempsolutions]
if all([self.isValidSolution(s) and self.isValidSolution(s) for s in jointsolutions]):
solutions.append(AST.SolverSolution(var.name,jointevalsin=jointsolutions,isHinge=self.isHinge(var.name)))
solutions[-1].equationsused = equationsused
continue
except self.CannotSolveError,e:
log.debug(e)
except NotImplementedError:
pass
if numcvar == 0 and numsvar == 0:
tempsolutions = solve(eqnew,var)
jointsolutions = [self.trigsimp(s.subs(symbols),othersolvedvars) for s in tempsolutions]
if all([self.isValidSolution(s) and s != S.Zero for s in jointsolutions]) and len(jointsolutions) > 0:
solutions.append(AST.SolverSolution(var.name,jointeval=jointsolutions,isHinge=self.isHinge(var.name)))
solutions[-1].equationsused = equationsused
continue
try:
solution = self.solveHighDegreeEquationsHalfAngle([eqnew],varsym,symbols)
solutions.append(solution.subs(symbols))
solutions[-1].equationsused = equationsused
except self.CannotSolveError,e:
log.debug(e)
if len(solutions) > 0:
return solutions
return [self.solveHighDegreeEquationsHalfAngle(eqns,varsym)]
[ドキュメント] def solvePairVariables(self,raweqns,var0,var1,othersolvedvars,maxcomplexity=50,unknownvars=None):
# make sure both variables are hinges
if not self.isHinge(var0.name) or not self.isHinge(var1.name):
raise self.CannotSolveError('pairwise variables only supports hinge joints')
varsym0 = self.Variable(var0)
varsym1 = self.Variable(var1)
cvar0,svar0 = varsym0.cvar, varsym0.svar
cvar1,svar1 = varsym1.cvar, varsym1.svar
varsubs=varsym0.subs+varsym1.subs
varsubsinv = varsym0.subsinv+varsym1.subsinv
unknownvars=[cvar0,svar0,cvar1,svar1]
reducesubs = [(svar0**2,1-cvar0**2),(svar1**2,1-cvar1**2)]
eqns = [eq.subs(varsubs).subs(reducesubs).expand() for eq in raweqns if eq.has(var0,var1)]
if len(eqns) <= 1:
raise self.CannotSolveError('not enough equations')
# group equations with single variables
symbolgen = cse_main.numbered_symbols('const')
orgeqns = []
allsymbols = []
for eq in eqns:
eqnew, symbols = self.groupTerms(eq, unknownvars, symbolgen)
allsymbols += symbols
orgeqns.append([self.codeComplexity(eq),Poly(eqnew,*unknownvars)])
orgeqns.sort(lambda x, y: x[0]-y[0])
neweqns = orgeqns[:]
pairwisesubs = [(svar0*cvar1,Symbol('s0c1')),(svar0*svar1,Symbol('s0s1')),(cvar0*cvar1,Symbol('c0c1')),(cvar0*svar1,Symbol('c0s1')),(cvar0*svar0,Symbol('s0c0')),(cvar1*svar1,Symbol('c1s1'))]
pairwiseinvsubs = [(f[1],f[0]) for f in pairwisesubs]
pairwisevars = [f[1] for f in pairwisesubs]
reduceeqns = [Poly(eq.as_expr().subs(pairwisesubs),*pairwisevars) for rank,eq in orgeqns if rank < 4*maxcomplexity]
for i,eq in enumerate(reduceeqns):
if eq.TC != S.Zero and not eq.TC().is_Symbol:
n=symbolgen.next()
allsymbols.append((n,eq.TC().subs(allsymbols)))
reduceeqns[i] += n-eq.TC()
# try to at least subtract as much paired variables out
eqcombs = [c for c in combinations(reduceeqns,2)]
while len(eqcombs) > 0 and len(neweqns) < 20:
eq0,eq1 = eqcombs.pop()
eq0dict = eq0.as_dict()
eq1dict = eq1.as_dict()
for i in range(6):
monom = [0,0,0,0,0,0]
monom[i] = 1
eq0value = eq0dict.get(tuple(monom),S.Zero)
eq1value = eq1dict.get(tuple(monom),S.Zero)
if eq0value != 0 and eq1value != 0:
tempeq = (eq0.as_expr()*eq1value-eq0value*eq1.as_expr()).subs(allsymbols+pairwiseinvsubs).expand()
if self.codeComplexity(tempeq) > 200:
continue
eq = simplify(tempeq)
if eq == S.Zero:
continue
peq = Poly(eq,*pairwisevars)
if max(peq.degree_list()) > 0 and self.codeComplexity(eq) > maxcomplexity:
# don't need such complex equations
continue
if not self.isExpressionUnique(eqns,eq) or not self.isExpressionUnique(eqns,-eq):
continue
if eq.has(*unknownvars): # be a little strict about new candidates
eqns.append(eq)
eqnew, symbols = self.groupTerms(eq, unknownvars, symbolgen)
allsymbols += symbols
neweqns.append([self.codeComplexity(eq),Poly(eqnew,*unknownvars)])
orgeqns = neweqns[:]
# try to solve for all pairwise variables
systemofequations = []
for i in range(len(reduceeqns)):
if reduceeqns[i].has(pairwisevars[4],pairwisevars[5]):
continue
if not all([__builtin__.sum(m) <= 1 for m in reduceeqns[i].monoms()]):
continue
arr = [S.Zero]*5
for m,c in reduceeqns[i].terms():
if __builtin__.sum(m) == 1:
arr[list(m).index(1)] = c
else:
arr[4] = c
systemofequations.append(arr)
if len(systemofequations) >= 4:
singleeqs = None
for eqs in combinations(systemofequations,4):
M = zeros((4,4))
B = zeros((4,1))
for i,arr in enumerate(eqs):
for j in range(4):
M[i,j] = arr[j]
B[i] = -arr[4]
det = self.det_bareis(M,*(self.pvars+unknownvars)).subs(allsymbols)
if det.evalf() != S.Zero:
X = M.adjugate()*B
singleeqs = []
for i in range(4):
eq = (pairwisesubs[i][0]*det - X[i]).subs(allsymbols)
eqnew, symbols = self.groupTerms(eq, unknownvars, symbolgen)
allsymbols += symbols
singleeqs.append([self.codeComplexity(eq),Poly(eqnew,*unknownvars)])
break
if singleeqs is not None:
neweqns += singleeqs
neweqns.sort(lambda x, y: x[0]-y[0])
# check if any equations are at least degree 1 (if not, try to compute some)
for ivar in range(2):
polyunknown = []
for rank,eq in orgeqns:
p = Poly(eq,unknownvars[2*ivar],unknownvars[2*ivar+1])
if sum(p.degree_list()) == 1 and __builtin__.sum(p.LM()) == 1:
polyunknown.append((rank,p))
if len(polyunknown) > 0:
break
if len(polyunknown) == 0:
addedeqs = eqns[:]
polyeqs = []
for ivar in range(2):
polyunknown = []
for rank,eq in orgeqns:
p = Poly(eq,unknownvars[2*ivar],unknownvars[2*ivar+1])
polyunknown.append(Poly(p.subs(unknownvars[2*ivar+1]**2,1-unknownvars[2*ivar]**2),unknownvars[2*ivar],unknownvars[2*ivar+1]))
if len(polyunknown) >= 2:
monomtoremove = [[polyunknown,(2,0)],[polyunknown,(1,1)]]
for curiter in range(2):
# remove the square
polyunknown,monom = monomtoremove[curiter]
pbase = [p for p in polyunknown if p.as_dict().get(monom,S.Zero) != S.Zero]
if len(pbase) == 0:
continue
pbase = pbase[0]
pbasedict = pbase.as_dict()
for i in range(len(polyunknown)):
eq = (polyunknown[i]*pbasedict.get(monom,S.Zero)-pbase*polyunknown[i].as_dict().get(monom,S.Zero)).as_expr().subs(allsymbols).expand()
if eq != S.Zero and self.isExpressionUnique(addedeqs,eq) and self.isExpressionUnique(addedeqs,-eq):
eqnew, symbols = self.groupTerms(eq, unknownvars, symbolgen)
allsymbols += symbols
p = Poly(eqnew,*pbase.gens)
if p.as_dict().get((1,1),S.Zero) != S.Zero and curiter == 0:
monomtoremove[1][0].insert(0,p)
polyeqs.append([self.codeComplexity(eqnew),Poly(eqnew,*unknownvars)])
addedeqs.append(eq)
neweqns += polyeqs
neweqns.sort(lambda x,y: x[0]-y[0])
rawsolutions = []
# try single variable solution, only return if a single solution has been found
# returning multiple solutions when only one exists can lead to wrong results.
try:
rawsolutions += self.solveSingleVariable([e.as_expr().subs(varsubsinv).expand() for score,e in neweqns if not e.has(cvar1,svar1,var1)],var0,othersolvedvars,subs=allsymbols,unknownvars=unknownvars)
except self.CannotSolveError:
pass
try:
rawsolutions += self.solveSingleVariable([e.as_expr().subs(varsubsinv).expand() for score,e in neweqns if not e.has(cvar0,svar0,var0)],var1,othersolvedvars,subs=allsymbols,unknownvars=unknownvars)
except self.CannotSolveError:
pass
if len(rawsolutions) > 0:
solutions = []
for s in rawsolutions:
try:
solutions.append(s.subs(allsymbols))
except self.CannotSolveError:
pass
if len(solutions) > 0:
return solutions
groups=[]
for i,unknownvar in enumerate(unknownvars):
listeqs = []
listeqscmp = []
for rank,eq in neweqns:
# if variable ever appears, it should be alone
if all([m[i] == 0 or (__builtin__.sum(m) == m[i] and m[i]>0) for m in eq.monoms()]) and any([m[i] > 0 for m in eq.monoms()]):
# make sure there's only one monom that includes other variables
othervars = [__builtin__.sum(m) - m[i] > 0 for m in eq.monoms()]
if __builtin__.sum(othervars) <= 1:
eqcmp = self.removecommonexprs(eq.subs(allsymbols).as_expr(),onlynumbers=False,onlygcd=True)
if self.isExpressionUnique(listeqscmp,eqcmp) and self.isExpressionUnique(listeqscmp,-eqcmp):
listeqs.append(eq)
listeqscmp.append(eqcmp)
groups.append(listeqs)
# find a group that has two or more equations:
useconic=False
goodgroup = [(i,g) for i,g in enumerate(groups) if len(g) >= 2]
if len(goodgroup) == 0:
# might have a set of equations that can be solved with conics
# look for equations where the variable and its complement are alone
groups=[]
for i in [0,2]:
unknownvar = unknownvars[i]
complementvar = unknownvars[i+1]
listeqs = []
listeqscmp = []
for rank,eq in neweqns:
# if variable ever appears, it should be alone
addeq = False
if all([__builtin__.sum(m) == m[i]+m[i+1] for m in eq.monoms()]):
addeq = True
else:
# make sure there's only one monom that includes other variables
othervars = 0
for m in eq.monoms():
if __builtin__.sum(m) > m[i]+m[i+1]:
if m[i] == 0 and m[i+1]==0:
othervars += 1
else:
othervars = 10000
if othervars <= 1:
addeq = True
if addeq:
eqcmp = self.removecommonexprs(eq.subs(allsymbols).as_expr(),onlynumbers=False,onlygcd=True)
if self.isExpressionUnique(listeqscmp,eqcmp) and self.isExpressionUnique(listeqscmp,-eqcmp):
listeqs.append(eq)
listeqscmp.append(eqcmp)
groups.append(listeqs)
groups.append([]) # necessary to get indices correct
goodgroup = [(i,g) for i,g in enumerate(groups) if len(g) >= 2]
useconic=True
if len(goodgroup) == 0:
try:
return self.solvePairVariablesHalfAngle(raweqns,var0,var1,othersolvedvars)
except self.CannotSolveError,e:
log.warn('%s',e)
# try a separate approach where the two variables are divided on both sides
neweqs = []
for rank,eq in neweqns:
p = Poly(eq,unknownvars[0],unknownvars[1])
iscoupled = False
for m,c in p.terms():
if __builtin__.sum(m) > 0:
if c.has(unknownvars[2],unknownvars[3]):
iscoupled = True
break
if not iscoupled:
neweqs.append([p-p.TC(),Poly(-p.TC(),unknownvars[2],unknownvars[3])])
if len(neweqs) > 0:
for ivar in range(2):
lineareqs = [eq for eq in neweqs if __builtin__.sum(eq[ivar].LM())==1]
for paireq0,paireq1 in combinations(lineareqs,2):
log.info('solving separated equations with linear terms')
eq0 = paireq0[ivar]
eq0dict = eq0.as_dict()
eq1 = paireq1[ivar]
eq1dict = eq1.as_dict()
disc = (eq0dict.get((1,0),S.Zero)*eq1dict.get((0,1),S.Zero) - eq0dict.get((0,1),S.Zero)*eq1dict.get((1,0),S.Zero)).subs(allsymbols).expand()
if disc == S.Zero:
continue
othereq0 = paireq0[1-ivar].as_expr() - eq0.TC()
othereq1 = paireq1[1-ivar].as_expr() - eq1.TC()
csol = - eq1dict.get((0,1),S.Zero) * othereq0 + eq0dict.get((0,1),S.Zero) * othereq1
ssol = eq1dict.get((1,0),S.Zero) * othereq0 - eq0dict.get((1,0),S.Zero) * othereq1
polysymbols = paireq0[1-ivar].gens
totaleq = (csol**2+ssol**2-disc**2).subs(allsymbols).expand()
if self.codeComplexity(totaleq) < 4000:
log.info('simplifying final equation to %d',self.codeComplexity(totaleq))
totaleq = simplify(totaleq)
ptotal_cos = Poly(Poly(totaleq,*polysymbols).subs(polysymbols[0]**2,1-polysymbols[1]**2).subs(polysymbols[1]**2,1-polysymbols[0]**2),*polysymbols)
ptotal_sin = Poly(S.Zero,*polysymbols)
for m,c in ptotal_cos.terms():
if m[1] > 0:
assert(m[1] == 1)
ptotal_sin = ptotal_sin.sub(Poly.from_dict({(m[0],0):c},*ptotal_sin.gens))
ptotal_cos = ptotal_cos.sub(Poly.from_dict({m:c},*ptotal_cos.gens))
finaleq = (ptotal_cos.as_expr()**2 - (1-polysymbols[0]**2)*ptotal_sin.as_expr()**2).expand()
# sometimes denominators can accumulate
pfinal = Poly(self.removecommonexprs(finaleq,onlygcd=False,onlynumbers=True),polysymbols[0])
pfinal = self.checkFinalEquation(pfinal)
if pfinal is not None:
jointsol = atan2(ptotal_cos.as_expr()/ptotal_sin.as_expr(), polysymbols[0])
var = var1 if ivar == 0 else var0
solution = AST.SolverPolynomialRoots(jointname=var.name,poly=pfinal,jointeval=[jointsol],isHinge=self.isHinge(var.name))
solution.postcheckforzeros = [ptotal_sin.as_expr()]
solution.postcheckfornonzeros = []
solution.postcheckforrange = []
return [solution]
# if maxnumeqs is any less, it will miss linearly independent equations
lineareqs = self.solveSingleVariableLinearly(raweqns,var0,[var1],maxnumeqs=len(raweqns))
if len(lineareqs) > 0:
try:
return [self.solveHighDegreeEquationsHalfAngle(lineareqs,varsym1)]
except self.CannotSolveError,e:
log.warn('%s',e)
raise self.CannotSolveError('cannot cleanly separate pair equations')
varindex=goodgroup[0][0]
var = var0 if varindex < 2 else var1
varsym = varsym0 if varindex < 2 else varsym1
unknownvar=unknownvars[goodgroup[0][0]]
eqs = goodgroup[0][1][0:2]
simpleterms = []
complexterms = []
domagicsquare = False
for i in range(2):
if useconic:
terms=[(c,m) for m,c in eqs[i].terms() if __builtin__.sum(m) - m[varindex] - m[varindex+1] > 0]
else:
terms=[(c,m) for m,c in eqs[i].terms() if __builtin__.sum(m) - m[varindex] > 0]
if len(terms) > 0:
simpleterms.append(eqs[i].sub(Poly.from_dict({terms[0][1]:terms[0][0]},*eqs[i].gens)).as_expr()/terms[0][0]) # divide by the coeff
complexterms.append(Poly({terms[0][1]:S.One},*unknownvars).as_expr())
domagicsquare = True
else:
simpleterms.append(eqs[i].as_expr())
complexterms.append(S.Zero)
finaleq = None
checkforzeros = []
if domagicsquare:
# here is the magic transformation:
finaleq = self.trigsimp(expand(((complexterms[0]**2+complexterms[1]**2) - simpleterms[0]**2 - simpleterms[1]**2).subs(varsubsinv)),othersolvedvars+[var0,var1]).subs(varsubs)
denoms = [fraction(simpleterms[0])[1], fraction(simpleterms[1])[1], fraction(complexterms[0])[1], fraction(complexterms[1])[1]]
lcmvars = self.pvars+unknownvars
for othersolvedvar in othersolvedvars:
lcmvars += self.Variable(othersolvedvar).vars
denomlcm = Poly(S.One,*lcmvars)
for denom in denoms:
if denom != S.One:
checkforzeros.append(self.removecommonexprs(denom,onlygcd=False,onlynumbers=True))
denomlcm = Poly(lcm(denomlcm,denom),*lcmvars)
finaleq = simplify(finaleq*denomlcm.as_expr()**2)
complementvarindex = varindex-(varindex%2)+((varindex+1)%2)
complementvar = unknownvars[complementvarindex]
finaleq = simplify(finaleq.subs(complementvar**2,1-unknownvar**2)).subs(allsymbols).expand()
else:
# try to reduce finaleq
p0 = Poly(simpleterms[0],unknownvars[varindex],unknownvars[varindex+1])
p1 = Poly(simpleterms[1],unknownvars[varindex],unknownvars[varindex+1])
if max(p0.degree_list()) > 1 and max(p1.degree_list()) > 1 and max(p0.degree_list()) == max(p1.degree_list()) and p0.LM() == p1.LM():
finaleq = (p0*p1.LC()-p1*p0.LC()).as_expr()
finaleq = expand(simplify(finaleq.subs(allsymbols)))
if finaleq == S.Zero:
finaleq = expand(p0.as_expr().subs(allsymbols))
if finaleq is None:
log.warn('solvePairVariables: did not compute a final variable. This is a weird condition...')
return self.solvePairVariablesHalfAngle(raweqns,var0,var1,othersolvedvars)
if not self.isValidSolution(finaleq):
log.warn('failed to solve pairwise equation: %s'%str(finaleq))
return self.solvePairVariablesHalfAngle(raweqns,var0,var1,othersolvedvars)
newunknownvars = unknownvars[:]
newunknownvars.remove(unknownvar)
if finaleq.has(*newunknownvars):
log.warn('equation relies on unsolved variables(%s): %s',newunknownvars,finaleq)
return self.solvePairVariablesHalfAngle(raweqns,var0,var1,othersolvedvars)
if not finaleq.has(unknownvar):
# somehow removed all variables, so try the general method
return self.solvePairVariablesHalfAngle(raweqns,var0,var1,othersolvedvars)
try:
if self.codeComplexity(finaleq) > 100000:
return self.solvePairVariablesHalfAngle(raweqns,var0,var1,othersolvedvars)
except self.CannotSolveError:
pass
if useconic:
# conic roots solver not as robust as half-angle transform!
#return [SolverConicRoots(var.name,[finaleq],isHinge=self.isHinge(var.name))]
solution = self.solveHighDegreeEquationsHalfAngle([finaleq],varsym)
solution.checkforzeros += checkforzeros
return [solution]
# now that everything is with respect to one variable, simplify and solve the equation
eqnew, symbols = self.groupTerms(finaleq, unknownvars, symbolgen)
allsymbols += symbols
solutions=solve(eqnew,unknownvar)
log.info('pair solution: %s, %s', eqnew,solutions)
if solutions:
solversolution=AST.SolverSolution(var.name, isHinge=self.isHinge(var.name))
if (varindex%2)==0:
solversolution.jointevalcos=[self.trigsimp(s.subs(allsymbols+varsubsinv),othersolvedvars).subs(varsubs) for s in solutions]
else:
solversolution.jointevalsin=[self.trigsimp(s.subs(allsymbols+varsubsinv),othersolvedvars).subs(varsubs) for s in solutions]
return [solversolution]
return self.solvePairVariablesHalfAngle(raweqns,var0,var1,othersolvedvars)
#raise self.CannotSolveError('cannot solve pair equation')
## SymPy helper routines
@staticmethod
[ドキュメント] def isValidSolution(expr):
"""return true if solution does not contain any nan or inf terms"""
if expr.is_number:
e=expr.evalf()
if e.has(I) or isinf(e) or isnan(e):
return False
return True
if expr.is_Mul:
# first multiply all numbers
number = S.One
for arg in expr.args:
if arg.is_number:
number *= arg
elif not IKFastSolver.isValidSolution(arg):
return False
# finally evalute the multiplied form
return IKFastSolver.isValidSolution(number.evalf())
for arg in expr.args:
if not IKFastSolver.isValidSolution(arg):
return False
return True
@staticmethod
def _GetSumSquares(expr):
"""if expr is a sum of squares, returns the list of individual expressions that were squared. otherwise returns None
"""
if not expr.is_Add:
if expr.is_Pow and expr.args[1].is_number and expr.args[1] > 0 and (expr.args[1]%2) == 0:
return [expr.args[0]]
return None
values = []
for arg in expr.args:
if not arg.is_Pow or not arg.args[1].is_number or not arg.args[1] > 0 or (arg.args[1]%2) != 0:
return None
values.append(arg.args[0])
return values
@staticmethod
[ドキュメント] def recursiveFraction(expr):
if expr.is_Add:
allpoly = []
finaldenom = S.One
for arg in expr.args:
n,d = IKFastSolver.recursiveFraction(arg)
finaldenom = finaldenom*d
allpoly.append([n,d])
finalnum = S.Zero
for n,d in allpoly:
finalnum += n*(finaldenom/d)
return finalnum,finaldenom
elif expr.is_Mul:
finalnum = S.One
finaldenom = S.One
for arg in expr.args:
n,d = IKFastSolver.recursiveFraction(arg)
finalnum = finalnum * n
finaldenom = finaldenom * d
return finalnum,finaldenom
elif expr.is_Pow and expr.exp.is_number:
n,d=IKFastSolver.recursiveFraction(expr.base)
if expr.exp < 0:
exponent = -expr.exp
n,d = d,n
else:
exponent = expr.exp
return n**exponent,d**exponent
else:
return fraction(expr)
@staticmethod
[ドキュメント] def groupTerms(expr,vars,symbolgen = None):
"""Separates all terms that do have var in them"""
if symbolgen is None:
symbolgen = cse_main.numbered_symbols('const')
symbols = []
p = Poly(expr,*vars)
newexpr = S.Zero
for m,c in p.terms():
# make huge numbers into constants too
if (c.is_number and len(str(c)) > 40) or (not c.is_number and not c.is_Symbol):
# if it is a product of a symbol and a number, then ignore
if not c.is_Mul or not all([e.is_number or e.is_Symbol for e in c.args]):
sym = symbolgen.next()
symbols.append((sym,c))
c = sym
if __builtin__.sum(m) == 0:
newexpr += c
else:
for i,degree in enumerate(m):
c = c*vars[i]**degree
newexpr += c
return newexpr,symbols
@staticmethod
[ドキュメント] def replaceNumbers(expr,symbolgen = None):
"""Replaces all numbers with symbols, this is to make gcd faster when fractions get too big"""
if symbolgen is None:
symbolgen = cse_main.numbered_symbols('const')
symbols = []
if expr.is_number:
result = symbolgen.next()
symbols.append((result,expr))
elif expr.is_Mul:
result = S.One
for arg in expr.args:
newresult, newsymbols = IKFastSolver.replaceNumbers(arg,symbolgen)
result *= newresult
symbols += newsymbols
elif expr.is_Add:
result = S.Zero
for arg in expr.args:
newresult, newsymbols = IKFastSolver.replaceNumbers(arg,symbolgen)
result += newresult
symbols += newsymbols
elif expr.is_Pow:
# don't replace the exponent
newresult, newsymbols = IKFastSolver.replaceNumbers(expr.base,symbolgen)
symbols += newsymbols
result = newresult**expr.exp
else:
result = expr
return result,symbols
@staticmethod
[ドキュメント] def frontnumbers(eq):
if eq.is_Number:
return [eq]
if eq.is_Mul:
n = []
for arg in eq.args:
n += IKFastSolver.frontnumbers(arg)
return n
return []
@staticmethod
[ドキュメント] def removecommonexprs(eq,returncommon=False,onlygcd=False,onlynumbers=True):
"""removes common expressions from a sum. Assumes all the coefficients are rationals. For example:
a*c_0 + a*c_1 + a*c_2 = 0
will return in
c_0 + c_1 + c_2 = 0
"""
eq = eq.expand() # doesn't work otherwise
if eq.is_Add:
exprs = eq.args
totaldenom = S.One
common = S.One
if onlynumbers:
for i in range(len(exprs)):
denom = S.One
for d in IKFastSolver.frontnumbers(fraction(exprs[i])[1]):
denom *= d
if denom != S.One:
exprs = [expr*denom for expr in exprs]
totaldenom *= denom
if onlygcd:
common = None
for i in range(len(exprs)):
coeff = S.One
for n in IKFastSolver.frontnumbers(exprs[i]):
coeff *= n
if common == None:
common = coeff
else:
common = igcd(common,coeff)
if common == S.One:
break
else:
for i in range(len(exprs)):
denom = fraction(exprs[i])[1]
if denom != S.One:
exprs = [expr*denom for expr in exprs]
totaldenom *= denom
# there are no fractions, so can start simplifying
common = exprs[0]/fraction(cancel(exprs[0]/exprs[1]))[0]
for i in range(2,len(exprs)):
common = common/fraction(cancel(common/exprs[i]))[0]
if common.is_number:
common=S.One
# find the smallest number and divide by it
if not onlygcd:
smallestnumber = None
for expr in exprs:
if expr.is_number:
if smallestnumber is None or smallestnumber > Abs(expr):
smallestnumber = Abs(expr)
elif expr.is_Mul:
n = S.One
for arg in expr.args:
if arg.is_number:
n *= arg
if smallestnumber is None or smallestnumber > Abs(n):
smallestnumber = Abs(n)
if smallestnumber is not None:
common = common*smallestnumber
eq = S.Zero
for expr in exprs:
eq += expr/common
if returncommon:
return eq,common/totaldenom
elif eq.is_Mul:
coeff = S.One
for d in IKFastSolver.frontnumbers(eq):
coeff *= d
if returncommon:
return eq/coeff,coeff
return eq/coeff
if returncommon:
return eq,S.One
return eq
# def det_bareis(M,*vars,**kwargs):
# return M.det_bareis()
@staticmethod
[ドキュメント] def det_bareis(M,*vars,**kwargs):
"""Function from sympy with a couple of improvements.
Compute matrix determinant using Bareis' fraction-free
algorithm which is an extension of the well known Gaussian
elimination method. This approach is best suited for dense
symbolic matrices and will result in a determinant with
minimal number of fractions. It means that less term
rewriting is needed on resulting formulae.
TODO: Implement algorithm for sparse matrices (SFF).
Function from sympy/matrices/matrices.py
"""
if not M.is_square:
raise NonSquareMatrixException()
n = M.rows
M = M[:,:] # make a copy
if n == 1:
det = M[0, 0]
elif n == 2:
det = M[0, 0]*M[1, 1] - M[0, 1]*M[1, 0]
else:
sign = 1 # track current sign in case of column swap
for k in range(n-1):
# look for a pivot in the current column
# and assume det == 0 if none is found
if M[k, k] == 0:
for i in range(k+1, n):
if M[i, k] != 0:
M.row_swap(i, k)
sign *= -1
break
else:
return S.Zero
# proceed with Bareis' fraction-free (FF)
# form of Gaussian elimination algorithm
for i in range(k+1, n):
for j in range(k+1, n):
D = M[k, k]*M[i, j] - M[i, k]*M[k, j]
if k > 0:
if len(vars) > 0 and D != S.Zero and not M[k-1, k-1].is_number:
try:
D,r = div(Poly(D,*vars),M[k-1, k-1])
except UnificationFailed:
log.warn('unification failed, trying direct division')
D /= M[k-1, k-1]
else:
D /= M[k-1, k-1]
if D.is_Atom:
M[i, j] = D
else:
if len(vars) > 0:
M[i, j] = D
else:
M[i, j] = Poly.cancel(D)
det = sign * M[n-1, n-1]
return det.expand()
@staticmethod
[ドキュメント] def sequence_cross_product(*sequences):
"""iterates through the cross product of all items in the sequences"""
# visualize an odometer, with "wheels" displaying "digits"...:
wheels = map(iter, sequences)
digits = [it.next( ) for it in wheels]
while True:
yield tuple(digits)
for i in range(len(digits)-1, -1, -1):
try:
digits[i] = wheels[i].next( )
break
except StopIteration:
wheels[i] = iter(sequences[i])
digits[i] = wheels[i].next( )
else:
break
@staticmethod
[ドキュメント] def tolatex(e):
s = printing.latex(e)
s1 = re.sub('\\\\operatorname\{(sin|cos)\}\\\\left\(j_\{(\d)\}\\\\right\)','\g<1>_\g<2>',s)
s2 = re.sub('1\.(0*)([^0-9])','1\g<2>',s1)
s3 = re.sub('1 \\\\(sin|cos)','\g<1>',s2)
s4 = re.sub('(\d*)\.([0-9]*[1-9])(0*)([^0-9])','\g<1>.\g<2>\g<4>',s3)
s5 = re.sub('sj_','s_',s4)
s5 = re.sub('cj_','c_',s5)
s5 = re.sub('sin','s',s5)
s5 = re.sub('cos','c',s5)
replacements = [('px','p_x'),('py','p_y'),('pz','p_z'),('r00','r_{00}'),('r01','r_{01}'),('r02','r_{02}'),('r10','r_{10}'),('r11','r_{11}'),('r12','r_{12}'),('r20','r_{20}'),('r21','r_{21}'),('r022','r_{22}')]
for old,new in replacements:
s5 = re.sub(old,new,s5)
return s5
@staticmethod
[ドキュメント] def GetSolvers():
"""Returns a dictionary of all the supported solvers and their official identifier names"""
return {'transform6d':IKFastSolver.solveFullIK_6D,
'rotation3d':IKFastSolver.solveFullIK_Rotation3D,
'translation3d':IKFastSolver.solveFullIK_Translation3D,
'direction3d':IKFastSolver.solveFullIK_Direction3D,
'ray4d':IKFastSolver.solveFullIK_Ray4D,
'lookat3d':IKFastSolver.solveFullIK_Lookat3D,
'translationdirection5d':IKFastSolver.solveFullIK_TranslationDirection5D,
'translationxy2d':IKFastSolver.solveFullIK_TranslationXY2D,
'translationxyorientation3d':IKFastSolver.solveFullIK_TranslationXYOrientation3D,
'translationxaxisangle4d':IKFastSolver.solveFullIK_TranslationAxisAngle4D,
'translationyaxisangle4d':IKFastSolver.solveFullIK_TranslationAxisAngle4D,
'translationzaxisangle4d':IKFastSolver.solveFullIK_TranslationAxisAngle4D,
'translationxaxisangleznorm4d':IKFastSolver.solveFullIK_TranslationAxisAngle4D,
'translationyaxisanglexnorm4d':IKFastSolver.solveFullIK_TranslationAxisAngle4D,
'translationzaxisangleynorm4d':IKFastSolver.solveFullIK_TranslationAxisAngle4D
}
if __name__ == '__main__':
import openravepy
parser = OptionParser(description="""IKFast: The Robot Kinematics Compiler
Software License Agreement (Lesser GPL v3).
Copyright (C) 2009-2011 Rosen Diankov.
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.
IKFast is part of OpenRAVE. This program can be used with robots or kinbodies defined and is independent of the OpenRAVE databases.
Example usage for 7 DOF Barrett WAM where the 3rd joint is a free parameter:
python ikfast.py --robot=robots/barrettwam.robot.xml --baselink=0 --eelink=7 --savefile=ik.cpp --freeindex=2
""",version=__version__)
parser.add_option('--robot', action='store', type='string', dest='robot',default=None,
help='robot file (COLLADA or OpenRAVE XML)')
parser.add_option('--savefile', action='store', type='string', dest='savefile',default='ik.cpp',
help='filename where to store the generated c++ code')
parser.add_option('--baselink', action='store', type='int', dest='baselink',
help='base link index to start extraction of ik chain')
parser.add_option('--eelink', action='store', type='int', dest='eelink',
help='end effector link index to end extraction of ik chain')
parser.add_option('--freeindex', action='append', type='int', dest='freeindices',default=[],
help='Optional joint index specifying a free parameter of the manipulator. If not specified, assumes all joints not solving for are free parameters. Can be specified multiple times for multiple free parameters.')
parser.add_option('--iktype', action='store', dest='iktype',default='transform6d',
help='The iktype to generate the ik for. Possible values are: %s'%(', '.join(name for name,fn in IKFastSolver.GetSolvers().iteritems())))
parser.add_option('--lang', action='store',type='string',dest='lang',default='cpp',
help='The language to generate the code in (default=%default), available=('+','.join(name for name,value in CodeGenerators.iteritems())+')')
parser.add_option('--debug','-d', action='store', type='int',dest='debug',default=logging.INFO,
help='Debug level for python nose (smaller values allow more text).')
(options, args) = parser.parse_args()
if options.robot is None or options.baselink is None or options.eelink is None:
print('Error: Not all arguments specified')
sys.exit(1)
format = logging.Formatter('%(levelname)s: %(message)s')
handler = logging.StreamHandler(sys.stdout)
handler.setFormatter(format)
log.addHandler(handler)
log.setLevel(options.debug)
solvefn=IKFastSolver.GetSolvers()[options.iktype]
if options.robot is not None:
try:
env=openravepy.Environment()
kinbody=env.ReadRobotXMLFile(options.robot)
env.Add(kinbody)
solver = IKFastSolver(kinbody,kinbody)
chaintree = solver.generateIkSolver(options.baselink,options.eelink,options.freeindices,solvefn=solvefn)
code=solver.writeIkSolver(chaintree,lang=options.lang)
finally:
openravepy.RaveDestroy()
if len(code) > 0:
open(options.savefile,'w').write(code)