#!/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/>.
"""generates C++ code from the IKFastSolver AST.
"""
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')
import sys, copy, time, datetime
try:
from openravepy.metaclass import AutoReloader
except:
class AutoReloader:
pass
# import the correct iktypes from openravepy (if present)
try:
from openravepy import IkParameterization
IkType = IkParameterization.Type
except:
class IkType:
Transform6D=0x67000001
Rotation3D=0x34000002
Translation3D=0x33000003
Direction3D=0x23000004
Ray4D=0x46000005
Lookat3D=0x23000006
TranslationDirection5D=0x56000007
TranslationXY2D=0x22000008
TranslationXYOrientation3D=0x33000009
TranslationLocalGlobal6D=0x3600000a
TranslationXAxisAngle4D=0x4400000b
TranslationYAxisAngle4D=0x4400000c
TranslationZAxisAngle4D=0x4400000d
TranslationXAxisAngleZNorm4D=0x4400000e
TranslationYAxisAngleXNorm4D=0x4400000f
TranslationZAxisAngleYNorm4D=0x44000010
from sympy import *
try:
import re # for indenting
except ImportError:
pass
try:
from itertools import izip, combinations
except ImportError:
def combinations(items,n):
if n == 0: yield[]
else:
for i in xrange(len(items)):
for cc in combinations(items[i+1:],n-1):
yield [items[i]]+cc
import logging
log = logging.getLogger('ikfast')
from sympy.core import function # for sympy 0.7.1+
[ドキュメント]class fmod(function.Function):
nargs = 2
is_real = True
is_Function = True
[ドキュメント]class atan2check(atan2):
nargs = 2
is_real = True
is_Function = True
[ドキュメント]def evalNumbers(expr):
"""Replaces all numbers with symbols, this is to make gcd faster when fractions get too big"""
if expr.is_number:
return expr.evalf()
elif expr.is_Mul:
result = S.One
for arg in expr.args:
result *= evalNumbers(arg)
elif expr.is_Add:
# because the arguments can get to the thousands, do a tree for adding numbers
evalexprs = [evalNumbers(arg) for arg in expr.args]
N = len(evalexprs)
while N > 1:
for i in range(N/2):
evalexprs[2*i]+=evalexprs[2*i+1]
evalexprs[i] = evalexprs[2*i]
if N & 1:
evalexprs[N/2] = evalexprs[N-1]
N += 1
N /= 2
return evalexprs[0]
elif expr.is_Pow:
# don't replace the exponent
result = evalNumbers(expr.base)**expr.exp
elif expr.is_Function:
args = [evalNumbers(arg) for arg in expr.args]
return expr.func(*args)
else:
result = expr
return result
[ドキュメント]def customcse(rawexprs,symbols=None):
if not hasattr(rawexprs,'__iter__') and not hasattr(rawexprs,'__array__'):
rawexprs = [rawexprs]
if symbols is None:
symbols = cse_main.numbered_symbols('x')
# fractions can get big, so evaluate as many decimals as possible
reduced_exprs = []
allexprs = []
for iexpr,expr in enumerate(rawexprs):
evalexpr = evalNumbers(expr)
complexity = evalexpr.count_ops()
# need to threshold complexity or otherwise cse will not terminate
if complexity > 300:
reduced_exprs.append(evalexpr)
else:
allexprs.append(evalexpr)
reduced_exprs.append(None)
newreplacements = []
if len(allexprs)>0:
replacements,reduced_exprs2 = cse(allexprs,symbols=symbols)
# have to maintain the same order
for expr in reduced_exprs2:
for i in range(len(reduced_exprs)):
if reduced_exprs[i] is None:
reduced_exprs[i] = expr
break
assert(all([expr is not None for expr in reduced_exprs]))
# look for any expressions of the order of (x**(1/a))**b, usually computer wants x^(b/a)
for r in replacements:
newr = r[1]
if newr.is_Pow and newr.exp.is_number and newr.base.is_Symbol:
baseexpr = newr.base.subs(replacements)
if baseexpr.is_Pow and baseexpr.exp.is_number:
newreplacements.append((r[0],baseexpr.base**(newr.exp*baseexpr.exp)))
continue
newreplacements.append((r[0],newr))
return newreplacements,reduced_exprs
[ドキュメント]class CodeGenerator(AutoReloader):
"""Generates C++ code from an AST generated by IKFastSolver.
"""
def __init__(self,kinematicshash='',version='0'):
self.symbolgen = cse_main.numbered_symbols('x')
self.strprinter = printing.StrPrinter()
self.freevars = None # list of free variables in the solution
self.freevardependencies = None # list of variables depending on the free variables
self.functions = dict()
self.kinematicshash=kinematicshash
self.resetequations() # dictionary of symbols already written
self.version=version
[ドキュメント] def resetequations(self):
self.dictequations = [[],[]]
[ドキュメント] def copyequations(self,dictequations=None):
if dictequations is None:
dictequations=self.dictequations
return [copy.copy(dictequations[0]),copy.copy(dictequations[1])]
[ドキュメント] def generate(self, solvertree):
code = """/// autogenerated analytical inverse kinematics code from ikfast program part of OpenRAVE
/// \\author Rosen Diankov
///
/// Licensed under the Apache License, Version 2.0 (the "License");
/// you may not use this file except in compliance with the License.
/// You may obtain a copy of the License at
/// http://www.apache.org/licenses/LICENSE-2.0
///
/// Unless required by applicable law or agreed to in writing, software
/// distributed under the License is distributed on an "AS IS" BASIS,
/// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
/// See the License for the specific language governing permissions and
/// limitations under the License.
///
/// ikfast version %s generated on %s
/// To compile with gcc:
/// gcc -lstdc++ ik.cpp
/// To compile without any main function as a shared object (might need -llapack):
/// gcc -fPIC -lstdc++ -DIKFAST_NO_MAIN -DIKFAST_CLIBRARY -shared -Wl,-soname,libik.so -o libik.so ik.cpp
#define IKFAST_HAS_LIBRARY
#include "ikfast.h" // found inside share/openrave-X.Y/python/ikfast.h
using namespace ikfast;
// check if the included ikfast version matches what this file was compiled with
#define IKFAST_COMPILE_ASSERT(x) extern int __dummy[(int)x]
IKFAST_COMPILE_ASSERT(IKFAST_VERSION==%s);
#include <cmath>
#include <vector>
#include <limits>
#include <algorithm>
#include <complex>
#define IKFAST_STRINGIZE2(s) #s
#define IKFAST_STRINGIZE(s) IKFAST_STRINGIZE2(s)
#ifndef IKFAST_ASSERT
#include <stdexcept>
#include <sstream>
#include <iostream>
#ifdef _MSC_VER
#ifndef __PRETTY_FUNCTION__
#define __PRETTY_FUNCTION__ __FUNCDNAME__
#endif
#endif
#ifndef __PRETTY_FUNCTION__
#define __PRETTY_FUNCTION__ __func__
#endif
#define IKFAST_ASSERT(b) { if( !(b) ) { std::stringstream ss; ss << "ikfast exception: " << __FILE__ << ":" << __LINE__ << ": " <<__PRETTY_FUNCTION__ << ": Assertion '" << #b << "' failed"; throw std::runtime_error(ss.str()); } }
#endif
#if defined(_MSC_VER)
#define IKFAST_ALIGNED16(x) __declspec(align(16)) x
#else
#define IKFAST_ALIGNED16(x) x __attribute((aligned(16)))
#endif
#define IK2PI ((IkReal)6.28318530717959)
#define IKPI ((IkReal)3.14159265358979)
#define IKPI_2 ((IkReal)1.57079632679490)
#ifdef _MSC_VER
#ifndef isnan
#define isnan _isnan
#endif
#endif // _MSC_VER
// lapack routines
extern "C" {
void dgetrf_ (const int* m, const int* n, double* a, const int* lda, int* ipiv, int* info);
void zgetrf_ (const int* m, const int* n, std::complex<double>* a, const int* lda, int* ipiv, int* info);
void dgetri_(const int* n, const double* a, const int* lda, int* ipiv, double* work, const int* lwork, int* info);
void dgesv_ (const int* n, const int* nrhs, double* a, const int* lda, int* ipiv, double* b, const int* ldb, int* info);
void dgetrs_(const char *trans, const int *n, const int *nrhs, double *a, const int *lda, int *ipiv, double *b, const int *ldb, int *info);
void dgeev_(const char *jobvl, const char *jobvr, const int *n, double *a, const int *lda, double *wr, double *wi,double *vl, const int *ldvl, double *vr, const int *ldvr, double *work, const int *lwork, int *info);
}
using namespace std; // necessary to get std math routines
#ifdef IKFAST_NAMESPACE
namespace IKFAST_NAMESPACE {
#endif
inline float IKabs(float f) { return fabsf(f); }
inline double IKabs(double f) { return fabs(f); }
inline float IKsqr(float f) { return f*f; }
inline double IKsqr(double f) { return f*f; }
inline float IKlog(float f) { return logf(f); }
inline double IKlog(double f) { return log(f); }
// allows asin and acos to exceed 1
#ifndef IKFAST_SINCOS_THRESH
#define IKFAST_SINCOS_THRESH ((IkReal)0.000001)
#endif
// used to check input to atan2 for degenerate cases
#ifndef IKFAST_ATAN2_MAGTHRESH
#define IKFAST_ATAN2_MAGTHRESH ((IkReal)2e-6)
#endif
// minimum distance of separate solutions
#ifndef IKFAST_SOLUTION_THRESH
#define IKFAST_SOLUTION_THRESH ((IkReal)1e-6)
#endif
inline float IKasin(float f)
{
IKFAST_ASSERT( f > -1-IKFAST_SINCOS_THRESH && f < 1+IKFAST_SINCOS_THRESH ); // any more error implies something is wrong with the solver
if( f <= -1 ) return float(-IKPI_2);
else if( f >= 1 ) return float(IKPI_2);
return asinf(f);
}
inline double IKasin(double f)
{
IKFAST_ASSERT( f > -1-IKFAST_SINCOS_THRESH && f < 1+IKFAST_SINCOS_THRESH ); // any more error implies something is wrong with the solver
if( f <= -1 ) return -IKPI_2;
else if( f >= 1 ) return IKPI_2;
return asin(f);
}
// return positive value in [0,y)
inline float IKfmod(float x, float y)
{
while(x < 0) {
x += y;
}
return fmodf(x,y);
}
// return positive value in [0,y)
inline double IKfmod(double x, double y)
{
while(x < 0) {
x += y;
}
return fmod(x,y);
}
inline float IKacos(float f)
{
IKFAST_ASSERT( f > -1-IKFAST_SINCOS_THRESH && f < 1+IKFAST_SINCOS_THRESH ); // any more error implies something is wrong with the solver
if( f <= -1 ) return float(IKPI);
else if( f >= 1 ) return float(0);
return acosf(f);
}
inline double IKacos(double f)
{
IKFAST_ASSERT( f > -1-IKFAST_SINCOS_THRESH && f < 1+IKFAST_SINCOS_THRESH ); // any more error implies something is wrong with the solver
if( f <= -1 ) return IKPI;
else if( f >= 1 ) return 0;
return acos(f);
}
inline float IKsin(float f) { return sinf(f); }
inline double IKsin(double f) { return sin(f); }
inline float IKcos(float f) { return cosf(f); }
inline double IKcos(double f) { return cos(f); }
inline float IKtan(float f) { return tanf(f); }
inline double IKtan(double f) { return tan(f); }
inline float IKsqrt(float f) { if( f <= 0.0f ) return 0.0f; return sqrtf(f); }
inline double IKsqrt(double f) { if( f <= 0.0 ) return 0.0; return sqrt(f); }
inline float IKatan2(float fy, float fx) {
if( isnan(fy) ) {
IKFAST_ASSERT(!isnan(fx)); // if both are nan, probably wrong value will be returned
return float(IKPI_2);
}
else if( isnan(fx) ) {
return 0;
}
return atan2f(fy,fx);
}
inline double IKatan2(double fy, double fx) {
if( isnan(fy) ) {
IKFAST_ASSERT(!isnan(fx)); // if both are nan, probably wrong value will be returned
return IKPI_2;
}
else if( isnan(fx) ) {
return 0;
}
return atan2(fy,fx);
}
inline float IKsign(float f) {
if( f > 0 ) {
return float(1);
}
else if( f < 0 ) {
return float(-1);
}
return 0;
}
inline double IKsign(double f) {
if( f > 0 ) {
return 1.0;
}
else if( f < 0 ) {
return -1.0;
}
return 0;
}
"""%(self.version,str(datetime.datetime.now()),self.version)
code += solvertree.generate(self)
code += solvertree.end(self)
code += """
/// solves the inverse kinematics equations.
/// \param pfree is an array specifying the free joints of the chain.
IKFAST_API bool ComputeIk(const IkReal* eetrans, const IkReal* eerot, const IkReal* pfree, IkSolutionListBase<IkReal>& solutions) {
IKSolver solver;
return solver.ComputeIk(eetrans,eerot,pfree,solutions);
}
IKFAST_API const char* GetKinematicsHash() { return "%s"; }
IKFAST_API const char* GetIkFastVersion() { return IKFAST_STRINGIZE(IKFAST_VERSION); }
#ifdef IKFAST_NAMESPACE
} // end namespace
#endif
"""%(self.kinematicshash)
code += """
#ifndef IKFAST_NO_MAIN
#include <stdio.h>
#include <stdlib.h>
#ifdef IKFAST_NAMESPACE
using namespace IKFAST_NAMESPACE;
#endif
int main(int argc, char** argv)
{
if( argc != 12+GetNumFreeParameters()+1 ) {
printf("\\nUsage: ./ik r00 r01 r02 t0 r10 r11 r12 t1 r20 r21 r22 t2 free0 ...\\n\\n"
"Returns the ik solutions given the transformation of the end effector specified by\\n"
"a 3x3 rotation R (rXX), and a 3x1 translation (tX).\\n"
"There are %d free parameters that have to be specified.\\n\\n",GetNumFreeParameters());
return 1;
}
IkSolutionList<IkReal> solutions;
std::vector<IkReal> vfree(GetNumFreeParameters());
IkReal eerot[9],eetrans[3];
eerot[0] = atof(argv[1]); eerot[1] = atof(argv[2]); eerot[2] = atof(argv[3]); eetrans[0] = atof(argv[4]);
eerot[3] = atof(argv[5]); eerot[4] = atof(argv[6]); eerot[5] = atof(argv[7]); eetrans[1] = atof(argv[8]);
eerot[6] = atof(argv[9]); eerot[7] = atof(argv[10]); eerot[8] = atof(argv[11]); eetrans[2] = atof(argv[12]);
for(std::size_t i = 0; i < vfree.size(); ++i)
vfree[i] = atof(argv[13+i]);
bool bSuccess = ComputeIk(eetrans, eerot, vfree.size() > 0 ? &vfree[0] : NULL, solutions);
if( !bSuccess ) {
fprintf(stderr,"Failed to get ik solution\\n");
return -1;
}
printf("Found %d ik solutions:\\n", (int)solutions.GetNumSolutions());
std::vector<IkReal> solvalues(GetNumJoints());
for(std::size_t i = 0; i < solutions.GetNumSolutions(); ++i) {
const IkSolutionBase<IkReal>& sol = solutions.GetSolution(i);
printf("sol%d (free=%d): ", (int)i, (int)sol.GetFree().size());
std::vector<IkReal> vsolfree(sol.GetFree().size());
sol.GetSolution(&solvalues[0],vsolfree.size()>0?&vsolfree[0]:NULL);
for( std::size_t j = 0; j < solvalues.size(); ++j)
printf("%.15f, ", solvalues[j]);
printf("\\n");
}
return 0;
}
#endif
"""
return code
[ドキュメント] def getClassInit(self,node,iktype,userotation=7,usetranslation=7):
code = "IKFAST_API int GetNumFreeParameters() { return %d; }\n"%len(node.freejointvars)
if len(node.freejointvars) == 0:
code += "IKFAST_API int* GetFreeParameters() { return NULL; }\n"
else:
code += "IKFAST_API int* GetFreeParameters() { static int freeparams[] = {"
for i,freejointvar in enumerate(node.freejointvars):
code += "%d"%(freejointvar[1])
if i < len(node.freejointvars)-1:
code += ", "
code += "}; return freeparams; }\n"
code += "IKFAST_API int GetNumJoints() { return %d; }\n\n"%(len(node.freejointvars)+len(node.solvejointvars))
code += "IKFAST_API int GetIkRealSize() { return sizeof(IkReal); }\n\n"
code += 'IKFAST_API int GetIkType() { return 0x%x; }\n\n'%iktype
code += "class IKSolver {\npublic:\n"
usedvars = []
for var in node.solvejointvars:
usedvars += [var[0].name,'c'+var[0].name,'s'+var[0].name,'ht'+var[0].name]
for i in range(len(node.freejointvars)):
name = node.freejointvars[i][0].name
usedvars += [name,'c'+name,'s'+name,'ht'+name]
for i in range(3):
if userotation & (1<<i):
for j in range(3):
usedvars += ['new_r%d%d'%(i,j), 'r%d%d'%(i,j), 'rxp%d_%d'%(i,j)]
if usetranslation & 1:
usedvars += ['new_px', 'px', 'npx']
if usetranslation & 2:
usedvars += ['new_py', 'py', 'npy']
if usetranslation & 4:
usedvars += ['new_pz', 'pz', 'npz']
if usetranslation ==7:
usedvars.append('pp')
code += 'IkReal ' + ','.join(usedvars) + ';\n'
code += 'unsigned char ' + ','.join('_i%s[2], _n%s'%(var[0].name,var[0].name) for var in node.solvejointvars+node.freejointvars) + ';\n\n'
return code
[ドキュメント] def GetIkFunctionPreamble(self, node):
code = "bool ComputeIk(const IkReal* eetrans, const IkReal* eerot, const IkReal* pfree, IkSolutionListBase<IkReal>& solutions) {\n"
for var in node.solvejointvars:
code += '%s=numeric_limits<IkReal>::quiet_NaN(); _i%s[0] = -1; _i%s[1] = -1; _n%s = -1; '%(var[0].name,var[0].name,var[0].name,var[0].name)
for i in range(len(node.freejointvars)):
name = node.freejointvars[i][0].name
code += ' _i%s[0] = -1; _i%s[1] = -1; _n%s = 0; '%(name,name,name)
code += "\nfor(int dummyiter = 0; dummyiter < 1; ++dummyiter) {\n"
code += " solutions.Clear();\n"
return code
[ドキュメント] def getFKFunctionPreamble(self):
code = "/// solves the forward kinematics equations.\n"
code += "/// \\param pfree is an array specifying the free joints of the chain.\n"
code += "IKFAST_API void ComputeFk(const IkReal* j, IkReal* eetrans, IkReal* eerot) {\n"
return code
[ドキュメント] def generateChain(self, node):
self.freevars = []
self.freevardependencies = []
self.resetequations()
self.symbolgen = cse_main.numbered_symbols('x')
code = ''
if node.Tfk:
code += self.getFKFunctionPreamble()
allvars = node.solvejointvars + node.freejointvars
subexprs,reduced_exprs=customcse (node.Tfk[0:3,0:4].subs([(v[0],Symbol('j[%d]'%v[1])) for v in allvars]),self.symbolgen)
outputnames = ['eerot[0]','eerot[1]','eerot[2]','eetrans[0]','eerot[3]','eerot[4]','eerot[5]','eetrans[1]','eerot[6]','eerot[7]','eerot[8]','eetrans[2]']
fcode = ''
if len(subexprs) > 0:
vars = [var for var,expr in subexprs]
fcode = 'IkReal ' + ','.join(str(var) for var,expr in subexprs) + ';\n'
for var,expr in subexprs:
fcode += self.writeEquations(lambda k: str(var),collect(expr,vars))
for i in range(len(outputnames)):
fcode += self.writeEquations(lambda k: outputnames[i],reduced_exprs[i])
code += self.indentCode(fcode,4)
code += '}\n\n'
code += self.getClassInit(node,IkType.Transform6D)
code += self.GetIkFunctionPreamble(node)
fcode = ''
for i in range(len(node.freejointvars)):
name = node.freejointvars[i][0].name
fcode += '%s=pfree[%d]; c%s=cos(pfree[%d]); s%s=sin(pfree[%d]);\n'%(name,i,name,i,name,i)
for i in range(3):
for j in range(3):
fcode += "r%d%d = eerot[%d*3+%d];\n"%(i,j,i,j)
fcode += "px = eetrans[0]; py = eetrans[1]; pz = eetrans[2];\n\n"
psymbols = ["new_px","new_py","new_pz"]
for i in range(3):
for j in range(3):
fcode += self.writeEquations(lambda k: "new_r%d%d"%(i,j),node.Tee[4*i+j].evalf())
fcode += self.writeEquations(lambda k: psymbols[i],node.Tee[4*i+3].evalf())
for i in range(3):
for j in range(3):
fcode += "r%d%d = new_r%d%d; "%(i,j,i,j)
fcode += "px = new_px; py = new_py; pz = new_pz;\n"
if node.dictequations is not None:
for var,value in node.dictequations:
fcode += self.writeEquations(lambda k: var,value)
fcode += self.generateTree(node.jointtree)
code += self.indentCode(fcode,4) + "}\nreturn solutions.GetNumSolutions()>0;\n}\n"
# write other functions
for name,functioncode in self.functions.iteritems():
code += self.indentCode(functioncode,4)
code += "};\n"
return code
[ドキュメント] def endChain(self, node):
return ""
[ドキュメント] def generateIKChainRotation3D(self, node):
self.freevars = []
self.freevardependencies = []
self.resetequations()
self.symbolgen = cse_main.numbered_symbols('x')
code = ''
if node.Rfk:
code += self.getFKFunctionPreamble()
allvars = node.solvejointvars + node.freejointvars
subexprs,reduced_exprs=customcse (node.Rfk[0:3,0:3].subs([(v[0],Symbol('j[%d]'%v[1])) for v in allvars]),self.symbolgen)
outputnames = ['eerot[0]','eerot[1]','eerot[2]','eerot[3]','eerot[4]','eerot[5]','eerot[6]','eerot[7]','eerot[8]']
fcode = ''
if len(subexprs) > 0:
vars = [var for var,expr in subexprs]
fcode = 'IkReal ' + ','.join(str(var) for var,expr in subexprs) + ';\n'
for var,expr in subexprs:
fcode += self.writeEquations(lambda k: str(var),collect(expr,vars))
for i in range(len(outputnames)):
fcode += self.writeEquations(lambda k: outputnames[i],reduced_exprs[i])
code += self.indentCode(fcode,4)
code += '}\n\n'
code += self.getClassInit(node,IkType.Rotation3D,usetranslation=0)
code += self.GetIkFunctionPreamble(node)
fcode = ''
for i in range(len(node.freejointvars)):
name = node.freejointvars[i][0].name
fcode += '%s=pfree[%d]; c%s=cos(pfree[%d]); s%s=sin(pfree[%d]);\n'%(name,i,name,i,name,i)
for i in range(3):
for j in range(3):
fcode += "r%d%d = eerot[%d*3+%d];\n"%(i,j,i,j)
for i in range(3):
for j in range(3):
fcode += self.writeEquations(lambda k: "new_r%d%d"%(i,j),node.Ree[i,j].evalf())
for i in range(3):
for j in range(3):
fcode += "r%d%d = new_r%d%d; "%(i,j,i,j)
fcode += '\n'
if node.dictequations is not None:
for var,value in node.dictequations:
fcode += self.writeEquations(lambda k: var,value)
fcode += self.generateTree(node.jointtree)
code += self.indentCode(fcode,4) + "}\nreturn solutions.GetNumSolutions()>0;\n}\n"
# write other functions
for name,functioncode in self.functions.iteritems():
code += self.indentCode(functioncode,4)
code += "};\n"
return code
[ドキュメント] def endIKChainRotation3D(self, node):
return ""
[ドキュメント] def generateIKChainTranslation3D(self, node):
self.freevars = []
self.freevardependencies = []
self.resetequations()
self.symbolgen = cse_main.numbered_symbols('x')
code = ''
if node.Pfk:
code += self.getFKFunctionPreamble()
allvars = node.solvejointvars + node.freejointvars
allsubs = [(v[0],Symbol('j[%d]'%v[1])) for v in allvars]
eqs = []
for eq in node.Pfk[0:3]:
eqs.append(eq.subs(allsubs))
subexprs,reduced_exprs=customcse (eqs,self.symbolgen)
outputnames = ['eetrans[0]','eetrans[1]','eetrans[2]']
if node.uselocaltrans:
fcode = """
// necessary for local/global translation3d
eerot[0] = eerot[4] = eerot[8] = 0;
IkReal r00 = 0, r11 = 0, r22 = 0;
"""
else:
fcode = ''
if len(subexprs) > 0:
vars = [var for var,expr in subexprs]
fcode += 'IkReal ' + ','.join(str(var) for var,expr in subexprs) + ';\n'
for var,expr in subexprs:
fcode += self.writeEquations(lambda k: str(var),collect(expr,vars))
for i in range(len(outputnames)):
fcode += self.writeEquations(lambda k: outputnames[i],reduced_exprs[i])
code += self.indentCode(fcode,4)
code += '}\n\n'
if node.uselocaltrans:
code += self.getClassInit(node,IkType.TranslationLocalGlobal6D,userotation=7)
else:
code += self.getClassInit(node,IkType.Translation3D,userotation=0)
code += self.GetIkFunctionPreamble(node)
fcode = ''
for i in range(len(node.freejointvars)):
name = node.freejointvars[i][0].name
fcode += '%s=pfree[%d]; c%s=cos(pfree[%d]); s%s=sin(pfree[%d]);\n'%(name,i,name,i,name,i)
if node.uselocaltrans:
for i in range(3):
fcode += "r%d%d = eerot[%d];\n"%(i,i,4*i)
fcode += "px = eetrans[0]; py = eetrans[1]; pz = eetrans[2];\n\n"
psymbols = ["new_px","new_py","new_pz"]
for i in range(3):
fcode += self.writeEquations(lambda k: psymbols[i],node.Pee[i].evalf())
fcode += "px = new_px; py = new_py; pz = new_pz;\n"
if node.dictequations is not None:
for var,value in node.dictequations:
fcode += self.writeEquations(lambda k: var,value)
fcode += self.generateTree(node.jointtree)
code += self.indentCode(fcode,4) + "}\nreturn solutions.GetNumSolutions()>0;\n}\n"
# write other functions
for name,functioncode in self.functions.iteritems():
code += self.indentCode(functioncode,4)
code += "};\n"
return code
[ドキュメント] def endIKChainTranslation3D(self, node):
return ""
[ドキュメント] def generateIKChainTranslationXY2D(self, node):
self.freevars = []
self.freevardependencies = []
self.resetequations()
self.symbolgen = cse_main.numbered_symbols('x')
code = ''
if node.Pfk:
code += self.getFKFunctionPreamble()
allvars = node.solvejointvars + node.freejointvars
allsubs = [(v[0],Symbol('j[%d]'%v[1])) for v in allvars]
eqs = []
for eq in node.Pfk[0:2]:
eqs.append(eq.subs(allsubs))
subexprs,reduced_exprs=customcse (eqs,self.symbolgen)
outputnames = ['eetrans[0]','eetrans[1]']
fcode = ''
if len(subexprs) > 0:
vars = [var for var,expr in subexprs]
fcode = 'IkReal ' + ','.join(str(var) for var,expr in subexprs) + ';\n'
for var,expr in subexprs:
fcode += self.writeEquations(lambda k: str(var),collect(expr,vars))
for i in range(len(outputnames)):
fcode += self.writeEquations(lambda k: outputnames[i],reduced_exprs[i])
code += self.indentCode(fcode,4)
code += '}\n\n'
code += self.getClassInit(node,IkType.TranslationXY2D,userotation=0,usetranslation=3)
code += self.GetIkFunctionPreamble(node)
fcode = ''
for i in range(len(node.freejointvars)):
name = node.freejointvars[i][0].name
fcode += '%s=pfree[%d]; c%s=cos(pfree[%d]); s%s=sin(pfree[%d]);\n'%(name,i,name,i,name,i)
fcode += "px = eetrans[0]; py = eetrans[1];\n\n"
psymbols = ["new_px","new_py"]
for i in range(2):
fcode += self.writeEquations(lambda k: psymbols[i],node.Pee[i].evalf())
fcode += "px = new_px; py = new_py;\n"
if node.dictequations is not None:
for var,value in node.dictequations:
fcode += self.writeEquations(lambda k: var,value)
fcode += self.generateTree(node.jointtree)
code += self.indentCode(fcode,4) + "}\nreturn solutions.GetNumSolutions()>0;\n}\n"
# write other functions
for name,functioncode in self.functions.iteritems():
code += self.indentCode(functioncode,4)
code += "};\n"
return code
[ドキュメント] def endIKChainTranslationXY2D(self, node):
return ""
[ドキュメント] def generateIKChainDirection3D(self, node):
self.freevars = []
self.freevardependencies = []
self.resetequations()
self.symbolgen = cse_main.numbered_symbols('x')
code = ''
if node.Dfk:
code += self.getFKFunctionPreamble()
allvars = node.solvejointvars + node.freejointvars
allsubs = [(v[0],Symbol('j[%d]'%v[1])) for v in allvars]
eqs = []
for eq in node.Dfk:
eqs.append(eq.subs(allsubs))
subexprs,reduced_exprs=customcse (eqs,self.symbolgen)
outputnames = ['eerot[0]','eerot[1]','eerot[2]']
fcode = ''
if len(subexprs) > 0:
vars = [var for var,expr in subexprs]
fcode = 'IkReal ' + ','.join(str(var) for var,expr in subexprs) + ';\n'
for var,expr in subexprs:
fcode += self.writeEquations(lambda k: str(var),collect(expr,vars))
for i in range(len(outputnames)):
fcode += self.writeEquations(lambda k: outputnames[i],reduced_exprs[i])
code += self.indentCode(fcode,4)
code += '}\n\n'
code += self.getClassInit(node,IkType.Direction3D,userotation=1,usetranslation=0)
code += self.GetIkFunctionPreamble(node)
fcode = ''
for i in range(len(node.freejointvars)):
name = node.freejointvars[i][0].name
fcode += '%s=pfree[%d]; c%s=cos(pfree[%d]); s%s=sin(pfree[%d]);\n'%(name,i,name,i,name,i)
for i in range(3):
fcode += "r0%d = eerot[%d];\n"%(i,i)
for i in range(3):
fcode += self.writeEquations(lambda k: "new_r%d%d"%(0,i),node.Dee[i].evalf())
for i in range(3):
fcode += "r0%d = new_r0%d; "%(i,i)
if node.dictequations is not None:
for var,value in node.dictequations:
fcode += self.writeEquations(lambda k: var,value)
fcode += self.generateTree(node.jointtree)
code += self.indentCode(fcode,4) + "}\nreturn solutions.GetNumSolutions()>0;\n}\n"
# write other functions
for name,functioncode in self.functions.iteritems():
code += self.indentCode(functioncode,4)
code += "};\n"
return code
[ドキュメント] def endIKChainDirection3D(self, node):
return ''
[ドキュメント] def generateIKChainRay(self, node):
self.freevars = []
self.freevardependencies = []
self.resetequations()
self.symbolgen = cse_main.numbered_symbols('x')
code = ''
if node.Dfk and node.Pfk:
code += self.getFKFunctionPreamble()
allvars = node.solvejointvars + node.freejointvars
allsubs = [(v[0],Symbol('j[%d]'%v[1])) for v in allvars]
eqs = []
for eq in node.Pfk[0:3]:
eqs.append(eq.subs(allsubs))
for eq in node.Dfk[0:3]:
eqs.append(eq.subs(allsubs))
subexprs,reduced_exprs=customcse (eqs,self.symbolgen)
outputnames = ['eetrans[0]','eetrans[1]','eetrans[2]','eerot[0]','eerot[1]','eerot[2]']
fcode = ''
if len(subexprs) > 0:
vars = [var for var,expr in subexprs]
fcode = 'IkReal ' + ','.join(str(var) for var,expr in subexprs) + ';\n'
for var,expr in subexprs:
fcode += self.writeEquations(lambda k: str(var),collect(expr,vars))
for i in range(len(outputnames)):
fcode += self.writeEquations(lambda k: outputnames[i],reduced_exprs[i])
code += self.indentCode(fcode,4)
code += '}\n\n'
code += self.getClassInit(node,IkType.TranslationDirection5D if node.is5dray else IkType.Ray4D,userotation=1)
code += self.GetIkFunctionPreamble(node)
fcode = "px = eetrans[0]; py = eetrans[1]; pz = eetrans[2];\n\n"
for i in range(len(node.freejointvars)):
name = node.freejointvars[i][0].name
fcode += '%s=pfree[%d]; c%s=cos(pfree[%d]); s%s=sin(pfree[%d]);\n'%(name,i,name,i,name,i)
for i in range(3):
fcode += "r0%d = eerot[%d];\n"%(i,i)
fcode += "px = eetrans[0]; py = eetrans[1]; pz = eetrans[2];\n"
psymbols = ["new_px","new_py","new_pz"]
for i in range(3):
fcode += self.writeEquations(lambda k: "new_r%d%d"%(0,i),node.Dee[i].evalf())
fcode += self.writeEquations(lambda k: psymbols[i],node.Pee[i].evalf())
for i in range(3):
fcode += "r0%d = new_r0%d; "%(i,i)
if node.is5dray:
fcode += "px = new_px; py = new_py; pz = new_pz;\n\n"
else:
fcode += "\nIkReal new_pdotd = new_px*new_r00+new_py*new_r01+new_pz*new_r02;\n"
fcode += "px = new_px-new_pdotd * new_r00; py = new_py- new_pdotd * new_r01; pz = new_pz - new_pdotd * new_r02;\n\n"
if node.dictequations is not None:
for var,value in node.dictequations:
fcode += self.writeEquations(lambda k: var,value)
fcode += self.generateTree(node.jointtree)
code += self.indentCode(fcode,4) + "}\nreturn solutions.GetNumSolutions()>0;\n}\n"
# write other functions
for name,functioncode in self.functions.iteritems():
code += self.indentCode(functioncode,4)
code += "};\n"
return code
[ドキュメント] def endIKChainRay(self, node):
return ''
[ドキュメント] def generateIKChainLookat3D(self, node):
self.freevars = []
self.freevardependencies = []
self.resetequations()
self.symbolgen = cse_main.numbered_symbols('x')
code = ''
if node.Dfk and node.Pfk:
code += self.getFKFunctionPreamble()
allvars = node.solvejointvars + node.freejointvars
allsubs = [(v[0],Symbol('j[%d]'%v[1])) for v in allvars]
eqs = []
for eq in node.Pfk[0:3]:
eqs.append(eq.subs(allsubs))
for eq in node.Dfk[0:3]:
eqs.append(eq.subs(allsubs))
subexprs,reduced_exprs=customcse (eqs,self.symbolgen)
outputnames = ['eetrans[0]','eetrans[1]','eetrans[2]','eerot[0]','eerot[1]','eerot[2]']
fcode = ''
if len(subexprs) > 0:
vars = [var for var,expr in subexprs]
fcode = 'IkReal ' + ','.join(str(var) for var,expr in subexprs) + ';\n'
for var,expr in subexprs:
fcode += self.writeEquations(lambda k: str(var),collect(expr,vars))
for i in range(len(outputnames)):
fcode += self.writeEquations(lambda k: outputnames[i],reduced_exprs[i])
code += self.indentCode(fcode,4)
code += '}\n\n'
code += self.getClassInit(node,IkType.Lookat3D,userotation=0)
code += self.GetIkFunctionPreamble(node)
fcode = "px = eetrans[0]; py = eetrans[1]; pz = eetrans[2];\n\n"
for i in range(len(node.freejointvars)):
name = node.freejointvars[i][0].name
fcode += '%s=pfree[%d]; c%s=cos(pfree[%d]); s%s=sin(pfree[%d]);\n'%(name,i,name,i,name,i)
psymbols = ["new_px","new_py","new_pz"]
for i in range(3):
fcode += self.writeEquations(lambda k: psymbols[i],node.Pee[i].evalf())
fcode += "px = new_px; py = new_py; pz = new_pz;\n"
if node.dictequations is not None:
for var,value in node.dictequations:
fcode += self.writeEquations(lambda k: var,value)
fcode += self.generateTree(node.jointtree)
code += self.indentCode(fcode,4) + "}\nreturn solutions.GetNumSolutions()>0;\n}\n\n"
# write other functions
for name,functioncode in self.functions.iteritems():
code += self.indentCode(functioncode,4)
code += "};\n"
return code
[ドキュメント] def endIKChainLookat3D(self, node):
return ''
[ドキュメント] def generateSolverIKChainAxisAngle(self, node):
self.freevars = []
self.freevardependencies = []
self.resetequations()
self.symbolgen = cse_main.numbered_symbols('x')
code = ''
if node.anglefk and node.Pfk:
code += self.getFKFunctionPreamble()
code += "for(int dummyiter = 0; dummyiter < 1; ++dummyiter) {\n"
allvars = node.solvejointvars + node.freejointvars
allsubs = [(v[0],Symbol('j[%d]'%v[1])) for v in allvars]
eqs = []
for eq in node.Pfk[0:3]:
eqs.append(eq.subs(allsubs))
eqs.append(node.anglefk.subs(allsubs))
subexprs,reduced_exprs=customcse (eqs,self.symbolgen)
outputnames = ['eetrans[0]','eetrans[1]','eetrans[2]','eerot[0]']
fcode = ''
if len(subexprs) > 0:
vars = [var for var,expr in subexprs]
fcode = 'IkReal ' + ','.join(str(var) for var,expr in subexprs) + ';\n'
for var,expr in subexprs:
fcode += self.writeEquations(lambda k: str(var),collect(expr,vars))
for i in range(len(outputnames)):
fcode += self.writeEquations(lambda k: outputnames[i],reduced_exprs[i])
code += self.indentCode(fcode,4)
# if gets to the end of the for loop statement, return successfully. if exits for loop, assert(0)
code += 'return;\n}\nIKFAST_ASSERT(0);\n}\n\n'
code += self.getClassInit(node,node.iktype,userotation=1)
code += self.GetIkFunctionPreamble(node)
fcode = "px = eetrans[0]; py = eetrans[1]; pz = eetrans[2];\n\n"
for i in range(len(node.freejointvars)):
name = node.freejointvars[i][0].name
fcode += '%s=pfree[%d]; c%s=cos(pfree[%d]); s%s=sin(pfree[%d]);\n'%(name,i,name,i,name,i)
fcode += "r00 = eerot[0];\n"
fcode += "px = eetrans[0]; py = eetrans[1]; pz = eetrans[2];\n"
psymbols = ["new_px","new_py","new_pz"]
for i in range(3):
fcode += self.writeEquations(lambda k: psymbols[i],node.Pee[i].evalf())
fcode += self.writeEquations(lambda k: "new_r00",node.angleee.evalf())
fcode += "r00 = new_r00; "
fcode += "px = new_px; py = new_py; pz = new_pz;\n\n"
if node.dictequations is not None:
for var,value in node.dictequations:
fcode += self.writeEquations(lambda k: var,value)
fcode += self.generateTree(node.jointtree)
code += self.indentCode(fcode,4) + "}\nreturn solutions.GetNumSolutions()>0;\n}\n"
# write other functions
for name,functioncode in self.functions.iteritems():
code += self.indentCode(functioncode,4)
code += "};\n"
return code
[ドキュメント] def endSolverIKChainAxisAngle(self, node):
return ''
[ドキュメント] def generateSolution(self, node,declarearray=True,acceptfreevars=True):
"""writes the solution of one variable
:param declarearray: if False, will return the equations to be written without evaluating them. Used for conditioned solutions.
"""
code = ''
numsolutions = 0
eqcode = ''
name = node.jointname
node.HasFreeVar = False
allnumsolutions = 0
for var,value in node.dictequations:
eqcode += 'IkReal %s;\n'%var
eqcode += self.writeEquations(lambda k: var,value)
if node.jointeval is not None:
numsolutions = len(node.jointeval)
equations = []
names = []
for i,expr in enumerate(node.jointeval):
if acceptfreevars:
m = None
for freevar in self.freevars:
if expr.has(Symbol(freevar)):
# has free variables, so have to look for a*freevar+b form
a = Wild('a',exclude=[Symbol(freevar)])
b = Wild('b',exclude=[Symbol(freevar)])
m = expr.match(a*Symbol(freevar)+b)
if m is not None:
self.freevardependencies.append((freevar,name))
assert(len(node.jointeval)==1)
code += 'IkReal ' + self.writeEquations(lambda i: '%smul'%name, m[a])
code += self.writeEquations(lambda i: name, m[b])
node.HasFreeVar = True
return code
else:
log.error('failed to extract free variable %s for %s from: eq=%s', freevar,node.jointname, expr)
# m = dict()
# m[a] = Float(-1,30)
# m[b] = Float(0,30)
equations.append(expr)
names.append('%sarray[%d]'%(name,allnumsolutions+i))
equations.append(sin(Symbol('%sarray[%d]'%(name,allnumsolutions+i))))
names.append('s%sarray[%d]'%(name,allnumsolutions+i))
equations.append(cos(Symbol('%sarray[%d]'%(name,allnumsolutions+i))))
names.append('c%sarray[%d]'%(name,allnumsolutions+i))
eqcode += self.writeEquations(lambda i: names[i], equations)
if node.AddPiIfNegativeEq:
for i in range(numsolutions):
eqcode += '%sarray[%d] = %sarray[%d] > 0 ? %sarray[%d]-IKPI : %sarray[%d]+IKPI;\n'%(name,allnumsolutions+numsolutions+i,name,allnumsolutions+i,name,allnumsolutions+i,name,allnumsolutions+i)
eqcode += 's%sarray[%d] = -s%sarray[%d];\n'%(name,allnumsolutions+numsolutions+i,name,allnumsolutions+i)
eqcode += 'c%sarray[%d] = -c%sarray[%d];\n'%(name,allnumsolutions+numsolutions+i,name,allnumsolutions+i)
numsolutions *= 2
for i in range(numsolutions):
if node.isHinge:
eqcode += 'if( %sarray[%d] > IKPI )\n{\n %sarray[%d]-=IK2PI;\n}\nelse if( %sarray[%d] < -IKPI )\n{ %sarray[%d]+=IK2PI;\n}\n'%(name,allnumsolutions+i,name,allnumsolutions+i,name,allnumsolutions+i,name,allnumsolutions+i)
eqcode += '%svalid[%d] = true;\n'%(name,allnumsolutions+i)
allnumsolutions += numsolutions
# might also have cos solutions ...
if node.jointevalcos is not None:
numsolutions = 2*len(node.jointevalcos)
eqcode += self.writeEquations(lambda i: 'c%sarray[%d]'%(name,allnumsolutions+2*i),node.jointevalcos)
for i in range(len(node.jointevalcos)):
eqcode += 'if( c%sarray[%d] >= -1-IKFAST_SINCOS_THRESH && c%sarray[%d] <= 1+IKFAST_SINCOS_THRESH )\n{\n'%(name,allnumsolutions+2*i,name,allnumsolutions+2*i)
eqcode += ' %svalid[%d] = %svalid[%d] = true;\n'%(name,allnumsolutions+2*i,name,allnumsolutions+2*i+1)
eqcode += ' %sarray[%d] = IKacos(c%sarray[%d]);\n'%(name,allnumsolutions+2*i,name,allnumsolutions+2*i)
eqcode += ' s%sarray[%d] = IKsin(%sarray[%d]);\n'%(name,allnumsolutions+2*i,name,allnumsolutions+2*i)
# second solution
eqcode += ' c%sarray[%d] = c%sarray[%d];\n'%(name,allnumsolutions+2*i+1,name,allnumsolutions+2*i)
eqcode += ' %sarray[%d] = -%sarray[%d];\n'%(name,allnumsolutions+2*i+1,name,allnumsolutions+2*i)
eqcode += ' s%sarray[%d] = -s%sarray[%d];\n'%(name,allnumsolutions+2*i+1,name,allnumsolutions+2*i)
eqcode += '}\n'
eqcode += 'else if( isnan(c%sarray[%d]) )\n{\n'%(name,allnumsolutions+2*i)
eqcode += ' // probably any value will work\n'
eqcode += ' %svalid[%d] = true;\n'%(name,allnumsolutions+2*i)
eqcode += ' c%sarray[%d] = 1; s%sarray[%d] = 0; %sarray[%d] = 0;\n'%(name,allnumsolutions+2*i,name,allnumsolutions+2*i,name,allnumsolutions+2*i)
eqcode += '}\n'
allnumsolutions += numsolutions
if node.jointevalsin is not None:
numsolutions = 2*len(node.jointevalsin)
eqcode += self.writeEquations(lambda i: 's%sarray[%d]'%(name,allnumsolutions+2*i),node.jointevalsin)
for i in range(len(node.jointevalsin)):
eqcode += 'if( s%sarray[%d] >= -1-IKFAST_SINCOS_THRESH && s%sarray[%d] <= 1+IKFAST_SINCOS_THRESH )\n{\n'%(name,allnumsolutions+2*i,name,allnumsolutions+2*i)
eqcode += ' %svalid[%d] = %svalid[%d] = true;\n'%(name,allnumsolutions+2*i,name,allnumsolutions+2*i+1)
eqcode += ' %sarray[%d] = IKasin(s%sarray[%d]);\n'%(name,allnumsolutions+2*i,name,allnumsolutions+2*i)
eqcode += ' c%sarray[%d] = IKcos(%sarray[%d]);\n'%(name,allnumsolutions+2*i,name,allnumsolutions+2*i)
# second solution
eqcode += ' s%sarray[%d] = s%sarray[%d];\n'%(name,allnumsolutions+2*i+1,name,allnumsolutions+2*i)
eqcode += ' %sarray[%d] = %sarray[%d] > 0 ? (IKPI-%sarray[%d]) : (-IKPI-%sarray[%d]);\n'%(name,allnumsolutions+2*i+1,name,allnumsolutions+2*i,name,allnumsolutions+2*i,name,allnumsolutions+2*i)
eqcode += ' c%sarray[%d] = -c%sarray[%d];\n'%(name,allnumsolutions+2*i+1,name,allnumsolutions+2*i)
eqcode += '}\n'
eqcode += 'else if( isnan(s%sarray[%d]) )\n{\n'%(name,allnumsolutions+2*i)
eqcode += ' // probably any value will work\n'
eqcode += ' %svalid[%d] = true;\n'%(name,allnumsolutions+2*i)
eqcode += ' c%sarray[%d] = 1; s%sarray[%d] = 0; %sarray[%d] = 0;\n'%(name,allnumsolutions+2*i,name,allnumsolutions+2*i,name,allnumsolutions+2*i)
eqcode += '}\n'
allnumsolutions += numsolutions
if not declarearray:
return eqcode,allnumsolutions
code += '{\nIkReal %sarray[%d], c%sarray[%d], s%sarray[%d];\n'%(name,allnumsolutions,name,allnumsolutions,name,allnumsolutions)
code += 'bool %svalid[%d]={false};\n'%(name,allnumsolutions)
code += '_n%s = %d;\n'%(name,allnumsolutions)
code += eqcode
if allnumsolutions > 1:
if allnumsolutions >= 256:
log.error('num solutions is %d>=256, which exceeds unsigned char',allnumsolutions)
code += 'for(int i%s = 0; i%s < %d; ++i%s)\n{\n'%(name,name,allnumsolutions,name)
code += 'if( !%svalid[i%s] )\n{\n continue;\n}\n'%(name,name)
code += '_i%s[0] = i%s; _i%s[1] = -1;\n'%(name,name,name)
# check for a similar solution
code += 'for(int ii%s = i%s+1; ii%s < %d; ++ii%s)\n{\n'%(name,name,name,allnumsolutions,name)
code += 'if( %svalid[ii%s] && IKabs(c%sarray[i%s]-c%sarray[ii%s]) < IKFAST_SOLUTION_THRESH && IKabs(s%sarray[i%s]-s%sarray[ii%s]) < IKFAST_SOLUTION_THRESH )\n{\n %svalid[ii%s]=false; _i%s[1] = ii%s; break; \n}\n'%(name,name,name,name,name,name,name,name,name,name,name,name,name,name)
code += '}\n'
code += '%s = %sarray[i%s]; c%s = c%sarray[i%s]; s%s = s%sarray[i%s];\n'%(name,name,name,name,name,name,name,name,name)
if node.AddHalfTanValue:
code += 'ht%s = IKtan(%s/2);\n'%(name,name)
if node.getEquationsUsed() is not None and len(node.getEquationsUsed()) > 0:
code += '{\nIkReal evalcond[%d];\n'%len(node.getEquationsUsed())
code += self.writeEquations(lambda i: 'evalcond[%d]'%(i),node.getEquationsUsed())
code += 'if( '
for i in range(len(node.getEquationsUsed())):
if i != 0:
code += ' || '
# using smaller node.thresh increases the missing solution rate, really not sure whether the solutions themselves
# are bad due to double precision arithmetic, or due to singularities
code += 'IKabs(evalcond[%d]) > 0.000001 '%(i)
code += ' )\n{\n'
#code += 'cout <<'
#code += '<<'.join(['evalcond[%d]'%i for i in range(len(node.getEquationsUsed()))])
#code += '<< endl;'
code += 'continue;\n}\n'
code += '}\n'
code += '\n'
return code
[ドキュメント] def endSolution(self, node):
if node.HasFreeVar:
self.freevardependencies.pop()
return ''
return '}\n}\n'
[ドキュメント] def generateConditionedSolution(self, node):
name=node.solversolutions[0].jointname
assert all([name == s.jointname for s in node.solversolutions])
origequations = self.copyequations()
maxchecks = max([len(s.checkforzeros) for s in node.solversolutions])
allnumsolutions = 0
AddHalfTanValue = False
checkcode = ''
for var,value in node.dictequations:
checkcode += 'IkReal %s;\n'%var
checkcode += self.writeEquations(lambda k: var,value)
for solversolution in node.solversolutions:
assert len(solversolution.checkforzeros) > 0
if solversolution.AddHalfTanValue:
AddHalfTanValue = True
self.dictequations = self.copyequations(origequations)
checkcode += '{\n'
checkcode += self.writeEquations(lambda i: 'evalcond[%d]'%(i),solversolution.checkforzeros)
checkcode += 'if( '
for i in range(len(solversolution.checkforzeros)):
if i != 0:
checkcode += ' && '
checkcode += 'IKabs(evalcond[%d]) %s %.16f '%(i,'<=' if solversolution.FeasibleIsZeros else '>',node.thresh)
checkcode += ' )\n{\n'
scode,numsolutions = self.generateSolution(solversolution,declarearray=False,acceptfreevars=False)
scode += 'numsolutions%s = %d;\n'%(name,numsolutions)
allnumsolutions = max(allnumsolutions,numsolutions)
checkcode += self.indentCode(scode,4)
checkcode += '\n} else\n'
checkcode += '{\n continue;\n}\n' # if got here, then current solution branch is not good, so skip
checkcode += '}\n'*len(node.solversolutions)
checkcode += 'if( numsolutions%s == 0 )\n{\n continue;\n}\n'%name
code = '{\nIkReal evalcond[%d]; int numsolutions%s = 0;\n'%(maxchecks,name)
code += 'IkReal %sarray[%d], c%sarray[%d], s%sarray[%d];\n'%(name,allnumsolutions,name,allnumsolutions,name,allnumsolutions)
code += 'bool %svalid[%d]={false};\n'%(name,allnumsolutions)
code += '_n%s = %d;\n'%(name,allnumsolutions)
code += self.indentCode(checkcode,4)
if allnumsolutions > 1:
if allnumsolutions >= 256:
log.error('num solutions is %d>=256, which exceeds unsigned char',allnumsolutions)
code += 'for(int i%s = 0; i%s < numsolutions%s; ++i%s)\n{\n'%(name,name,name,name)
code += 'if( !%svalid[i%s] )\n{\n continue;\n}\n'%(name,name)
code += '_i%s[0] = i%s; _i%s[1] = -1;\n'%(name,name,name)
# check for a similar solution
code += 'for(int ii%s = i%s+1; ii%s < numsolutions%s; ++ii%s)\n{\n'%(name,name,name,name,name)
code += 'if( %svalid[ii%s] && IKabs(c%sarray[i%s]-c%sarray[ii%s]) < IKFAST_SOLUTION_THRESH && IKabs(s%sarray[i%s]-s%sarray[ii%s]) < IKFAST_SOLUTION_THRESH )\n{\n %svalid[ii%s]=false; _i%s[1] = ii%s; break; \n}\n'%(name,name,name,name,name,name,name,name,name,name,name,name,name,name)
code += '}\n'
code += '%s = %sarray[i%s]; c%s = c%sarray[i%s]; s%s = s%sarray[i%s];\n\n'%(name,name,name,name,name,name,name,name,name)
if AddHalfTanValue:
code += 'ht%s = IKtan(%s/2);\n'%(name,name)
self.dictequations = origequations
return code
[ドキュメント] def endConditionedSolution(self, node):
return '}\n}\n'
[ドキュメント] def generatePolynomialRoots(self, node):
D=node.poly.degree(0)
polyroots=self.using_polyroots(D)
name = node.jointname
polyvar = node.poly.gens[0].name
code = 'IkReal op[%d+1], zeror[%d];\nint numroots;\n'%(D,D)
numevals = 0
if node.postcheckforzeros is not None:
numevals = max(numevals,len(node.postcheckforzeros))
if node.postcheckfornonzeros is not None:
numevals = max(numevals,len(node.postcheckfornonzeros))
if node.postcheckforrange is not None:
numevals = max(numevals,len(node.postcheckforrange))
if numevals > 0:
code += 'IkReal %sevalpoly[%d];\n'%(name,numevals)
for var,value in node.dictequations:
code += 'IkReal %s;\n'%var
code += self.writeEquations(lambda k: var,value)
polydict = node.poly.as_dict()
code += self.writeEquations(lambda i: 'op[%d]'%(i),[polydict.get((i,),S.Zero) for i in range(D,-1,-1)])
code += "%s(op,zeror,numroots);\n"%(polyroots)
code += 'IkReal %sarray[%d], c%sarray[%d], s%sarray[%d], temp%sarray[%d];\n'%(name,len(node.jointeval)*D,name,len(node.jointeval)*D,name,len(node.jointeval)*D,name,len(node.jointeval))
code += 'int numsolutions = 0;\n'
code += 'for(int i%s = 0; i%s < numroots; ++i%s)\n{\n'%(name,name,name)
fcode = 'IkReal %s = zeror[i%s];\n'%(polyvar,name)
origequations = self.copyequations()
fcode += self.writeEquations(lambda i: 'temp%sarray[%d]'%(name,i), node.jointeval)
self.dictequations = origequations
fcode += 'for(int k%s = 0; k%s < %d; ++k%s)\n{\n'%(name,name,len(node.jointeval),name)
fcode += '%sarray[numsolutions] = temp%sarray[k%s];\n'%(name,name,name)
if node.isHinge:
fcode += 'if( %sarray[numsolutions] > IKPI )\n{\n %sarray[numsolutions]-=IK2PI;\n}\nelse if( %sarray[numsolutions] < -IKPI )\n{\n %sarray[numsolutions]+=IK2PI;\n}\n'%(name,name,name,name)
fcode += 's%sarray[numsolutions] = IKsin(%sarray[numsolutions]);\n'%(name,name)
fcode += 'c%sarray[numsolutions] = IKcos(%sarray[numsolutions]);\n'%(name,name)
fcode += 'numsolutions++;\n'
fcode += '}\n'
code += self.indentCode(fcode,4)
code += '}\n'
allnumsolutions = D*len(node.jointeval)
if allnumsolutions >= 256:
log.error('num solutions is %d>=256, which exceeds unsigned char',allnumsolutions)
code += 'bool %svalid[%d]={%s};\n'%(name,allnumsolutions,','.join(['true']*allnumsolutions))
code += '_n%s = %d;\n'%(name,allnumsolutions)
code += 'for(int i%s = 0; i%s < numsolutions; ++i%s)\n {\n'%(name,name,name)
code += 'if( !%svalid[i%s] )\n{\n continue;\n}\n'%(name,name)
code += ' %s = %sarray[i%s]; c%s = c%sarray[i%s]; s%s = s%sarray[i%s];\n'%(name,name,name,name,name,name,name,name,name)
if node.AddHalfTanValue:
code += 'ht%s = IKtan(%s/2);\n'%(name,name)
code += '\n'
if node.postcheckforzeros is not None and len(node.postcheckforzeros) > 0:
fcode = self.writeEquations(lambda i: '%sevalpoly[%d]'%(name,i),node.postcheckforzeros)
fcode += 'if( '
for i in range(len(node.postcheckforzeros)):
if i != 0:
fcode += ' || '
fcode += 'IKabs(%sevalpoly[%d]) < %.16f '%(name,i,node.thresh)
fcode += ' )\n{\n continue;\n}\n'
code += self.indentCode(fcode,4)
if node.postcheckfornonzeros is not None and len(node.postcheckfornonzeros) > 0:
fcode = self.writeEquations(lambda i: '%sevalpoly[%d]'%(name,i),node.postcheckfornonzeros)
fcode += 'if( '
for i in range(len(node.postcheckfornonzeros)):
if i != 0:
fcode += ' || '
fcode += 'IKabs(%sevalpoly[%d]) > %.16f '%(name,i,node.thresh)
fcode += ' )\n{\n continue;\n}\n'
code += self.indentCode(fcode,4)
if node.postcheckforrange is not None and len(node.postcheckforrange) > 0:
fcode = self.writeEquations(lambda i: '%sevalpoly[%d]'%(name,i),node.postcheckforrange)
fcode += 'if( '
for i in range(len(node.postcheckforrange)):
if i != 0:
fcode += ' || '
fcode += ' (%sevalpoly[%d] < %.16f || %sevalpoly[%d] > %.16f) '%(name,i,-1.0-node.thresh,name,i,1.0+node.thresh)
fcode += ' )\n{\n continue;\n}\n'
code += self.indentCode(fcode,4)
# passed all tests
code += '_i%s[0] = i%s; _i%s[1] = -1;\n'%(name,name,name)
# check for a similar solution
code += 'for(int ii%s = i%s+1; ii%s < numsolutions; ++ii%s)\n{\n'%(name,name,name,name)
code += 'if( %svalid[ii%s] && IKabs(c%sarray[i%s]-c%sarray[ii%s]) < IKFAST_SOLUTION_THRESH && IKabs(s%sarray[i%s]-s%sarray[ii%s]) < IKFAST_SOLUTION_THRESH )\n{\n %svalid[ii%s]=false; _i%s[1] = ii%s; break; \n}\n'%(name,name,name,name,name,name,name,name,name,name,name,name,name,name)
code += '}\n'
return code
[ドキュメント] def endPolynomialRoots(self, node):
return ' }\n'
[ドキュメント] def generateCoeffFunction(self, node):
assert(len(node.jointnames) == len(node.jointeval))
firstname = node.jointnames[0]
if node.exportfnname == 'solvedialyticpoly16lep':
fnname=self.using_solvedialyticpoly16lep()
elif node.exportfnname == 'solvedialyticpoly12qep':
fnname=self.using_solvedialyticpoly12qep()
elif node.exportfnname == 'solvedialyticpoly8qep':
fnname=self.using_solvedialyticpoly8qep()
else:
fnname = 'unknownfn'
code = 'IkReal op[%d], zeror[%d];\nint numroots;\n'%(len(node.exportcoeffeqs),node.rootmaxdim*len(node.jointnames))
numevals = 0
code += self.writeEquations(lambda i: 'op[%d]'%(i),node.exportcoeffeqs)
code += "%s(op,zeror,numroots);\n"%(fnname)
code += 'IkReal '
for i,name in enumerate(node.jointnames):
code += '%sarray[%d], c%sarray[%d], s%sarray[%d]'%(name,node.rootmaxdim,name,node.rootmaxdim,name,node.rootmaxdim)
if i+1 < len(node.jointnames):
code += ', '
else:
code += ';\n'
code += 'int numsolutions = 0;\n'
code += 'for(int i%s = 0; i%s < numroots; i%s += %d)\n{\n'%(firstname,firstname,firstname,len(node.jointnames))
fcode = 'IkReal '
for i in range(len(node.exportvar)):
fcode += '%s = zeror[i%s+%d]'%(node.exportvar[i],firstname,i)
if i+1<len(node.exportvar):
fcode += ', '
else:
fcode += ';\n'
origequations = self.copyequations()
fcode += self.writeEquations(lambda i: '%sarray[numsolutions]'%(node.jointnames[i]), node.jointeval)
fcode += self.writeEquations(lambda i: 'c%sarray[numsolutions]'%(node.jointnames[i]), node.jointevalcos)
fcode += self.writeEquations(lambda i: 's%sarray[numsolutions]'%(node.jointnames[i]), node.jointevalsin)
self.dictequations = origequations
for i in range(len(node.jointnames)):
if node.isHinges[i]:
fcode += 'if( %sarray[numsolutions] > IKPI )\n{\n %sarray[numsolutions]-=IK2PI;\n}\nelse if( %sarray[numsolutions] < -IKPI )\n{\n %sarray[numsolutions]+=IK2PI;\n}\n'%(node.jointnames[i],node.jointnames[i],node.jointnames[i],node.jointnames[i])
fcode += 'numsolutions++;\n'
# fcode += 'bool valid = true;\n'
# # test all the solutions up to now for validity
# fcode += 'for( int k%s = 0; k%s < numsolutions; ++k%s)\n{\n'%(firstname,firstname,firstname)
# fcode += ' if( '
# for name in node.jointnames:
# fcode += 'IKabs(c%sarray[k%s]-c%sarray[numsolutions]) < IKFAST_SOLUTION_THRESH && IKabs(s%sarray[k%s]-s%sarray[numsolutions]) < IKFAST_SOLUTION_THRESH &&'%(name,firstname,name,name, firstname,name)
# fcode += ' 1 )\n {\n valid=false; break;\n }\n'
# fcode += '}\n'
# fcode += 'if( valid ) { numsolutions++; }\n'
fcode += '}\n'
code += self.indentCode(fcode,4)
code += 'bool %svalid[%d]={%s};\n'%(firstname,node.rootmaxdim,','.join(['true']*node.rootmaxdim))
code += '_n%s = %d;\n'%(firstname,node.rootmaxdim)
for name in node.jointnames[1:]:
code += '_n%s = 1;\n'%name
if node.rootmaxdim >= 256:
log.error('num solutions is %d>=256, which exceeds unsigned char',node.rootmaxdim)
code += 'for(int i%s = 0; i%s < numsolutions; ++i%s)\n {\n'%(firstname,firstname,firstname)
code += 'if( !%svalid[i%s] )\n{\n continue;\n}\n'%(firstname,firstname)
code += '_i%s[0] = i%s; _i%s[1] = -1;\n'%(firstname,firstname,firstname)
for name in node.jointnames[1:]:
code += '_i%s[0] = 0; _i%s[1] = -1;\n'%(name,name)
# check for a similar solution
code += 'for(int ii%s = i%s+1; ii%s < numsolutions; ++ii%s)\n{\n'%(firstname,firstname,firstname,firstname)
code += 'if( !%svalid[ii%s] ) { continue; }\n'%(firstname,firstname)
code += 'if( '
for name in node.jointnames:
code += 'IKabs(c%sarray[i%s]-c%sarray[ii%s]) < IKFAST_SOLUTION_THRESH && IKabs(s%sarray[i%s]-s%sarray[ii%s]) < IKFAST_SOLUTION_THRESH && '%(name,firstname,name,firstname,name,firstname,name,firstname)
code += ' 1 )\n{\n %svalid[ii%s]=false; '%(firstname,firstname)
code += '_i%s[1] = ii%s; '%(firstname,firstname)
for name in node.jointnames[1:]:
code += '_i%s[1] = 0; '%name
code += ' break; \n}\n'
code += '}\n'
for name in node.jointnames:
code += ' %s = %sarray[i%s]; c%s = c%sarray[i%s]; s%s = s%sarray[i%s];\n\n'%(name,name,firstname,name,name,firstname,name,name,firstname)
return code
[ドキュメント] def endCoeffFunction(self, node):
return ' }\n'
[ドキュメント] def generateMatrixInverse(self, node):
# lapack takes matrices in column order
assert( node.A.shape[0] == node.A.shape[1] )
matrixinverse=self.using_matrixinverse()
code = ''
# for some reason things work even if the determinant is 0....
# if len(node.checkforzeros) > 0:
# code = 'IkReal matrixcondition[%d];\n'%(len(node.checkforzeros))
# code += self.writeEquations(lambda i: 'matrixcondition[%d]'%(i),node.checkforzeros)
# code += 'if( '
# for i in range(len(node.checkforzeros)):
# if i != 0:
# code += ' || '
# code += 'IKabs(matrixcondition[%d]) < 1e-14 '%(i)
# code += ' )\n{ continue;\n}\n'
code += 'IkReal IKFAST_ALIGNED16(matrixinvcoeffs[%d]);\n'%(node.A.shape[0]*node.A.shape[1])
code += self.writeEquations(lambda i: 'matrixinvcoeffs[%d]'%(i),node.A.transpose()[:])
code += 'if( !%s<%d>(matrixinvcoeffs) ) {\ncontinue;\n}\n'%(matrixinverse,node.A.shape[0]);
# create the variables
mcode = ''
for i in range(len(node.Asymbols)):
for j in range(len(node.Asymbols[i])):
if node.Asymbols[i][j] is not None:
if len(mcode) > 0:
mcode += ', '
else:
mcode = 'IkReal '
mcode += '%s=matrixinvcoeffs[%d]'%(node.Asymbols[i][j],i+j*node.A.shape[0])
if len(mcode)> 0:
code += mcode + ';\n'
return code
[ドキュメント] def endMatrixInverse(self,node):
return ''
[ドキュメント] def generateBranchConds(self, node):
origequations = self.copyequations()
code = '{\n'
numevals = None
for branch in node.jointbranches:
if branch[0] is not None:
if numevals is None or numevals < len(branch[0]):
numevals=len(branch[0])
if numevals is not None:
code += 'IkReal evalcond[%d];\n'%numevals
for branch in node.jointbranches:
self.dictequations = self.copyequations(origequations)
if branch[0] is None:
branchcode = 'if( 1 )\n{\n'
else:
branchcode = self.writeEquations(lambda x: 'evalcond[%d]'%x,branch[0])
branchcode += 'if( '
for i in range(len(branch[0])):
if i != 0:
branchcode += ' && '
branchcode += 'IKabs(evalcond[%d]) < %.16f '%(i,node.thresh)
branchcode += ' )\n{\n'
for n in branch[1]:
branchcode += n.generate(self)
for n in reversed(branch[1]):
branchcode += n.end(self)
code += self.indentCode(branchcode,4)+'\n} else\n{\n'
code += '}\n'*(len(node.jointbranches)+1)
self.dictequations = origequations
return code
[ドキュメント] def endBranchConds(self, node):
return ''
[ドキュメント] def generateCheckZeros(self, node):
origequations = self.copyequations()
name = node.jointname if node.jointname is None else 'dummy'
code = 'IkReal %seval[%d];\n'%(name,len(node.jointcheckeqs))
for var,value in node.dictequations:
code += 'IkReal %s;\n'%var
code += self.writeEquations(lambda k: var,value)
code += self.writeEquations(lambda i: '%seval[%d]'%(name,i),node.jointcheckeqs)
if len(node.jointcheckeqs) > 0:
code += 'if( '
for i in range(len(node.jointcheckeqs)):
if i != 0:
if node.anycondition:
code += ' || '
else:
code += ' && '
code += 'IKabs(%seval[%d]) < %.16f '%(name,i,node.thresh)
code += ' )\n{\n'
self.dictequations = self.copyequations(origequations)
code += self.indentCode(self.generateTree(node.zerobranch),4)
code += '\n} else\n'
code += '{\n'
self.dictequations = self.copyequations(origequations)
code += self.indentCode(self.generateTree(node.nonzerobranch),4)
code += '\n}\n'
self.dictequations = origequations
return '{\n' + self.indentCode(code,4) + '\n}\n'
[ドキュメント] def endCheckZeros(self, node):
return ''
[ドキュメント] def generateFreeParameter(self, node):
#'free variable ',node.jointname,': ',self.freevars
self.freevars.append(node.jointname)
self.freevardependencies.append((node.jointname,node.jointname))
code = 'IkReal %smul = 1;\n%s=0;\n'%(node.jointname,node.jointname)
return code+self.generateTree(node.jointtree)
[ドキュメント] def endFreeParameter(self, node):
self.freevars.pop()
self.freevardependencies.pop()
return ''
[ドキュメント] def generateBreak(self,node):
return 'continue;\n'
[ドキュメント] def endBreak(self,node):
return ''
[ドキュメント] def generateRotation(self, node):
if not node.functionid in self.functions:
code = 'inline void rotationfunction%d(IkSolutionListBase<IkReal>& solutions) {\n'%(node.functionid)
code += 'for(int rotationiter = 0; rotationiter < 1; ++rotationiter) {\n'
origequations = self.dictequations
self.resetequations()
listequations = []
names = []
for i in range(3):
for j in range(3):
listequations.append(node.T[i,j])
names.append(Symbol('new_r%d%d'%(i,j)))
code += self.indentCode(self.writeEquations(lambda i: names[i],listequations),4)
code += self.indentCode(self.generateTree(node.jointtree),4)
code += '}\n}'
self.dictequations = origequations
self.functions[node.functionid] = code
return 'rotationfunction%d(solutions);\n'%(node.functionid)
[ドキュメント] def endRotation(self, node):
return ''
[ドキュメント] def generateDirection(self, node):
code = ''
listequations = []
names = []
for i in range(3):
listequations.append(node.D[i])
names.append(Symbol('new_r%d%d'%(0,i)))
code += self.writeEquations(lambda i: names[i],listequations)
code += self.generateTree(node.jointtree)
return code
[ドキュメント] def endDirection(self, node):
return ''
[ドキュメント] def generateStoreSolution(self, node):
code = ''
if node.checkgreaterzero is not None and len(node.checkgreaterzero) > 0:
origequations = self.copyequations()
code += 'IkReal soleval[%d];\n'%(len(node.checkgreaterzero))
code += self.writeEquations(lambda i: 'soleval[%d]'%(i),node.checkgreaterzero)
code += 'if( '
for i in range(len(node.checkgreaterzero)):
if i != 0:
code += ' && '
code += 'soleval[%d] > %.16f '%(i,node.thresh)
code += ' )\n'
self.dictequations = origequations
code += '{\n'
code += 'std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(%d);\n'%len(node.alljointvars)
for i,var in enumerate(node.alljointvars):
offsetvalue = '+%.15e'%node.offsetvalues[i] if node.offsetvalues is not None else ''
code += 'vinfos[%d].jointtype = %d;\n'%(i,0x01 if node.isHinge[i] else 0x11)
code += 'vinfos[%d].foffset = %s%s;\n'%(i,var,offsetvalue)
vardeps = [vardep for vardep in self.freevardependencies if vardep[1]==var.name]
if len(vardeps) > 0:
freevarname = vardeps[0][0]
ifreevar = [j for j in range(len(self.freevars)) if freevarname==self.freevars[j]]
code += 'vinfos[%d].fmul = %smul;\n'%(i,var.name)
code += 'vinfos[%d].freeind = %d;\n'%(i,ifreevar[0])
code += 'vinfos[%d].maxsolutions = 0;\n'%(i)
else:
code += 'vinfos[%d].indices[0] = _i%s[0];\n'%(i,var)
code += 'vinfos[%d].indices[1] = _i%s[1];\n'%(i,var)
code += 'vinfos[%d].maxsolutions = _n%s;\n'%(i,var)
code += 'std::vector<int> vfree(%d);\n'%len(self.freevars)
for i,varname in enumerate(self.freevars):
ind = [j for j in range(len(node.alljointvars)) if varname==node.alljointvars[j].name]
code += 'vfree[%d] = %d;\n'%(i,ind[0])
code += 'solutions.AddSolution(vinfos,vfree);\n'
code += '}\n'
return code
[ドキュメント] def endStoreSolution(self, node):
return ''
[ドキュメント] def generateSequence(self, node):
code = ''
for tree in node.jointtrees:
code += self.generateTree(tree)
return code
[ドキュメント] def endSequence(self, node):
return ''
[ドキュメント] def generateTree(self,tree):
code = ''
for n in tree:
code += n.generate(self)
for n in reversed(tree):
code += n.end(self)
return code
[ドキュメント] def writeEquations(self, varnamefn, allexprs):
if not hasattr(allexprs,'__iter__') and not hasattr(allexprs,'__array__'):
allexprs = [allexprs]
code = ''
# calling cse on many long expressions will freeze it, so try to divide the problem
complexity = [expr.count_ops() for expr in allexprs]
complexitythresh = 4000
exprs = []
curcomplexity = 0
for i,expr in enumerate(allexprs):
curcomplexity += complexity[i]
exprs.append(expr)
if curcomplexity > complexitythresh or i == len(allexprs)-1:
code += self._writeEquations(varnamefn,exprs,i+1-len(exprs))
exprs = []
curcomplexity = 0
assert(len(exprs)==0)
return code
def _writeEquations(self, varnamefn, exprs,ioffset):
code = ''
replacements,reduced_exprs = customcse(exprs,symbols=self.symbolgen)
N = len(self.dictequations[0])
for rep in replacements:
comparerep = rep[1].subs(self.dictequations[0]).expand()
found = False
complexity = rep[1].count_ops()
maxcomplexity = 3 if N > 1000 else 2
if complexity > maxcomplexity: # check only long expressions
for i in range(N):
if self.dictequations[1][i] is not None and comparerep-self.dictequations[1][i]==S.Zero:
#self.dictequations.append((rep[0],self.dictequations[0][i][0],self.dictequations[1][i]))
code += 'IkReal %s=%s;\n'%(rep[0],self.dictequations[0][i][0])
found = True
break
else:
comparerep = None
if not found:
self.dictequations[0].append(rep)
self.dictequations[1].append(comparerep)
code2,sepcode2 = self.writeExprCode(rep[1])
code += sepcode2+'IkReal %s=%s;\n'%(rep[0],code2)
for i,rexpr in enumerate(reduced_exprs):
code2,sepcode2 = self.writeExprCode(rexpr)
code += sepcode2+'%s=%s;\n'%(varnamefn(i+ioffset), code2)
return code
[ドキュメント] def writeExprCode(self, expr):
# go through all arguments and chop them
code = ''
sepcode = ''
if expr.is_Function:
if expr.func == Abs:
code += 'IKabs('
code2,sepcode = self.writeExprCode(expr.args[0])
code += code2
elif expr.func == sign:
code += 'IKsign('
code2,sepcode = self.writeExprCode(expr.args[0])
code += code2
elif expr.func == acos:
code += 'IKacos('
code2,sepcode = self.writeExprCode(expr.args[0])
code += code2
sepcode += 'if( (%s) < -1-IKFAST_SINCOS_THRESH || (%s) > 1+IKFAST_SINCOS_THRESH )\n continue;\n'%(code2,code2)
elif expr.func == asin:
code += 'IKasin('
code2,sepcode = self.writeExprCode(expr.args[0])
code += code2
sepcode += 'if( (%s) < -1-IKFAST_SINCOS_THRESH || (%s) > 1+IKFAST_SINCOS_THRESH )\n continue;\n'%(code2,code2)
elif expr.func == atan2check:
code += 'IKatan2('
# check for divides by 0 in arguments, this could give two possible solutions?!?
# if common arguments is nan! solution is lost!
code2,sepcode = self.writeExprCode(expr.args[0])
code += code2+', '
code3,sepcode2 = self.writeExprCode(expr.args[1])
code += code3
sepcode += sepcode2
sepcode += 'if( IKabs(%s) < IKFAST_ATAN2_MAGTHRESH && IKabs(%s) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr(%s)+IKsqr(%s)-1) <= IKFAST_SINCOS_THRESH )\n continue;\n'%(code2,code3,code2,code3)
elif expr.func == atan2:
code += 'IKatan2('
# check for divides by 0 in arguments, this could give two possible solutions?!?
# if common arguments is nan! solution is lost!
code2,sepcode = self.writeExprCode(expr.args[0])
code += code2+', '
code3,sepcode2 = self.writeExprCode(expr.args[1])
code += code3
sepcode += sepcode2
sepcode += 'if( IKabs(%s) < IKFAST_ATAN2_MAGTHRESH && IKabs(%s) < IKFAST_ATAN2_MAGTHRESH )\n continue;\n'%(code2,code3)
elif expr.func == sin:
# if expr.args[0].is_Symbol and expr.args[0].name[0] == 'j':
# # probably already have initialized
# code += '(s%s'%expr.args[0].name
# else:
code += 'IKsin('
code2,sepcode = self.writeExprCode(expr.args[0])
code += code2
elif expr.func == cos:
# if expr.args[0].is_Symbol and expr.args[0].name[0] == 'j':
# # probably already have initialized
# code += '(c%s'%expr.args[0].name
# else:
code += 'IKcos('
code2,sepcode = self.writeExprCode(expr.args[0])
code += code2
elif expr.func == fmod:
code += 'IKfmod('
code2,sepcode = self.writeExprCode(expr.args[0])
code += code2+', '
code3,sepcode2 = self.writeExprCode(expr.args[1])
code += code3
sepcode += sepcode2
else:
code += expr.func.__name__ + '('
for arg in expr.args:
code2,sepcode2 = self.writeExprCode(arg)
code += code2
sepcode += sepcode2
if not arg == expr.args[-1]:
code += ','
return code + ')',sepcode
elif expr.is_number:
return 'IkReal('+self.strprinter.doprint(expr.evalf())+')',sepcode
elif expr.is_Mul:
code += '('
for arg in expr.args:
code2,sepcode2 = self.writeExprCode(arg)
code += '('+code2+')'
sepcode += sepcode2
if not arg == expr.args[-1]:
code += '*'
return code + ')',sepcode
elif expr.is_Pow:
exprbase,sepcode = self.writeExprCode(expr.base)
if expr.exp.is_number:
if expr.exp.is_integer and expr.exp.evalf() > 0:
code += '('+exprbase+')'
for i in range(1,expr.exp.evalf()):
code += '*('+exprbase+')'
return code,sepcode
elif expr.exp-0.5 == S.Zero:
sepcode += 'if( (%s) < (IkReal)-0.00001 )\n continue;\n'%exprbase
return 'IKsqrt('+exprbase+')',sepcode
elif expr.exp+1 == S.Zero:
# check if exprbase is 0
return '((IKabs('+exprbase+') != 0)?((IkReal)1/('+exprbase+')):(IkReal)1.0e30)',sepcode
elif expr.exp.is_integer and expr.exp.evalf() < 0:
# check if exprbase is 0
fcode = '('+exprbase+')'
for i in range(1,-expr.exp.evalf()):
fcode += '*('+exprbase+')'
return '((IKabs('+exprbase+') != 0)?((IkReal)1/('+fcode+')):(IkReal)1.0e30)',sepcode
elif expr.exp < 0:
# check if exprbase is 0
return '((IKabs('+exprbase+') != 0)?(pow(' + exprbase + ',' + str(expr.exp.evalf()) + ')):(IkReal)1.0e30)',sepcode
exprexp,sepcode2 = self.writeExprCode(expr.exp)
sepcode += sepcode2
return 'pow(' + exprbase + ',' + exprexp + ')',sepcode
elif expr.is_Add:
code += '('
for arg in expr.args:
code2,sepcode2 = self.writeExprCode(arg)
code += '('+code2+')'
sepcode += sepcode2
if not arg == expr.args[-1]:
code += '+'
return code + ')',sepcode
return self.strprinter.doprint(expr.evalf()),sepcode
[ドキュメント] def indentCode(self, code, numspaces):
# actually a lot of time can be wasted in this phase...
if True:
return code
numspaces /= 4
try:
return re.sub('\n','\n'+' '*numspaces,s)
except:
lcode = list(code)
locations = [i for i in range(len(lcode)-1) if lcode[i]=='\n']
locations.reverse()
insertcode = [' ' for i in range(numspaces)]
for loc in locations:
lcode[loc+1:0] = insertcode
lcode[:0] = insertcode
return ''.join(lcode)
[ドキュメント] def using_polyroots(self, deg):
name = 'polyroots%d'%deg
if not name in self.functions:
if deg == 1:
fcode = """static inline void %s(IkReal rawcoeffs[1+1], IkReal rawroots[1], int& numroots) {
numroots=0;
if( rawcoeffs[0] != 0 ) {
rawroots[0] = -rawcoeffs[1]/rawcoeffs[0];
numroots=1;
}
}
"""%name
elif deg == 2:
fcode = """static inline void %s(IkReal rawcoeffs[2+1], IkReal rawroots[2], int& numroots) {
IkReal det = rawcoeffs[1]*rawcoeffs[1]-4*rawcoeffs[0]*rawcoeffs[2];
if( det < 0 ) {
numroots=0;
}
else if( det == 0 ) {
rawroots[0] = -0.5*rawcoeffs[1]/rawcoeffs[0];
numroots = 1;
}
else {
det = IKsqrt(det);
rawroots[0] = (-rawcoeffs[1]+det)/(2*rawcoeffs[0]);
rawroots[1] = (-rawcoeffs[1]-det)/(2*rawcoeffs[0]);//rawcoeffs[2]/(rawcoeffs[0]*rawroots[0]);
numroots = 2;
}
}
"""%name
elif False:#deg == 3: # amazing, cubic formula is not as accurate as iterative method...
fcode = """static inline void %s(IkReal rawcoeffs[%d+1], IkReal rawroots[%d], int& numroots)
{
using std::complex;
IKFAST_ASSERT(rawcoeffs[0] != 0);
IkReal a0 = rawcoeffs[3]/rawcoeffs[0], a1 = rawcoeffs[2]/rawcoeffs[0], a2 = rawcoeffs[1]/rawcoeffs[0];
IkReal a2_3 = a2/3.0;
IkReal Q = (3*a1-a2*a2)/9, R = (9*a2*a1-27*a0-2*a2*a2*a2)/54;
complex<IkReal> D(Q*Q*Q+R*R,0.0);
complex<IkReal> Dsqrt = sqrt(D);
complex<IkReal> S, T;
if( imag(Dsqrt) != 0 ) {
S = pow(complex<IkReal>(R,0)+Dsqrt,IkReal(1.0/3.0));
T = pow(complex<IkReal>(R,0)-Dsqrt,IkReal(1.0/3.0));
}
else {
IkReal temp = R+real(Dsqrt);
S = pow(IKabs(temp),IkReal(1.0/3.0));
if( temp < 0 ) {
S = -S;
}
temp = R-real(Dsqrt);
T = pow(IKabs(temp),IkReal(1.0/3.0));
if( temp < 0 ) {
T = -T;
}
}
complex<IkReal> B = S+T, A = S-T;
numroots = 0;
if( IKabs(imag(B)) < 40*std::numeric_limits<IkReal>::epsilon() ) {
rawroots[numroots++] = -a2_3+real(B);
}
complex<IkReal> Arot = complex<IkReal>(0,SQRT_3)*A;
if( IKabs(imag(B-Arot)) < 40*std::numeric_limits<IkReal>::epsilon() ) {
rawroots[numroots++] = -a2_3-0.5*real(B-Arot);
rawroots[numroots++] = -a2_3-0.5*real(B+Arot);
}
}
"""%name
else:
# Durand-Kerner polynomial root finding method
# In case of multiple roots, see Pierre Fraigniaud's work on Quadratic-Like Convergence of the Mean
# http://www.springerlink.com/content/t72g1635574u10q3/
# the following poly has 2 double roots and requires 105-110 steps to get accurate roots (-2.48003364697210, -0.266182031226875, -0.399192786087098, 5.23118700753098, 1.42932474708659, -3.56915690046190, 1.51649127129379, -0.100045756315578, -1.36239190484780)
# on universalrobots-ur6-85-5-a.zae (deg 8), 110 steps is 87.5%, 150 steps is 88.8%, performance difference is 200ms
fcode = """static inline void %(name)s(IkReal rawcoeffs[%(deg)d+1], IkReal rawroots[%(deg)d], int& numroots)
{
using std::complex;
if( rawcoeffs[0] == 0 ) {
// solve with one reduced degree
%(reducedpolyroots)s(&rawcoeffs[1], &rawroots[0], numroots);
return;
}
IKFAST_ASSERT(rawcoeffs[0] != 0);
const IkReal tol = 128.0*std::numeric_limits<IkReal>::epsilon();
const IkReal tolsqrt = sqrt(std::numeric_limits<IkReal>::epsilon());
complex<IkReal> coeffs[%(deg)d];
const int maxsteps = 110;
for(int i = 0; i < %(deg)d; ++i) {
coeffs[i] = complex<IkReal>(rawcoeffs[i+1]/rawcoeffs[0]);
}
complex<IkReal> roots[%(deg)d];
IkReal err[%(deg)d];
roots[0] = complex<IkReal>(1,0);
roots[1] = complex<IkReal>(0.4,0.9); // any complex number not a root of unity works
err[0] = 1.0;
err[1] = 1.0;
for(int i = 2; i < %(deg)d; ++i) {
roots[i] = roots[i-1]*roots[1];
err[i] = 1.0;
}
for(int step = 0; step < maxsteps; ++step) {
bool changed = false;
for(int i = 0; i < %(deg)d; ++i) {
if ( err[i] >= tol ) {
changed = true;
// evaluate
complex<IkReal> x = roots[i] + coeffs[0];
for(int j = 1; j < %(deg)d; ++j) {
x = roots[i] * x + coeffs[j];
}
for(int j = 0; j < %(deg)d; ++j) {
if( i != j ) {
if( roots[i] != roots[j] ) {
x /= (roots[i] - roots[j]);
}
}
}
roots[i] -= x;
err[i] = abs(x);
}
}
if( !changed ) {
break;
}
}
numroots = 0;
bool visited[%(deg)d] = {false};
for(int i = 0; i < %(deg)d; ++i) {
if( !visited[i] ) {
// might be a multiple root, in which case it will have more error than the other roots
// find any neighboring roots, and take the average
complex<IkReal> newroot=roots[i];
int n = 1;
for(int j = i+1; j < %(deg)d; ++j) {
if( abs(roots[i]-roots[j]) < 8*tolsqrt ) {
newroot += roots[j];
n += 1;
visited[j] = true;
}
}
if( n > 1 ) {
newroot /= n;
}
// there are still cases where even the mean is not accurate enough, until a better multi-root algorithm is used, need to use the sqrt
if( IKabs(imag(newroot)) < tolsqrt ) {
rawroots[numroots++] = real(newroot);
}
}
}
}
"""%{'name':name, 'deg':deg, 'reducedpolyroots':self.using_polyroots(deg-1) }
self.functions[name] = fcode
return name
[ドキュメント] def using_checkconsistency12(self):
name = 'checkconsistency12'
if not name in self.functions:
fcode = """
static inline bool %s(const IkReal* Breal)
{
IkReal norm = 0.1;
for(int i = 0; i < 11; ++i) {
norm += IKabs(Breal[i]);
}
IkReal tol = 1e-6*norm; // have to increase the threshold since many computations are involved
return IKabs(Breal[0]*Breal[0]-Breal[1]) < tol && IKabs(Breal[0]*Breal[2]-Breal[3]) < tol && IKabs(Breal[1]*Breal[2]-Breal[4]) < tol && IKabs(Breal[2]*Breal[2]-Breal[5]) < tol && IKabs(Breal[0]*Breal[5]-Breal[6]) < tol && IKabs(Breal[1]*Breal[5]-Breal[7]) < tol && IKabs(Breal[2]*Breal[5]-Breal[8]) < tol && IKabs(Breal[0]*Breal[8]-Breal[9]) < tol && IKabs(Breal[1]*Breal[8]-Breal[10]) < tol;
}
"""%name
self.functions[name] = fcode
return name
[ドキュメント] def using_matrixinverse(self):
name = 'matrixinverse'
if not name in self.functions:
fcode = """
template<int D>
static inline bool %s(IkReal* A)
{
int n = D;
int info;
IkReal IKFAST_ALIGNED16(work[D*D*(D-1)]);
int ipiv[D];
dgetrf_(&n, &n, A, &n, &ipiv[0], &info);
if( info != 0 ) {
return false;
}
int worksize=D*D*(D-1);
dgetri_(&n, A, &n, &ipiv[0], &work[0], &worksize, &info);
return info==0;
}
"""%(name)
self.functions[name] = fcode
return name
[ドキュメント] def using_solvedialyticpoly12qep(self):
name = 'solvedialyticpoly12qep'
checkconsistency12=self.using_checkconsistency12()
if not name in self.functions:
fcode = """
/// \\brief Solve the det Ax^2+Bx+C = 0 problem using the Manocha and Canny method (1994)
///
/// matcoeffs is of length 54*3, for 3 matrices
static inline void %s(const IkReal* matcoeffs, IkReal* rawroots, int& numroots)
{
const IkReal tol = 128.0*std::numeric_limits<IkReal>::epsilon();
IkReal IKFAST_ALIGNED16(M[24*24]) = {0};
IkReal IKFAST_ALIGNED16(A[12*12]);
IkReal IKFAST_ALIGNED16(work[24*24*23]);
int ipiv[12];
int info, coeffindex;
const int worksize=24*24*23;
const int matrixdim = 12;
const int matrixdim2 = 24;
numroots = 0;
// first setup M = [0 I; -C -B] and A
coeffindex = 0;
for(int j = 0; j < 6; ++j) {
for(int k = 0; k < 9; ++k) {
M[matrixdim+(j+6)+2*matrixdim*k] = M[matrixdim+j+2*matrixdim*(k+3)] = -matcoeffs[coeffindex++];
}
}
for(int j = 0; j < 6; ++j) {
for(int k = 0; k < 9; ++k) {
M[matrixdim+(j+6)+2*matrixdim*k+matrixdim*2*matrixdim] = M[matrixdim+j+2*matrixdim*(k+3)+matrixdim*2*matrixdim] = -matcoeffs[coeffindex++];
}
}
for(int j = 0; j < 6; ++j) {
for(int k = 0; k < 9; ++k) {
A[(j+6)+matrixdim*k] = A[j+matrixdim*(k+3)] = matcoeffs[coeffindex++];
}
for(int k = 0; k < 3; ++k) {
A[j+matrixdim*k] = A[(j+6)+matrixdim*(k+9)] = 0;
}
}
const IkReal lfpossibilities[4][4] = {{1,-1,1,1},{1,0,-2,1},{1,1,2,0},{1,-1,4,1}};
int lfindex = -1;
bool bsingular = true;
do {
dgetrf_(&matrixdim,&matrixdim,A,&matrixdim,&ipiv[0],&info);
if( info == 0 ) {
bsingular = false;
for(int j = 0; j < matrixdim; ++j) {
if( IKabs(A[j*matrixdim+j]) < 100*tol ) {
bsingular = true;
break;
}
}
if( !bsingular ) {
break;
}
}
if( lfindex == 3 ) {
break;
}
// transform by the linear functional
lfindex++;
const IkReal* lf = lfpossibilities[lfindex];
// have to reinitialize A
coeffindex = 0;
for(int j = 0; j < 6; ++j) {
for(int k = 0; k < 9; ++k) {
IkReal a = matcoeffs[coeffindex+108], b = matcoeffs[coeffindex+54], c = matcoeffs[coeffindex];
A[(j+6)+matrixdim*k] = A[j+matrixdim*(k+3)] = lf[0]*lf[0]*a+lf[0]*lf[2]*b+lf[2]*lf[2]*c;
M[matrixdim+(j+6)+2*matrixdim*k] = M[matrixdim+j+2*matrixdim*(k+3)] = -(lf[1]*lf[1]*a + lf[1]*lf[3]*b + lf[3]*lf[3]*c);
M[matrixdim+(j+6)+2*matrixdim*k+matrixdim*2*matrixdim] = M[matrixdim+j+2*matrixdim*(k+3)+matrixdim*2*matrixdim] = -(2*lf[0]*lf[1]*a + (lf[0]*lf[3]+lf[1]*lf[2])*b + 2*lf[2]*lf[3]*c);
coeffindex++;
}
for(int k = 0; k < 3; ++k) {
A[j+matrixdim*k] = A[(j+6)+matrixdim*(k+9)] = 0;
}
}
} while(lfindex<4);
if( bsingular ) {
return;
}
dgetrs_("No transpose", &matrixdim, &matrixdim2, A, &matrixdim, &ipiv[0], &M[matrixdim], &matrixdim2, &info);
if( info != 0 ) {
return;
}
// set identity in upper corner
for(int j = 0; j < matrixdim; ++j) {
M[matrixdim*2*matrixdim+j+matrixdim*2*j] = 1;
}
IkReal IKFAST_ALIGNED16(wr[24]);
IkReal IKFAST_ALIGNED16(wi[24]);
IkReal IKFAST_ALIGNED16(vr[24*24]);
int one=1;
dgeev_("N", "V", &matrixdim2, M, &matrixdim2, wr, wi,NULL, &one, vr, &matrixdim2, work, &worksize, &info);
if( info != 0 ) {
return;
}
IkReal Breal[matrixdim-1];
for(int i = 0; i < matrixdim2; ++i) {
if( IKabs(wi[i]) < tol*100 ) {
IkReal* ev = vr+matrixdim2*i;
if( IKabs(wr[i]) > 1 ) {
ev += matrixdim;
}
// consistency has to be checked!!
if( IKabs(ev[0]) < tol ) {
continue;
}
IkReal iconst = 1/ev[0];
for(int j = 1; j < matrixdim; ++j) {
Breal[j-1] = ev[j]*iconst;
}
if( %s(Breal) ) {
if( lfindex >= 0 ) {
const IkReal* lf = lfpossibilities[lfindex];
rawroots[numroots++] = (wr[i]*lf[0]+lf[1])/(wr[i]*lf[2]+lf[3]);
}
else {
rawroots[numroots++] = wr[i];
}
bool bsmall0=IKabs(ev[0]) > IKabs(ev[3]);
bool bsmall1=IKabs(ev[0]) > IKabs(ev[1]);
if( bsmall0 && bsmall1 ) {
rawroots[numroots++] = ev[3]/ev[0];
rawroots[numroots++] = ev[1]/ev[0];
}
else if( bsmall0 && !bsmall1 ) {
rawroots[numroots++] = ev[5]/ev[2];
rawroots[numroots++] = ev[2]/ev[1];
}
else if( !bsmall0 && bsmall1 ) {
rawroots[numroots++] = ev[9]/ev[6];
rawroots[numroots++] = ev[10]/ev[9];
}
else if( !bsmall0 && !bsmall1 ) {
rawroots[numroots++] = ev[11]/ev[8];
rawroots[numroots++] = ev[11]/ev[10];
}
}
}
}
}"""%(name,checkconsistency12)
self.functions[name] = fcode
return name
[ドキュメント] def using_checkconsistency8(self):
name = 'checkconsistency8'
if not name in self.functions:
fcode = """
static inline bool %s(const IkReal* Breal)
{
IkReal norm = 0.1;
for(int i = 0; i < 7; ++i) {
norm += IKabs(Breal[i]);
}
IkReal tol = 1e-5*norm; // have to increase the threshold since many computations are involved
return IKabs(Breal[0]*Breal[1]-Breal[2]) < tol && IKabs(Breal[1]*Breal[1]-Breal[3]) < tol && IKabs(Breal[0]*Breal[3]-Breal[4]) < tol && IKabs(Breal[1]*Breal[3]-Breal[5]) < tol && IKabs(Breal[0]*Breal[5]-Breal[6]) < tol;
}"""%name
self.functions[name] = fcode
return name
[ドキュメント] def using_solvedialyticpoly8qep(self):
name = 'solvedialyticpoly8qep'
checkconsistency8=self.using_checkconsistency8()
if not name in self.functions:
fcode = """
/// \\brief Solve the det Ax^2+Bx+C = 0 problem using the Manocha and Canny method (1994)
///
/// matcoeffs is of length 54*3, for 3 matrices
static inline void %s(const IkReal* matcoeffs, IkReal* rawroots, int& numroots)
{
const IkReal tol = 128.0*std::numeric_limits<IkReal>::epsilon();
IkReal IKFAST_ALIGNED16(M[16*16]) = {0};
IkReal IKFAST_ALIGNED16(A[8*8]);
IkReal IKFAST_ALIGNED16(work[16*16*15]);
int ipiv[8];
int info, coeffindex;
const int worksize=16*16*15;
const int matrixdim = 8;
const int matrixdim2 = 16;
numroots = 0;
// first setup M = [0 I; -C -B] and A
coeffindex = 0;
for(int j = 0; j < 4; ++j) {
for(int k = 0; k < 6; ++k) {
M[matrixdim+(j+4)+2*matrixdim*k] = M[matrixdim+j+2*matrixdim*(k+2)] = -matcoeffs[coeffindex++];
}
}
for(int j = 0; j < 4; ++j) {
for(int k = 0; k < 6; ++k) {
M[matrixdim+(j+4)+2*matrixdim*k+matrixdim*2*matrixdim] = M[matrixdim+j+2*matrixdim*(k+2)+matrixdim*2*matrixdim] = -matcoeffs[coeffindex++];
}
}
for(int j = 0; j < 4; ++j) {
for(int k = 0; k < 6; ++k) {
A[(j+4)+matrixdim*k] = A[j+matrixdim*(k+2)] = matcoeffs[coeffindex++];
}
for(int k = 0; k < 2; ++k) {
A[j+matrixdim*k] = A[(j+4)+matrixdim*(k+6)] = 0;
}
}
const IkReal lfpossibilities[4][4] = {{1,-1,1,1},{1,0,-2,1},{1,1,2,0},{1,-1,4,1}};
int lfindex = -1;
bool bsingular = true;
do {
dgetrf_(&matrixdim,&matrixdim,A,&matrixdim,&ipiv[0],&info);
if( info == 0 ) {
bsingular = false;
for(int j = 0; j < matrixdim; ++j) {
if( IKabs(A[j*matrixdim+j]) < 100*tol ) {
bsingular = true;
break;
}
}
if( !bsingular ) {
break;
}
}
if( lfindex == 3 ) {
break;
}
// transform by the linear functional
lfindex++;
const IkReal* lf = lfpossibilities[lfindex];
// have to reinitialize A
coeffindex = 0;
for(int j = 0; j < 4; ++j) {
for(int k = 0; k < 6; ++k) {
IkReal a = matcoeffs[coeffindex+48], b = matcoeffs[coeffindex+24], c = matcoeffs[coeffindex];
A[(j+4)+matrixdim*k] = A[j+matrixdim*(k+2)] = lf[0]*lf[0]*a+lf[0]*lf[2]*b+lf[2]*lf[2]*c;
M[matrixdim+(j+4)+2*matrixdim*k] = M[matrixdim+j+2*matrixdim*(k+2)] = -(lf[1]*lf[1]*a + lf[1]*lf[3]*b + lf[3]*lf[3]*c);
M[matrixdim+(j+4)+2*matrixdim*k+matrixdim*2*matrixdim] = M[matrixdim+j+2*matrixdim*(k+2)+matrixdim*2*matrixdim] = -(2*lf[0]*lf[1]*a + (lf[0]*lf[3]+lf[1]*lf[2])*b + 2*lf[2]*lf[3]*c);
coeffindex++;
}
for(int k = 0; k < 2; ++k) {
A[j+matrixdim*k] = A[(j+4)+matrixdim*(k+6)] = 0;
}
}
} while(lfindex<4);
if( bsingular ) {
return;
}
dgetrs_("No transpose", &matrixdim, &matrixdim2, A, &matrixdim, &ipiv[0], &M[matrixdim], &matrixdim2, &info);
if( info != 0 ) {
return;
}
// set identity in upper corner
for(int j = 0; j < matrixdim; ++j) {
M[matrixdim*2*matrixdim+j+matrixdim*2*j] = 1;
}
IkReal IKFAST_ALIGNED16(wr[16]);
IkReal IKFAST_ALIGNED16(wi[16]);
IkReal IKFAST_ALIGNED16(vr[16*16]);
int one=1;
dgeev_("N", "V", &matrixdim2, M, &matrixdim2, wr, wi,NULL, &one, vr, &matrixdim2, work, &worksize, &info);
if( info != 0 ) {
return;
}
IkReal Breal[matrixdim-1];
for(int i = 0; i < matrixdim2; ++i) {
if( IKabs(wi[i]) < tol*100 ) {
IkReal* ev = vr+matrixdim2*i;
if( IKabs(wr[i]) > 1 ) {
ev += matrixdim;
}
// consistency has to be checked!!
if( IKabs(ev[0]) < tol ) {
continue;
}
IkReal iconst = 1/ev[0];
for(int j = 1; j < matrixdim; ++j) {
Breal[j-1] = ev[j]*iconst;
}
if( %s(Breal) ) {
if( lfindex >= 0 ) {
const IkReal* lf = lfpossibilities[lfindex];
rawroots[numroots++] = (wr[i]*lf[0]+lf[1])/(wr[i]*lf[2]+lf[3]);
}
else {
rawroots[numroots++] = wr[i];
}
bool bsmall0=IKabs(ev[0]) > IKabs(ev[2]);
bool bsmall1=IKabs(ev[0]) > IKabs(ev[1]);
if( bsmall0 && bsmall1 ) {
rawroots[numroots++] = ev[2]/ev[0];
rawroots[numroots++] = ev[1]/ev[0];
}
else if( bsmall0 && !bsmall1 ) {
rawroots[numroots++] = ev[3]/ev[1];
rawroots[numroots++] = ev[1]/ev[0];
}
else if( !bsmall0 && bsmall1 ) {
rawroots[numroots++] = ev[6]/ev[4];
rawroots[numroots++] = ev[7]/ev[6];
}
else if( !bsmall0 && !bsmall1 ) {
rawroots[numroots++] = ev[7]/ev[5];
rawroots[numroots++] = ev[7]/ev[6];
}
}
}
}
}"""%(name,checkconsistency8)
self.functions[name] = fcode
return name
[ドキュメント] def using_checkconsistency16(self):
name = 'checkconsistency16'
if not name in self.functions:
fcode = """
static inline bool %s(const IkReal* Breal)
{
IkReal norm = 0.1;
for(int i = 0; i < 15; ++i) {
norm += IKabs(Breal[i]);
}
IkReal tol = 5e-5*norm; // have to increase the threshold since many computations are involved
return IKabs(Breal[2]*Breal[2]-Breal[3]) < tol && IKabs(Breal[0]*Breal[4]-Breal[5]) < tol && IKabs(Breal[1]*Breal[4]-Breal[6]) < tol && IKabs(Breal[2]*Breal[4]-Breal[7]) < tol && IKabs(Breal[3]*Breal[4]-Breal[8]) < tol && IKabs(Breal[4]*Breal[4]-Breal[9]) < tol && IKabs(Breal[2]*Breal[9]-Breal[10]) < tol && IKabs(Breal[3]*Breal[9]-Breal[11]) < tol && IKabs(Breal[4]*Breal[9]-Breal[12]) < tol && IKabs(Breal[2]*Breal[12]-Breal[13]) < tol && IKabs(Breal[3]*Breal[12]-Breal[14]) < tol;
};
"""%name
self.functions[name] = fcode
return name
[ドキュメント] def using_solvedialyticpoly16lep(self):
name = 'solvedialyticpoly16lep'
checkconsistency16=self.using_checkconsistency16()
if not name in self.functions:
fcode = """
/// \\brief Solve the det Bx-A = 0 problem
///
/// matcoeffs is of length 88*2, for 2 matrices
static inline void %s(const IkReal* matcoeffs, IkReal* rawroots, int& numroots)
{
const IkReal tol = std::numeric_limits<IkReal>::epsilon();
const int matrixdim = 16;
IkReal IKFAST_ALIGNED16(M[2][matrixdim*matrixdim]);
const int worksize=matrixdim*matrixdim*(matrixdim-1);
IkReal IKFAST_ALIGNED16(work[worksize]);
int ipiv[matrixdim];
int info, coeffindex;
const int secondrowindices[11] = {0,1,2,3,4,5,8,9,10,11,12};
numroots = 0;
coeffindex = 0;
for(int degree = 0; degree < 2; ++degree) {
for(int j = 0; j < 8; ++j) {
for(int k = 0; k < 11; ++k) {
M[degree][(j+8)+matrixdim*secondrowindices[k]] = M[degree][j+matrixdim*(k+5)] = matcoeffs[coeffindex++];
}
M[degree][j+matrixdim*0] = M[degree][j+matrixdim*1] = M[degree][j+matrixdim*2] = M[degree][j+matrixdim*3] = M[degree][j+matrixdim*4] = 0;
M[degree][(j+8)+matrixdim*6] = M[degree][(j+8)+matrixdim*7] = M[degree][(j+8)+matrixdim*13] = M[degree][(j+8)+matrixdim*14] = M[degree][(j+8)+matrixdim*15] = 0;
}
}
const IkReal lfpossibilities[2][4] = {{1,-1,1,1},{1,0,-2,1}};
int lfindex = -1;
IkReal polymultiplier = 0;
bool bsingular = false;
do {
dgetrf_(&matrixdim,&matrixdim,M[1],&matrixdim,&ipiv[0],&info);
if( info == 0 ) {
bsingular = false;
for(int j = 0; j < matrixdim; ++j) {
if( IKabs(M[1][j*matrixdim+j]) < 100*tol ) {
bsingular = true;
break;
}
}
if( !bsingular ) {
break;
}
}
else {
bsingular = true;
}
if( lfindex == 1 ) {
break;
}
// transform by the linear functional
lfindex++;
const IkReal* lf = lfpossibilities[lfindex];
coeffindex = 0;
for(int j = 0; j < 8; ++j) {
for(int k = 0; k < 11; ++k) {
IkReal b = matcoeffs[coeffindex+88], a = matcoeffs[coeffindex];
M[0][(j+8)+matrixdim*secondrowindices[k]] = M[0][j+matrixdim*(k+5)] = lf[1]*b+lf[3]*a;
M[1][(j+8)+matrixdim*secondrowindices[k]] = M[1][j+matrixdim*(k+5)] = lf[0]*b+lf[2]*a;
++coeffindex;
}
for(int degree = 0; degree < 2; ++degree ) {
M[degree][j+matrixdim*0] = M[degree][j+matrixdim*1] = M[degree][j+matrixdim*2] = M[degree][j+matrixdim*3] = M[degree][j+matrixdim*4] = 0;
M[degree][(j+8)+matrixdim*6] = M[degree][(j+8)+matrixdim*7] = M[degree][(j+8)+matrixdim*13] = M[degree][(j+8)+matrixdim*14] = M[degree][(j+8)+matrixdim*15] = 0;
}
}
} while(lfindex<2);
if( bsingular ) {
// solve the generalized eigenvalue problem (qz algorithm)
return;
}
dgetrs_("No transpose", &matrixdim, &matrixdim, M[1], &matrixdim, &ipiv[0], M[0], &matrixdim, &info);
if( info != 0 ) {
return;
}
IkReal IKFAST_ALIGNED16(wr[matrixdim]);
IkReal IKFAST_ALIGNED16(wi[matrixdim]);
IkReal IKFAST_ALIGNED16(vr[matrixdim*matrixdim]);
int one=1;
dgeev_("N", "V", &matrixdim, M[0], &matrixdim, wr, wi,NULL, &one, vr, &matrixdim, work, &worksize, &info);
if( info != 0 ) {
return;
}
// eigen values are negatives of the real solution
IkReal Breal[matrixdim-1];
for(int i = 0; i < matrixdim; ++i) {
if( IKabs(wi[i]) < tol*10000 ) {
IkReal* ev = vr+matrixdim*i;
// consistency has to be checked!!
if( IKabs(ev[0]) < tol ) {
continue;
}
IkReal iconst = 1/ev[0];
IkReal constsign = ev[0] > 0 ? IkReal(1) : IkReal(-1);
for(int j = 1; j < matrixdim; ++j) {
Breal[j-1] = ev[j]*iconst;
}
if( %s(Breal) ) {
if( lfindex >= 0 ) {
const IkReal* lf = lfpossibilities[lfindex];
rawroots[numroots++] = (-wr[i]*lf[0]+lf[1])/(-wr[i]*lf[2]+lf[3]);
}
else {
rawroots[numroots++] = -wr[i];
}
bool bsmall0=IKabs(ev[0]) > IKabs(ev[5]);
bool bsmall1=IKabs(ev[0]) > IKabs(ev[3]);
if( bsmall0 && bsmall1 ) {
rawroots[numroots++] = ev[5]/ev[0];
rawroots[numroots++] = ev[3]/ev[0];
rawroots[numroots++] = IKatan2(constsign*ev[1],constsign*ev[2]);
}
else if( bsmall0 && !bsmall1 ) {
rawroots[numroots++] = ev[8]/ev[3];
rawroots[numroots++] = ev[4]/ev[3];
rawroots[numroots++] = IKatan2(constsign*ev[1],constsign*ev[2]);
}
else if( !bsmall0 && bsmall1 ) {
rawroots[numroots++] = ev[13]/ev[10];
rawroots[numroots++] = ev[14]/ev[13];
if( rawroots[numroots-2] < 0 ) {
constsign = -constsign;
}
rawroots[numroots++] = IKatan2(constsign*ev[6],constsign*ev[7]);
}
else if( !bsmall0 && !bsmall1 ) {
rawroots[numroots++] = ev[15]/ev[12];
rawroots[numroots++] = ev[15]/ev[14];
if( rawroots[numroots-2] < 0 ) {
constsign = -constsign;
}
rawroots[numroots++] = IKatan2(constsign*ev[6],constsign*ev[7]);
}
}
}
}
}
"""%(name,checkconsistency16)
self.functions[name] = fcode
return name