├── .gitignore ├── .travis.yml ├── LICENSE.txt ├── README.md ├── changelog.md ├── run_tests.sh ├── setup.py ├── sympybotics ├── __init__.py ├── _compatibility_.py ├── dynamics │ ├── __init__.py │ ├── dyn_parm_dep.py │ ├── dynamics.py │ ├── extra_dyn.py │ ├── regressor.py │ ├── rne.py │ ├── rne_khalil.py │ └── rne_park.py ├── dynident │ ├── __init__.py │ └── regression.py ├── geometry.py ├── kinematics.py ├── robotcodegen.py ├── robotdef.py ├── robotmodel.py ├── symcode │ ├── __init__.py │ ├── generation.py │ └── subexprs.py ├── tests │ └── test_results.py ├── tools │ ├── __init__.py │ ├── cache.py │ └── qepcad.py └── utils.py └── todo.md /.gitignore: -------------------------------------------------------------------------------- 1 | *~ 2 | *.pyc 3 | *.kdev4 4 | *.desktop 5 | -------------------------------------------------------------------------------- /.travis.yml: -------------------------------------------------------------------------------- 1 | language: python 2 | python: 3 | - "2.7" 4 | - "3.3" 5 | before_install: 6 | - pip install sympy --use-mirrors 7 | - pip install numpy --use-mirrors 8 | install: 9 | - python setup.py install 10 | before_script: 11 | - cd sympybotics 12 | script: 13 | - ../run_tests.sh 14 | -------------------------------------------------------------------------------- /LICENSE.txt: -------------------------------------------------------------------------------- 1 | Copyright (c) 2012, 2013 Cristóvão D. Sousa 2 | 3 | All rights reserved. 4 | 5 | Redistribution and use in source and binary forms, with or without 6 | modification, are permitted provided that the following conditions are met: 7 | 8 | a. Redistributions of source code must retain the above copyright notice, 9 | this list of conditions and the following disclaimer. 10 | b. Redistributions in binary form must reproduce the above copyright 11 | notice, this list of conditions and the following disclaimer in the 12 | documentation and/or other materials provided with the distribution. 13 | c. Neither the name of SymPyBotics nor the names of its contributors 14 | may be used to endorse or promote products derived from this software 15 | without specific prior written permission. 16 | 17 | 18 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 19 | AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 20 | IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 21 | ARE DISCLAIMED. IN NO EVENT SHALL THE REGENTS OR CONTRIBUTORS BE LIABLE FOR 22 | ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 23 | DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 24 | SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 25 | CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 26 | LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY 27 | OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH 28 | DAMAGE. -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | SymPyBotics 2 | =========== 3 | 4 | Symbolic Framework for Modeling and Identification of Robot Dynamics 5 | 6 | Uses [Sympy](http://sympy.org) and [Numpy](http://www.numpy.org/) libraries. 7 | 8 | [![Build Status](https://travis-ci.org/cdsousa/SymPyBotics.png?branch=master)](https://travis-ci.org/cdsousa/SymPyBotics) 9 | 10 | ##### Citation: 11 | [![DOI](https://zenodo.org/badge/920/cdsousa/SymPyBotics.png)](http://dx.doi.org/10.5281/zenodo.11365) 12 | 13 | 14 | Example 15 | ------- 16 | 17 | Definition of a 2 DOF example robot: 18 | 19 | ```Python 20 | >>> import sympy 21 | >>> import sympybotics 22 | >>> rbtdef = sympybotics.RobotDef('Example Robot', # robot name 23 | ... [('-pi/2', 0, 0, 'q+pi/2'), # list of tuples with Denavit-Hartenberg parameters 24 | ... ( 'pi/2', 0, 0, 'q-pi/2')], # (alpha, a, d, theta) 25 | ... dh_convention='standard' # either 'standard' or 'modified' 26 | ... ) 27 | >>> rbtdef.frictionmodel = {'Coulomb', 'viscous'} # options are None or a combination of 'Coulomb', 'viscous' and 'offset' 28 | >>> rbtdef.gravityacc = sympy.Matrix([0.0, 0.0, -9.81]) # optional, this is the default value 29 | 30 | ``` 31 | 32 | ```Python 33 | >>> rbtdef.dynparms() 34 | [L_1xx, L_1xy, L_1xz, L_1yy, L_1yz, L_1zz, l_1x, l_1y, l_1z, m_1, fv_1, fc_1, L_2xx, L_2xy, L_2xz, L_2yy, L_2yz, L_2zz, l_2x, l_2y, l_2z, m_2, fv_2, fc_2] 35 | 36 | ``` 37 | 38 | `L` is the link inertia tensor computed about the link frame; 39 | `l` is the link first moment of inertia; 40 | `m` is the link mass. 41 | These are the so-called barycentric parameters, with respect to which the dynamic model is linear. 42 | 43 | 44 | Generation of geometric, kinematic and dynamic models: 45 | 46 | ```Python 47 | >>> rbt = sympybotics.RobotDynCode(rbtdef, verbose=True) 48 | generating geometric model 49 | generating kinematic model 50 | generating inverse dynamics code 51 | generating gravity term code 52 | generating coriolis term code 53 | generating coriolis matrix code 54 | generating inertia matrix code 55 | generating regressor matrix code 56 | generating friction term code 57 | done 58 | 59 | ``` 60 | 61 | ```Python 62 | >>> rbt.geo.T[-1] 63 | Matrix([ 64 | [-sin(q1)*sin(q2), -cos(q1), sin(q1)*cos(q2), 0], 65 | [ sin(q2)*cos(q1), -sin(q1), -cos(q1)*cos(q2), 0], 66 | [ cos(q2), 0, sin(q2), 0], 67 | [ 0, 0, 0, 1]]) 68 | 69 | ``` 70 | 71 | ```Python 72 | >>> rbt.kin.J[-1] 73 | Matrix([ 74 | [0, 0], 75 | [0, 0], 76 | [0, 0], 77 | [0, -cos(q1)], 78 | [0, -sin(q1)], 79 | [1, 0]]) 80 | 81 | ``` 82 | 83 | C function generation: 84 | 85 | ```Python 86 | >>> tau_str = sympybotics.robotcodegen.robot_code_to_func('C', rbt.invdyn_code, 'tau_out', 'tau', rbtdef) 87 | 88 | 89 | ``` 90 | Doing `print(tau_str)`, function code will be output: 91 | 92 | ```C 93 | void tau( double* tau_out, const double* parms, const double* q, const double* dq, const double* ddq ) 94 | { 95 | double x0 = sin(q[1]); 96 | double x1 = -dq[0]; 97 | double x2 = -x1; 98 | double x3 = x0*x2; 99 | double x4 = cos(q[1]); 100 | double x5 = x2*x4; 101 | double x6 = parms[13]*x5 + parms[15]*dq[1] + parms[16]*x3; 102 | double x7 = parms[14]*x5 + parms[16]*dq[1] + parms[17]*x3; 103 | double x8 = -ddq[0]; 104 | double x9 = -x4; 105 | double x10 = dq[1]*x1; 106 | double x11 = x0*x10 + x8*x9; 107 | double x12 = -x0*x8 - x10*x4; 108 | double x13 = 9.81*x0; 109 | double x14 = 9.81*x4; 110 | double x15 = parms[12]*x5 + parms[13]*dq[1] + parms[14]*x3; 111 | 112 | tau_out[0] = -parms[3]*x8 + x0*(parms[14]*x11 + parms[16]*ddq[1] + parms[17]*x12 - dq[1]*x15 - parms[19]*x14 + x5*x6) - x9*(parms[12]*x11 + parms[13]*ddq[1] + parms[14]*x12 + dq[1]*x7 + parms[19]*x13 - x3*x6); 113 | tau_out[1] = parms[13]*x11 + parms[15]*ddq[1] + parms[16]*x12 - parms[18]*x13 + parms[20]*x14 + x15*x3 - x5*x7; 114 | 115 | return; 116 | } 117 | ``` 118 | 119 | Dynamic base parameters: 120 | 121 | ```Python 122 | >>> rbt.calc_base_parms() 123 | >>> rbt.dyn.baseparms 124 | Matrix([ 125 | [L_1yy + L_2zz], 126 | [ fv_1], 127 | [ fc_1], 128 | [L_2xx - L_2zz], 129 | [ L_2xy], 130 | [ L_2xz], 131 | [ L_2yy], 132 | [ L_2yz], 133 | [ l_2x], 134 | [ l_2z], 135 | [ fv_2], 136 | [ fc_2]]) 137 | 138 | ``` 139 | 140 | Author 141 | ------ 142 | 143 | [Cristóvão Duarte Sousa](https://github.com/cdsousa) 144 | 145 | Install 146 | ------- 147 | 148 | From git source: 149 | 150 | git clone https://github.com/cdsousa/SymPyBotics.git 151 | cd sympybotics 152 | python setup.py install 153 | 154 | License 155 | ------- 156 | 157 | New BSD license. See [License File](LICENSE.txt) 158 | -------------------------------------------------------------------------------- /changelog.md: -------------------------------------------------------------------------------- 1 | Changelog 2 | ========= 3 | 4 | 0.3 5 | --- 6 | 7 | - Added modified Denavit-Hartenberg convention support (DH convention is now a mandatory parameter to RobotDef) 8 | - Added tests to assert correctness of generated equations -------------------------------------------------------------------------------- /run_tests.sh: -------------------------------------------------------------------------------- 1 | #!/bin/sh 2 | nosetests --with-doctest --doctest-extension=md $@ 3 | -------------------------------------------------------------------------------- /setup.py: -------------------------------------------------------------------------------- 1 | from os.path import exists 2 | from setuptools import setup 3 | 4 | setup( 5 | name='sympybotics', 6 | version='1.0-dev', 7 | author='Cristovao D. Sousa', 8 | author_email='crisjss@gmail.com', 9 | description='Robot dynamic symbolic model generator', 10 | license='BSD', 11 | keywords='robot dynamics', 12 | url='http://github.com/cdsousa/sympybotics', 13 | packages=['sympybotics', 14 | 'sympybotics.dynamics', 15 | 'sympybotics.dynident', 16 | 'sympybotics.symcode', 17 | 'sympybotics.tools'], 18 | long_description=open('README.md').read() if exists('README.md') else '', 19 | classifiers=[ 20 | 'Development Status :: 4 - Beta', 21 | 'Intended Audience :: Developers', 22 | 'License :: OSI Approved :: BSD License', 23 | 'Operating System :: OS Independent', 24 | 'Programming Language :: Python', 25 | 'Intended Audience :: Education', 26 | 'Intended Audience :: Manufacturing', 27 | 'Intended Audience :: Science/Research', 28 | 'Topic :: Scientific/Engineering :: Physics', 29 | 'Topic :: Software Development :: Code Generators', 30 | ], 31 | ) 32 | -------------------------------------------------------------------------------- /sympybotics/__init__.py: -------------------------------------------------------------------------------- 1 | """Symbolic manipulation of robot geometric, kinematic and dynamic models.""" 2 | 3 | __version__ = '1.0-dev' 4 | 5 | from .robotdef import RobotDef, q 6 | from .geometry import Geometry 7 | from .kinematics import Kinematics 8 | from .dynamics import Dynamics 9 | from .robotcodegen import robot_code_to_func 10 | from .robotmodel import RobotAllSymb, RobotDynCode 11 | -------------------------------------------------------------------------------- /sympybotics/_compatibility_.py: -------------------------------------------------------------------------------- 1 | """ Python 2/3 compatibility utils """ 2 | 3 | import sys 4 | pyversion = sys.version_info[0] 5 | 6 | # exec (from https://bitbucket.org/gutworth/six/): 7 | if pyversion is 3: 8 | import builtins 9 | exec_ = getattr(builtins, "exec") 10 | del builtins 11 | elif pyversion is 2: 12 | def exec_(_code_, _globs_=None, _locs_=None): 13 | """Execute code in a namespace.""" 14 | if _globs_ is None: 15 | frame = sys._getframe(1) 16 | _globs_ = frame.f_globals 17 | if _locs_ is None: 18 | _locs_ = frame.f_locals 19 | del frame 20 | elif _locs_ is None: 21 | _locs_ = _globs_ 22 | exec("""exec _code_ in _globs_, _locs_""") 23 | -------------------------------------------------------------------------------- /sympybotics/dynamics/__init__.py: -------------------------------------------------------------------------------- 1 | from .dynamics import Dynamics 2 | -------------------------------------------------------------------------------- /sympybotics/dynamics/dyn_parm_dep.py: -------------------------------------------------------------------------------- 1 | import numpy 2 | 3 | 4 | def find_dyn_parm_deps(dof, parm_num, regressor_func): 5 | ''' 6 | Find dynamic parameter dependencies (i.e., regressor column dependencies). 7 | ''' 8 | 9 | samples = 10000 10 | round = 10 11 | 12 | pi = numpy.pi 13 | 14 | Z = numpy.zeros((dof * samples, parm_num)) 15 | 16 | for i in range(samples): 17 | q = [float(numpy.random.random() * 2.0 * pi - pi) for j in range(dof)] 18 | dq = [float(numpy.random.random() * 2.0 * pi - pi) for j in range(dof)] 19 | ddq = [float(numpy.random.random() * 2.0 * pi - pi) 20 | for j in range(dof)] 21 | Z[i * dof: i * dof + dof, :] = numpy.matrix( 22 | regressor_func(q, dq, ddq)).reshape(dof, parm_num) 23 | 24 | R1_diag = numpy.linalg.qr(Z, mode='economic').diagonal().round(round) 25 | dbi = [] 26 | ddi = [] 27 | for i, e in enumerate(R1_diag): 28 | if e != 0: 29 | dbi.append(i) 30 | else: 31 | ddi.append(i) 32 | dbn = len(dbi) 33 | 34 | P = numpy.mat(numpy.eye(parm_num))[:, dbi + ddi] 35 | Pb = P[:, :dbn] 36 | Pd = P[:, dbn:] 37 | 38 | Rbd1 = numpy.mat(numpy.linalg.qr(Z * P, mode='r')) 39 | Rb1 = Rbd1[:dbn, :dbn] 40 | Rd1 = Rbd1[:dbn, dbn:] 41 | 42 | Kd = numpy.mat((numpy.linalg.inv(Rb1) * Rd1).round(round)) 43 | 44 | return Pb, Pd, Kd 45 | -------------------------------------------------------------------------------- /sympybotics/dynamics/dynamics.py: -------------------------------------------------------------------------------- 1 | import sympy 2 | import numpy 3 | 4 | from .rne import rne, gravityterm, coriolisterm, coriolismatrix,\ 5 | frictionterm, inertiamatrix 6 | from .regressor import regressor 7 | from .dyn_parm_dep import find_dyn_parm_deps 8 | 9 | 10 | class Dynamics(object): 11 | 12 | def __init__(self, rbtdef, geom): 13 | 14 | self.rbtdef = rbtdef 15 | self.geom = geom 16 | self.dof = rbtdef.dof 17 | 18 | self.dynparms = sympy.Matrix(rbtdef.dynparms()) 19 | self.n_dynparms = len(self.dynparms) 20 | 21 | def gen_invdyn(self, ifunc=None): 22 | self.invdyn = rne(self.rbtdef, self.geom, ifunc) 23 | 24 | def gen_gravityterm(self, ifunc=None): 25 | self.g = gravityterm(self.rbtdef, self.geom, ifunc) 26 | 27 | def gen_coriolisterm(self, ifunc=None): 28 | self.c = coriolisterm( 29 | self.rbtdef, self.geom, ifunc) 30 | 31 | def gen_coriolismatrix(self, ifunc=None): 32 | self.C = coriolismatrix( 33 | self.rbtdef, self.geom, ifunc) 34 | 35 | def gen_frictionterm(self, ifunc=None): 36 | self.f = frictionterm(self.rbtdef, ifunc) 37 | 38 | def gen_inertiamatrix(self, ifunc=None): 39 | self.M = inertiamatrix( 40 | self.rbtdef, self.geom, ifunc) 41 | 42 | def gen_regressor(self, ifunc=None): 43 | self.H = regressor(self.rbtdef, self.geom, ifunc) 44 | 45 | def gen_all(self, ifunc=None): 46 | self.gen_invdyn(ifunc) 47 | self.gen_gravityterm(ifunc) 48 | self.gen_coriolisterm(ifunc) 49 | self.gen_inertiamatrix(ifunc) 50 | self.gen_regressor(ifunc) 51 | 52 | def calc_base_parms(self, regressor_func): 53 | 54 | Pb, Pd, Kd = find_dyn_parm_deps( 55 | self.dof, self.n_dynparms, regressor_func) 56 | 57 | self.Pb = sympy.Matrix(Pb).applyfunc(lambda x: x.nsimplify()) 58 | self.Pd = sympy.Matrix(Pd).applyfunc(lambda x: x.nsimplify()) 59 | self.Kd = sympy.Matrix(Kd).applyfunc(lambda x: x.nsimplify()) 60 | 61 | self.base_idxs = \ 62 | (numpy.matrix([[i for i in range(self.n_dynparms)]]) * 63 | numpy.matrix(Pb)).astype(float).astype(int).tolist()[0] 64 | 65 | self.baseparms = (self.Pb.T + self.Kd * self.Pd.T) * self.dynparms 66 | self.n_base = len(self.baseparms) 67 | -------------------------------------------------------------------------------- /sympybotics/dynamics/extra_dyn.py: -------------------------------------------------------------------------------- 1 | """ Extra dynamic terms modelation """ 2 | 3 | from sympy import zeros, sign 4 | from ..utils import identity 5 | 6 | 7 | _frictionterms = set(['Coulomb', 'viscous', 'offset']) 8 | 9 | 10 | def frictionforce(rbtdef, ifunc=None): 11 | '''Generate friction forces (Coulomb/viscouse model plus offset).''' 12 | 13 | if not ifunc: 14 | ifunc = identity 15 | 16 | fric = zeros(rbtdef.dof, 1) 17 | 18 | if rbtdef.frictionmodel is None or len(rbtdef.frictionmodel) == 0: 19 | pass 20 | else: 21 | askedterms = set(rbtdef.frictionmodel) 22 | if askedterms.issubset(_frictionterms): 23 | if 'viscous' in askedterms: 24 | for i in range(rbtdef.dof): 25 | fric[i] += rbtdef.fv[i] * rbtdef.dq[i] 26 | if 'Coulomb' in askedterms: 27 | for i in range(rbtdef.dof): 28 | fric[i] += rbtdef.fc[i] * sign(rbtdef.dq[i]) 29 | if 'offset' in askedterms: 30 | for i in range(rbtdef.dof): 31 | fric[i] += rbtdef.fo[i] 32 | fric[i] = ifunc(fric[i]) 33 | else: 34 | raise Exception( 35 | 'Friction model terms \'%s\' not understanded. Use None or a' 36 | ' combination of %s.' % 37 | (str(askedterms - _frictionterms), _frictionterms)) 38 | 39 | return fric 40 | 41 | 42 | def driveinertiaterm(rbtdef, ifunc=None): 43 | '''Generate drive inertia term (Siplified, neglets gyroscopic effects).''' 44 | 45 | if not ifunc: 46 | ifunc = identity 47 | 48 | driveinertia = zeros(rbtdef.dof, 1) 49 | 50 | if rbtdef.driveinertiamodel is None: 51 | pass 52 | elif rbtdef.driveinertiamodel == 'simplified': 53 | for i in range(rbtdef.dof): 54 | driveinertia[i] = rbtdef.Ia[i] * rbtdef.ddq[i] 55 | driveinertia[i] = ifunc(driveinertia[i]) 56 | else: 57 | raise Exception('Drive inertia model \'%s\' not understanded. Use' 58 | ' None or \'simplified\'.' % rbtdef.driveinertiamodel) 59 | 60 | return driveinertia 61 | -------------------------------------------------------------------------------- /sympybotics/dynamics/regressor.py: -------------------------------------------------------------------------------- 1 | from copy import copy, deepcopy 2 | from sympy import zeros, Matrix 3 | from ..utils import identity 4 | from .rne import rne_forward, rne_backward 5 | 6 | 7 | def regressor(rbtdef, geom, ifunc=None): 8 | '''Generate regression matrix.''' 9 | 10 | if not ifunc: 11 | ifunc = identity 12 | 13 | fw_results = rne_forward(rbtdef, geom, ifunc) 14 | 15 | rbtdeftmp = deepcopy(rbtdef) 16 | 17 | dynparms = rbtdef.dynparms() 18 | 19 | Y = zeros(rbtdef.dof, len(dynparms)) 20 | 21 | for p, parm in enumerate(dynparms): 22 | 23 | for i in range(rbtdef.dof): 24 | rbtdeftmp.Le[i] = list(map( 25 | lambda x: 1 if x == parm else 0, rbtdef.Le[i])) 26 | rbtdeftmp.l[i] = Matrix(rbtdef.l[i]).applyfunc( 27 | lambda x: 1 if x == parm else 0) 28 | rbtdeftmp.m[i] = 1 if rbtdef.m[i] == parm else 0 29 | rbtdeftmp.Ia[i] = 1 if rbtdef.Ia[i] == parm else 0 30 | rbtdeftmp.fv[i] = 1 if rbtdef.fv[i] == parm else 0 31 | rbtdeftmp.fc[i] = 1 if rbtdef.fc[i] == parm else 0 32 | rbtdeftmp.fo[i] = 1 if rbtdef.fo[i] == parm else 0 33 | 34 | Y[:, p] = rne_backward(rbtdeftmp, geom, fw_results, ifunc=ifunc) 35 | 36 | return Y 37 | -------------------------------------------------------------------------------- /sympybotics/dynamics/rne.py: -------------------------------------------------------------------------------- 1 | from copy import deepcopy 2 | from sympy import zeros 3 | from ..utils import identity 4 | from ..geometry import Geometry 5 | from .rne_park import rne_park_forward, rne_park_backward 6 | from .rne_khalil import rne_khalil_forward, rne_khalil_backward 7 | from .extra_dyn import frictionforce 8 | 9 | 10 | def rne_forward(rbtdef, geom, ifunc=None): 11 | if rbtdef._dh_convention == 'standard': 12 | rne_forward = rne_park_forward 13 | elif rbtdef._dh_convention == 'modified': 14 | rne_forward = rne_khalil_forward 15 | return rne_forward(rbtdef, geom, ifunc) 16 | 17 | 18 | def rne_backward(rbtdef, geom, fw_results, ifunc=None): 19 | if rbtdef._dh_convention == 'standard': 20 | rne_backward = rne_park_backward 21 | elif rbtdef._dh_convention == 'modified': 22 | rne_backward = rne_khalil_backward 23 | return rne_backward(rbtdef, geom, fw_results, ifunc) 24 | 25 | 26 | def rne(rbtdef, geom, ifunc=None): 27 | '''Generate joints generic forces/torques equation.''' 28 | 29 | fw_results = rne_forward(rbtdef, geom, ifunc) 30 | invdyn = rne_backward(rbtdef, geom, fw_results, ifunc=ifunc) 31 | 32 | return invdyn 33 | 34 | 35 | def gravityterm(rbtdef, geom, ifunc=None): 36 | '''Generate gravity force equation.''' 37 | if not ifunc: 38 | ifunc = identity 39 | rbtdeftmp = deepcopy(rbtdef) 40 | rbtdeftmp.dq = zeros(rbtdeftmp.dof, 1) 41 | rbtdeftmp.ddq = zeros(rbtdeftmp.dof, 1) 42 | rbtdeftmp.frictionmodel = None 43 | geomtmp = Geometry(rbtdeftmp) 44 | return rne(rbtdeftmp, geomtmp, ifunc) 45 | 46 | 47 | def coriolisterm(rbtdef, geom, ifunc=None): 48 | '''Generate Coriolis and centriptal forces equation.''' 49 | if not ifunc: 50 | ifunc = identity 51 | rbtdeftmp = deepcopy(rbtdef) 52 | rbtdeftmp.gravityacc = zeros(3, 1) 53 | rbtdeftmp.ddq = zeros(rbtdeftmp.dof, 1) 54 | rbtdeftmp.frictionmodel = None 55 | geomtmp = Geometry(rbtdeftmp) 56 | return rne(rbtdeftmp, geomtmp, ifunc) 57 | 58 | 59 | def coriolismatrix(rbtdef, geom, ifunc=None): 60 | '''Generate Coriolis matrix (non-unique).''' 61 | 62 | if not ifunc: 63 | ifunc = identity 64 | 65 | C = zeros(rbtdef.dof, rbtdef.dof) 66 | 67 | rbtdeftmp = deepcopy(rbtdef) 68 | rbtdeftmp.gravityacc = zeros(3, 1) 69 | rbtdeftmp.ddq = zeros(rbtdeftmp.dof, 1) 70 | rbtdeftmp.frictionmodel = None 71 | 72 | # # Coriolis vector c element k is given by 73 | # # c[k] = dq.T * A[k] * dq[k] 74 | # # where A[k] is a (non-unique) matrix 75 | 76 | # # Find upper triangular A[k]'s 77 | 78 | A = [zeros(rbtdef.dof, rbtdef.dof) for k in range(rbtdef.dof)] 79 | 80 | for i in range(rbtdef.dof): 81 | rbtdeftmp.dq = zeros(rbtdeftmp.dof, 1) 82 | rbtdeftmp.dq[i] = 1 83 | geomtmp = Geometry(rbtdeftmp) 84 | fw_results = rne_forward(rbtdeftmp, geomtmp, ifunc) 85 | c = rne_backward(rbtdeftmp, geomtmp, fw_results, ifunc) 86 | for k in range(rbtdef.dof): 87 | A[k][i, i] = c[k] 88 | 89 | for i in range(rbtdef.dof): 90 | for j in range(i+1, rbtdef.dof): 91 | rbtdeftmp.dq = zeros(rbtdeftmp.dof, 1) 92 | rbtdeftmp.dq[i] = rbtdeftmp.dq[j] = 1 93 | geomtmp = Geometry(rbtdeftmp) 94 | fw_results = rne_forward(rbtdeftmp, geomtmp, ifunc) 95 | c = rne_backward(rbtdeftmp, geomtmp, fw_results, ifunc) 96 | for k in range(rbtdef.dof): 97 | A[k][i, j] = ifunc(c[k] - A[k][i, i] - A[k][j, j]) 98 | 99 | # # Coriolis vector c is 100 | # # c[k] = dq.T * A[k] * dq[k] 101 | # # thus, Coriolis matrix is 102 | # # C[k, :] = dq.T * A[k] 103 | # # so that 104 | # # c[k] = C[k, :] * dq 105 | 106 | for k in range(rbtdef.dof): 107 | C[k, :] = rbtdef.dq.T * A[k] 108 | 109 | C = ifunc(C) 110 | 111 | return C 112 | 113 | 114 | def frictionterm(rbtdef, ifunc=None): 115 | '''Generate friction forces expression.''' 116 | return frictionforce(rbtdef, ifunc) 117 | 118 | 119 | def inertiamatrix(rbtdef, geom, ifunc=None): 120 | '''Generate mass matrix.''' 121 | 122 | if not ifunc: 123 | ifunc = identity 124 | 125 | M = zeros(rbtdef.dof, rbtdef.dof) 126 | 127 | rbtdeftmp = deepcopy(rbtdef) 128 | rbtdeftmp.gravityacc = zeros(3, 1) 129 | rbtdeftmp.frictionmodel = None 130 | rbtdeftmp.dq = zeros(rbtdeftmp.dof, 1) 131 | 132 | for i in range(M.rows): 133 | rbtdeftmp.ddq = zeros(rbtdeftmp.dof, 1) 134 | rbtdeftmp.ddq[i] = 1 135 | geomtmp = Geometry(rbtdeftmp) 136 | 137 | fw_results = rne_forward(rbtdeftmp, geomtmp, ifunc) 138 | Mcoli = rne_backward(rbtdeftmp, geomtmp, fw_results, ifunc=ifunc) 139 | 140 | # It's done like this since M is symmetric: 141 | M[:, i] = (M[i, :i].T) .col_join(Mcoli[i:, :]) 142 | 143 | return M 144 | -------------------------------------------------------------------------------- /sympybotics/dynamics/rne_khalil.py: -------------------------------------------------------------------------------- 1 | from sympy import zeros, eye, Matrix 2 | from .extra_dyn import frictionforce, driveinertiaterm 3 | from ..utils import sym_skew as skew 4 | from ..utils import identity 5 | 6 | 7 | def rne_khalil_forward(rbtdef, geom, ifunc=None): 8 | '''RNE forward pass.''' 9 | 10 | if not ifunc: 11 | ifunc = identity 12 | 13 | w = list(range(0, rbtdef.dof + 1)) 14 | dw = list(range(0, rbtdef.dof + 1)) 15 | dV = list(range(0, rbtdef.dof + 1)) 16 | U = list(range(0, rbtdef.dof + 1)) 17 | 18 | w[-1] = zeros(3, 1) 19 | dw[-1] = zeros(3, 1) 20 | dV[-1] = -rbtdef.gravityacc 21 | U[-1] = zeros(3, 3) 22 | 23 | z = Matrix([0, 0, 1]) 24 | 25 | # Forward 26 | for i in range(rbtdef.dof): 27 | 28 | s = rbtdef._links_sigma[i] 29 | ns = 1 - s 30 | 31 | w_pj = geom.Rdh[i].T * w[i - 1] 32 | 33 | w[i] = w_pj + ns * rbtdef.dq[i] * z 34 | w[i] = ifunc(w[i]) 35 | 36 | dw[i] = geom.Rdh[i].T * dw[i - 1] + ns * \ 37 | (rbtdef.ddq[i] * z + w_pj.cross(rbtdef.dq[i] * z).reshape(3, 1)) 38 | dw[i] = ifunc(dw[i]) 39 | 40 | dV[i] = geom.Rdh[i].T * (dV[i - 1] + U[i - 1] * geom.pdh[i]) + s * ( 41 | rbtdef.ddq[i] * z + 2 * w_pj.cross(rbtdef.dq[i] * z).reshape(3, 1)) 42 | dV[i] = ifunc(dV[i]) 43 | 44 | U[i] = skew(dw[i]) + skew(w[i]) ** 2 45 | U[i] = ifunc(U[i]) 46 | 47 | return w, dw, dV, U 48 | 49 | 50 | def rne_khalil_backward(rbtdef, geom, fw_results, ifunc=None): 51 | '''RNE backward pass.''' 52 | 53 | w, dw, dV, U = fw_results 54 | 55 | if not ifunc: 56 | ifunc = identity 57 | 58 | # extend Rdh so that Rdh[dof] return identity 59 | Rdh = geom.Rdh + [eye(3)] 60 | # extend pdh so that pRdh[dof] return zero 61 | pdh = geom.pdh + [zeros(3, 1)] 62 | 63 | F = list(range(rbtdef.dof)) 64 | M = list(range(rbtdef.dof)) 65 | f = list(range(rbtdef.dof + 1)) 66 | m = list(range(rbtdef.dof + 1)) 67 | 68 | f[rbtdef.dof] = zeros(3, 1) 69 | m[rbtdef.dof] = zeros(3, 1) 70 | 71 | z = Matrix([0, 0, 1]) 72 | 73 | tau = zeros(rbtdef.dof, 1) 74 | 75 | fric = frictionforce(rbtdef) 76 | Idrive = driveinertiaterm(rbtdef) 77 | 78 | # Backward 79 | for i in range(rbtdef.dof - 1, -1, -1): 80 | 81 | s = rbtdef._links_sigma[i] 82 | ns = 1 - s 83 | 84 | F[i] = rbtdef.m[i] * dV[i] + U[i] * Matrix(rbtdef.l[i]) 85 | F[i] = ifunc(F[i]) 86 | 87 | M[i] = rbtdef.L[i] * dw[i] + w[i].cross( 88 | rbtdef.L[i] * w[i]).reshape(3, 1) + \ 89 | Matrix(rbtdef.l[i]).cross(dV[i]).reshape(3, 1) 90 | M[i] = ifunc(M[i]) 91 | 92 | f_nj = Rdh[i + 1] * f[i + 1] 93 | 94 | f[i] = F[i] + f_nj # + f_e[i] 95 | f[i] = ifunc(f[i]) 96 | 97 | m[i] = M[i] + Rdh[i + 1] * m[i + 1] + \ 98 | pdh[i + 1].cross(f_nj).reshape(3, 1) # + m_e[i] 99 | m[i] = ifunc(m[i]) 100 | 101 | tau[i] = ifunc(((s * f[i] + ns * m[i]).T * z)[0] + fric[i] + Idrive[i]) 102 | 103 | return tau 104 | -------------------------------------------------------------------------------- /sympybotics/dynamics/rne_park.py: -------------------------------------------------------------------------------- 1 | from sympy import zeros, eye 2 | from .extra_dyn import frictionforce, driveinertiaterm 3 | from ..utils import sym_skew as skew 4 | from ..utils import identity 5 | 6 | 7 | def Adj(G, g): 8 | R = G[0:3, 0:3] 9 | p = G[0:3, 3] 10 | return (R.row_join(zeros(3))).col_join( 11 | (skew(p) * R).row_join(R)) * g 12 | 13 | 14 | def Adjdual(G, g): 15 | R = G[0:3, 0:3] 16 | p = G[0:3, 3] 17 | return ((R.row_join(zeros(3))).col_join( 18 | (skew(p) * R).row_join(R))).transpose() * g 19 | 20 | 21 | def adj(g, h): 22 | wg = g[0:3, 0] 23 | vg = g[3:6, 0] 24 | return (skew(wg).row_join(zeros(3))).col_join( 25 | (skew(vg)).row_join(skew(wg))) * h 26 | 27 | 28 | def adjdual(g, h): 29 | wg = g[0:3, 0] 30 | vg = g[3:6, 0] 31 | return ((skew(wg).row_join(zeros(3))).col_join( 32 | (skew(vg)).row_join(skew(wg)))).transpose() * h 33 | 34 | 35 | def rne_park_forward(rbtdef, geom, ifunc=None): 36 | '''RNE forward pass.''' 37 | 38 | if not ifunc: 39 | ifunc = identity 40 | 41 | V = list(range(0, rbtdef.dof + 1)) 42 | dV = list(range(0, rbtdef.dof + 1)) 43 | 44 | V[-1] = zeros(6, 1) 45 | dV[-1] = - zeros(3, 1).col_join(rbtdef.gravityacc) 46 | 47 | # Forward 48 | for i in range(rbtdef.dof): 49 | 50 | V[i] = ifunc(Adj(geom.Tdh_inv[i], V[i - 1])) + \ 51 | ifunc(geom.S[i] * rbtdef.dq[i]) 52 | V[i] = ifunc(V[i]) 53 | 54 | dV[i] = ifunc(geom.S[i] * rbtdef.ddq[i]) + \ 55 | ifunc(Adj(geom.Tdh_inv[i], dV[i - 1])) + \ 56 | ifunc(adj(ifunc(Adj(geom.Tdh_inv[i], V[i - 1])), 57 | ifunc(geom.S[i] * rbtdef.dq[i]))) 58 | dV[i] = ifunc(dV[i]) 59 | 60 | return V, dV 61 | 62 | 63 | def rne_park_backward(rbtdef, geom, fw_results, ifunc=None): 64 | '''RNE backward pass.''' 65 | 66 | V, dV = fw_results 67 | 68 | if not ifunc: 69 | ifunc = identity 70 | 71 | # extend Tdh_inv so that Tdh_inv[dof] return identity 72 | Tdh_inv = geom.Tdh_inv + [eye(4)] 73 | 74 | F = list(range(rbtdef.dof + 1)) 75 | F[rbtdef.dof] = zeros(6, 1) 76 | 77 | tau = zeros(rbtdef.dof, 1) 78 | 79 | fric = frictionforce(rbtdef) 80 | Idrive = driveinertiaterm(rbtdef) 81 | 82 | # Backward 83 | for i in range(rbtdef.dof - 1, -1, -1): 84 | 85 | Llm = (rbtdef.L[i].row_join(skew(rbtdef.l[i]))).col_join( 86 | (-skew(rbtdef.l[i])).row_join(eye(3) * rbtdef.m[i])) 87 | 88 | F[i] = Adjdual(Tdh_inv[i + 1], F[i + 1]) + \ 89 | Llm * dV[i] - adjdual(V[i], Llm * V[i]) 90 | F[i] = ifunc(F[i]) 91 | 92 | tau[i] = ifunc((geom.S[i].transpose() * F[i])[0] + fric[i] + Idrive[i]) 93 | 94 | return tau 95 | -------------------------------------------------------------------------------- /sympybotics/dynident/__init__.py: -------------------------------------------------------------------------------- 1 | from . import regression 2 | -------------------------------------------------------------------------------- /sympybotics/dynident/regression.py: -------------------------------------------------------------------------------- 1 | 2 | class NotAvailableError(Exception): 3 | def __init__(self, function_name): 4 | msg = 'Function %s not available since cvxopt package '\ 5 | 'was not found' % function_name 6 | Exception.__init__(self, msg) 7 | 8 | try: 9 | import cvxopt 10 | except ImportError: 11 | cvxopt = None 12 | else: 13 | import cvxopt 14 | import cvxopt.solvers 15 | 16 | 17 | import numpy 18 | import sympy 19 | 20 | 21 | def mrepl(m,repl): 22 | return m.applyfunc(lambda x: x.xreplace(repl)) 23 | 24 | def skew(v): 25 | return sympy.Matrix( [ [ 0, -v[2], v[1] ], 26 | [ v[2], 0, -v[0] ], 27 | [ -v[1], v[0], 0 ] ] ) 28 | 29 | def regr_matrices( dof, parm_num, q, dq, ddq, tau, regr_func): 30 | 31 | sn = q.shape[0] 32 | 33 | H_S = numpy.matrix( numpy.zeros( ( dof*sn, parm_num ) ) ) 34 | tau_S = numpy.matrix( numpy.zeros( dof*sn ) ).T 35 | 36 | for i in range(sn): 37 | H_S[ i*dof : i*dof+dof , : ] = numpy.array( regr_func( q[i], dq[i], ddq[i] ) ).reshape(dof,parm_num) 38 | 39 | for i in range(sn): 40 | tau_S[ i*dof : i*dof+dof ] = numpy.mat( tau[i] ).T 41 | 42 | return H_S,tau_S 43 | 44 | 45 | 46 | 47 | def get_diag_blocks(M): 48 | b = [] 49 | n = M.shape[0] 50 | c = 0 51 | while c < n-1: 52 | for l in range(n-1,c,-1): 53 | if M[l,c] != 0 or M[c,l] != 0: 54 | break 55 | elif l == c+1: 56 | b.append(l) 57 | c = l 58 | return b + [n] 59 | 60 | 61 | 62 | 63 | 64 | def prepare_sdp( var_symbs, LMI_matrix, split_diag_blocks=True): 65 | v = var_symbs 66 | vn = len(v) 67 | 68 | if isinstance( LMI_matrix, list ) or isinstance( LMI_matrix, tuple ): 69 | 70 | blocks_LMI_matrix = LMI_matrix 71 | 72 | else: 73 | 74 | if split_diag_blocks: 75 | 76 | blocks = get_diag_blocks(LMI_matrix) 77 | blocks_LMI_matrix = [] 78 | for a,b in zip( [0]+blocks[:-1] , blocks ): 79 | blocks_LMI_matrix.append( LMI_matrix[ a:b, a:b ] ) 80 | print('Split into %d diagonal blocks.'%(len(blocks))) 81 | 82 | else: 83 | 84 | blocks_LMI_matrix = [ LMI_matrix ] 85 | 86 | 87 | blocks_Fi = [] 88 | 89 | zero_subs = dict(zip(v,[0]*vn)) 90 | 91 | for LMI_matrix in blocks_LMI_matrix: 92 | 93 | Fi = [0]*(1+vn) 94 | 95 | Fsym = mrepl( LMI_matrix, zero_subs ) 96 | Fi[0] = numpy.matrix(Fsym).astype(float) 97 | 98 | for i,s in enumerate(v): 99 | Fsym = LMI_matrix.applyfunc(lambda x: 0 if not x.coeff(s) else x.coeff(s)) 100 | Fi[i+1] = numpy.matrix(Fsym).astype(float) 101 | 102 | blocks_Fi.append( Fi ) 103 | 104 | return blocks_Fi 105 | 106 | 107 | def sdp( c, Ai_blocks, primalstar=None, dualstart=None, solver='dsdp', verbose=0, interpret=False, maxiters=1000, tolerance=10e-7 ): 108 | if cvxopt is None: 109 | raise NotAvailableError(sdp.__name__) 110 | 111 | 112 | c = cvxopt.matrix( c ) 113 | 114 | h = [] 115 | G = [] 116 | 117 | for A in Ai_blocks: 118 | h.append( cvxopt.matrix( A[0].astype(float).tolist() ) ) 119 | G.append( cvxopt.matrix( [ (-A[i]).flatten().astype(float).tolist()[0] for i in range(1,len(A)) ] ) ) 120 | 121 | if solver != 'dsdp' and solver != 'conelp': 122 | raise Exception('error: unknown solver (available: dsdp or conelp)') 123 | 124 | if solver == 'conelp': solver == '' 125 | 126 | cvxopt.solvers.options['show_progress'] = (1 if verbose > 0 else 0) #True/False (default: True) 127 | cvxopt.solvers.options['maxiters'] = maxiters #positive integer (default: 100) 128 | # cvxopt.solvers.options['refinement'] #positive integer (default: 1) 129 | cvxopt.solvers.options['abstol'] = tolerance #scalar (default: 1e-7) 130 | cvxopt.solvers.options['reltol'] = tolerance #scalar (default: 1e-6) 131 | cvxopt.solvers.options['feastol'] = tolerance #scalar (default: 1e-7). 132 | 133 | cvxopt.solvers.options['DSDP_Monitor'] = (1 if verbose > 0 else 0) #True/False (default: 0) 134 | cvxopt.solvers.options['DSDP_MaxIts'] = maxiters #positive integer 135 | cvxopt.solvers.options['DSDP_GapTolerance'] = tolerance #scalar (default: 1e-5). 136 | 137 | sol = cvxopt.solvers.sdp(c, Gs=G, hs=h, solver=solver) 138 | 139 | if not interpret: 140 | if verbose >= 0: print(sol['status']) 141 | return sol 142 | 143 | if verbose > 0: 144 | print('------------------------------\nsol[\'status\'] : '+sol['status']+'\n\n____________________________________') 145 | 146 | if sol['status'] == 'optimal': 147 | if verbose >= 0: print('optimal') 148 | return numpy.matrix(sol['x']) 149 | 150 | else: 151 | 152 | sp = 'residual as primal infeasibility certificate' 153 | sd = 'residual as dual infeasibility certificate' 154 | 155 | if solver == 'dsdp' and sol['status'] == 'unknown': 156 | 157 | if sol[sp] is not None: 158 | s = 'primal infeasible' 159 | elif sol[sp] is not None: 160 | s = 'dual infeasible' 161 | else: 162 | s = 'unknown' 163 | 164 | if verbose >= 0: print(s) 165 | if verbose >= 0: print(sp + ': ' + str(sol[sp]) + '\n' + sd + ': ' + str(sol[sd])) 166 | return 167 | 168 | 169 | else: 170 | 171 | if verbose >= 0: print(sol['status']) 172 | if verbose >= 0: print(sp + ': ' + str(sol[sp]) + '\n' + sd + ': ' + str(sol[sd])) 173 | return 174 | 175 | 176 | -------------------------------------------------------------------------------- /sympybotics/geometry.py: -------------------------------------------------------------------------------- 1 | import sympy 2 | 3 | from .robotdef import _joint_i_symb 4 | 5 | _id = lambda x: x 6 | 7 | 8 | class Geometry(object): 9 | 10 | """Robot symbolic geometric transformations.""" 11 | 12 | def __init__(self, robotdef, ifunc=None): 13 | 14 | if not ifunc: 15 | ifunc = _id 16 | 17 | self.rbtdef = robotdef 18 | self.dof = self.rbtdef.dof 19 | 20 | def inverse_T(T): 21 | return T[0:3, 0:3].transpose().row_join( 22 | - T[0:3, 0:3].transpose() * T[0:3, 3]).col_join( 23 | sympy.zeros(1, 3).row_join(sympy.eye(1))) 24 | 25 | (alpha, a, d, theta) = sympy.symbols('alpha,a,d,theta', real=True) 26 | 27 | self.Tdh = list(range(self.rbtdef.dof)) 28 | self.Tdh_inv = list(range(self.rbtdef.dof)) 29 | self.Rdh = list(range(self.rbtdef.dof)) 30 | self.pdh = list(range(self.rbtdef.dof)) 31 | 32 | dh_transfmat_inv = inverse_T(self.rbtdef._dh_transfmat) 33 | 34 | q_subs = dict([(_joint_i_symb(i + 1), self.rbtdef.q[i]) 35 | for i in range(self.rbtdef.dof)]) 36 | 37 | for l in range(self.rbtdef.dof): 38 | 39 | subs_dict = dict( 40 | zip(self.rbtdef._dh_symbols, self.rbtdef._dh_parms[l])) 41 | 42 | self.Tdh[l] = self.rbtdef._dh_transfmat.subs( 43 | subs_dict).subs(q_subs) 44 | self.Tdh_inv[l] = dh_transfmat_inv.subs(subs_dict).subs(q_subs) 45 | self.Rdh[l] = self.Tdh[l][0:3, 0:3] 46 | self.pdh[l] = self.Tdh[l][0:3, 3] 47 | 48 | self.T = list(range(self.rbtdef.dof)) 49 | 50 | # set T[-1] so that T[l-1] for l=0 is correctly assigned 51 | # T[-1] is override after 52 | self.T[-1] = sympy.eye(4) 53 | for l in range(self.rbtdef.dof): 54 | self.T[l] = ifunc(self.T[l - 1] * self.Tdh[l]) 55 | 56 | self.R = list(range(self.rbtdef.dof)) 57 | self.p = list(range(self.rbtdef.dof)) 58 | self.z = list(range(self.rbtdef.dof)) 59 | 60 | for l in range(self.rbtdef.dof): 61 | self.R[l] = self.T[l][0:3, 0:3] 62 | self.p[l] = self.T[l][0:3, 3] 63 | self.z[l] = self.R[l][0:3, 2] 64 | 65 | # 66 | # for screw theory: 67 | 68 | if self.rbtdef._dh_convention == 'standard': 69 | cos = sympy.cos 70 | sin = sympy.sin 71 | 72 | Mr = sympy.Matrix([[1, 0, 0, a], 73 | [0, cos(alpha), -sin(alpha), 0], 74 | [0, sin(alpha), cos(alpha), d], 75 | [0, 0, 0, 1]]) 76 | Pr = sympy.Matrix([[0, -1, 0, 0], 77 | [1, 0, 0, 0], 78 | [0, 0, 0, 0], 79 | [0, 0, 0, 0]]) 80 | # from Frank Park paper: 81 | # Mp = sympy.Matrix( 82 | # [[cos(theta), -sin(theta) * cos(alpha), 0, a * cos(theta)], 83 | # [sin(theta), cos(theta) * cos(alpha), -sin(alpha), 84 | # a * sin(theta)], 85 | # [0, sin(alpha), cos(alpha), 0], 86 | # [0, 0, 0, 1]]) 87 | # my own: 88 | Mp = sympy.Matrix( 89 | [[cos(theta), -sin(theta) * cos(alpha), 90 | sin(theta) * sin(alpha), a * cos(theta)], 91 | [sin(theta), cos(theta) * cos(alpha), 92 | -cos(theta) * sin(alpha), a * sin(theta)], 93 | [0, sin(alpha), cos(alpha), 0], 94 | [0, 0, 0, 1]]) 95 | Pp = sympy.Matrix([[0, 0, 0, 0], 96 | [0, 0, 0, 0], 97 | [0, 0, 0, 1], 98 | [0, 0, 0, 0]]) 99 | 100 | # if 0: 101 | # D_exp2trig = { exp(SR.wild(0)) : exp(SR.wild(0).real_part()) 102 | # * ( cos(SR.wild(0).imag_part()) + 103 | # sympy.I*sin(SR.wild(0).imag_part()) ) } 104 | # ePtM_r = exp( Pr*theta ) * Mr 105 | # ePtM_r = ePtM_r.expand().subs(D_exp2trig).simplify_rational() 106 | # ePtM_p = exp( Pp*d ) * Mp 107 | # ePtM_p = ePtM_p.expand().subs(D_exp2trig).simplify_rational() 108 | # if bool( ePtM_r != self.rbtdef._dh_transfmat or ePtM_p ! 109 | # self.rbtdef._dh_transfmat ): 110 | # raise Exception('Geom: interlink transformation does not 111 | # follows implemented DH formulation') 112 | 113 | Sr = (inverse_T(Mr) * Pr * Mr).applyfunc( 114 | lambda x: sympy.trigsimp(x)) 115 | Sp = (inverse_T(Mp) * Pp * Mp).applyfunc( 116 | lambda x: sympy.trigsimp(x)) 117 | 118 | def sym_se3_unskew(g): 119 | w = sympy.Matrix([g[2, 1], g[0, 2], g[1, 0]]) 120 | v = g[0:3, 3] 121 | return w.col_join(v) 122 | 123 | self.S = list(range(self.rbtdef.dof)) 124 | for l in range(self.rbtdef.dof): 125 | if self.rbtdef._links_sigma[l]: 126 | S = Sp 127 | else: 128 | S = Sr 129 | self.S[l] = ifunc(sym_se3_unskew(S.subs(dict(zip( 130 | self.rbtdef._dh_symbols, self.rbtdef._dh_parms[l]))))) 131 | -------------------------------------------------------------------------------- /sympybotics/kinematics.py: -------------------------------------------------------------------------------- 1 | 2 | import sympy 3 | 4 | _id = lambda x: x 5 | 6 | 7 | class Kinematics(object): 8 | 9 | """Robot symbolic Jacobians. 10 | 11 | kinobj.J: list of link frame Jacobians - complete (6 x N): 12 | [linear_velocity 13 | angular_velocity] = J * joint_velocities 14 | kinobj.Jc: list of link center-of-mass Jacobians - complete 15 | kinobj.Jp: list of link frame Jacobians - linear velocity part only 16 | kinobj.Jo: list of link frame Jacobians - angular velocity part only 17 | kinobj.Jcp: list of link center-of-mass Jacobians - linear part 18 | kinobj.Jco: list of link center-of-mass Jacobians - angular part 19 | """ 20 | 21 | def __init__(self, robotdef, geom, ifunc=None): 22 | 23 | if not ifunc: 24 | ifunc = _id 25 | 26 | self.rbtdef = robotdef 27 | self.geom = geom 28 | self.dof = self.rbtdef.dof 29 | 30 | def sym_skew(v): 31 | return sympy.Matrix([[0, -v[2], v[1]], 32 | [v[2], 0, -v[0]], 33 | [-v[1], v[0], 0]]) 34 | 35 | if self.rbtdef._dh_convention == 'standard': 36 | 37 | # extend z and p so that z[-1] and p[-1] return values from base 38 | # frame 39 | z_ext = geom.z + [sympy.Matrix([0, 0, 1])] 40 | p_ext = geom.p + [sympy.zeros(3, 1)] 41 | 42 | self.Jp = list(range(self.rbtdef.dof)) 43 | for l in range(self.rbtdef.dof): 44 | self.Jp[l] = sympy.zeros(3, self.rbtdef.dof) 45 | for j in range(l + 1): 46 | if self.rbtdef._links_sigma[j]: 47 | self.Jp[l][0:3, j] = ifunc(z_ext[j - 1]) 48 | else: 49 | self.Jp[l][0:3, j] = ifunc(z_ext[j - 1].cross( 50 | (p_ext[l] - p_ext[j - 1])).reshape(3, 1)) 51 | 52 | self.Jo = list(range(self.rbtdef.dof)) 53 | for l in range(self.rbtdef.dof): 54 | self.Jo[l] = sympy.zeros(3, self.rbtdef.dof) 55 | for j in range(l + 1): 56 | if self.rbtdef._links_sigma[j]: 57 | self.Jo[l][0:3, j] = sympy.zeros(3, 1) 58 | else: 59 | self.Jo[l][0:3, j] = ifunc(z_ext[j - 1]) 60 | 61 | elif self.rbtdef._dh_convention == 'modified': 62 | 63 | self.Jp = list(range(self.rbtdef.dof)) 64 | for l in range(self.rbtdef.dof): 65 | self.Jp[l] = sympy.zeros(3, self.rbtdef.dof) 66 | for j in range(l + 1): 67 | if self.rbtdef._links_sigma[j]: 68 | self.Jp[l][0:3, j] = ifunc(geom.z[j]) 69 | else: 70 | self.Jp[l][0:3, j] = ifunc(geom.z[j].cross( 71 | (geom.p[l] - geom.p[j])).reshape(3, 1)) 72 | 73 | self.Jo = list(range(self.rbtdef.dof)) 74 | for l in range(self.rbtdef.dof): 75 | self.Jo[l] = sympy.zeros(3, self.rbtdef.dof) 76 | for j in range(l + 1): 77 | if self.rbtdef._links_sigma[j]: 78 | self.Jo[l][0:3, j] = sympy.zeros(3, 1) 79 | else: 80 | self.Jo[l][0:3, j] = ifunc(geom.z[j]) 81 | 82 | self.J = list(range(self.rbtdef.dof)) 83 | for l in range(self.rbtdef.dof): 84 | self.J[l] = self.Jp[l].col_join(self.Jo[l]) 85 | 86 | self.Jcp = list(range(self.rbtdef.dof)) 87 | self.Jco = self.Jo 88 | for l in range(self.rbtdef.dof): 89 | self.Jcp[l] = ifunc(self.Jp[l] - sym_skew( 90 | geom.R[l] * sympy.Matrix(self.rbtdef.l[l])) * self.Jo[l]) 91 | 92 | self.Jc = list(range(self.rbtdef.dof)) 93 | for l in range(self.rbtdef.dof): 94 | self.Jc[l] = self.Jcp[l].col_join(self.Jco[l]) 95 | -------------------------------------------------------------------------------- /sympybotics/robotcodegen.py: -------------------------------------------------------------------------------- 1 | 2 | import sympy 3 | 4 | from . import symcode 5 | 6 | 7 | def _gen_q_dq_ddq_subs(rbtdef, offset=0): 8 | subs = {} 9 | for i in reversed(range(rbtdef.dof)): 10 | subs[rbtdef.ddq[i]] = 'ddq[' + str(i+offset) + ']' 11 | subs[rbtdef.dq[i]] = 'dq[' + str(i+offset) + ']' 12 | subs[rbtdef.q[i]] = 'q[' + str(i+offset) + ']' 13 | return subs 14 | 15 | 16 | def _gen_parms_subs(parms_symbols, name='parms', offset=0): 17 | subs = {} 18 | for i, parm in enumerate(parms_symbols): 19 | subs[parm] = name + '[' + str(i+offset) + ']' 20 | return subs 21 | 22 | 23 | def robot_code_to_func(lang, code, outputname, funcname, rbtdef): 24 | func_parms = [] 25 | subs_pairs = {} 26 | 27 | idxoffset = 1 if lang.lower() in ['julia', 'jl'] else 0 28 | 29 | if not isinstance(code[1], list): 30 | code = (code[0], [code[1]]) 31 | if not isinstance(outputname, list): 32 | outputname = [outputname] 33 | 34 | code_symbols = set() 35 | for iv, se in code[0]: 36 | code_symbols.update(se.free_symbols) 37 | for e in code[1]: 38 | for ei in e: 39 | code_symbols.update(ei.free_symbols) 40 | 41 | if not code_symbols.isdisjoint(set(rbtdef.dynparms())): 42 | func_parms.append('parms') 43 | subs_pairs.update(_gen_parms_subs(rbtdef.dynparms(), 'parms', 44 | offset=idxoffset)) 45 | 46 | qderivlevel = -1 47 | if not code_symbols.isdisjoint(set(rbtdef.q)): 48 | qderivlevel = 0 49 | if not code_symbols.isdisjoint(set(rbtdef.dq)): 50 | qderivlevel = 1 51 | if not code_symbols.isdisjoint(set(rbtdef.ddq)): 52 | qderivlevel = 2 53 | 54 | if qderivlevel >= 0: 55 | for i in range(qderivlevel + 1): 56 | func_parms.append('d'*i + 'q') 57 | subs_pairs.update(_gen_q_dq_ddq_subs(rbtdef, offset=idxoffset)) 58 | 59 | return symcode.generation.code_to_func(lang, code, outputname, funcname, 60 | func_parms, subs_pairs) 61 | -------------------------------------------------------------------------------- /sympybotics/robotdef.py: -------------------------------------------------------------------------------- 1 | 2 | import sympy 3 | 4 | from .utils import sym_skew as skew 5 | 6 | 7 | def _new_sym(name): 8 | return sympy.symbols(name, real=True) 9 | 10 | 11 | def _elements_to_tensor(elems): 12 | return sympy.Matrix([[elems[0], elems[1], elems[2]], 13 | [elems[1], elems[3], elems[4]], 14 | [elems[2], elems[4], elems[5]]]) 15 | 16 | 17 | def _elementslist_to_tensorlist(elementslist): 18 | 19 | return [_elements_to_tensor(elems) for elems in elementslist] 20 | 21 | 22 | _joint_symb = _new_sym('q') 23 | 24 | 25 | def _joint_i_symb(i): 26 | return _new_sym('q' + str(i)) 27 | 28 | q = _joint_symb 29 | 30 | _dh_alpha, _dh_a, _dh_d, _dh_theta = _new_sym('alpha,a,d,theta') 31 | default_dh_symbols = (_dh_alpha, _dh_a, _dh_d, _dh_theta) 32 | 33 | _cos = sympy.cos 34 | _sin = sympy.sin 35 | _standard_dh_transfmat = sympy.Matrix([ 36 | [_cos(_dh_theta), -_sin(_dh_theta) * _cos(_dh_alpha), 37 | _sin(_dh_theta) * _sin(_dh_alpha), _dh_a * _cos(_dh_theta)], 38 | [_sin(_dh_theta), _cos(_dh_theta) * _cos(_dh_alpha), - 39 | _cos(_dh_theta) * _sin(_dh_alpha), _dh_a * _sin(_dh_theta)], 40 | [0, _sin(_dh_alpha), _cos(_dh_alpha), _dh_d], 41 | [0, 0, 0, 1]]) 42 | _modified_dh_transfmat = sympy.Matrix([ 43 | [_cos(_dh_theta), -_sin(_dh_theta), 0, _dh_a], 44 | [_sin(_dh_theta) * _cos(_dh_alpha), _cos(_dh_theta) * 45 | _cos(_dh_alpha), -_sin(_dh_alpha), -_sin(_dh_alpha) * _dh_d], 46 | [_sin(_dh_theta) * _sin(_dh_alpha), _cos(_dh_theta) * 47 | _sin(_dh_alpha), _cos(_dh_alpha), _cos(_dh_alpha) * _dh_d], 48 | [0, 0, 0, 1]]) 49 | 50 | default_frictionmodel = None 51 | default_driveinertiamodel = None 52 | default_gravityacc = sympy.Matrix([[0.0], [0.0], [-9.81]]) 53 | 54 | 55 | class RobotDef(object): 56 | 57 | """Class that generates and holds robot definitions and symbols.""" 58 | 59 | def __init__(self, name, dh_parms, dh_convention, shortname=None): 60 | """ 61 | Create RobotDef instance with data structures for robot geometry and 62 | symbols of robot dynamics. 63 | """ 64 | 65 | self.dof = len(dh_parms) 66 | 67 | self.name = str(name) 68 | if shortname is not None: 69 | self.shortname = str(shortname).replace(' ', '_').replace('.', '_') 70 | else: 71 | self.shortname = ''.join(c for c in name.replace( 72 | ' ', '_').replace('.', '_') if c.isalnum() or c == '_') 73 | 74 | dh_convention = dh_convention.lower() 75 | if dh_convention in ['standard', 'std', 'dh', 'sdh']: 76 | self._dh_convention = 'standard' 77 | self._dh_transfmat = _standard_dh_transfmat 78 | elif dh_convention in ['modified', 'mod', 'mdh']: 79 | self._dh_convention = 'modified' 80 | self._dh_transfmat = _modified_dh_transfmat 81 | else: 82 | raise ValueError( 83 | "DH convention %s not known/implemented (use 'standard' or" 84 | "'modified')" % dh_convention) 85 | 86 | self._dh_symbols = default_dh_symbols 87 | 88 | self._dyn_parms_order = 'Khalil' 89 | 90 | # can be None or a list containing any combination of the strings 91 | # 'Coulomb', 'viscous' and 'offset', e.g., {'Coulomb', 'viscous'} 92 | self.frictionmodel = default_frictionmodel 93 | 94 | # can be None or 'simplified' 95 | self.driveinertiamodel = default_driveinertiamodel 96 | 97 | self.gravityacc = default_gravityacc 98 | 99 | self._gen_symbols() 100 | 101 | self._set_dh_parms(dh_parms) 102 | 103 | @property 104 | def dh_convention(self): 105 | return self._dh_convention 106 | 107 | @property 108 | def dyn_parms_order(self): 109 | return self._dyn_parms_order 110 | 111 | @property 112 | def dh_parms(self): 113 | return self._dh_parms 114 | 115 | def __str__(self): 116 | return 'RobotDef instance: ' + self.name 117 | 118 | def description(self): 119 | from sympy import sstr 120 | 121 | s = 'RobotDef instance: ' + self.name + '\n' 122 | s += ' DH parameters - ' + str(self.dh_convention) + ' convention:\n' 123 | s += ' joint - alpha, a, d, theta\n' 124 | for i, dh in enumerate(self.dh_parms): 125 | s += ' %2i - ' % (i+1) + \ 126 | sstr(dh).replace('(', '').replace(')', '') + '\n' 127 | s += ' gravity acceleration: ' + sstr(list(self.gravityacc)) + '^T\n' 128 | s += ' friction model: ' + str(self.frictionmodel) + '\n' 129 | return s 130 | 131 | @property 132 | def L(self): 133 | return _elementslist_to_tensorlist(self.Le) 134 | 135 | @property 136 | def I(self): 137 | return _elementslist_to_tensorlist(self.Ie) 138 | 139 | def _gen_symbols(self): 140 | """Generate robot dynamic symbols and populates RobotDef instance with 141 | them. (internal function)""" 142 | 143 | dof = self.dof 144 | 145 | # q = self.q = [ _new_sym('q_'+str(i+1)) for i in range(self.dof) ] 146 | # dq = self.dq = [ _new_sym('dq_'+str(i+1)) for i in range(self.dof) ] 147 | # ddq = self.ddq = [ _new_sym('ddq_'+str(i+1)) for i in range(self.dof) 148 | # ] 149 | self.q = sympy.Matrix([[_new_sym('q' + str(i + 1))] 150 | for i in range(self.dof)]) 151 | self.dq = sympy.Matrix([[_new_sym(r'\dot{q}_' + str(i + 1))] 152 | for i in range(self.dof)]) 153 | self.ddq = sympy.Matrix( 154 | [[_new_sym(r'\ddot{q}_' + str(i + 1))] for i in range(self.dof)]) 155 | 156 | self.non_latex_symbols = {} 157 | for i in range(self.dof): 158 | self.non_latex_symbols[self.q[i]] = r'q' + str(i + 1) 159 | self.non_latex_symbols[ 160 | self.dq[i]] = r'dq' + str(i + 1) 161 | self.non_latex_symbols[ 162 | self.ddq[i]] = r'ddq' + str(i + 1) 163 | 164 | m = self.m = list(range(self.dof)) 165 | l = self.l = list(range(self.dof)) 166 | Le = self.Le = list(range(self.dof)) 167 | 168 | r = self.r = list(range(self.dof)) 169 | Ie = self.Ie = list(range(self.dof)) 170 | 171 | Ia = self.Ia = list(range(self.dof)) 172 | 173 | fv = self.fv = list(range(self.dof)) 174 | fc = self.fc = list(range(self.dof)) 175 | fo = self.fo = list(range(self.dof)) 176 | 177 | for i in range(dof): 178 | 179 | m[i] = _new_sym('m_' + str(i + 1)) 180 | l[i] = sympy.Matrix([_new_sym('l_' + str(i + 1) + dim) 181 | for dim in ['x', 'y', 'z']]) 182 | Le[i] = [_new_sym('L_' + str(i + 1) + elem) 183 | for elem in ['xx', 'xy', 'xz', 'yy', 'yz', 'zz']] 184 | 185 | r[i] = sympy.Matrix([_new_sym('r_' + str(i + 1) + dim) 186 | for dim in ['x', 'y', 'z']]) 187 | Ie[i] = [_new_sym('I_' + str(i + 1) + elem) 188 | for elem in ['xx', 'xy', 'xz', 'yy', 'yz', 'zz']] 189 | 190 | Ia[i] = _new_sym('Ia_' + str(i + 1)) 191 | 192 | fv[i] = _new_sym('fv_' + str(i + 1)) 193 | fc[i] = _new_sym('fc_' + str(i + 1)) 194 | fo[i] = _new_sym('fo_' + str(i + 1)) 195 | 196 | I = self.I 197 | L = self.L 198 | 199 | I_funcof_L = self.I_funcof_L = list(range(self.dof)) 200 | L_funcof_I = self.L_funcof_I = list(range(self.dof)) 201 | 202 | dict_I2Lexp = self.dict_I2Lexp = dict() 203 | dict_L2Iexp = self.dict_L2Iexp = dict() 204 | dict_l2mr = self.dict_l2mr = dict() 205 | dict_r2lm = self.dict_r2lm = dict() 206 | 207 | for i in range(dof): 208 | 209 | L_funcof_I[i] = I[i] + m[i] * skew(r[i]).T * skew(r[i]) 210 | I_funcof_L[i] = L[i] - m[i] * skew(r[i]).T * skew(r[i]) 211 | 212 | for elem, exprss in enumerate(I_funcof_L[i]): 213 | dict_I2Lexp[I[i][elem]] = exprss 214 | 215 | for elem, exprss in enumerate(L_funcof_I[i]): 216 | dict_L2Iexp[L[i][elem]] = exprss 217 | 218 | for elem in range(3): 219 | dict_l2mr[l[i][elem]] = m[i] * r[i][elem] 220 | dict_r2lm[r[i][elem]] = l[i][elem] / m[i] 221 | 222 | return self 223 | 224 | def _set_dh_parms(self, dh_parms_list): 225 | """ 226 | Define the RobotDef geometry using Denavit-Hartenberg notation. 227 | """ 228 | 229 | if len(dh_parms_list) != self.dof: 230 | raise Exception('RobotDef.set_geometry(): provided number of links' 231 | 'differ from robot dof (%d vs %d).' % 232 | (len(dh_parms_list), self.dof)) 233 | 234 | self._dh_parms = [] 235 | 236 | self._links_sigma = [0] * self.dof 237 | theta_index = self._dh_symbols.index(sympy.Symbol('theta', real=True)) 238 | d_index = self._dh_symbols.index(sympy.Symbol('d', real=True)) 239 | 240 | for i in range(self.dof): 241 | 242 | if len(dh_parms_list[i]) != 4: 243 | raise Exception( 244 | 'RobotDef.set_dh_parms: wrong number of Denavit-Hartenberg' 245 | 'parameters (must be 4 per link).') 246 | 247 | link_dh_parms = [] 248 | for p in dh_parms_list[i]: 249 | 250 | p = sympy.sympify(p) 251 | 252 | for s in p.free_symbols: 253 | if str(s) == str(_joint_symb): 254 | p = p.subs(s, _joint_i_symb(i + 1)) 255 | 256 | link_dh_parms.append(p) 257 | 258 | self._dh_parms.append(tuple(link_dh_parms)) 259 | 260 | try: 261 | if self._dh_parms[i][theta_index].has(self.q[i]): 262 | self._links_sigma[i] = 0 263 | # print 'joint',i+1,'is revolute' 264 | except: 265 | pass 266 | try: 267 | if self._dh_parms[i][d_index].has(self.q[i]): 268 | self._links_sigma[i] = 1 269 | # print 'joint',il+1,'is prismatic' 270 | except: 271 | pass 272 | 273 | return self 274 | 275 | def dynparms(self, parm_order=None): 276 | """Return list of RobotDef symbolic dynamic parameters.""" 277 | 278 | if not parm_order: 279 | parm_order = self._dyn_parms_order 280 | parm_order = parm_order.lower() 281 | 282 | parms = [] 283 | for i in range(0, self.dof): 284 | 285 | if parm_order == 'khalil' or parm_order == 'tensor first': 286 | # Lxx Lxy Lxz Lyy Lyz Lzz lx ly lz m 287 | parms += self.Le[i] 288 | parms += sympy.flatten(self.l[i]) 289 | parms += [self.m[i]] 290 | 291 | elif parm_order == 'siciliano' or parm_order == 'mass first': 292 | # m lx ly lz Lxx Lxy Lxz Lyy Lyz Lzz 293 | parms += [self.m[i]] 294 | parms += sympy.flatten(self.l[i]) 295 | parms += self.Le[i] 296 | 297 | else: 298 | raise Exception( 299 | 'RobotDef.Parms(): dynamic parameters order \'' 300 | + parm_order + '\' not know.') 301 | 302 | if self.driveinertiamodel == 'simplified': 303 | parms += [self.Ia[i]] 304 | 305 | if self.frictionmodel is not None: 306 | if 'viscous' in self.frictionmodel: 307 | parms += [self.fv[i]] 308 | if 'Coulomb' in self.frictionmodel: 309 | parms += [self.fc[i]] 310 | if 'offset' in self.frictionmodel: 311 | parms += [self.fo[i]] 312 | 313 | return parms 314 | 315 | #def _set_new_dynparms(self, dynparms): 316 | #"""Define new symbols to dynamic parameters.""" 317 | #symparms = self.dynparms() 318 | #parmsidx = dict(zip(symparms, range(len(symparms)))) 319 | #for i in range(self.dof): 320 | #for x, Le_xx in enumerate(self.Le[i]): 321 | #self.Le[i][x] = dynparms[parmsidx[Le_xx]] 322 | #for x, l_x in enumerate(self.l[i]): 323 | #self.l[i][x] = dynparms[parmsidx[l_x]] 324 | #self.m[i] = dynparms[parmsidx[self.m[i]]] 325 | #if self.frictionmodel == 'simple': 326 | #self.fv[i] = dynparms[parmsidx[self.fv[i]]] 327 | #self.fc[i] = dynparms[parmsidx[self.fc[i]]] 328 | -------------------------------------------------------------------------------- /sympybotics/robotmodel.py: -------------------------------------------------------------------------------- 1 | 2 | import sys 3 | import numpy 4 | 5 | from .geometry import Geometry 6 | from .kinematics import Kinematics 7 | from .dynamics import Dynamics 8 | from .symcode import Subexprs, code_to_func 9 | from ._compatibility_ import exec_ 10 | 11 | def _fprint(x): 12 | print(x) 13 | sys.stdout.flush() 14 | 15 | 16 | class RobotAllSymb(object): 17 | """ 18 | Robot geometric, kinematic, and dynamic models in single symbolic 19 | expressions. 20 | """ 21 | 22 | def __init__(self, rbtdef): 23 | 24 | self.rbtdef = rbtdef 25 | self.dof = rbtdef.dof 26 | 27 | self.geo = Geometry(self.rbtdef) 28 | self.kin = Kinematics(self.rbtdef, self.geo) 29 | 30 | self.dyn = Dynamics(self.rbtdef, self.geo) 31 | self.dyn.gen_all() 32 | 33 | 34 | class RobotDynCode(object): 35 | 36 | """Robot dynamic model in code form.""" 37 | 38 | def __init__(self, rbtdef, verbose=False): 39 | 40 | if verbose: 41 | p = _fprint 42 | else: 43 | p = lambda x: None 44 | 45 | self.rbtdef = rbtdef 46 | self.dof = rbtdef.dof 47 | 48 | p('generating geometric model') 49 | self.geo = Geometry(self.rbtdef) 50 | 51 | p('generating kinematic model') 52 | self.kin = Kinematics(self.rbtdef, self.geo) 53 | 54 | self.dyn = Dynamics(self.rbtdef, self.geo) 55 | 56 | p('generating inverse dynamics code') 57 | invdyn_se = Subexprs() 58 | self.dyn.gen_invdyn(invdyn_se.collect) 59 | self.invdyn_code = invdyn_se.get(self.dyn.invdyn) 60 | 61 | p('generating gravity term code') 62 | g_se = Subexprs() 63 | self.dyn.gen_gravityterm(g_se.collect) 64 | self.g_code = g_se.get(self.dyn.g) 65 | 66 | p('generating coriolis term code') 67 | c_se = Subexprs() 68 | self.dyn.gen_coriolisterm(c_se.collect) 69 | self.c_code = c_se.get(self.dyn.c) 70 | 71 | p('generating coriolis matrix code') 72 | C_se = Subexprs() 73 | self.dyn.gen_coriolismatrix(C_se.collect) 74 | self.C_code = C_se.get(self.dyn.C) 75 | 76 | p('generating inertia matrix code') 77 | M_se = Subexprs() 78 | self.dyn.gen_inertiamatrix(M_se.collect) 79 | self.M_code = M_se.get(self.dyn.M) 80 | 81 | p('generating regressor matrix code') 82 | H_se = Subexprs() 83 | self.dyn.gen_regressor(H_se.collect) 84 | self.H_code = H_se.get(self.dyn.H) 85 | self._H_se = H_se._subexp_iv 86 | 87 | self._codes = ['invdyn_code', 'g_code', 'c_code', 'C_code', 'M_code', 88 | 'H_code'] 89 | 90 | if self.rbtdef.frictionmodel is not None: 91 | p('generating friction term code') 92 | f_se = Subexprs() 93 | self.dyn.gen_frictionterm(f_se.collect) 94 | self.f_code = f_se.get(self.dyn.f) 95 | self._codes.append('f_code') 96 | 97 | p('done') 98 | 99 | def calc_base_parms(self, verbose=False): 100 | 101 | q_subs = {q: 'q[%d]' % i for i, q in enumerate(self.rbtdef.q)} 102 | q_subs.update( 103 | {dq: 'dq[%d]' % i for i, dq in enumerate(self.rbtdef.dq)}) 104 | q_subs.update( 105 | {ddq: 'ddq[%d]' % i for i, ddq in enumerate(self.rbtdef.ddq)}) 106 | func_def_regressor = code_to_func( 107 | 'python', self.H_code, 'regressor', 'regressor_func', 108 | ['q', 'dq', 'ddq'], q_subs) 109 | 110 | global sin, cos, sign 111 | sin = numpy.sin 112 | cos = numpy.cos 113 | sign = numpy.sign 114 | 115 | l = locals() 116 | exec_(func_def_regressor, globals(), l) 117 | regressor_func = l['regressor_func'] 118 | 119 | if verbose: 120 | _fprint('calculating base parameters and regressor code') 121 | 122 | self.dyn.calc_base_parms(regressor_func) 123 | 124 | H_se = Subexprs() 125 | H_se._subexp_iv = self._H_se 126 | self.Hb_code = H_se.get(self.dyn.H * self.dyn.Pb) 127 | 128 | self._codes.append('Hb_code') 129 | 130 | if verbose: 131 | _fprint('done') 132 | -------------------------------------------------------------------------------- /sympybotics/symcode/__init__.py: -------------------------------------------------------------------------------- 1 | """Collect sub-expressions from SymPy expressions and generate C and Python 2 | code.""" 3 | 4 | from .subexprs import Subexprs 5 | from .generation import code_to_func, code_back_to_exprs, code_to_string, \ 6 | codestring_count 7 | from . import generation 8 | -------------------------------------------------------------------------------- /sympybotics/symcode/generation.py: -------------------------------------------------------------------------------- 1 | 2 | import copy 3 | import sympy 4 | import re 5 | 6 | 7 | options = {} 8 | options['unroll_square'] = True 9 | 10 | 11 | def apply_func(code, func, apply_to_ivs=True): 12 | if apply_to_ivs: 13 | code_ivs = [(func(iv), func(se)) for iv, se in code[0]] 14 | else: 15 | code_ivs = [(iv, func(se)) for iv, se in code[0]] 16 | code_exprs = [] 17 | for expr in code[1]: 18 | if isinstance(expr, sympy.MatrixBase): 19 | expr = expr.applyfunc(func) 20 | else: 21 | expr = func(expr) 22 | code_exprs.append(expr) 23 | return code_ivs, code_exprs 24 | 25 | 26 | def xreplace(code, xreplace_dict): 27 | return apply_func(code, lambda x: x.xreplace(xreplace_dict)) 28 | 29 | 30 | def code_back_to_exprs(code): 31 | 32 | ivars = copy.deepcopy(code[0]) 33 | exps = copy.deepcopy(code[1]) 34 | 35 | for i in range(len(ivars)): 36 | 37 | Dxreplace = {ivars[i][0]: ivars[i][1]} 38 | 39 | for j in range(i + 1, len(ivars)): 40 | if ivars[j][1].has(ivars[i][0]): 41 | ivars[j] = (ivars[j][0], ivars[j][1].xreplace(Dxreplace)) 42 | 43 | for j in range(len(exps)): 44 | if sympy.sympify(exps[j]).has(ivars[i][0]): 45 | exps[j] = exps[j].xreplace(Dxreplace) 46 | 47 | return exps 48 | 49 | 50 | def _ccode(expr, ): 51 | code = sympy.ccode(expr) 52 | if options['unroll_square']: 53 | return re.sub(r'pow\(([^,]*), 2\)', r'((\1)*(\1))', code) 54 | else: 55 | return code 56 | 57 | 58 | def _juliacode(expr, ): 59 | code = sympy.printing.lambdarepr.lambdarepr(expr) 60 | return code.replace('**', '^') 61 | 62 | 63 | def code_to_string(code, out_parms, printer, indent='', realtype='', 64 | line_end='', outidxoffset=0): 65 | 66 | codestr = '' 67 | 68 | if realtype: 69 | realtype += ' ' 70 | 71 | for i in range(len(code[0])): 72 | codestr += indent + realtype + \ 73 | printer(code[0][i][0]) + ' = ' + printer( 74 | code[0][i][1]) + line_end + '\n' 75 | 76 | for c, out in enumerate(out_parms): 77 | codestr += '\n' 78 | for i in range(len(code[1][c])): 79 | codestr += indent + out + \ 80 | '[' + str(i+outidxoffset) + '] = ' + \ 81 | printer(code[1][c][i]) + line_end + '\n' 82 | 83 | return codestr 84 | 85 | 86 | def codestring_count(codestring, resume=False): 87 | ops = [] 88 | ops += [('=', int(codestring.count('=')))] 89 | ops += [('+', int(codestring.count('+')))] 90 | ops += [('-', int(codestring.count('-')))] 91 | ops += [('*', int(codestring.count('*')))] 92 | ops += [('/', int(codestring.count('/')))] 93 | ops += [('pow', int(codestring.count('pow')))] 94 | ops += [('sin', int(codestring.count('sin')))] 95 | ops += [('cos', int(codestring.count('cos')))] 96 | if not resume: 97 | return ops 98 | else: 99 | adds = int(codestring.count('+')) + int(codestring.count('-')) 100 | muls = int(codestring.count('*')) + int( 101 | codestring.count('/')) + int(codestring.count('pow')) 102 | return ops, {'add': adds, 'mul': muls, 'total': adds + muls} 103 | 104 | 105 | def gen_py_func(code, out_parms, func_parms, func_name='func'): 106 | 107 | indent = 4 * ' ' 108 | 109 | pycode = 'def ' + func_name + '(' 110 | pycode += ', '.join(func_parms) 111 | pycode += '):\n\n' 112 | 113 | for i, out in enumerate(out_parms): 114 | pycode += indent + out + ' = [0]*' + str(len(code[1][i])) + '\n' 115 | 116 | pycode += '\n' 117 | 118 | mainpycode = code_to_string(code, out_parms, 119 | sympy.printing.lambdarepr.lambdarepr, indent) 120 | 121 | pycode += mainpycode 122 | 123 | pycode += '\n' + indent + 'return ' 124 | pycode += ', '.join(out_parms) 125 | 126 | pycode = pycode.replace('\n\n', '\n#\n') 127 | 128 | return pycode 129 | 130 | 131 | def gen_c_func(code, out_parms, func_parms, func_name='func'): 132 | 133 | indent = 2 * ' ' 134 | 135 | ccode = 'void ' + func_name + '( double* ' 136 | 137 | ccode += ', double* '.join(out_parms) 138 | 139 | ccode += ', const double* ' 140 | ccode += ', const double* '.join(func_parms) 141 | 142 | ccode += ' )\n{\n' 143 | 144 | mainccode = code_to_string(code, out_parms, _ccode, indent, 'double', ';') 145 | 146 | ccode += mainccode + '\n' + indent + 'return;\n}' 147 | 148 | ccode = ccode.replace('\n\n', '\n//\n') 149 | 150 | return ccode 151 | 152 | 153 | def gen_julia_func(code, out_parms, func_parms, func_name='func'): 154 | 155 | indent = 4 * ' ' 156 | 157 | ccode = 'function ' + func_name + '(' 158 | 159 | ccode += ', '.join(out_parms) 160 | 161 | ccode += ', ' 162 | ccode += ', '.join(func_parms) 163 | 164 | ccode += ')\n\n' 165 | 166 | code = code[0], [(e.T if isinstance(e, sympy.MatrixBase) else e) 167 | for e in code[1]] 168 | 169 | mainccode = code_to_string(code, out_parms, _juliacode, indent, '', '', 170 | outidxoffset=1) 171 | 172 | ## pass from 0-idexed to 1-indexed arrays 173 | #mainccode = re.sub( 174 | #r"\[([0-9]+)\]", 175 | #lambda m: '[' + str(int(m.group(1))+1) + ']', 176 | #mainccode) 177 | 178 | ccode += mainccode + '\n' + indent + 'return ' 179 | ccode += ', '.join(out_parms) 180 | ccode += '\nend' 181 | 182 | ccode = ccode.replace('\n\n', '\n#\n') 183 | 184 | return ccode 185 | 186 | 187 | def code_to_func(lang, code, out_parms, func_name, func_parms, symb_replace): 188 | 189 | if not isinstance(code[1], list): 190 | code = (code[0], [code[1]]) 191 | if not isinstance(out_parms, list): 192 | out_parms = [out_parms] 193 | 194 | lang = lang.lower() 195 | if lang in ['python', 'py']: 196 | gen_func = gen_py_func 197 | elif lang in ['c', 'c++']: 198 | gen_func = gen_c_func 199 | elif lang in ['julia', 'jl']: 200 | gen_func = gen_julia_func 201 | else: 202 | raise Exception('chosen language not supported.') 203 | 204 | if symb_replace: 205 | sympified_replace = {} 206 | for k, v in symb_replace.items(): 207 | if isinstance(k, str): 208 | k = sympy.Symbol(k) 209 | if isinstance(v, str): 210 | v = sympy.Symbol(v) 211 | sympified_replace[k] = v 212 | code = xreplace(code, sympified_replace) 213 | 214 | return gen_func(code, out_parms, func_parms, func_name) 215 | -------------------------------------------------------------------------------- /sympybotics/symcode/subexprs.py: -------------------------------------------------------------------------------- 1 | import sympy 2 | import sympy.utilities 3 | from sympy.simplify.cse_main import preprocess_for_cse, postprocess_for_cse 4 | 5 | import collections 6 | 7 | 8 | class Subexprs(object): 9 | 10 | def __init__(self, optimizations=None, postprocess=None): 11 | 12 | if optimizations is None: 13 | optimizations = [] 14 | self._optimizations = optimizations 15 | self._postprocess = postprocess 16 | 17 | self._tmp_symbols = sympy.utilities.iterables.numbered_symbols( 18 | 'tmp', start=0, real=True) 19 | 20 | self._subexp_iv = dict() 21 | self._commutatives = dict() 22 | 23 | class _ordered_len(object): 24 | 25 | def __init__(self): 26 | self.lenidxs = [0] 27 | 28 | def insert(self, list, item): 29 | l = len(item) 30 | over = l - (len(self.lenidxs) - 1) 31 | if over > 0: 32 | self.lenidxs.extend([self.lenidxs[-1]] * over) 33 | list.insert(self.lenidxs[l], item) 34 | for j in range(l, len(self.lenidxs)): 35 | self.lenidxs[j] += 1 36 | 37 | def pop(self, list, index): 38 | l = len(list.pop(index)) 39 | for j in range(l, len(self.lenidxs)): 40 | self.lenidxs[j] -= 1 41 | 42 | def _parse_commutative(self, expr): 43 | 44 | exprtype = type(expr) 45 | args_input = set(expr.args) 46 | 47 | if exprtype not in self._commutatives: 48 | argsets = [] 49 | argset_orderlens = self._ordered_len() 50 | argset_orderlens.insert(argsets, args_input) 51 | self._commutatives[exprtype] = (argsets, argset_orderlens) 52 | ivar = next(self._tmp_symbols) 53 | self._subexp_iv[expr] = ivar 54 | return ivar 55 | 56 | argsets = self._commutatives[exprtype][0] 57 | argset_orderlens = self._commutatives[exprtype][1] 58 | 59 | ivar = None 60 | args_to_remove = [] 61 | args_to_insert = [] 62 | 63 | # for 2 args input exprs, bypass comparison with other 2 args exprs 64 | if len(args_input) == 2: 65 | init = argset_orderlens.lenidxs[2] 66 | else: 67 | init = 0 68 | 69 | for i in range(init, len(argsets)): 70 | args_other = argsets[i] 71 | 72 | com = args_input.intersection(args_other) 73 | if len(com) > 1: 74 | 75 | diff_args_input = args_input.difference(com) 76 | diff_args_other = args_other.difference(com) 77 | 78 | if not diff_args_input: 79 | # args_input is strict subset of args_other 80 | 81 | ivar = next(self._tmp_symbols) 82 | self._subexp_iv[exprtype(*args_input)] = ivar 83 | args_to_insert.append(args_input) 84 | 85 | args_other = diff_args_other 86 | args_other.add(ivar) 87 | self._subexp_iv[exprtype(*args_other)] = \ 88 | self._subexp_iv.pop(exprtype(*argsets[i])) 89 | args_to_remove.append(i) 90 | args_to_insert.append(args_other) 91 | 92 | break 93 | 94 | elif not diff_args_other: 95 | # args_other is strict subset of args_input 96 | 97 | args_input = diff_args_input 98 | args_input.add(self._subexp_iv[exprtype(*args_other)]) 99 | 100 | ivar = self._subexp_iv.get(exprtype(*args_input), None) 101 | 102 | if ivar or len(args_input) == 2: 103 | break 104 | 105 | else: # args_input != com != args_other 106 | 107 | ivar_com = next(self._tmp_symbols) 108 | self._subexp_iv[exprtype(*com)] = ivar_com 109 | args_to_insert.append(com) # argsets.append(com) 110 | 111 | args_other = diff_args_other 112 | args_other.add(ivar_com) 113 | self._subexp_iv[exprtype(*args_other)] = \ 114 | self._subexp_iv.pop(exprtype(*argsets[i])) 115 | args_to_remove.append(i) 116 | args_to_insert.append(args_other) 117 | 118 | args_input = diff_args_input 119 | args_input.add(ivar_com) 120 | 121 | if len(args_input) == 2: 122 | break 123 | 124 | if ivar is None: 125 | ivar = next(self._tmp_symbols) 126 | self._subexp_iv[exprtype(*args_input)] = ivar 127 | args_to_insert.append(args_input) 128 | 129 | for i in reversed(sorted(args_to_remove)): 130 | argset_orderlens.pop(argsets, i) 131 | for args in args_to_insert: 132 | argset_orderlens.insert(argsets, args) 133 | 134 | return ivar 135 | 136 | def _parse(self, expr): 137 | 138 | if expr.is_Atom: 139 | # Exclude atoms, since there is no point in renaming them. 140 | return expr 141 | 142 | if sympy.iterables.iterable(expr): 143 | return expr 144 | 145 | subexpr = type(expr)(*map(self._parse, expr.args)) 146 | 147 | if subexpr in self._subexp_iv: 148 | return self._subexp_iv[subexpr] 149 | 150 | if subexpr.is_Mul or subexpr.is_Add: 151 | return self._parse_commutative(subexpr) 152 | else: 153 | ivar = next(self._tmp_symbols) 154 | self._subexp_iv[subexpr] = ivar 155 | return ivar 156 | 157 | def collect(self, exprs): 158 | 159 | if isinstance(exprs, sympy.Basic): # if only one expression is passed 160 | exprs = [exprs] 161 | is_single_expr = True 162 | else: 163 | is_single_expr = False 164 | 165 | # Preprocess the expressions to give us better optimization 166 | # opportunities. 167 | prep_exprs = [preprocess_for_cse(e, self._optimizations) 168 | for e in exprs] 169 | 170 | out_exprs = list(map(self._parse, prep_exprs)) 171 | 172 | if is_single_expr: 173 | return out_exprs[0] 174 | elif isinstance(exprs, sympy.Matrix): 175 | return sympy.Matrix(exprs.rows, exprs.cols, out_exprs) 176 | else: 177 | return out_exprs 178 | 179 | def get(self, exprs=None, symbols=None): 180 | 181 | if symbols is None: 182 | symbols = sympy.utilities.iterables.numbered_symbols() 183 | else: 184 | # In case we get passed an iterable with an __iter__ method 185 | # instead of an actual iterator. 186 | symbols = iter(symbols) 187 | 188 | if isinstance(exprs, sympy.Basic): # if only one expression is passed 189 | exprs = [exprs] 190 | 191 | # Find all of the repeated subexpressions. 192 | 193 | ivar_se = {iv: se for se, iv in self._subexp_iv.items()} 194 | 195 | used_ivs = set() 196 | repeated = set() 197 | 198 | def _find_repeated_subexprs(subexpr): 199 | if subexpr.is_Atom: 200 | symbs = [subexpr] 201 | else: 202 | symbs = subexpr.args 203 | for symb in symbs: 204 | if symb in ivar_se: 205 | if symb not in used_ivs: 206 | _find_repeated_subexprs(ivar_se[symb]) 207 | used_ivs.add(symb) 208 | else: 209 | repeated.add(symb) 210 | 211 | for expr in exprs: 212 | if expr.is_Matrix: 213 | expr.applyfunc(_find_repeated_subexprs) 214 | else: 215 | _find_repeated_subexprs(expr) 216 | 217 | # Substitute symbols for all of the repeated subexpressions. 218 | # remove temporary replacements that weren't used more than once 219 | 220 | tmpivs_ivs = dict() 221 | ordered_iv_se = collections.OrderedDict() 222 | 223 | def _get_subexprs(subexpr): 224 | if subexpr.is_Atom: 225 | symb = subexpr 226 | if symb in ivar_se: 227 | if symb in tmpivs_ivs: 228 | return tmpivs_ivs[symb] 229 | else: 230 | subexpr = ivar_se[symb] 231 | args = list(map(_get_subexprs, subexpr.args)) 232 | subexpr = type(subexpr)(*args) 233 | if symb in repeated: 234 | ivar = next(symbols) 235 | ordered_iv_se[ivar] = subexpr 236 | tmpivs_ivs[symb] = ivar 237 | return ivar 238 | else: 239 | return subexpr 240 | else: 241 | return symb 242 | else: 243 | args = list(map(_get_subexprs, subexpr.args)) 244 | subexpr = type(subexpr)(*args) 245 | return subexpr 246 | 247 | out_exprs = [] 248 | for expr in exprs: 249 | if expr.is_Matrix: 250 | out_exprs.append(expr.applyfunc(_get_subexprs)) 251 | else: 252 | out_exprs.append(_get_subexprs(expr)) 253 | 254 | # Postprocess the expressions to return the expressions to canonical 255 | # form. 256 | ordered_iv_se_notopt = ordered_iv_se 257 | ordered_iv_se = collections.OrderedDict() 258 | for i, (ivar, subexpr) in enumerate(ordered_iv_se_notopt.items()): 259 | subexpr = postprocess_for_cse(subexpr, self._optimizations) 260 | ordered_iv_se[ivar] = subexpr 261 | out_exprs = [postprocess_for_cse(e, self._optimizations) 262 | for e in out_exprs] 263 | 264 | if isinstance(exprs, sympy.Matrix): 265 | out_exprs = sympy.Matrix(exprs.rows, exprs.cols, out_exprs) 266 | if self._postprocess is None: 267 | return list(ordered_iv_se.items()), out_exprs 268 | return self._postprocess(list(ordered_iv_se.items()), out_exprs) 269 | 270 | 271 | def fast_cse(exprs, symbols='aux'): 272 | se = Subexprs() 273 | return se.get(se.collect(exprs)) 274 | 275 | 276 | class WholeSubexprs(object): 277 | 278 | def __init__(self, *args, **kwargs): 279 | 280 | self._tmp_symbols = sympy.utilities.iterables.numbered_symbols() 281 | self._subexp_iv = list() 282 | 283 | def collect(self, exprs): 284 | 285 | if isinstance(exprs, sympy.Basic): # if only one expression is passed 286 | exprs = [exprs] 287 | is_single_expr = True 288 | else: 289 | is_single_expr = False 290 | 291 | out_exprs = [] 292 | for expr in exprs: 293 | if expr.is_Atom or (-expr).is_Atom: 294 | out_exprs.append(expr) 295 | else: 296 | iv = next(self._tmp_symbols) 297 | self._subexp_iv.append((iv, expr)) 298 | out_exprs.append(iv) 299 | 300 | if is_single_expr: 301 | return out_exprs[0] 302 | elif isinstance(exprs, sympy.Matrix): 303 | return sympy.Matrix(exprs.rows, exprs.cols, out_exprs) 304 | else: 305 | return out_exprs 306 | 307 | def get(self, exprs=None, symbols=None): 308 | return self._subexp_iv, exprs 309 | -------------------------------------------------------------------------------- /sympybotics/tests/test_results.py: -------------------------------------------------------------------------------- 1 | import sympy 2 | import numpy 3 | from math import cos, sin 4 | 5 | import sympybotics 6 | from sympybotics._compatibility_ import exec_ 7 | 8 | 9 | 10 | 11 | 12 | def test_scara_dh_sym_geo_kin(): 13 | 14 | pi = sympy.pi 15 | q = sympybotics.robotdef.q 16 | 17 | a1, a2, d3, d4 = sympy.symbols('a1, a2, d3, d4') 18 | 19 | scara = sympybotics.robotdef.RobotDef( 20 | 'SCARA - Spong', 21 | [( 0, a1, 0, q), 22 | (pi, a2, 0, q), 23 | ( 0, 0, q, 0), 24 | ( 0, 0, d4, q)], 25 | dh_convention='standard') 26 | 27 | scara_geo = sympybotics.geometry.Geometry(scara) 28 | scara_kin = sympybotics.kinematics.Kinematics(scara, scara_geo) 29 | 30 | cos, sin = sympy.cos, sympy.sin 31 | q1, q2, q3, q4 = sympy.flatten(scara.q) 32 | 33 | T_spong = sympy.Matrix([ 34 | [(-sin(q1)*sin(q2) + cos(q1)*cos(q2))*cos(q4) + (sin(q1)*cos(q2) + 35 | sin(q2)*cos(q1))*sin(q4), -(-sin(q1)*sin(q2) + 36 | cos(q1)*cos(q2))*sin(q4) + (sin(q1)*cos(q2) + 37 | sin(q2)*cos(q1))*cos(q4), 0, a1*cos(q1) - a2*sin(q1)*sin(q2) + 38 | a2*cos(q1)*cos(q2)], 39 | [(sin(q1)*sin(q2) - cos(q1)*cos(q2))*sin(q4) + (sin(q1)*cos(q2) + 40 | sin(q2)*cos(q1))*cos(q4), (sin(q1)*sin(q2) - 41 | cos(q1)*cos(q2))*cos(q4) - (sin(q1)*cos(q2) + 42 | sin(q2)*cos(q1))*sin(q4), 0, a1*sin(q1) + a2*sin(q1)*cos(q2) + 43 | a2*sin(q2)*cos(q1)], 44 | [0, 0, -1, -d4 - q3], 45 | [0, 0, 0, 1]]) 46 | 47 | J_spong = sympy.Matrix([[-a1*sin(q1) - a2*sin(q1)*cos(q2) - 48 | a2*sin(q2)*cos(q1), -a2*sin(q1)*cos(q2) - 49 | a2*sin(q2)*cos(q1), 0, 0], 50 | [a1*cos(q1) - a2*sin(q1)*sin(q2) + 51 | a2*cos(q1)*cos(q2), -a2*sin(q1)*sin(q2) + 52 | a2*cos(q1)*cos(q2), 0, 0], 53 | [0, 0, -1, 0], 54 | [0, 0, 0, 0], 55 | [0, 0, 0, 0], 56 | [1, 1, 0, -1]]) 57 | 58 | assert (scara_geo.T[-1] - T_spong).expand() == sympy.zeros(4) 59 | assert (scara_kin.J[-1] - J_spong).expand() == sympy.zeros(6, 4) 60 | 61 | 62 | def test_puma_dh_num_geo_kin_dyn(): 63 | pi = sympy.pi 64 | q = sympybotics.robotdef.q 65 | 66 | puma560_def = sympybotics.RobotDef( 67 | 'Puma 560 Robot', 68 | [( pi/2, 0, 0, q), 69 | ( 0, 0.4318, 0, q), 70 | ('-pi/2', '0.0203', '0.15005', 'q'), # test sympify 71 | ( pi/2, 0, 0.4318, q), 72 | ( -pi/2, 0, 0, q), 73 | ( 0, 0, 0, q)], 74 | dh_convention='standard') 75 | 76 | puma560_def.frictionmodel = None 77 | 78 | puma560 = sympybotics.RobotDynCode(puma560_def) 79 | 80 | q_test = [0.7504516826728697, 0.8395156106908136, 0.16851233582594916, 0.3849629637427072, 0.5252993946810777, 0.6701207256444748] 81 | dq_test = [0.24721855939629367, 0.9805915670454258, 0.9895299755642817, 0.7861135739668947, 0.273842245476577, 0.17182358900767503] 82 | ddq_test = [0.707405815485141, 0.25295715193420953, 0.9763909835998361, 0.8412822676113918, 0.4867768296473465, 0.11480270540937143] 83 | 84 | dynparm_test = [0.0, -0, -0, 0.34999999999999998, -0, 0.0, 0.0, 0.0, 0.0, 0, 1.03118515, 0.037980719999999996, 1.4401022999999999, 85 | 3.7274564059999999, -0.023751000000000001, 2.8425240559999998, -6.33012, 0.10439999999999999, 3.9585, 17.4, 86 | 0.090474288000000014, -0.0013739039999999998, 0.0068208000000000001, 0.111498032, 0.0047375999999999998, 87 | 0.015432319999999999, -0.09743999999999998, -0.06767999999999999, 0.336, 4.8, 0.0020960200000000001, -0, -0, 88 | 0.0012999999999999999, -0, 0.0020960200000000001, 0.0, 0.015579999999999998, 0.0, 0.82, 0.00029999999999999997, -0, -0, 89 | 0.00040000000000000002, -0, 0.00029999999999999997, 0.0, 0.0, 0.0, 0.34, 0.00024216, -0, -0, 0.00024216, -0, 90 | 4.0000000000000003e-05, 0.0, 0.0, 0.0028799999999999997, 0.09] 91 | 92 | dynparm_test2 = [0.47804562306292275, 0.5871876506259908, 0.9487349009746813, 0.35387185413632094, 0.28071604959871554, 0.368556182617345, 93 | 0.24355010647230801, 0.6753463418802456, 0.9728151452953864, 0.6620741264406734, 0.34669638996014096, 0.01593886435340608, 94 | 0.3521748260592804, 0.5384045845183812, 0.021600503502885116, 0.4654003203805651, 0.5202014161122065, 0.33744920539722967, 95 | 0.052363297799702835, 0.07051826001770234, 0.7389222546505236, 0.771434543548951, 0.3652539269897015, 0.2603059367721896, 96 | 0.4310648491411889, 0.7071252186366607, 0.16320122542325732, 0.44948421506462655, 0.48085540250421277, 0.08408482356412372, 97 | 0.923593157615906, 0.46852453511703684, 0.6670004526434297, 0.573634975268657, 0.12665814747855264, 0.3152822779549781, 98 | 0.21725524421221942, 0.5727451645276381, 0.7984025459787872, 0.7630923700903489, 0.27670044727992893, 0.919667661316697, 99 | 0.8828363383223908, 0.3618378476984413, 0.20690997158181246, 0.8321536435628591, 0.556153477173109, 0.3985225289975399, 100 | 0.7128303168401632, 0.433898343199971, 0.35116090526732047, 0.551636779637059, 0.03397585970570116, 0.28333059714010744, 101 | 0.4785548774382852, 0.16258401709779136, 0.8246056496848533, 0.23027686557606952, 0.26655111443159285, 0.6087446254045791] 102 | 103 | q_num_subs = dict(zip(puma560_def.q, q_test)) 104 | 105 | T = puma560.geo.T[-1] 106 | T = T.subs(q_num_subs) 107 | T = numpy.matrix(T).astype(numpy.float64) 108 | 109 | J = puma560.kin.J[-1] 110 | J = J.subs(q_num_subs) 111 | J = numpy.matrix(J).astype(numpy.float64) 112 | 113 | M_func_def = sympybotics.robotcodegen.robot_code_to_func( 114 | 'python', puma560.M_code, 'M', 'M_puma560', puma560_def) 115 | c_func_def = sympybotics.robotcodegen.robot_code_to_func( 116 | 'python', puma560.c_code, 'c', 'c_puma560', puma560_def) 117 | C_func_def = sympybotics.robotcodegen.robot_code_to_func( 118 | 'python', puma560.C_code, 'C', 'C_puma560', puma560_def) 119 | g_func_def = sympybotics.robotcodegen.robot_code_to_func( 120 | 'python', puma560.g_code, 'g', 'g_puma560', puma560_def) 121 | tau_func_def = sympybotics.robotcodegen.robot_code_to_func( 122 | 'python', puma560.invdyn_code, 'tau', 'tau_puma560', puma560_def) 123 | H_func_def = sympybotics.robotcodegen.robot_code_to_func( 124 | 'python', puma560.H_code, 'H', 'H_puma560', puma560_def) 125 | 126 | l = locals() 127 | exec_(M_func_def, globals(), l) 128 | exec_(c_func_def, globals(), l) 129 | exec_(C_func_def, globals(), l) 130 | exec_(g_func_def, globals(), l) 131 | exec_(tau_func_def, globals(), l) 132 | exec_(H_func_def, globals(), l) 133 | tau_puma560 = l['tau_puma560'] 134 | g_puma560 = l['g_puma560'] 135 | c_puma560 = l['c_puma560'] 136 | C_puma560 = l['C_puma560'] 137 | M_puma560 = l['M_puma560'] 138 | H_puma560 = l['H_puma560'] 139 | 140 | tau = tau_puma560(dynparm_test, q_test, dq_test, ddq_test) 141 | tau = numpy.matrix(tau).T.astype(numpy.float64) 142 | 143 | g = g_puma560(dynparm_test, q_test) 144 | g = numpy.matrix(g).T.astype(numpy.float64) 145 | 146 | c = c_puma560(dynparm_test, q_test, dq_test) 147 | c = numpy.matrix(c).T.astype(numpy.float64) 148 | 149 | C = C_puma560(dynparm_test, q_test, dq_test) 150 | C = numpy.matrix(C).reshape(puma560.dof, puma560.dof).astype(numpy.float64) 151 | C_dq = C * numpy.matrix(dq_test).T 152 | 153 | M = M_puma560(dynparm_test, q_test) 154 | M = numpy.matrix(M).reshape(puma560.dof, puma560.dof).astype(numpy.float64) 155 | 156 | H = H_puma560(q_test, dq_test, ddq_test) 157 | H = numpy.matrix(H).reshape(puma560.dof, puma560.dyn.n_dynparms 158 | ).astype(numpy.float64) 159 | 160 | tau_t2 = tau_puma560(dynparm_test2, q_test, dq_test, ddq_test) 161 | tau_t2 = numpy.matrix(tau_t2).T.astype(numpy.float64) 162 | 163 | g_t2 = g_puma560(dynparm_test2, q_test) 164 | g_t2 = numpy.matrix(g_t2).T.astype(numpy.float64) 165 | 166 | c_t2 = c_puma560(dynparm_test2, q_test, dq_test) 167 | c_t2 = numpy.matrix(c_t2).T.astype(numpy.float64) 168 | 169 | C_t2 = C_puma560(dynparm_test2, q_test, dq_test) 170 | C_t2 = numpy.matrix(C_t2).reshape(puma560.dof, puma560.dof 171 | ).astype(numpy.float64) 172 | C_dq_t2 = C_t2 * numpy.matrix(dq_test).T 173 | 174 | M_t2 = M_puma560(dynparm_test2, q_test) 175 | M_t2 = numpy.matrix(M_t2).reshape(puma560.dof, puma560.dof 176 | ).astype(numpy.float64) 177 | 178 | T_pcorke = numpy.matrix([[-0.655113870655343, -0.474277925274361, -0.588120962109346, 0.0540498757911011], 179 | [ 0.524340309498464, 0.275038163391149, -0.805866768463298, -0.154761559833806], 180 | [ 0.543960528264717, -0.836310025215453, 0.0685017183295358, 0.568944734102513], 181 | [ 0.0, 0.0, 0.0, 1.0]]) 182 | 183 | J_pcorke = numpy.matrix([[ 0.154761559833806, -0.416115317270121, -0.181051500179202, 0.0, 0.0, 0.0], 184 | [ 0.0540498757911011, -0.388002774727404, -0.16881975145483, 0.0, 0.0, 0.0], 185 | [-1.56125112837913e-17, -0.0660115669805396, -0.354377730397368, 0.0, 0.0, 0.0], 186 | [ 2.77555756156289e-17, 0.681969181663071, 0.681969181663071, -0.618588326585383, 0.778592276991744, -0.588120962109346], 187 | [ 1.04083408558608e-16, -0.731380909828662, -0.731380909828662, -0.576796808883883, -0.541217849732837, -0.805866768463298], 188 | [ 1.0, 5.72458747072346e-17, 5.72458747072346e-17, 0.533529683779323, 0.317611878460765, 0.0685017183295358]]) 189 | 190 | tau_pcorke = numpy.matrix([[ 0.986185688341252], 191 | [ 16.1055550633721], 192 | [ -6.54839494827661], 193 | [0.00626452648190803], 194 | [-0.0208972707132494], 195 | [5.59805923645945e-5]]) 196 | 197 | g_pcorke = numpy.matrix([[1.0295354054077e-15], 198 | [ 16.8135891283022], 199 | [ -7.29033633567802], 200 | [0.00449992182015992], 201 | [ -0.026719893613902], 202 | [ 0.0]]) 203 | 204 | c_pcorke = numpy.matrix([[ -0.240237883763956], 205 | [ -1.0843848843681], 206 | [ 0.365583586254475], 207 | [-0.000123404998742911], 208 | [ 0.00359951257553373], 209 | [ 1.1075729558421e-5]]) 210 | 211 | M_pcorke = numpy.matrix([[ 2.05591694561963, -0.607149114401804, -0.0772414386669633, 0.00110264316988551, 0.000264135330891356, 2.74006873318143e-6], 212 | [ -0.607149114401804, 2.00048563124708, 0.306870038373922, -0.000227846077158843, 0.000780985328855223, 7.53260796283046e-6], 213 | [ -0.0772414386669632, 0.306870038373922, 0.361368447500762, -0.000267122764221431, 0.00156301630001611, 7.53260796283046e-6], 214 | [ 0.00110264316988551, -0.000227846077158844, -0.000267122764221432, 0.00169083802882858, 1.45380752747483e-20, 3.4606953693396e-5], 215 | [0.000264135330891356, 0.000780985328855223, 0.00156301630001611, 2.33135442795155e-20, 0.00064216, 2.44929359829471e-21], 216 | [ 2.74006873318143e-6, 7.53260796283046e-6, 7.53260796283046e-6, 3.4606953693396e-5, 2.44929359829471e-21, 4.0e-5]]) 217 | 218 | tau_assrt2 = numpy.matrix([[ 4.55384377546122], 219 | [-18.3482765770679], 220 | [-18.5150406032816], 221 | [-3.48354572715293], 222 | [-4.22434630683546], 223 | [-8.50580534103007]]) 224 | 225 | g_assrt2 = numpy.matrix([[-4.44089209850063e-16], 226 | [ -16.5628257742538], 227 | [ -23.0524142097215], 228 | [ -4.2536826692411], 229 | [ -8.37451830056579], 230 | [ -7.9940463468124]]) 231 | 232 | c_assrt2 = numpy.matrix([[ 3.79031173486171], 233 | [ -6.37866680030809], 234 | [ 0.668430906458299], 235 | [ 0.0446113764650349], 236 | [ 2.68800462309735], 237 | [-0.0871119983741941]]) 238 | 239 | M_assrt2 = numpy.matrix([[ 2.80338554010218, -0.884144436072495, -1.36196693847736, 0.914512998462046, -0.799840112619873, -0.402048288616148], 240 | [-0.884144436072495, 5.09045072475534, 3.75033319331688, -0.822003775709123, 2.00576876282064, -0.136034063369629], 241 | [ -1.36196693847736, 3.75033319331688, 3.69036945486662, -0.784876338891255, 1.91719457979014, 0.0657267986145439], 242 | [ 0.914512998462046, -0.822003775709123, -0.784876338891255, 1.86573777350968, -1.06272697183618, 0.00496876753726561], 243 | [-0.799840112619873, 2.00576876282064, 1.91719457979014, -1.06272697183618, 1.2083737144556, -0.396167549434339], 244 | [-0.402048288616148, -0.136034063369629, 0.0657267986145439, 0.00496876753726561, -0.396167549434339, 0.162584017097791]]) 245 | 246 | assert_precision = 10 247 | 248 | assert not numpy.any(numpy.round(T_pcorke - T, assert_precision)) 249 | assert not numpy.any(numpy.round(J_pcorke - J, assert_precision)) 250 | assert not numpy.any(numpy.round(tau_pcorke - tau, assert_precision)) 251 | assert not numpy.any(numpy.round(g_pcorke - g, assert_precision)) 252 | assert not numpy.any(numpy.round(c_pcorke - c, assert_precision)) 253 | assert not numpy.any(numpy.round(C_dq - c, assert_precision)) 254 | assert not numpy.any(numpy.round(M_pcorke - M, assert_precision)) 255 | assert not numpy.any(numpy.round( 256 | tau_pcorke - H * numpy.matrix(dynparm_test).T, assert_precision)) 257 | assert not numpy.any(numpy.round(tau_assrt2 - tau_t2, assert_precision)) 258 | assert not numpy.any(numpy.round(g_assrt2 - g_t2, assert_precision)) 259 | assert not numpy.any(numpy.round(c_assrt2 - c_t2, assert_precision)) 260 | assert not numpy.any(numpy.round(C_dq_t2 - c_t2, assert_precision)) 261 | assert not numpy.any(numpy.round(M_assrt2 - M_t2, assert_precision)) 262 | assert not numpy.any(numpy.round( 263 | tau_assrt2 - H * numpy.matrix(dynparm_test2).T, assert_precision)) 264 | 265 | 266 | def test_puma_mdh_num_geo_kin_dyn(): 267 | pi = sympy.pi 268 | q = sympybotics.robotdef.q 269 | 270 | puma560_def = sympybotics.RobotDef( 271 | 'Puma Robot 560 mdh', 272 | [( 0., 0., 0., q), 273 | (-pi/2., 0., 0.2435, q), 274 | ( 0., 0.4318, -0.0934, q), 275 | ( pi/2., -0.0203, 0.4331, q), 276 | (-pi/2., 0., 0., q), 277 | ( pi/2., 0., 0., q)], 278 | dh_convention='modified') 279 | 280 | puma560_def.frictionmodel = None 281 | 282 | puma560 = sympybotics.RobotDynCode(puma560_def) 283 | 284 | q_test = [0.7504516826728697, 0.8395156106908136, 0.16851233582594916, 0.3849629637427072, 0.5252993946810777, 0.6701207256444748] 285 | dq_test = [0.24721855939629367, 0.9805915670454258, 0.9895299755642817, 0.7861135739668947, 0.273842245476577, 0.17182358900767503] 286 | ddq_test = [0.707405815485141, 0.25295715193420953, 0.9763909835998361, 0.8412822676113918, 0.4867768296473465, 0.11480270540937143] 287 | 288 | dynparm_test = [0.0, 0.0, 0.0, 0.0, 0.0, 0.34999999999999998, 0, 0, 0, 0, 0.1350808, -0.0070992, 0.018931199999999999, 0.60891200000000001, 289 | 0.0016703999999999998, 0.62008400000000008, 1.1832, 0.10439999999999999, -0.2784, 17.4, 0.090460800000000008, 0.0, 0.0, 290 | 0.013440800000000001, 0.0047039999999999998, 0.089520000000000002, 0.0, -0.336, 0.0672, 4.8, 0.0020960200000000001, 0.0, 291 | 0.0, 0.0020960200000000001, 0.0, 0.0012999999999999999, 0.0, 0.0, -0.015579999999999998, 0.82, 0.00029999999999999997, 0.0, 292 | 0.0, 0.00029999999999999997, 0.0, 0.00040000000000000002, 0.0, 0.0, 0.0, 0.34, 0.00024216, 0.0, 0.0, 0.00024216, 0.0, 293 | 4.0000000000000003e-05, 0.0, 0, 0.0028799999999999997, 0.09] 294 | 295 | dynparm_test2 = [0.47804562306292275, 0.5871876506259908, 0.9487349009746813, 0.35387185413632094, 0.28071604959871554, 0.368556182617345, 296 | 0.24355010647230801, 0.6753463418802456, 0.9728151452953864, 0.6620741264406734, 0.34669638996014096, 0.01593886435340608, 297 | 0.3521748260592804, 0.5384045845183812, 0.021600503502885116, 0.4654003203805651, 0.5202014161122065, 0.33744920539722967, 298 | 0.052363297799702835, 0.07051826001770234, 0.7389222546505236, 0.771434543548951, 0.3652539269897015, 0.2603059367721896, 299 | 0.4310648491411889, 0.7071252186366607, 0.16320122542325732, 0.44948421506462655, 0.48085540250421277, 0.08408482356412372, 300 | 0.923593157615906, 0.46852453511703684, 0.6670004526434297, 0.573634975268657, 0.12665814747855264, 0.3152822779549781, 301 | 0.21725524421221942, 0.5727451645276381, 0.7984025459787872, 0.7630923700903489, 0.27670044727992893, 0.919667661316697, 302 | 0.8828363383223908, 0.3618378476984413, 0.20690997158181246, 0.8321536435628591, 0.556153477173109, 0.3985225289975399, 303 | 0.7128303168401632, 0.433898343199971, 0.35116090526732047, 0.551636779637059, 0.03397585970570116, 0.28333059714010744, 304 | 0.4785548774382852, 0.16258401709779136, 0.8246056496848533, 0.23027686557606952, 0.26655111443159285, 0.6087446254045791] 305 | 306 | q_num_subs = dict(zip(puma560_def.q, q_test)) 307 | 308 | T = puma560.geo.T[-1] 309 | T = T.subs(q_num_subs) 310 | T = numpy.matrix(T).astype(numpy.float64) 311 | 312 | J = puma560.kin.J[-1] 313 | J = J.subs(q_num_subs) 314 | J = numpy.matrix(J).astype(numpy.float64) 315 | 316 | M_func_def = sympybotics.robotcodegen.robot_code_to_func( 317 | 'python', puma560.M_code, 'M', 'M_puma560', puma560_def) 318 | c_func_def = sympybotics.robotcodegen.robot_code_to_func( 319 | 'python', puma560.c_code, 'c', 'c_puma560', puma560_def) 320 | C_func_def = sympybotics.robotcodegen.robot_code_to_func( 321 | 'python', puma560.C_code, 'C', 'C_puma560', puma560_def) 322 | g_func_def = sympybotics.robotcodegen.robot_code_to_func( 323 | 'python', puma560.g_code, 'g', 'g_puma560', puma560_def) 324 | tau_func_def = sympybotics.robotcodegen.robot_code_to_func( 325 | 'python', puma560.invdyn_code, 'tau', 'tau_puma560', puma560_def) 326 | H_func_def = sympybotics.robotcodegen.robot_code_to_func( 327 | 'python', puma560.H_code, 'H', 'H_puma560', puma560_def) 328 | 329 | l = locals() 330 | exec_(M_func_def, globals(), l) 331 | exec_(c_func_def, globals(), l) 332 | exec_(C_func_def, globals(), l) 333 | exec_(g_func_def, globals(), l) 334 | exec_(tau_func_def, globals(), l) 335 | exec_(H_func_def, globals(), l) 336 | tau_puma560 = l['tau_puma560'] 337 | g_puma560 = l['g_puma560'] 338 | c_puma560 = l['c_puma560'] 339 | C_puma560 = l['C_puma560'] 340 | M_puma560 = l['M_puma560'] 341 | H_puma560 = l['H_puma560'] 342 | 343 | tau = tau_puma560(dynparm_test, q_test, dq_test, ddq_test) 344 | tau = numpy.matrix(tau).T.astype(numpy.float64) 345 | 346 | g = g_puma560(dynparm_test, q_test) 347 | g = numpy.matrix(g).T.astype(numpy.float64) 348 | 349 | c = c_puma560(dynparm_test, q_test, dq_test) 350 | c = numpy.matrix(c).T.astype(numpy.float64) 351 | 352 | C = C_puma560(dynparm_test, q_test, dq_test) 353 | C = numpy.matrix(C).reshape(puma560.dof, puma560.dof).astype(numpy.float64) 354 | C_dq = C * numpy.matrix(dq_test).T 355 | 356 | M = M_puma560(dynparm_test, q_test) 357 | M = numpy.matrix(M).reshape(puma560.dof, puma560.dof).astype(numpy.float64) 358 | 359 | H = H_puma560(q_test, dq_test, ddq_test) 360 | H = numpy.matrix(H).reshape(puma560.dof, puma560.dyn.n_dynparms 361 | ).astype(numpy.float64) 362 | 363 | tau_t2 = tau_puma560(dynparm_test2, q_test, dq_test, ddq_test) 364 | tau_t2 = numpy.matrix(tau_t2).T.astype(numpy.float64) 365 | 366 | g_t2 = g_puma560(dynparm_test2, q_test) 367 | g_t2 = numpy.matrix(g_t2).T.astype(numpy.float64) 368 | 369 | c_t2 = c_puma560(dynparm_test2, q_test, dq_test) 370 | c_t2 = numpy.matrix(c_t2).T.astype(numpy.float64) 371 | 372 | C_t2 = C_puma560(dynparm_test2, q_test, dq_test) 373 | C_t2 = numpy.matrix(C_t2).reshape(puma560.dof, puma560.dof 374 | ).astype(numpy.float64) 375 | C_dq_t2 = C_t2 * numpy.matrix(dq_test).T 376 | 377 | M_t2 = M_puma560(dynparm_test2, q_test) 378 | M_t2 = numpy.matrix(M_t2).reshape(puma560.dof, puma560.dof 379 | ).astype(numpy.float64) 380 | 381 | T_pcorke = numpy.matrix([[-0.655113870655343, -0.474277925274361, 0.588120962109346, 0.368531204501578], 382 | [ 0.524340309498464, 0.275038163391149, 0.805866768463298, 0.548861637700235], 383 | [-0.543960528264717, 0.836310025215453, 0.0685017183295358, -0.0731561881633544], 384 | [ 0.0, 0.0, 0.0, 1.0]]) 385 | 386 | J_pcorke = numpy.matrix([[ -0.548861637700235, -0.053505039458511, 0.181558777632408, 0.0, 0.0, 0.0], 387 | [ 0.368531204501578, -0.0498902657753524, 0.169292757497222, 0.0, 0.0, 0.0], 388 | [ 2.66713734431434e-17, -0.643843409557299, -0.35547724614047, 0.0, 0.0, 0.0], 389 | [-2.77555756156289e-17, -0.681969181663072, -0.681969181663072, 0.618588326585383, -0.778592276991744, 0.588120962109346], 390 | [-1.04083408558608e-16, 0.731380909828662, 0.731380909828662, 0.576796808883883, 0.541217849732837, 0.805866768463298], 391 | [ 1.0, 5.72458747072346e-17, 5.72458747072346e-17, 0.533529683779323, 0.317611878460765, 0.0685017183295358]]) 392 | 393 | tau_pcorke = numpy.matrix([[ 2.71610302653938], 394 | [ -28.7944111980059], 395 | [ -7.07013200484716], 396 | [0.00682888994276653], 397 | [-0.0220889100229662], 398 | [5.59805923645946e-5]]) 399 | 400 | g_pcorke = numpy.matrix([[2.88527195030991e-15], 401 | [ -31.1492065095002], 402 | [ -7.04528104551999], 403 | [ 0.00449992182016002], 404 | [ -0.026719893613902], 405 | [ 0.0]]) 406 | 407 | c_pcorke = numpy.matrix([[ 0.634177626441184], 408 | [ 1.12398760139525], 409 | [ -0.373925639216401], 410 | [-8.54577359118394e-5], 411 | [ 0.00162606594660281], 412 | [ 1.1075729558421e-5]]) 413 | 414 | M_pcorke = numpy.matrix([[ 2.88092539107775, 0.454967124647697, -0.0748835030743064, 0.00187610810095465, 0.00080480941146298, 2.74006873318143e-6], 415 | [ 0.454967124647696, 2.16866309038072, 0.368189596401607, -0.000307104503389342, 0.00234804941178354, 7.53260796283046e-6], 416 | [-0.0748835030743065, 0.368189596401607, 0.315830104422493, -0.000267827816326753, 0.00156601844062265, 7.53260796283046e-6], 417 | [0.00187610810095465, -0.000307104503389342, -0.000267827816326753, 0.00169083802882858, 1.98357371583556e-20, 3.4606953693396e-5], 418 | [0.00080480941146298, 0.00234804941178354, 0.00156601844062265, 2.86112061631228e-20, 0.00064216, 2.44929359829471e-21], 419 | [2.74006873318143e-6, 7.53260796283046e-6, 7.53260796283046e-6, 3.4606953693396e-5, 2.44929359829471e-21, 4.0e-5]]) 420 | 421 | tau_assrt2 = numpy.matrix([[ 5.46017281703168], 422 | [ -8.53813836226581], 423 | [-0.490552199963579], 424 | [ 16.4258371315422], 425 | [ 6.18803453956645], 426 | [ 7.99017179545593]]) 427 | 428 | g_assrt2 = numpy.matrix([[-1.77635683940025e-15], 429 | [ -12.2719368424473], 430 | [ -5.98183554268347], 431 | [ 18.5386690234033], 432 | [ 0.512467781908107], 433 | [ 7.9940463468124]]) 434 | 435 | c_assrt2 = numpy.matrix([[ 3.69185531689127], 436 | [ -1.34087787458136], 437 | [ 1.44951875057327], 438 | [ -2.77730540324791], 439 | [ 3.31210163468715], 440 | [-0.788977149763209]]) 441 | 442 | M_assrt2 = numpy.matrix([[ 3.65840168922149, -0.555774017452474, -1.39220275090878, 0.985927680308992, -0.439435427090482, 0.563816026114764], 443 | [-0.555774017452474, 5.33926241554461, 4.32747701210166, -1.24139932228482, 1.97231403813762, -0.207221260322988], 444 | [ -1.39220275090878, 4.32747701210166, 4.13345124166948, -1.07958817736071, 1.65363538346271, -0.00546039833881495], 445 | [ 0.985927680308992, -1.24139932228482, -1.07958817736071, 1.75689881264024, -0.358749490679038, 0.276358110012213], 446 | [-0.439435427090482, 1.97231403813762, 1.65363538346271, -0.358749490679038, 1.67868951032001, 0.396167549434339], 447 | [ 0.563816026114764, -0.207221260322988, -0.00546039833881495, 0.276358110012213, 0.396167549434339, 0.162584017097791]]) 448 | 449 | assert_precision = 10 450 | 451 | assert not numpy.any(numpy.round(T_pcorke - T, assert_precision)) 452 | assert not numpy.any(numpy.round(J_pcorke - J, assert_precision)) 453 | assert not numpy.any(numpy.round(tau_pcorke - tau, assert_precision)) 454 | assert not numpy.any(numpy.round(g_pcorke - g, assert_precision)) 455 | assert not numpy.any(numpy.round(c_pcorke - c, assert_precision)) 456 | assert not numpy.any(numpy.round(C_dq - c, assert_precision)) 457 | assert not numpy.any(numpy.round(M_pcorke - M, assert_precision)) 458 | assert not numpy.any(numpy.round( 459 | tau_pcorke - H * numpy.matrix(dynparm_test).T, assert_precision)) 460 | assert not numpy.any(numpy.round(tau_assrt2 - tau_t2, assert_precision)) 461 | assert not numpy.any(numpy.round(g_assrt2 - g_t2, assert_precision)) 462 | assert not numpy.any(numpy.round(c_assrt2 - c_t2, assert_precision)) 463 | assert not numpy.any(numpy.round(C_dq_t2 - c_t2, assert_precision)) 464 | assert not numpy.any(numpy.round(M_assrt2 - M_t2, assert_precision)) 465 | assert not numpy.any(numpy.round( 466 | tau_assrt2 - H * numpy.matrix(dynparm_test2).T, assert_precision)) 467 | -------------------------------------------------------------------------------- /sympybotics/tools/__init__.py: -------------------------------------------------------------------------------- 1 | 2 | from . import cache 3 | from . import qepcad 4 | 5 | -------------------------------------------------------------------------------- /sympybotics/tools/cache.py: -------------------------------------------------------------------------------- 1 | import pickle 2 | import hashlib 3 | import os 4 | import time 5 | import re 6 | 7 | 8 | 9 | def memoized( func, extra_deps='', cache_folder=None, hash_args_by_str=False, debug=False ): 10 | 11 | encode_pkl = lambda x: pickle.dumps( x ) 12 | #encode_str = lambda x: str( x ).encode() 13 | encode_str = lambda x: re.sub( ' at 0x[0-9a-f]+', ' at 0xXXXXXXXX', str(x) ).encode() 14 | 15 | if hash_args_by_str: 16 | encode_args = encode_str 17 | else: 18 | encode_args = encode_pkl 19 | 20 | def decorated_function( *args, **kwargs ): 21 | 22 | funcstr = encode_pkl(func) 23 | argsstr = encode_args(args) 24 | kwargsstr = encode_args(kwargs) 25 | extrastr = encode_args(extra_deps) 26 | 27 | if debug: 28 | print('funcstr ',hashlib.sha1(funcstr).hexdigest()) 29 | print('argsstr ',hashlib.sha1(argsstr).hexdigest()) 30 | print('kwargsstr',hashlib.sha1(kwargsstr).hexdigest()) 31 | print('extrastr ',hashlib.sha1(extrastr).hexdigest()) 32 | 33 | bytestr = funcstr + argsstr + kwargsstr + extrastr 34 | 35 | sha1 = hashlib.sha1(bytestr).hexdigest() 36 | 37 | if debug: 38 | print('all ',sha1) 39 | 40 | filename = func.__name__ + '-' + sha1 + '.pkl' 41 | 42 | pathfile = os.path.join( cache_folder, filename ) 43 | 44 | try: 45 | with open( pathfile, 'rb' ) as file: 46 | cached_return = pickle.load( file ) 47 | 48 | except: 49 | print('(memoization cache miss ...)') 50 | 51 | if cache_folder and not os.path.exists( cache_folder ): 52 | os.makedirs( cache_folder ) 53 | 54 | t0 = time.time() 55 | 56 | call_return = func( *args, **kwargs ) 57 | 58 | print( '(... memoization call wall time: %.3f)' % (time.time()-t0) ) 59 | 60 | if debug: 61 | print('call ',hashlib.sha1(str(call_return).encode()).hexdigest()) 62 | 63 | with open( pathfile, 'wb' ) as file: 64 | pickle.dump( call_return, file ) 65 | 66 | return call_return 67 | 68 | print('(memoization cache hit)') 69 | 70 | if debug: 71 | print('cache ',hashlib.sha1(str(cached_return).encode()).hexdigest()) 72 | 73 | return cached_return 74 | 75 | return decorated_function 76 | -------------------------------------------------------------------------------- /sympybotics/tools/qepcad.py: -------------------------------------------------------------------------------- 1 | 2 | import subprocess 3 | 4 | 5 | class VarMaps(object): 6 | pass 7 | 8 | def gen_qepcad_varmaps( var_list ): 9 | 10 | forwmap = {} 11 | 12 | for v in var_list: 13 | 14 | v = str(v) 15 | if v in forwmap: 16 | raise 'Error: Two variables with same string representation' 17 | 18 | vm = v.replace('_','') 19 | 20 | if vm not in forwmap.values(): 21 | forwmap[v] = vm 22 | else: 23 | raise 'Error: Two variables with same map' 24 | 25 | backmap = dict( zip( forwmap.values(), forwmap.keys()) ) 26 | 27 | varmaps = VarMaps(); 28 | 29 | varmaps.forward = forwmap 30 | varmaps.backward = backmap 31 | 32 | return varmaps 33 | 34 | 35 | 36 | def sym_to_qepcad( symexpr, vardictforw=None ): 37 | if vardictforw is None: 38 | vardictforw = {} 39 | 40 | symexpr = str(symexpr).replace('**','^').replace('*',' ') 41 | 42 | for var in vardictforw: 43 | symexpr = symexpr.replace(var,vardictforw[var]) 44 | 45 | elements = symexpr.split() 46 | 47 | for i,e in enumerate(elements): 48 | if '/' in e: 49 | l,r = e.split('/') 50 | if not l.isnumeric(): 51 | elements[i] = l + ' 1/' + r 52 | 53 | symexpr = ' '.join(elements) 54 | 55 | return symexpr 56 | 57 | 58 | 59 | def qepcad_to_sym( qepcad_rel, vardictback=None ): 60 | if vardictforw is None: 61 | vardictforw = {} 62 | 63 | elements = qepcad_rel.split() 64 | 65 | out_elements = [] 66 | 67 | last_isalnum = False 68 | 69 | for i,e in enumerate(elements): 70 | 71 | e0 = e.split('^')[0] 72 | e = e.replace('^','**') 73 | 74 | isalnum = e0.isalnum() 75 | 76 | if isalnum and last_isalnum: 77 | out_elements.append('*') 78 | 79 | last_isalnum = isalnum 80 | 81 | if isalnum and e0 in vardictback: 82 | e = e.replace( e0, str(vardictback[e0]) ) 83 | 84 | out_elements.append( e ) 85 | 86 | return ' '.join(out_elements) 87 | 88 | 89 | def gen_qepcad_input( freevars, qvars, prenex, vardictforw=None ): 90 | if vardictforw is None: 91 | vardictforw = {} 92 | 93 | vars = '(' + ','.join( [ sym_to_qepcad(var) for var in freevars ] + [ sym_to_qepcad(var) for var in qvars ] ) + ')' 94 | 95 | for var in vardictforw: 96 | vars = vars.replace(var,vardictforw[var]) 97 | 98 | freevarnum = str(len(freevars)) 99 | 100 | prenex += '.' if prenex[-1] != '.' else '' 101 | 102 | 103 | qepcad_input = '[]\n'+vars+'\n'+freevarnum+'\n'+prenex+'\nfinish\n' 104 | 105 | return qepcad_input 106 | 107 | 108 | def run_qepcad( qepcad_cmd, qepcad_input ): 109 | 110 | proc = subprocess.Popen(qepcad_cmd, shell=True, stdin=subprocess.PIPE, stdout=subprocess.PIPE ) 111 | 112 | proc.stdin.write(qepcad_input.encode()) 113 | 114 | out = proc.communicate() 115 | 116 | outstr = out[0].decode("utf-8") 117 | 118 | #print(outstr) 119 | 120 | outlines = outstr.splitlines() 121 | 122 | return outlines[ outlines.index('An equivalent quantifier-free formula:') + 2 ] 123 | -------------------------------------------------------------------------------- /sympybotics/utils.py: -------------------------------------------------------------------------------- 1 | """ Utilities """ 2 | 3 | from sympy import Matrix 4 | 5 | 6 | identity = lambda x: x 7 | 8 | 9 | def sym_skew(v): 10 | return Matrix([[0, -v[2], v[1]], 11 | [v[2], 0, -v[0]], 12 | [-v[1], v[0], 0]]) 13 | -------------------------------------------------------------------------------- /todo.md: -------------------------------------------------------------------------------- 1 | TODO 2 | ==== 3 | 4 | - more/better documentation 5 | - more tests 6 | - add motor rotor inertia to model 7 | - add custom motor to joint models 8 | - add Coriolis matrix computation 9 | - add Lagrangian recursive method (maybe) 10 | - add Link/DH parameters class - Link(alpha=x, a=y, ....) 11 | - support tree robots - use tree to model gear and rotor chain 12 | --------------------------------------------------------------------------------