diff --git a/.gitignore b/.gitignore index 3d691a8..c75b45b 100644 --- a/.gitignore +++ b/.gitignore @@ -1,221 +1,56 @@ -################# -## Eclipse -################# - -*.pydevproject -.idea -.project -.metadata -bin/ -tmp/ -*.tmp -*.bak +# tmp files *.swp -*~.nib -local.properties -.classpath -.settings/ -.loadpath - -# External tool builders -.externalToolBuilders/ - -# Locally stored "Eclipse launch configurations" -*.launch - -# CDT-specific -.cproject - -# PDT-specific -.buildpath - - -################# -## Visual Studio -################# - -## Ignore Visual Studio temporary files, build results, and -## files generated by popular Visual Studio add-ons. - -# User-specific files -*.suo -*.user -*.sln.docstates - -# Build results - -[Dd]ebug/ -[Rr]elease/ -x64/ -build/ -[Bb]in/ -[Oo]bj/ - -# MSTest test Results -[Tt]est[Rr]esult*/ -[Bb]uild[Ll]og.* - -*_i.c -*_p.c -*.ilk -*.meta -*.obj -*.pch -*.pdb -*.pgc -*.pgd -*.rsp -*.sbr -*.tlb -*.tli -*.tlh *.tmp -*.tmp_proj -*.log -*.vspscc -*.vssscc -.builds -*.pidb -*.log -*.scc - -# Visual C++ cache files -ipch/ -*.aps -*.ncb -*.opensdf -*.sdf -*.cachefile - -# Visual Studio profiler -*.psess -*.vsp -*.vspx - -# Guidance Automation Toolkit -*.gpState - -# ReSharper is a .NET coding add-in -_ReSharper*/ -*.[Rr]e[Ss]harper - -# TeamCity is a build add-in -_TeamCity* - -# DotCover is a Code Coverage Tool -*.dotCover - -# NCrunch -*.ncrunch* -.*crunch*.local.xml - -# Installshield output folder -[Ee]xpress/ - -# DocProject is a documentation generator add-in -DocProject/buildhelp/ -DocProject/Help/*.HxT -DocProject/Help/*.HxC -DocProject/Help/*.hhc -DocProject/Help/*.hhk -DocProject/Help/*.hhp -DocProject/Help/Html2 -DocProject/Help/html - -# Click-Once directory -publish/ - -# Publish Web Output -*.Publish.xml -*.pubxml - -# NuGet Packages Directory -## TODO: If you have NuGet Package Restore enabled, uncomment the next line -#packages/ - -# Windows Azure Build Output -csx -*.build.csdef - -# Windows Store app package directory -AppPackages/ - -# Others -sql/ -*.Cache -ClientBin/ -[Ss]tyle[Cc]op.* -~$* +*.bak *~ -*.dbmdl -*.[Pp]ublish.xml -*.pfx -*.publishsettings - -# RIA/Silverlight projects -Generated_Code/ - -# Backup & report files from converting an old project file to a newer -# Visual Studio version. Backup files are not needed, because we have git ;-) -_UpgradeReport_Files/ -Backup*/ -UpgradeLog*.XML -UpgradeLog*.htm - -# SQL Server files -App_Data/*.mdf -App_Data/*.ldf - -############# -## Windows detritus -############# - -# Windows image file caches -Thumbs.db -ehthumbs.db - -# Folder config file -Desktop.ini - -# Recycle Bin used on file shares -$RECYCLE.BIN/ - -# Mac crap -.DS_Store +# Byte-compiled / optimized / DLL files +__pycache__/ +*.py[cod] -############# -## Python -############# +# C extensions +*.so -*.py[co] -*.txt -# Packages -*.egg -*.egg-info -dist/ +# Distribution / packaging build/ +develop-eggs/ +dist/ eggs/ +lib/ +lib64/ parts/ -var/ sdist/ -develop-eggs/ -old/ -validation/ -symoro math/ -Test RX90/ -pysymoro/core/robots +var/ +*.egg-info/ .installed.cfg +*.egg # Installer logs pip-log.txt +pip-delete-this-directory.txt # Unit test / coverage reports +.tox/ .coverage -.tox +.cache +nosetests.xml +coverage.xml -#Translations +# Translations *.mo -#Mr Developer +# Mr Developer .mr.developer.cfg +.project +.pydevproject + +# Rope +.ropeproject + +# Django stuff: +*.log +*.pot + +# Sphinx documentation +docs/_build/ + diff --git a/Core/Robots/RX90/RX90.par b/Core/Robots/RX90/RX90.par deleted file mode 100644 index aef7411..0000000 --- a/Core/Robots/RX90/RX90.par +++ /dev/null @@ -1,57 +0,0 @@ -(* Robotname = 'RX90' *) -NL = 6 -NJ = 6 -NF = 6 -Type = 0 -is_mobile = 0 - -(* Geometric parameters *) -ant = {0,1,2,3,4,5} -sigma = {0,0,0,0,0,0} -b = {0,0,0,0,0,0} -d = {0,0,D3,0,0,0} -r = {0,0,0,RL4,0,0} -gamma = {0,0,0,0,0,0} -alpha = {0,pi/2,0,-pi/2,pi/2,-pi/2} -mu = {1,1,1,1,1,1} -theta = {th1,th2,th3,th4,th5,th6} - -(* Dynamic parameters and external forces *) -XX = {XX1,XX2,XX3,XX4,XX5,XX6} -XY = {XY1,XY2,XY3,XY4,XY5,XY6} -XZ = {XZ1,XZ2,XZ3,XZ4,XZ5,XZ6} -YY = {YY1,YY2,YY3,YY4,YY5,YY6} -YZ = {YZ1,YZ2,YZ3,YZ4,YZ5,YZ6} -ZZ = {ZZ1,ZZ2,ZZ3,ZZ4,ZZ5,ZZ6} -MX = {MX1,MX2,MX3,MX4,MX5,MX6} -MY = {MY1,MY2,MY3,MY4,MY5,MY6} -MZ = {MZ1,MZ2,MZ3,MZ4,MZ5,MZ6} -M = {M1,M2,M3,M4,M5,M6} -IA = {IA1,IA2,IA3,IA4,IA5,IA6} -FV = {FV1,FV2,FV3,FV4,FV5,FV6} -FS = {FS1,FS2,FS3,FS4,FS5,FS6} -FX = {0,0,0,0,0,FX6} -FY = {0,0,0,0,0,FY6} -FZ = {0,0,0,0,0,FZ6} -CX = {0,0,0,0,0,CX6} -CY = {0,0,0,0,0,CY6} -CZ = {0,0,0,0,0,CZ6} - -(* Joint parameters *) -QP = {QP1,QP2,QP3,QP4,QP5,QP6} -QDP = {QDP1,QDP2,QDP3,QDP4,QDP5,QDP6} -GAM = {GAM1,GAM2,GAM3,GAM4,GAM5,GAM6} - -(* Speed and acceleration of the base *) -W0 = {0,0,0} -WP0 = {0,0,0} -V0 = {0,0,0} -VP0 = {0,0,0} - -(* Acceleration of gravity *) -G = {0,0,G3} - -(* Transformation of 0 frame position fT0 *) -Z = {1,0,0,0,0,1,0,0,0,0,1,0,0,0,0,1} - -(* End of definition *) diff --git a/LICENCE b/LICENCE new file mode 100644 index 0000000..fe1f19b --- /dev/null +++ b/LICENCE @@ -0,0 +1,22 @@ +The MIT License (MIT) + +Copyright (c) 2014 Wisama Khalil, Institut de Recherche en Communications et +Cybernétique de Nantes (IRCCyN) + +Permission is hereby granted, free of charge, to any person obtaining a copy +of this software and associated documentation files (the "Software"), to deal +in the Software without restriction, including without limitation the rights +to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +copies of the Software, and to permit persons to whom the Software is +furnished to do so, subject to the following conditions: + +The above copyright notice and this permission notice shall be included in +all copies or substantial portions of the Software. + +THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN +THE SOFTWARE. diff --git a/README.md b/README.md index bb3f23a..7d4d869 100644 --- a/README.md +++ b/README.md @@ -1,9 +1,46 @@ SYMORO ====== -SYmbolic MOdeling of RObots software. The language is python and the key library is sympy. +SYOMRO is a software package for SYmbolic MOdeling of RObots. -This is an open-source version of SYMORO software written in Python. Hence you do not need Mathematica license to use it. +This software package is developed as part of the OpenSYMORO project by +the robotics team at [IRCCyN][lk:irccyn] under the supervision of Wisama +Khalil. + +For details on the algorithms used, please see [the paper][lk:hal] +published in the AIM 2014 conference. + + +Requirements +------------ ++ python (>= 2.7,    3.* is not supported) ++ sympy (== 0.7.3) ++ numpy (>= 1.6.1) ++ wxPython (>= 2.8.12) ++ OpenGL (>= 3.0.1b2) + + +Getting Started +--------------- ++ For setting up SYMORO, see [Setup][lk:setup]. ++ If you have any queries, contact [Aravind][el:aravind]. + + +Licence +------- +See [LICENCE][lk:licence]. + + +Contributors +------------ +See [Contributors][lk:contributors]. + + +[lk:irccyn]: http://www.irccyn.ec-nantes.fr/ +[lk:hal]: http://hal.archives-ouvertes.fr/hal-01025919 +[lk:setup]: https://github.com/symoro/symoro/wiki/Setup +[el:aravind]: mailto:Aravindkumar.Vijayalingam@eleves.ec-nantes.fr +[lk:licence]: https://github.com/symoro/symoro/blob/master/LICENCE +[lk:contributors]: https://github.com/symoro/symoro/graphs/contributors -For more details on SYMORO, please see http://www.irccyn.ec-nantes.fr/spip.php?article601&lang=en diff --git a/bin/symoro-bin b/bin/symoro-bin new file mode 120000 index 0000000..f11bf78 --- /dev/null +++ b/bin/symoro-bin @@ -0,0 +1 @@ +symoro-bin.py \ No newline at end of file diff --git a/bin/symoro-bin.py b/bin/symoro-bin.py new file mode 100644 index 0000000..2934322 --- /dev/null +++ b/bin/symoro-bin.py @@ -0,0 +1,40 @@ +#!/usr/bin/env python +# -*- coding: utf-8 -*- + + +# This file is part of the OpenSYMORO project. Please see +# https://github.com/symoro/symoro/blob/master/LICENCE for the licence. + + +""" +This is the main executable script of SYMORO. +""" + + +import os +import sys + +import wx + +from symoroui import layout +from symoroui import labels as ui_labels + + +def main(): + app = wx.App(redirect=False) + style = wx.DEFAULT_FRAME_STYLE ^ wx.MAXIMIZE_BOX ^ wx.RESIZE_BORDER + frame = layout.MainFrame( + parent=None, + id=wx.ID_ANY, + title=ui_labels.MAIN_WIN['window_title'], + size=(-1, -1), + style=style + ) + frame.Show() + app.MainLoop() + + +if __name__ == "__main__": + main() + + diff --git a/launch.sh b/launch.sh new file mode 100755 index 0000000..a78c2b3 --- /dev/null +++ b/launch.sh @@ -0,0 +1,6 @@ +#!/bin/sh + +DIR="$(dirname $0)" + +PYTHONPATH="$DIR":"$PYTHONPATH" python "$DIR"/bin/symoro-bin.py + diff --git a/pysymoro/baseparams.py b/pysymoro/baseparams.py new file mode 100644 index 0000000..4c18073 --- /dev/null +++ b/pysymoro/baseparams.py @@ -0,0 +1,288 @@ +# -*- coding: utf-8 -*- + + +# This file is part of the OpenSYMORO project. Please see +# https://github.com/symoro/symoro/blob/master/LICENCE for the licence. + + +""" +This module of SYMORO package contains function to compute the base +inertial parameters. +""" + + +import sympy +from sympy import Matrix + +from pysymoro.geometry import compute_rot_trans +from pysymoro.geometry import Transform +from symoroutils import symbolmgr +from symoroutils import tools + + +inert_names = ('XXR', 'XYR', 'XZR', 'YYR', 'YZR', + 'ZZR', 'MXR', 'MYR', 'MZR', 'MR') + + +# TODO:Finish base parameters computation +def base_inertial_parameters(robo, symo): + """Computes grouped inertia parameters. New parametrization + contains less parameters but generates the same dynamic model + + Parameters + ========== + robo : Robot + Instance of robot description container + + Returns + ======= + symo.sydi : dictionary + Dictionary with the information of all the sybstitution + """ + lam = [0 for i in xrange(robo.NL)] + # init transformation + antRj, antPj = compute_rot_trans(robo, symo) + for j in reversed(xrange(1, robo.NL)): + if robo.sigma[j] == 0: + # general grouping + compute_lambda(robo, symo, j, antRj, antPj, lam) + group_param_rot(robo, symo, j, lam) + # special grouping + group_param_rot_spec(robo, symo, j, lam, antRj) + pass + elif robo.sigma[j] == 1: + # general grouping + group_param_prism(robo, symo, j, antRj) + # special grouping + group_param_prism_spec(robo, symo, j, antRj, antPj) + pass + elif robo.sigma[j] == 2: + # fixed joint, group everuthing + compute_lambda(robo, symo, j, antRj, antPj, lam) + group_param_fix(robo, symo, j, lam) + symo.write_line('*=*') + + +def vec_mut_J(v, u): + """Internal function. Needed for inertia parameters transformation + + Parameters + ========== + v, u : Matrix 3x1 + two axis vectors + Returns : Matrix 6x1 + """ + return Matrix([v[0]*u[0], v[0]*u[1], v[0]*u[2], + v[1]*u[1], v[1]*u[2], v[2]*u[2]]) + + +def vec_mut_MS(v, P): + """Internal function. Needed for inertia parameters transformation + + Parameters + ========== + v : Matrix 3x1 + axis vector + P : Matrix 3x1 + position vector + + Returns : Matrix 6x1 + """ + U = - tools.skew(v)*tools.skew(P) + return Matrix([2*U[0, 0], U[0, 1] + U[1, 0], U[0, 2] + U[2, 0], + 2*U[1, 1], U[1, 2] + U[2, 1], 2*U[2, 2]]) + + +def vec_mut_M(P): + """Internal function. Needed for inertia parameters transformation + + Parameters + ========== + P : Matrix 3x1 + position vector + + Returns : Matrix 6x1 + """ + U = -tools.skew(P)*tools.skew(P) + return Matrix([U[0, 0], U[0, 1], U[0, 2], U[1, 1], U[1, 2], U[2, 2]]) + + +def compute_lambda(robo, symo, j, antRj, antPj, lam): + """Internal function. Computes the inertia parameters + transformation matrix + + Notes + ===== + lam is the output paramete + """ + lamJJ_list = [] + lamJMS_list = [] + for e1 in xrange(3): + for e2 in xrange(e1, 3): + u = vec_mut_J(antRj[j][:, e1], antRj[j][:, e2]) + if e1 != e2: + u += vec_mut_J(antRj[j][:, e2], antRj[j][:, e1]) + lamJJ_list.append(u.T) + for e1 in xrange(3): + v = vec_mut_MS(antRj[j][:, e1], antPj[j]) + lamJMS_list.append(v.T) + lamJJ = Matrix(lamJJ_list).T # , 'LamJ', j) + lamJMS = symo.mat_replace(Matrix(lamJMS_list).T, 'LamMS', j) + lamJM = symo.mat_replace(vec_mut_M(antPj[j]), 'LamM', j) + lamJ = lamJJ.row_join(lamJMS).row_join(lamJM) + lamMS = sympy.zeros(3, 6).row_join(antRj[j]).row_join(antPj[j]) + lamM = sympy.zeros(1, 10) + lamM[9] = 1 + lam[j] = Matrix([lamJ, lamMS, lamM]) + + +def group_param_rot(robo, symo, j, lam): + """Internal function. Groups inertia parameters according to the + general rule for a rotational joint. + + Notes + ===== + robo is the output paramete + """ + Kj = robo.get_inert_param(j) + + lam03 = lam[j][:, 0] + lam[j][:, 3] + lam03 = lam03.applyfunc(symo.C2S2_simp) + for i in (3, 8, 9): + Kj[i] = symo.replace(Kj[i], inert_names[i], j) + if robo.ant[j] != -1: + Kant = robo.get_inert_param(robo.ant[j]) + Kant += lam03*Kj[3] + lam[j][:, 8]*Kj[8] + lam[j][:, 9]*Kj[9] + robo.put_inert_param(Kant, robo.ant[j]) + Kj[0] -= Kj[3] # XX + Kj[3] = 0 # YY + Kj[8] = 0 # MZ + Kj[9] = 0 # M + robo.put_inert_param(Kj, j) + + +def group_param_rot_spec(robo, symo, j, lam, antRj): + """Internal function. Groups inertia parameters according to the + special rule for a rotational joint. + + Notes + ===== + robo is the output paramete + """ + chainj = robo.chain(j) + r1, r2, orthog = Transform.find_r12(robo, chainj, antRj, j) + kRj, all_paral = Transform.kRj(robo, antRj, r1, chainj) + Kj = robo.get_inert_param(j) + to_replace = {0, 1, 2, 4, 5, 6, 7} + if Transform.z_paral(kRj): + Kj[0] = 0 # XX + Kj[1] = 0 # XY + Kj[2] = 0 # XZ + Kj[4] = 0 # YZ + to_replace -= {0, 1, 2, 4} + joint_axis = antRj[chainj[-1]].col(2) + if all_paral and robo.G.norm() == sympy.Abs(joint_axis.dot(robo.G)): + Kj[6] = 0 # MX + Kj[7] = 0 # MY + to_replace -= {6, 7} + if j == r1 or(j == r2 and orthog): + Kj[5] += robo.IA[j] # ZZ + robo.IA[j] = 0 + for i in to_replace: + Kj[i] = symo.replace(Kj[i], inert_names[i], j) + robo.put_inert_param(Kj, j) + + +def group_param_fix(robo, symo, j, lam): + """Internal function. Groups inertia parameters according to the + general rule for a fixed joint joint. + + Notes + ===== + robo is the output paramete + """ + Kj = robo.get_inert_param(j) + for i in xrange(10): + Kj[i] = symo.replace(Kj[i], inert_names[i], j) + if robo.ant[j] != -1: + Kant = robo.get_inert_param(robo.ant[j]) + Kant += lam[j]*Kj + robo.put_inert_param(Kant, robo.ant[j]) + robo.put_inert_param(sympy.zeros(10, 1), j) + + +def group_param_prism(robo, symo, j, antRj): + """Internal function. Groups inertia parameters according to the + general rule for a prismatic joint. + + Notes + ===== + robo is the output paramete + """ + Kj = robo.get_inert_param(j) + for i in xrange(6): + Kj[i] = symo.replace(Kj[i], inert_names[i], j) + robo.put_inert_param(Kj, j) + if robo.ant[j] != -1: + antJj = antRj[j]*robo.J[j]*antRj[j].T + robo.J[robo.ant[j]] += antJj + robo.J[j] = sympy.zeros(3, 3) + + +def group_param_prism_spec(robo, symo, j, antRj, antPj): + """Internal function. Groups inertia parameters according to the + special rule for a prismatic joint. + + Notes + ===== + robo is the output paramete + """ + chainj = robo.chain(j) + r1, r2, orthog = Transform.find_r12(robo, chainj, antRj, j) + Kj = robo.get_inert_param(j) + kRj, all_paral = Transform.kRj(robo, antRj, r1, chainj) + to_replace = {6, 7, 8, 9} + if r1 < j and j < r2: + if Transform.z_paral(kRj): + Kj[8] = 0 # MZ + for i in (6, 7): + Kj[i] = symo.replace(Kj[i], inert_names[i], j) + robo.MS[robo.ant[j]] += antRj[j]*Matrix([Kj[6], Kj[7], 0]) + robo.JJ[2, 2] -= Kj[6]*antPj[j][0] + Kj[7]*antPj[j][1] + Kj[6] = 0 # MX + Kj[7] = 0 # MY + to_replace -= {6, 7, 8} + else: + jar1 = kRj.row(2) + if jar1[2] != 0: + Kj[6] -= jar1[0]/jar1[2]*Kj[8] + Kj[7] -= jar1[1]/jar1[2]*Kj[8] + Kj[8] = 0 # MZ + to_replace -= {8} + elif jar1[0]*jar1[1] != 0: + Kj[6] -= jar1[0]/jar1[1]*Kj[7] + Kj[7] = 0 # MY + to_replace -= {7} + elif jar1[0] != 0: + Kj[7] = 0 # MY + to_replace -= {7} + else: + Kj[6] = 0 # MX + to_replace -= {6} + elif j < r1: + Kj[6] = 0 # MX + Kj[7] = 0 # MY + Kj[8] = 0 # MZ + to_replace -= {6, 7, 8} + #TOD: rewrite + dotGa = Transform.sna(antRj[j])[2].dot(robo.G) + if dotGa == tools.ZERO: + revol_align = robo.ant[robo.ant[j]] == 0 and robo.ant[j] == tools.ZERO + if robo.ant[j] == 0 or revol_align: + Kj[9] += robo.IA[j] + robo.IA[j] = 0 + for i in to_replace: + Kj[i] = symo.replace(Kj[i], inert_names[i], j) + robo.put_inert_param(Kj, j) + + diff --git a/pysymoro/core/dynamics.py b/pysymoro/core/dynamics.py deleted file mode 100644 index 289b3bd..0000000 --- a/pysymoro/core/dynamics.py +++ /dev/null @@ -1,801 +0,0 @@ -""" -This module of SYMORO package provides symbolic -modeling of robots' dynamics. - -The core symbolic library is sympy. -Needed modules : symoro.py, geometry.py, kinematics.py - -ECN - ARIA1 2013 -""" -from copy import copy, deepcopy -import sympy -from sympy import Matrix -from symoro import Symoro, Init, hat, ZERO -from geometry import compute_screw_transform -from geometry import compute_rot_trans, Transform -from kinematics import compute_vel_acc -from kinematics import compute_omega - -chars = 'ABCDEFGHJKLMNPQRSTUVWXYZ' -inert_names = ('XXR', 'XYR', 'XZR', 'YYR', 'YZR', - 'ZZR', 'MXR', 'MYR', 'MZR', 'MR') - - -def Newton_Euler(robo, symo): - """Internal function. Computes Inverse Dynamic Model using - Newton-Euler formulation - - Parameters - ========== - robo : Robot - Instance of robot description container - symo : Symoro - Instance of symbolic manager - """ - # init external forces - Fex = copy(robo.Fex) - Nex = copy(robo.Nex) - # init transformation - antRj, antPj = compute_rot_trans(robo, symo) - # init velocities and accelerations - w, wdot, vdot, U = compute_vel_acc(robo, symo, antRj, antPj) - # init forces vectors - F = Init.init_vec(robo) - N = Init.init_vec(robo) - Fjnt = Init.init_vec(robo) - Njnt = Init.init_vec(robo) - for j in xrange(1, robo.NL): - compute_wrench(robo, symo, j, w, wdot, U, vdot, F, N) - for j in reversed(xrange(1, robo.NL)): - compute_joint_wrench(robo, symo, j, antRj, antPj, vdot, - Fjnt, Njnt, F, N, Fex, Nex) - for j in xrange(1, robo.NL): - compute_torque(robo, symo, j, Fjnt, Njnt) - - -def dynamic_identification_NE(robo): - """Computes Dynamic Identification Model using - Newton-Euler formulation - - Parameters - ========== - robo : Robot - Instance of robot description container - - Returns - ======= - symo.sydi : dictionary - Dictionary with the information of all the sybstitution - """ - - # init forces vectors - Fjnt = Init.init_vec(robo) - Njnt = Init.init_vec(robo) - # init file output, writing the robot description - symo = Symoro() - symo.file_open(robo, 'dim') - title = "Dynamic identification model using Newton - Euler Algorith" - symo.write_params_table(robo, title, inert=True, dynam=True) - # init transformation - antRj, antPj = compute_rot_trans(robo, symo) - # init velocities and accelerations - w, wdot, vdot, U = compute_vel_acc(robo, symo, antRj, antPj) - # virtual robot with only one non-zero parameter at once - robo_tmp = deepcopy(robo) - robo_tmp.IA = sympy.zeros(robo.NL, 1) - robo_tmp.FV = sympy.zeros(robo.NL, 1) - robo_tmp.FS = sympy.zeros(robo.NL, 1) - for k in xrange(1, robo.NL): - param_vec = robo.get_inert_param(k) - F = Init.init_vec(robo) - N = Init.init_vec(robo) - for i in xrange(10): - if param_vec[i] == ZERO: - continue - # change link names according to current non-zero parameter - robo_tmp.num = [str(l) + str(param_vec[i]) - for l in xrange(k + 1)] - # set the parameter to 1 - mask = sympy.zeros(10, 1) - mask[i] = 1 - robo_tmp.put_inert_param(mask, k) - # compute the total forcec of the link k - compute_wrench(robo_tmp, symo, k, w, wdot, U, vdot, F, N) - # init external forces - Fex = copy(robo.Fex) - Nex = copy(robo.Nex) - for j in reversed(xrange(k + 1)): - compute_joint_wrench(robo_tmp, symo, j, antRj, antPj, - vdot, Fjnt, Njnt, F, N, Fex, Nex) - for j in xrange(k + 1): - compute_torque(robo_tmp, symo, j, Fjnt, Njnt, 'DG') - # reset all the parameters to zero - robo_tmp.put_inert_param(sympy.zeros(10, 1), k) - # compute model for the joint parameters - compute_joint_torque_deriv(symo, robo.IA[k], - robo.qddot[k], k) - compute_joint_torque_deriv(symo, robo.FS[k], - sympy.sign(robo.qdot[k]), k) - compute_joint_torque_deriv(symo, robo.FV[k], - robo.qdot[k], k) - # closing the output file - symo.file_close() - return symo - - -def direct_dynamic_NE(robo): - """Computes Direct Dynamic Model using - Newton-Euler formulation - - Parameters - ========== - robo : Robot - Instance of robot description container - - Returns - ======= - symo.sydi : dictionary - Dictionary with the information of all the sybstitution - """ - wi = Init.init_vec(robo) - # antecedent angular velocity, projected into jth frame - w = Init.init_w(robo) - jaj = Init.init_vec(robo, 6) - jTant = Init.init_mat(robo, 6) # Twist transform list of Matrices 6x6 - beta_star = Init.init_vec(robo, 6) - grandJ = Init.init_mat(robo, 6) - link_acc = Init.init_vec(robo, 6) - H_inv = Init.init_scalar(robo) - juj = Init.init_vec(robo, 6) # Jj*aj / Hj - Tau = Init.init_scalar(robo) - grandVp = Init.init_vec(robo, 6) - grandVp.append(Matrix([robo.vdot0 - robo.G, robo.w0])) - symo = Symoro() - symo.file_open(robo, 'ddm') - title = 'Direct dynamic model using Newton - Euler Algorith' - symo.write_params_table(robo, title, inert=True, dynam=True) - - # init transformation - antRj, antPj = compute_rot_trans(robo, symo) - for j in xrange(1, robo.NL): - compute_omega(robo, symo, j, antRj, w, wi) - compute_screw_transform(robo, symo, j, antRj, antPj, jTant) - if robo.sigma[j] == 0: - jaj[j] = Matrix([0, 0, 0, 0, 0, 1]) - elif robo.sigma[j] == 1: - jaj[j] = Matrix([0, 0, 1, 0, 0, 0]) - for j in xrange(1, robo.NL): - compute_beta(robo, symo, j, w, beta_star) - compute_link_acc(robo, symo, j, antRj, antPj, link_acc, w, wi) - grandJ[j] = inertia_spatial(robo.J[j], robo.MS[j], robo.M[j]) - for j in reversed(xrange(1, robo.NL)): - replace_beta_J_star(robo, symo, j, grandJ, beta_star) - compute_Tau(robo, symo, j, grandJ, beta_star, jaj, juj, H_inv, Tau) - if robo.ant[j] != - 1: - compute_beta_J_star(robo, symo, j, grandJ, jaj, juj, Tau, - beta_star, jTant, link_acc) - for j in xrange(1, robo.NL): - compute_acceleration(robo, symo, j, jTant, grandVp, - juj, H_inv, jaj, Tau, link_acc) - for j in xrange(1, robo.NL): - compute_coupled_forces(robo, symo, j, grandVp, grandJ, beta_star) - symo.file_close() - return symo - - -def inertia_matrix(robo): - """Computes Inertia Matrix using composed link - - Parameters - ========== - robo : Robot - Instance of robot description container - - Returns - ======= - symo.sydi : dictionary - Dictionary with the information of all the sybstitution - """ - Jplus, MSplus, Mplus = Init.init_Jplus(robo) - AJE1 = Init.init_vec(robo) - f = Init.init_vec(robo, ext=1) - n = Init.init_vec(robo, ext=1) - A = sympy.zeros(robo.NL, robo.NL) - symo = Symoro() - symo.file_open(robo, 'inm') - title = 'Inertia Matrix using composite links' - symo.write_params_table(robo, title, inert=True, dynam=True) - # init transformation - antRj, antPj = compute_rot_trans(robo, symo) - for j in reversed(xrange(-1, robo.NL)): - replace_Jplus(robo, symo, j, Jplus, MSplus, Mplus) - if j != - 1: - compute_Jplus(robo, symo, j, antRj, antPj, - Jplus, MSplus, Mplus, AJE1) - for j in xrange(1, robo.NL): - compute_A_diagonal(robo, symo, j, Jplus, MSplus, Mplus, f, n, A) - ka = j - while ka != - 1: - k = ka - ka = robo.ant[ka] - compute_A_triangle(robo, symo, j, k, ka, - antRj, antPj, f, n, A, AJE1) - symo.mat_replace(A, 'A', forced=True, symmet=True) - J_base = inertia_spatial(Jplus[-1], MSplus[-1], Mplus[-1]) - symo.mat_replace(J_base, 'JP', 0, forced=True, symmet=True) - symo.file_close() - return symo - - -def inverse_dynamic_NE(robo): - """Computes Inverse Dynamic Model using - Newton-Euler formulation - - Parameters - ========== - robo : Robot - Instance of robot description container - - Returns - ======= - symo.sydi : dictionary - Dictionary with the information of all the sybstitution - """ - symo = Symoro() - symo.file_open(robo, 'idm') - title = 'Inverse dynamic model using Newton - Euler Algorith' - symo.write_params_table(robo, title, inert=True, dynam=True) - Newton_Euler(robo, symo) - symo.file_close() - return symo - - -def pseudo_force_NE(robo): - """Computes Coriolis, Centrifugal, Gravity, Friction and external - torques using Newton-Euler formulation - - Parameters - ========== - robo : Robot - Instance of robot description container - - Returns - ======= - symo.sydi : dictionary - Dictionary with the information of all the sybstitution - """ - robo_pseudo = deepcopy(robo) - robo_pseudo.qddot = sympy.zeros(robo_pseudo.NL, 1) - symo = Symoro() - symo.file_open(robo, 'ccg') - title = 'Pseudo forces using Newton - Euler Algorith' - symo.write_params_table(robo, title, inert=True, dynam=True) - Newton_Euler(robo_pseudo, symo) - symo.file_close() - return symo - - -def compute_wrench(robo, symo, j, w, wdot, U, vdot, F, N): - """Internal function. Computes total wrench (torques and forces) - of the link j - - Notes - ===== - F, N are the output parameters - """ - F[j] = robo.M[j]*vdot[j] + U[j]*robo.MS[j] - symo.mat_replace(F[j], 'F', j) - Psi = symo.mat_replace(robo.J[j]*w[j], 'PSI', j) - N[j] = robo.J[j]*wdot[j] + hat(w[j])*Psi - symo.mat_replace(N[j], 'No', j) - - -def compute_joint_wrench(robo, symo, j, antRj, antPj, vdot, - Fjnt, Njnt, F, N, Fex, Nex): - """Internal function. Computes wrench (torques and forces) - of the joint j - - Notes - ===== - Fjnt, Njnt, Fex, Nex are the output parameters - """ - Fjnt[j] = symo.mat_replace(F[j] + Fex[j], 'E', j) - Njnt[j] = N[j] + Nex[j] + hat(robo.MS[j])*vdot[j] - symo.mat_replace(Njnt[j], 'N', j) - f_ant = symo.mat_replace(antRj[j]*Fjnt[j], 'FDI', j) - if robo.ant[j] != - 1: - Fex[robo.ant[j]] += f_ant - Nex[robo.ant[j]] += antRj[j]*Njnt[j] + hat(antPj[j])*f_ant - - -def compute_torque(robo, symo, j, Fjnt, Njnt, name='GAM'): - """Internal function. Computes actuation torques - projection of - joint wrench on the joint axis - """ - if robo.sigma[j] != 2: - tau = (robo.sigma[j]*Fjnt[j] + (1 - robo.sigma[j])*Njnt[j]) - tau_total = tau[2] + robo.fric_s(j) + robo.fric_v(j) + robo.tau_ia(j) - symo.replace(tau_total, name, j, forced=True) - - -def inertia_spatial(J, MS, M): - return Matrix([(M*sympy.eye(3)).row_join(hat(MS).T), hat(MS).row_join(J)]) - - -def compute_joint_torque_deriv(symo, param, arg, index): - """Internal function. Computes joint reactive torques - in case if the parameter is 1 - - Parameters - ========== - symo : Symoro - symbol manager - param : var - Dynamic parameter - arg : var - The real torque is equal to arg*param - index : strig - identifies the parameter in the sybstituted symbol's name - """ - if param != ZERO and arg != ZERO: - index = str(index) + str(param) - symo.replace(arg, 'DG', index, forced=True) - - -def compute_beta(robo, symo, j, w, beta_star): - """Internal function. Computes link's wrench when - the joint accelerations are zero - - Notes - ===== - beta_star is the output parameter - """ - E1 = symo.mat_replace(robo.J[j]*w[j], 'JW', j) - E2 = symo.mat_replace(hat(w[j])*E1, 'KW', j) - E3 = hat(w[j])*robo.MS[j] - E4 = symo.mat_replace(hat(w[j])*E3, 'SW', j) - E5 = -robo.Nex[j] - E2 - E6 = -robo.Fex[j] - E4 - beta_star[j] = Matrix([E6, E5]) - - -def compute_link_acc(robo, symo, j, antRj, antPj, link_acc, w, wi): - """Internal function. Computes link's accelerations when - the joint accelerations are zero - - Notes - ===== - link_acc is the output parameter - """ - E1 = symo.mat_replace(hat(wi[j])*Matrix([0, 0, robo.qdot[j]]), - 'WQ', j) - E2 = (1 - robo.sigma[j])*E1 - E3 = 2*robo.sigma[j]*E1 - E4 = hat(w[robo.ant[j]])*antPj[j] - E5 = hat(w[robo.ant[j]])*E4 - E6 = antRj[j].T*E5 - E7 = symo.mat_replace(E6 + E3, 'LW', j) - link_acc[j] = Matrix([E7, E2]) - - -def replace_beta_J_star(robo, symo, j, grandJ, beta_star): - """Internal function. Makes symbol substitution in beta_star - and grandJ - """ - grandJ[j] = symo.mat_replace(grandJ[j], 'MJE', j, symmet=True) - beta_star[j] = symo.mat_replace(beta_star[j], 'VBE', j) - - -def compute_Tau(robo, symo, j, grandJ, beta_star, jaj, juj, H_inv, Tau): - """Internal function. Computes intermediat dynamic variables - - Notes - ===== - H_inv and Tau are the output parameters - """ - Jstar_jaj = grandJ[j]*jaj[j] - if robo.sigma[j] == 2: - Tau[j] = 0 - else: - H_inv[j] = 1 / (jaj[j].dot(Jstar_jaj) + robo.IA[j]) - H_inv[j] = symo.replace(H_inv[j], 'JD', j) - juj[j] = Jstar_jaj*H_inv[j] - symo.mat_replace(juj[j], 'JU', j) - joint_friction = robo.fric_s(j) + robo.fric_v(j) - Tau[j] = jaj[j].dot(beta_star[j]) + robo.GAM[j] - joint_friction - Tau[j] = symo.replace(Tau[j], 'GW', j) - - -def compute_beta_J_star(robo, symo, j, grandJ, jaj, juj, Tau, - beta_star, jTant, link_acc): - """Internal function. Computes intermediat dynamic variables - - Notes - ===== - grandJ and beta_star are the output parameters - """ - Jstar_jaj = grandJ[j]*jaj[j] - grandK = symo.mat_replace(grandJ[j] - juj[j]*Jstar_jaj.T, - 'GK', j) - E1 = symo.mat_replace(grandK*link_acc[j], 'NG', j) - E3 = symo.mat_replace(E1 + Tau[j]*juj[j], 'VS', j) - alpha = symo.mat_replace(E3 - beta_star[j], 'AP', j) - E4 = symo.mat_replace(jTant[j].T*grandK, 'GX', j) - E5 = symo.mat_replace(E4*jTant[j], 'TKT', j, symmet=True) - grandJ[robo.ant[j]] += E5 - beta_star[robo.ant[j]] -= jTant[j].T*alpha - - -def compute_acceleration(robo, symo, j, jTant, grandVp, - juj, H_inv, jaj, Tau, link_acc): - """Internal function. Computes joint accelerations and links' twists - - Notes - ===== - grandVp is the output parameter - """ - grandR = symo.mat_replace(jTant[j]*grandVp[robo.ant[j]] + link_acc[j], - 'VR', j) - E1 = symo.replace(juj[j].dot(grandR), 'GU', j) - if robo.sigma[j] == 2: - qddot = 0 - else: - qddot = H_inv[j]*Tau[j] - E1 - qddot = symo.replace(qddot, "QDP", j, forced=True) - grandVp[j] = (grandR + qddot*jaj[j]) - grandVp[j][3:, 0] = symo.mat_replace(grandVp[j][3:, 0], 'WP', j) - grandVp[j][:3, 0] = symo.mat_replace(grandVp[j][:3, 0], 'VP', j) - - -def compute_coupled_forces(robo, symo, j, grandVp, grandJ, beta_star): - """Internal function. - """ - E3 = symo.mat_replace(grandJ[j]*grandVp[j], 'DY', j) - couplforce = E3 - beta_star[j] - symo.mat_replace(couplforce[3:, 0], 'N', j) - symo.mat_replace(couplforce[:3, 0], 'E', j) - - -def replace_Jplus(robo, symo, j, Jplus, MSplus, Mplus): - """Internal function. Makes symbol substitutions inertia parameters - """ - symo.mat_replace(Jplus[j], 'JP', j) - symo.mat_replace(MSplus[j], 'MSP', j) - Mplus[j] = symo.replace(Mplus[j], 'MP', j) - - -def compute_Jplus(robo, symo, j, antRj, antPj, Jplus, MSplus, Mplus, AJE1): - """Internal function. Computes inertia parameters of composed link - - Notes - ===== - Jplus, MSplus, Mplus are the output parameters - """ - hat_antPj = hat(antPj[j]) - antMSj = symo.mat_replace(antRj[j]*MSplus[j], 'AS', j) - E1 = symo.mat_replace(antRj[j]*Jplus[j], 'AJ', j) - AJE1[j] = E1[:, 2] - E2 = symo.mat_replace(E1*antRj[j].T, 'AJA', j) - E3 = symo.mat_replace(hat_antPj*hat(antMSj), 'PAS', j) - Jplus[robo.ant[j]] += E2 - (E3 + E3.T) + hat_antPj*hat_antPj.T*Mplus[j] - MSplus[robo.ant[j]] += antMSj + antPj[j]*Mplus[j] - Mplus[robo.ant[j]] += Mplus[j] - - -def compute_A_diagonal(robo, symo, j, Jplus, MSplus, Mplus, f, n, A): - """Internal function. Computes diagonal elements - of the inertia matrix - - Notes - ===== - f, n, A are the output parameters - """ - if robo.sigma[j] == 0: - f[j] = Matrix([-MSplus[j][1], MSplus[j][0], 0]) - n[j] = Jplus[j][:, 2] - A[j, j] = Jplus[j][2, 2] + robo.IA[j] - elif robo.sigma[j] == 1: - f[j] = Matrix([0, 0, Mplus[j]]) - n[j] = Matrix([MSplus[j][1], - MSplus[j][0], 0]) - A[j, j] = Mplus[j] + robo.IA[j] - symo.mat_replace(f[j], 'E' + chars[j], j) - symo.mat_replace(n[j], 'N' + chars[j], j) - - -def compute_A_triangle(robo, symo, j, k, ka, antRj, antPj, f, n, A, AJE1): - """Internal function. Computes elements below and above diagonal - of the inertia matrix - - Notes - ===== - f, n, A are the output parameters - """ - f[ka] = antRj[k]*f[k] - if k == j and robo.sigma[j] == 0: - n[ka] = AJE1[k] + hat(antPj[k])*f[k] - else: - n[ka] = antRj[k]*n[k] + hat(antPj[k])*f[k] - if ka == - 1: - symo.mat_replace(f[ka], 'AV0') - symo.mat_replace(n[ka], 'AW0') - else: - symo.mat_replace(f[ka], 'E' + chars[j], ka) - symo.mat_replace(n[ka], 'N' + chars[j], ka) - if robo.sigma[ka] == 0: - A[j, ka] = n[ka][2] - elif robo.sigma[ka] == 1: - A[j, ka] = f[ka][2] - A[ka, j] = A[j, ka] - - -# TODO:Finish base parameters computation -def base_paremeters(robo_orig): - """Computes grouped inertia parameters. New parametrization - contains less parameters but generates the same dynamic model - - Parameters - ========== - robo : Robot - Instance of robot description container - - Returns - ======= - symo.sydi : dictionary - Dictionary with the information of all the sybstitution - """ - robo = copy(robo_orig) - lam = [0 for i in xrange(robo.NL)] - symo = Symoro() - symo.file_open(robo, 'regp') - title = 'Base parameters computation' - symo.write_params_table(robo, title, inert=True, dynam=True) - # init transformation - antRj, antPj = compute_rot_trans(robo, symo) - for j in reversed(xrange(1, robo.NL)): - if robo.sigma[j] == 0: - # general grouping - compute_lambda(robo, symo, j, antRj, antPj, lam) - group_param_rot(robo, symo, j, lam) - # special grouping - group_param_rot_spec(robo, symo, j, lam, antRj) - pass - elif robo.sigma[j] == 1: - # general grouping - group_param_prism(robo, symo, j, antRj) - # special grouping - group_param_prism_spec(robo, symo, j, antRj, antPj) - pass - elif robo.sigma[j] == 2: - # fixed joint, group everuthing - compute_lambda(robo, symo, j, antRj, antPj) - group_param_fix(robo, symo, j, lam) - pass - symo.write_line('*=*') - symo.write_line() - title = robo.name + ' grouped inertia parameters' - symo.write_params_table(robo, title, inert=True, equations=False) - symo.file_close() - return robo, symo.sydi - - -def vec_mut_J(v, u): - """Internal function. Needed for inertia parameters transformation - - Parameters - ========== - v, u : Matrix 3x1 - two axis vectors - Returns : Matrix 6x1 - """ - return Matrix([v[0]*u[0], v[0]*u[1], v[0]*u[2], - v[1]*u[1], v[1]*u[2], v[2]*u[2]]) - - -def vec_mut_MS(v, P): - """Internal function. Needed for inertia parameters transformation - - Parameters - ========== - v : Matrix 3x1 - axis vector - P : Matrix 3x1 - position vector - - Returns : Matrix 6x1 - """ - U = - hat(v)*hat(P) - return Matrix([2*U[0, 0], U[0, 1] + U[1, 0], U[0, 2] + U[2, 0], - 2*U[1, 1], U[1, 2] + U[2, 1], 2*U[2, 2]]) - - -def vec_mut_M(P): - """Internal function. Needed for inertia parameters transformation - - Parameters - ========== - P : Matrix 3x1 - position vector - - Returns : Matrix 6x1 - """ - U = -hat(P)*hat(P) - return Matrix([U[0, 0], U[0, 1], U[0, 2], U[1, 1], U[1, 2], U[2, 2]]) - - -def compute_lambda(robo, symo, j, antRj, antPj, lam): - """Internal function. Computes the inertia parameters - transformation matrix - - Notes - ===== - lam is the output paramete - """ - lamJJ_list = [] - lamJMS_list = [] - for e1 in xrange(3): - for e2 in xrange(e1, 3): - u = vec_mut_J(antRj[j][:, e1], antRj[j][:, e2]) - if e1 != e2: - u += vec_mut_J(antRj[j][:, e2], antRj[j][:, e1]) - lamJJ_list.append(u.T) - for e1 in xrange(3): - v = vec_mut_MS(antRj[j][:, e1], antPj[j]) - lamJMS_list.append(v.T) - lamJJ = Matrix(lamJJ_list).T # , 'LamJ', j) - lamJMS = symo.mat_replace(Matrix(lamJMS_list).T, 'LamMS', j) - lamJM = symo.mat_replace(vec_mut_M(antPj[j]), 'LamM', j) - lamJ = lamJJ.row_join(lamJMS).row_join(lamJM) - lamMS = sympy.zeros(3, 6).row_join(antRj[j]).row_join(antPj[j]) - lamM = sympy.zeros(1, 10) - lamM[9] = 1 - lam[j] = Matrix([lamJ, lamMS, lamM]) - - -def group_param_rot(robo, symo, j, lam): - """Internal function. Groups inertia parameters according to the - general rule for a rotational joint. - - Notes - ===== - robo is the output paramete - """ - Kj = robo.get_inert_param(j) - - lam03 = lam[j][:, 0] + lam[j][:, 3] - lam03 = lam03.applyfunc(symo.C2S2_simp) - for i in (3, 8, 9): - Kj[i] = symo.replace(Kj[i], inert_names[i], j) - if robo.ant[j] != -1: - Kant = robo.get_inert_param(robo.ant[j]) - Kant += lam03*Kj[3] + lam[j][:, 8]*Kj[8] + lam[j][:, 9]*Kj[9] - robo.put_inert_param(Kant, robo.ant[j]) - Kj[0] -= Kj[3] # XX - Kj[3] = 0 # YY - Kj[8] = 0 # MZ - Kj[9] = 0 # M - robo.put_inert_param(Kj, j) - - -def group_param_rot_spec(robo, symo, j, lam, antRj): - """Internal function. Groups inertia parameters according to the - special rule for a rotational joint. - - Notes - ===== - robo is the output paramete - """ - chainj = robo.chain(j) - r1, r2, orthog = Transform.find_r12(robo, chainj, antRj, j) - kRj, all_paral = Transform.kRj(robo, antRj, r1, chainj) - Kj = robo.get_inert_param(j) - to_replace = {0, 1, 2, 4, 5, 6, 7} - if Transform.z_paral(kRj): - Kj[0] = 0 # XX - Kj[1] = 0 # XY - Kj[2] = 0 # XZ - Kj[4] = 0 # YZ - to_replace -= {0, 1, 2, 4} - joint_axis = antRj[chainj[-1]].col(2) - if all_paral and robo.G.norm() == sympy.Abs(joint_axis.dot(robo.G)): - Kj[6] = 0 # MX - Kj[7] = 0 # MY - to_replace -= {6, 7} - if j == r1 or(j == r2 and orthog): - Kj[5] += robo.IA[j] # ZZ - robo.IA[j] = 0 - for i in to_replace: - Kj[i] = symo.replace(Kj[i], inert_names[i], j) - robo.put_inert_param(Kj, j) - - -def group_param_fix(robo, symo, j, lam): - """Internal function. Groups inertia parameters according to the - general rule for a fixed joint joint. - - Notes - ===== - robo is the output paramete - """ - Kj = robo.get_inert_param(j) - for i in xrange(10): - Kj[i] = symo.replace(Kj[i], inert_names[i], j) - if robo.ant[j] != -1: - Kant = robo.get_inert_param(robo.ant[j]) - Kant += lam[j]*Kj - robo.put_inert_param(Kant, robo.ant[j]) - robo.put_inert_param(sympy.zeros(10, 1), j) - - -def group_param_prism(robo, symo, j, antRj): - """Internal function. Groups inertia parameters according to the - general rule for a prismatic joint. - - Notes - ===== - robo is the output paramete - """ - Kj = robo.get_inert_param(j) - for i in xrange(6): - Kj[i] = symo.replace(Kj[i], inert_names[i], j) - robo.put_inert_param(Kj, j) - if robo.ant[j] != -1: - antJj = antRj[j]*robo.J[j]*antRj[j].T - robo.J[robo.ant[j]] += antJj - robo.J[j] = sympy.zeros(3, 3) - - -def group_param_prism_spec(robo, symo, j, antRj, antPj): - """Internal function. Groups inertia parameters according to the - special rule for a prismatic joint. - - Notes - ===== - robo is the output paramete - """ - chainj = robo.chain(j) - r1, r2, orthog = Transform.find_r12(robo, chainj, antRj, j) - Kj = robo.get_inert_param(j) - kRj, all_paral = Transform.kRj(robo, antRj, r1, chainj) - to_replace = {6, 7, 8, 9} - if r1 < j and j < r2: - if Transform.z_paral(kRj): - Kj[8] = 0 # MZ - for i in (6, 7): - Kj[i] = symo.replace(Kj[i], inert_names[i], j) - robo.MS[robo.ant[j]] += antRj[j]*Matrix([Kj[6], Kj[7], 0]) - robo.JJ[2, 2] -= Kj[6]*antPj[j][0] + Kj[7]*antPj[j][1] - Kj[6] = 0 # MX - Kj[7] = 0 # MY - to_replace -= {6, 7, 8} - else: - jar1 = kRj.row(2) - if jar1[2] != 0: - Kj[6] -= jar1[0]/jar1[2]*Kj[8] - Kj[7] -= jar1[1]/jar1[2]*Kj[8] - Kj[8] = 0 # MZ - to_replace -= {8} - elif jar1[0]*jar1[1] != 0: - Kj[6] -= jar1[0]/jar1[1]*Kj[7] - Kj[7] = 0 # MY - to_replace -= {7} - elif jar1[0] != 0: - Kj[7] = 0 # MY - to_replace -= {7} - else: - Kj[6] = 0 # MX - to_replace -= {6} - elif j < r1: - Kj[6] = 0 # MX - Kj[7] = 0 # MY - Kj[8] = 0 # MZ - to_replace -= {6, 7, 8} - #TOD: rewrite - dotGa = Transform.sna(antRj[j])[2].dot(robo.G) - if dotGa == ZERO: - revol_align = robo.ant[robo.ant[j]] == 0 and robo.ant[j] == ZERO - if robo.ant[j] == 0 or revol_align: - Kj[9] += robo.IA[j] - robo.IA[j] = 0 - for i in to_replace: - Kj[i] = symo.replace(Kj[i], inert_names[i], j) - robo.put_inert_param(Kj, j) diff --git a/pysymoro/core/geometry.py b/pysymoro/core/geometry.py deleted file mode 100644 index 3652feb..0000000 --- a/pysymoro/core/geometry.py +++ /dev/null @@ -1,486 +0,0 @@ -""" -This module of SYMORO package provides geometric models' computation. - -The core symbolic library is sympy. - -Needed modules: symoro.py - -ECN - ARIA1 2013 -""" - -from sympy import Matrix, zeros, eye, sin, cos -from symoro import Symoro, Init, hat - -Z_AXIS = Matrix([0, 0, 1]) - - -class Transform(): - @classmethod - def sna(self, T): - """Extracts the s, n, a vector basis of rotation 3x3 matrix - from 4x4 transformation matrix - - Parameters - ========== - T: Matrix 4x4 - Transformation matrix - - Returns - ======= - s: Matrix 3x1 - n: Matrix 3x1 - a: Matrix 3x1 - """ - R = Transform.R(T) - return R.col(0), R.col(1), R.col(2) - - @classmethod - def R(self, T): - """Extracts rotation 3x3 matrix from 4x4 transformation matrix - - Parameters - ========== - T: Matrix 4x4 - Transformation matrix - - Returns - ======= - get_r: Matrix 3x3 - """ - return T[:3, :3] - - @classmethod - def P(self, T): - """Extracts translation vector from 4x4 transformation matrix - - Parameters - ========== - T: Matrix 4x4 - Transformation matrix - - Returns - ======= - get_p: Matrix 3x1 - """ - return T[:3, 3] - - @classmethod - def kRj(self, robo, antRj, k, chainj): - T = eye(3) - all_paral = True - for i in chainj: - if i > k: - T = antRj[i]*T - if antRj[i].col(2) != Z_AXIS and robo.ant[i] != 0: - all_paral = False - return T, all_paral - - @classmethod - def find_r12(self, robo, chainj, antRj, j): - r1 = robo.NL - r2 = robo.NL - rot12 = eye(3) - orthog = False - for i in reversed(chainj): - if robo.sigma[i] == 0: - rot12 *= antRj[i] - if r1 == robo.NL: - r1 = i - elif r2 == robo.NL and rot12.col(2) != Matrix([0, 0, 1]): - r2 = i - if Matrix([0, 0, 1]).dot(rot12.col(2)) == 0: - orthog = True - break - return r1, r2, orthog - - @classmethod - def z_paral(self, T): - return T.col(2) == Z_AXIS - - -def _transform(robo, j, invert=False): - """Transform matrix between frames j and ant[j] - - Parameters - ========== - j: int - Frame index. - invert: bool, optional - Defines the transformation direction - - Returns - ======= - transform: Matrix 4x4 - Transformation matrix. If invert is True then j_T_ant, - else ant_T_j. - """ - if not invert: - R1 = _rot_trans('z', robo.gamma[j], robo.b[j]) - R2 = _rot_trans('x', robo.alpha[j], robo.d[j]) - R3 = _rot_trans('z', robo.theta[j], robo.r[j]) - return R1*R2*R3 - else: - R1 = _rot_trans('z', -robo.gamma[j], -robo.b[j]) - R2 = _rot_trans('x', -robo.alpha[j], -robo.d[j]) - R3 = _rot_trans('z', -robo.theta[j], -robo.r[j]) - return R3*R2*R1 - - -#TODO: rewrite the description -def _transform_const_sep(robo, j, invert=False): - """Transform matrix between frames j and ant[j] - - Parameters - ========== - j: int - Frame index. - invert: bool, optional - Defines the transformation direction - - Returns - ======= - transform: Matrix 4x4 - Transformation matrix. If invert is True then j_T_ant, - else ant_T_j. - """ - if not invert: - R1 = _rot_trans('z', robo.gamma[j], robo.b[j]) - R2 = _rot_trans('x', robo.alpha[j], robo.d[j]) - R3 = _rot_trans('z', th=robo.theta[j]) - R4 = _rot_trans('z', p=robo.r[j]) - return R1, R2, R3, R4 - else: - R1 = _rot_trans('z', -robo.gamma[j], -robo.b[j]) - R2 = _rot_trans('x', -robo.alpha[j], -robo.d[j]) - R3 = _rot_trans('z', th=-robo.theta[j]) - R4 = _rot_trans('z', p=-robo.r[j]) - return R1, R2, R3, R4 - - -def compute_transform(robo, symo, j, antRj, antPj): - """Internal function. Computes rotation matrix and translation vector - of ant_T_j homogenuous transform. Does the trigonometric subsctitution - and saves the symbols into symo.sydi - - Notes - ===== - antPj and antRj are the output parameters - """ - antTj = _transform(robo, j) - for angle, name in robo.get_angles(j): - antTj = symo.trig_replace(antTj, angle, name) - antRj[j] = symo.mat_replace(Transform.R(antTj), 'A', j) - antPj[j] = symo.mat_replace(Transform.P(antTj), 'L', j) - - -def compute_screw_transform(robo, symo, j, antRj, antPj, jTant): - """Internal function. Computes the screw transformation matrix - between ant[j] and j frames. - - Notes - ===== - jTant is an output parameter - """ - jRant = antRj[j].T - ET = symo.mat_replace(-jRant*hat(antPj[j]), 'JPR', j) - jTant[j] = (Matrix([jRant.row_join(ET), - zeros(3, 3).row_join(jRant)])) - - -def _trans_name(robo, i, j, pattern='T{0}T{1}'): - return 'T%sT%s' % (i, j) - - -def _dgm_left(robo, symo, i, j, trig_subs=True, sep_const=False): - k = robo.common_root(i, j) - chain1 = robo.chain(j, k) - chain2 = robo.chain(i, k) - chain2.reverse() - complete_chain = (chain1 + chain2 + [None]) - T_out = {(j, j): eye(4)} - T_res = eye(4) - T = eye(4) - for indx, x in enumerate(complete_chain[:-1]): - inverted = indx >= len(chain1) - T = _transform(robo, x, inverted) * T - if trig_subs: - for ang, name in robo.get_angles(x): - symo.trig_replace(T, ang, name) - T = T.expand() - T = T.applyfunc(symo.CS12_simp) - x_next = complete_chain[indx + 1] - if inverted: - t_name = (x, j) - else: - t_name = (robo.ant[x], j) - T_out[t_name] = T * T_res - if robo.paral(x, x_next): - continue - T_res = T_out[t_name] - T = eye(4) - return T_out - - -def _dgm_right(robo, symo, i, j, trig_subs=True, sep_const=False): - k = robo.common_root(i, j) - chain1 = robo.chain(i, k) - chain2 = robo.chain(j, k) - chain2.reverse() - complete_chain = (chain1 + chain2 + [None]) - T_out = {(i, i): eye(4)} - T_res = eye(4) - T = eye(4) - for indx, x in enumerate(complete_chain[:-1]): - inverted = indx < len(chain1) - T = T * _transform(robo, x, inverted) - if trig_subs: - for ang, name in robo.get_angles(x): - symo.trig_replace(T, ang, name) - T = T.expand() - T = T.applyfunc(symo.CS12_simp) - x_next = complete_chain[indx + 1] - if inverted: - t_name = (i, robo.ant[x]) - else: - t_name = (i, x) - T_out[t_name] = T_res * T - if robo.paral(x, x_next): - continue - T_res = T_out[t_name] - T = eye(4) - return T_out - - -def _dgm_one(robo, symo, i, j, fast_form=True, - forced=False, trig_subs=True): - k = robo.common_root(i, j) - is_loop = i > robo.NL and j > robo.NL - chain1 = robo.chain(j, k) - chain2 = robo.chain(i, k) - chain2.reverse() - complete_chain = (chain1 + chain2 + [None]) - T_res = eye(4) - T = eye(4) - for indx, x in enumerate(complete_chain[:-1]): - inverted = indx >= len(chain1) - T = _transform(robo, x, inverted) * T - if trig_subs: - for ang, name in robo.get_angles(x): - symo.trig_replace(T, ang, name) - T = T.applyfunc(symo.CS12_simp) - if is_loop: - T = T.applyfunc(symo.C2S2_simp) - x_next = complete_chain[indx + 1] - if robo.paral(x, x_next): # false if x_next is None - continue - T_res = T * T_res - T = eye(4) - if fast_form: - _dgm_rename(robo, symo, T_res, x, i, j, inverted, forced) - if not fast_form and forced: - _dgm_rename(robo, symo, T_res, x, i, j, inverted, forced) - return T_res - - -def _dgm_rename(robo, symo, T_res, x, i, j, inverted, forced): - if inverted: - name = _trans_name(robo, x, j) - forced_now = x == i - else: - name = _trans_name(robo, robo.ant[x], j) - forced_now = robo.ant[x] == i - symo.mat_replace(T_res, name, forced=forced and forced_now, skip=1) - - -#TODO: implemet returning all the matrices -# sep_const: bool, optional -# If True, transform will be represented as a tuple of -# form (Cpref, T, Cpost) where the requirad transform is -# represented by Cpref*T*Cpost and Cpref and Cpost contain no -# joint variables, just constant values. Only for 'left' and 'right' -def dgm(robo, symo, i, j, key='one', fast_form=True, forced=False, - trig_subs=True): - """must be the final DGM function - - Parameters - ========== - symo: Symoro - Instance of Symoro. All the substitutions will - be put into symo.sydi - i: int - To-frame index. - j: int - From-frame index. - key: {'one','left','right'} - Defines whether return just one transform or all the chain - with multiplication from left and right - fast_form: bool, optional - If False, result will be in unfolded mode (triginimetric - substitutions only) - forced: bool, optional - If True, all the symbols of the last transformation - matrix will be rplaced, aplicable only if fast_form is True - for key='one' - trig_subs: bool, optional - If True, all the sin(x) and cos(x) will be replaced by symbols - SX and CX with adding them to the dictionary - - """ - if key == 'left': - return _dgm_left(robo, symo, i, j, trig_subs) - elif key == 'right': - return _dgm_right(robo, symo, i, j, trig_subs) - else: - return _dgm_one(robo, symo, i, j, fast_form, forced, trig_subs) - - -def _rot(axis='z', th=0): - """Rotation matrix about axis - - Parameters - ========== - axis: {'x', 'y', 'z'} - Rotation axis - th: var - Rotation angle - - Returns - ======= - rot: Matrix 3x3 - """ - if axis == 'x': - return Matrix([[1, 0, 0], - [0, cos(th), - sin(th)], - [0, sin(th), cos(th)]]) - elif axis == 'y': - return Matrix([[cos(th), 0, sin(th)], - [0, 1, 0], - [-sin(th), 0, cos(th)]]) - else: - return Matrix([[cos(th), - sin(th), 0], - [sin(th), cos(th), 0], - [0, 0, 1]]) - - -def _trans_vect(axis='z', p=0): - """Translation vector along axis - - Parameters - ========== - axis: {'x', 'y', 'z'} - Translation axis - p: var - Translation distance - - Returns - ======= - v: Matrix 3x1 - """ - axis_dict = {'x': 0, 'y': 1, 'z': 2} - v = zeros(3, 1) - v[axis_dict[axis]] = p - return v - - -def _trans(axis='z', p=0): - """Translation matrix along axis - - Parameters - ========== - axis: {'x', 'y', 'z'} - Translation axis - p: var - Translation distance - - Returns - ======= - trans: Matrix 4x4 - """ - return Matrix([eye(3).row_join(_trans_vect(axis, p)), - [0, 0, 0, 1]]) - - -def _rot_trans(axis='z', th=0, p=0): - """Transformation matrix with rotation about and - translation along axis - - Parameters - ========== - axis: {'x', 'y', 'z'} - Transformation axis - p: var - Translation distance - th: var - Rotation angle - - Returns - ======= - rot_trans: Matrix 4x4 - """ - return Matrix([_rot(axis, th).row_join(_trans_vect(axis, p)), - [0, 0, 0, 1]]) - - -def compute_rot_trans(robo, symo): - #init transformation - antRj = Init.init_mat(robo) - antPj = Init.init_vec(robo) - for j in xrange(robo.NL): - compute_transform(robo, symo, j, antRj, antPj) - return antRj, antPj - - -#TODO: validate for different structures -def direct_geometric_fast(robo, i, j): - """Computes trensformation matrix iTj. - - Parameters - ========== - robo: Robot - Instance of robot description container - i: int - the to-frame - j: int - the from-frame - - Returns - ======= - symo: Symoro - Instance that contains all the relations of the computed model - """ - symo = Symoro() - symo.file_open(robo, 'fgm') - symo.write_params_table(robo, 'Direct Geometrix model') - dgm(robo, symo, i, j, fast_form=True, forced=True) - symo.file_close() - return symo - - -def direct_geometric(robo, frames, trig_subs): - """Computes trensformation matrix iTj. - - Parameters - ========== - robo: Robot - Instance of robot description container - frames: list of tuples of type (i,j) - Defines list of required transformation matrices iTj - trig_subs: bool, optional - If True, all the sin(x) and cos(x) will be replaced by symbols - SX and CX with adding them to the dictionary - - Returns - ======= - symo: Symoro - Instance that contains all the relations of the computed model - """ - symo = Symoro() - symo.file_open(robo, 'trm') - symo.write_params_table(robo, 'Direct Geometrix model') - for i, j in frames: - symo.write_line('Tramsformation matrix %s T %s' % (i, j)) - print dgm(robo, symo, i, j, fast_form=False, - forced=True, trig_subs=trig_subs) - symo.write_line() - symo.file_close() - return symo diff --git a/pysymoro/core/parfile.py b/pysymoro/core/parfile.py deleted file mode 100644 index 0759406..0000000 --- a/pysymoro/core/parfile.py +++ /dev/null @@ -1,142 +0,0 @@ -import re -import symoro - -_keywords = ['ant', 'sigma', 'b', 'd', 'r', - 'gamma', 'alpha', 'mu', 'theta', - 'XX', 'XY', 'XZ', 'YY', 'YZ', 'ZZ', - 'MX', 'MY', 'MZ', 'M', - 'IA', 'FV', 'FS', 'FX', 'FY', 'FZ', - 'CX', 'CY', 'CZ', 'QP', 'QDP', 'GAM', - 'W0', 'WP0', 'V0', 'VP0', - 'Z', 'G'] - -_NF = ['ant', 'sigma', 'b', 'd', 'r', - 'gamma', 'alpha', 'mu', 'theta'] -_NJ = ['QP', 'QDP', 'GAM'] -_NL = ['XX', 'XY', 'XZ', 'YY', 'YZ', 'ZZ', - 'MX', 'MY', 'MZ', 'M', - 'IA', 'FV', 'FS', 'FX', 'FY', 'FZ', - 'CX', 'CY', 'CZ'] -_VEC = ['W0', 'WP0', 'V0', 'VP0'] -_ZERO_BASED = {'W0', 'WP0', 'V0', 'VP0', 'Z', 'G'} -_bool_dict = {'True': True, 'False': False, - 'true': True, 'false': False, - '1': True, '0': False} - -_keyword_repl = {'Ant': 'ant', 'Sigma': 'sigma', 'B': 'b', 'R': 'r', - 'Alpha': 'alpha', 'Mu': 'mu', 'Theta': 'theta'} - - -def _extract_vals(robo, key, line): - line = line.replace('{', '') - line = line.replace('}', '') - if key in _ZERO_BASED: - k = 0 - else: - k = 1 - items = line.split(',') - items_proc = [] - prev_item = False - for i, v in enumerate(items): - if v.find('atan2') == -1 and not prev_item: - items_proc.append(v) - elif prev_item: - items_proc.append('%s,%s' % (items[i-1], v)) - prev_item = False - else: - prev_item = True - for i, v in enumerate(items_proc): - if robo.put_val(i+k, key, v.strip()) == symoro.FAIL: - return symoro.FAIL - - -def _write_par_list(robo, f, key, N0, N): - f.write('%s = {%s' % (key, robo.get_val(N0, key))) - for i in xrange(N0 + 1, N): - f.write(',%s' % robo.get_val(i, key)) - f.write('}\n') - - -def writepar(robo): - fname = symoro.make_fname(robo) - with open(fname, 'w') as f: - f.write('(* Robotname = \'%s\' *)\n' % robo.name) - f.write('NL = %s\n' % robo.nl) - f.write('NJ = %s\n' % robo.nj) - f.write('NF = %s\n' % robo.nf) - f.write('Type = %s\n' % symoro.TYPES.index(robo.structure)) - f.write('is_mobile = %s\n' % int(robo.is_mobile)) - f.write('\n(* Geometric parameters *)\n') - if robo.is_mobile: - N0 = 0 - else: - N0 = 1 - for key in _NF: - _write_par_list(robo, f, key, 1, robo.NF) - f.write('\n(* Dynamic parameters and external forces *)\n') - for key in _NL: - _write_par_list(robo, f, key, N0, robo.NL) - f.write('\n(* Joint parameters *)\n') - for key in _NJ: - _write_par_list(robo, f, key, 1, robo.NJ) - f.write('\n(* Speed and acceleration of the base *)\n') - for key in _VEC: - _write_par_list(robo, f, key, 0, 3) - f.write('\n(* Acceleration of gravity *)\n') - _write_par_list(robo, f, 'G', 0, 3) - f.write('\n(* Transformation of 0 frame position fT0 *)\n') - _write_par_list(robo, f, 'Z', 0, 16) - f.write('\n(* End of definition *)\n') - - -def readpar(directory, robo_name): - """Return: - robo: an instance of Robot, read from file - flag: indicates if any errors accured. (symoro.FAIL) - """ - fname = '%s\\%s.par' % (directory, robo_name) - with open(fname, 'r') as f: - #initialize the Robot instance - f.seek(0) - d = {} - is_mobile = False - for line in f.readlines(): - for s in ('NJ', 'NL', 'Type'): - match = re.match(r'^%s.*=([\d\s]*)(\(\*.*)?' % s, line) - if match: - d[s] = int(match.group(1)) - continue - match = re.match(r'^is_mobile.*=([\d\s]*)(\(\*.*)?', line) - if match: - is_mobile = _bool_dict[(match.group(1).strip())] - if len(d) < 2: - return None, symoro.FAIL - NF = d['NJ']*2 - d['NL'] - robo = symoro.Robot(robo_name, d['NL'], d['NJ'], NF, - is_mobile, symoro.TYPES[d['Type']]) - robo.directory = directory - #fitting the data - acc_line = '' - key = '' - f.seek(0) - flag = symoro.OK - for line in f.readlines(): - if line.find('(*') != -1: - continue - line = line.replace('Pi', 'pi') - match = re.match(r'^(.*)=.*\{(.*)', line) - if match: - acc_line == '' - key = match.group(1).strip() - acc_line = match.group(2).strip() - else: - acc_line += line - if acc_line.find('}') != -1: - if key in _keyword_repl: - key = _keyword_repl[key] - if key in _keywords: - if _extract_vals(robo, key, acc_line) == symoro.FAIL: - flag = symoro.FAIL - acc_line = '' - key = '' - return robo, flag diff --git a/pysymoro/core/robots/RX90/RX90.par b/pysymoro/core/robots/RX90/RX90.par deleted file mode 100644 index aef7411..0000000 --- a/pysymoro/core/robots/RX90/RX90.par +++ /dev/null @@ -1,57 +0,0 @@ -(* Robotname = 'RX90' *) -NL = 6 -NJ = 6 -NF = 6 -Type = 0 -is_mobile = 0 - -(* Geometric parameters *) -ant = {0,1,2,3,4,5} -sigma = {0,0,0,0,0,0} -b = {0,0,0,0,0,0} -d = {0,0,D3,0,0,0} -r = {0,0,0,RL4,0,0} -gamma = {0,0,0,0,0,0} -alpha = {0,pi/2,0,-pi/2,pi/2,-pi/2} -mu = {1,1,1,1,1,1} -theta = {th1,th2,th3,th4,th5,th6} - -(* Dynamic parameters and external forces *) -XX = {XX1,XX2,XX3,XX4,XX5,XX6} -XY = {XY1,XY2,XY3,XY4,XY5,XY6} -XZ = {XZ1,XZ2,XZ3,XZ4,XZ5,XZ6} -YY = {YY1,YY2,YY3,YY4,YY5,YY6} -YZ = {YZ1,YZ2,YZ3,YZ4,YZ5,YZ6} -ZZ = {ZZ1,ZZ2,ZZ3,ZZ4,ZZ5,ZZ6} -MX = {MX1,MX2,MX3,MX4,MX5,MX6} -MY = {MY1,MY2,MY3,MY4,MY5,MY6} -MZ = {MZ1,MZ2,MZ3,MZ4,MZ5,MZ6} -M = {M1,M2,M3,M4,M5,M6} -IA = {IA1,IA2,IA3,IA4,IA5,IA6} -FV = {FV1,FV2,FV3,FV4,FV5,FV6} -FS = {FS1,FS2,FS3,FS4,FS5,FS6} -FX = {0,0,0,0,0,FX6} -FY = {0,0,0,0,0,FY6} -FZ = {0,0,0,0,0,FZ6} -CX = {0,0,0,0,0,CX6} -CY = {0,0,0,0,0,CY6} -CZ = {0,0,0,0,0,CZ6} - -(* Joint parameters *) -QP = {QP1,QP2,QP3,QP4,QP5,QP6} -QDP = {QDP1,QDP2,QDP3,QDP4,QDP5,QDP6} -GAM = {GAM1,GAM2,GAM3,GAM4,GAM5,GAM6} - -(* Speed and acceleration of the base *) -W0 = {0,0,0} -WP0 = {0,0,0} -V0 = {0,0,0} -VP0 = {0,0,0} - -(* Acceleration of gravity *) -G = {0,0,G3} - -(* Transformation of 0 frame position fT0 *) -Z = {1,0,0,0,0,1,0,0,0,0,1,0,0,0,0,1} - -(* End of definition *) diff --git a/pysymoro/core/symoro.py b/pysymoro/core/symoro.py deleted file mode 100644 index 540066d..0000000 --- a/pysymoro/core/symoro.py +++ /dev/null @@ -1,1484 +0,0 @@ -""" -This module of SYMORO package provides description -of the robot parametrizaion container and symbol replacer class. - -The core symbolic library is sympy. - -ECN - ARIA1 2013 -""" -import os -import re -from copy import copy -from itertools import combinations -from sympy import sin, cos, sign, pi -from sympy import Symbol, Matrix, Expr, Integer -from sympy import Mul, Add, factor, zeros, var, sympify, eye - -ZERO = Integer(0) -ONE = Integer(1) -CLOSED_LOOP = 'Closed loop' -SIMPLE = 'Simple' -TREE = 'Tree' -TYPES = [SIMPLE, TREE, CLOSED_LOOP] -FAIL = 1 -OK = 0 -INT_KEYS = ['ant', 'sigma', 'mu'] - - -#TODO: write consistency check -#TODO: Ask about QP QDP file writing. Number of joints is different -#from number of links -class Robot: - """Container of the robot parametric description. - Responsible for low-level geometric transformation - and direct geometric model generation. - Also provides different representations of parameters.""" - def __init__(self, name, NL=0, NJ=0, NF=0, is_mobile=False, - structure=TREE): - # member variables: - self.name = name - """ name of the robot: string""" - self.directory = os.path.join('robots', name) - """ directory name""" - self.is_mobile = is_mobile - """ whethere the base frame is floating: bool""" - self.nl = NL - """ number of links: int""" - self.nj = NJ - """ number of joints: int""" - self.nf = NF - """ number of frames: int""" - self.structure = structure - """ type of robot's structure""" - self.sigma = [0 for i in xrange(NF + 1)] - """ joint type: list of int""" - self.ant = range(-1, self.NF - 1) - """ index of antecedent joint: list of int""" - self.mu = [0 for i in xrange(NF + 1)] - """motorization, if 1, then the joint im motorized""" - self.theta = [0] + [var('th%s' % (i+1)) for i in xrange(NF)] - """ geometrical parameter: list of var""" - self.r = [0 for i in xrange(NF + 1)] - """ geometrical parameter: list of var""" - self.alpha = [0 for i in xrange(NF + 1)] - """ geometrical parameter: list of var""" - self.d = [0 for i in xrange(NF + 1)] - """ geometrical parameter: list of var""" - self.gamma = [0 for i in xrange(NF + 1)] - """ geometrical parameter: list of var""" - self.b = [0 for i in xrange(NF + 1)] - """ geometrical parameter: list of var""" - self.Z = eye(4) - """ transformation from reference frame to zero frame""" - num = range(self.NL) - numj = range(self.NJ) - self.w0 = zeros(3, 1) - """ base angular velocity: 3x1 matrix""" - self.wdot0 = zeros(3, 1) - """ base angular acceleration: 3x1 matrix""" - self.v0 = zeros(3, 1) - """ base linear velocity: 3x1 matrix""" - self.vdot0 = zeros(3, 1) - """ base linear acceleration: 3x1 matrix""" - self.qdot = [var('QP{0}'.format(i)) for i in numj] - """ joint speed: list of var""" - self.qddot = [var('QDP{0}'.format(i)) for i in numj] - """ joint acceleration: list of var""" - self.Nex = [zeros(3, 1) for i in num] - """ external moment of link: list of 3x1 matrix""" - self.Nex[-1] = Matrix(var('CX{0}, CY{0}, CZ{0}'.format(self.NL - 1))) - self.Fex = [zeros(3, 1) for i in num] - """ external force of link: list of 3x1 matrix""" - self.Fex[-1] = Matrix(var('FX{0}, FY{0}, FZ{0}'.format(self.NL - 1))) - self.FS = [var('FS{0}'.format(i)) for i in num] - """ dry friction coefficient: list of ver""" - self.IA = [var('IA{0}'.format(i)) for i in num] - """ joint actuator inertia: list of var""" - self.FV = [var('FV{0}'.format(i)) for i in num] - """ viscous friction coefficient: list of var""" - self.MS = [Matrix(var('MX{0}, MY{0}, MZ{0}'.format(i))) for i in num] - """ first momentum of link: list of 3x1 matrix""" - self.M = [var('M{0}'.format(i)) for i in num] - """ mass of link: list of var""" - self.GAM = [var('GAM{0}'.format(i)) for i in numj] - """ joint torques: list of var""" - J_str = 'XX{0},XY{0},XZ{0},XY{0},YY{0},YZ{0},XZ{0},YZ{0},ZZ{0}' - self.J = [Matrix(3, 3, var(J_str.format(i))) for i in num] - """ inertia tensor of link: list of 3x3 matrix""" - self.G = Matrix([0, 0, var('G3')]) - """ gravity vector: 3x1 matrix""" - - # member methods: - def put_val(self, j, name, val): - #TODO: write proper parser - #accepts tuple - try: - if isinstance(val, str) or isinstance(val, unicode): - val = sympify(val) - assert isinstance(val, Expr) - geom_head = self.get_geom_head() - if name in INT_KEYS: - val = int(val) - except: - return FAIL - base_vel_head = self.get_base_vel_head() - ext_dynam_head = self.get_ext_dynam_head() - dynam_head = self.get_dynam_head() - ext_head = ext_dynam_head[7:] + ['IA'] - f_ex_head = ext_dynam_head[1:4] - n_ex_head = ext_dynam_head[4:7] - if name in ext_head + geom_head + base_vel_head: - X = getattr(self, name) - X[j] = val - elif name in f_ex_head: - self.Fex[j][f_ex_head.index(name)] = val - elif name in n_ex_head: - self.Nex[j][n_ex_head.index(name)] = val - elif name in dynam_head: - params = self.get_inert_param(j) - i = dynam_head.index(name) - params[i-1] = val - self.put_inert_param(params, j) - elif name == 'Z': - self.Z[j] = val - return OK - - def get_val(self, j, name): - geom_head = self.get_geom_head() - base_vel_head = self.get_base_vel_head() - ext_dynam_head = self.get_ext_dynam_head() - dynam_head = self.get_dynam_head() - ext_head = ext_dynam_head[7:] + ['IA'] - f_ex_head = ext_dynam_head[1:4] - n_ex_head = ext_dynam_head[4:7] - if name in ext_head + geom_head + base_vel_head: - X = getattr(self, name) - return X[j] - elif name in f_ex_head: - return self.Fex[j][f_ex_head.index(name)] - elif name in n_ex_head: - return self.Nex[j][n_ex_head.index(name)] - elif name in self.get_dynam_head(): - params = self.get_inert_param(j) - i = dynam_head.index(name) - return params[i-1] - elif name == 'Z': - return self.Z[j] - - def get_q_chain(self, j, k=0): - """Generates vector of joint variables in chain - between j, k (zero by default) - """ - chain = self.chain(j, k) - q = [] - for i in reversed(chain): - if int(self.sigma[i]) == 0: - q.append(self.theta[i]) - elif int(self.sigma[i]) == 1: - q.append(self.r[i]) - return q - - def get_q(self, i): - """ Returns symbol of joint variable - or 0 if joint is fixed - """ - if self.sigma[i] == 0: - return self.theta[i] - elif self.sigma[i] == 1: - return self.r[i] - else: - return 0 - - @property - def q_vec(self): - """Generates vector of joint variables - """ - qs = [] - for i in xrange(1, self.NJ): - if self.sigma[i] != 2: - qs.append(self.get_q(i)) - return qs - - @property - def endeffectors(self): - return set(range(1, self.NJ + 1)) - set(self.ant) - - @property - def q_passive(self): - """Generates vector of passive joint variables (including cut!) - """ - q = list() - for i in xrange(1, self.NJ): - if self.mu[i] == 0: - q.append(self.get_q(i)) - return q - - @property - def q_active(self): - """Generates vector of active joint variables (including cut!) - """ - q = list() - for i in xrange(1, self.NJ): - if self.mu[i] == 1: - q.append(self.get_q(i)) - return q - - @property - def indx_passive(self): - """Generates vector of passive joint indices - """ - return [i for i in xrange(1, self.NL) if self.mu[i] == 0] - - @property - def indx_active(self): - """Generates vector of active joint indices - """ - return [i for i in xrange(1, self.NL) if self.mu[i] == 1] - - @property - def indx_cut(self): - """Generates vector of cut joint indices - """ - return range(self.NL, self.NJ) - - def fric_v(self, j): - """Fluid friction torque - - Parameters - ========== - j: int - Joint index. - - Returns - ======= - fric_v: sympy expression - Expression for fluid friction torque of joint j - """ - return self.FV[j] * self.qdot[j] - - def fric_s(self, j): - """Dry friction torque - - Parameters - ========== - j: int - Joint index. - - Returns - ======= - fric_s: sympy expression - Expression for dry friction torque of joint j - """ - return self.FS[j] * sign(self.qdot[j]) - - @property - def W0(self): - return self.w0 - - @property - def WP0(self): - return self.wdot0 - - @property - def V0(self): - return self.w0 - - @property - def VP0(self): - return self.wdot0 - - @property - def QP(self): - return self.qdot - - @property - def QDP(self): - return self.qddot - - @property - def NJ(self): - """ Actual number of joints counting 0 - """ - return self.nj + 1 - - @property - def NL(self): - """ Actual number of links counting 0 - """ - return self.nl + 1 - - @property - def NF(self): - """ Actual number of frames counting 0 - """ - return self.nf + 1 - - @property - def loop_terminals(self): - B = self.NJ - self.NL - return [(i, i+B) for i in xrange(self.NL, self.NJ)] - - def paral(self, i, j): - if j is None: - return False - elif self.ant[i] == j: - return sin(self.alpha[i]) == 0 - elif self.ant[j] == i: - return sin(self.alpha[j]) == 0 - elif self.ant[j] == self.ant[i]: - return sin(self.alpha[j] - self.alpha[i]) == 0 - else: - return False - - def tau_ia(self, j): - """Actuator inertia torque - - Parameters - ========== - j: int - Joint index. - - Returns - ======= - fric_v: sympy expression - Expression for actuator inertia torque of joint j - """ - return self.IA[j] * self.qddot[j] - - def get_angles(self, j): - """List of non-constant angles of frame j - - Parameters - ========== - j: int - Frame index. - - Returns - ======= - get_angles: list of touples (var, name) - Returns list of touples, where: - var - the angle symbol, - name - brief name for cos and sin abbreviation - """ - angs = [] - if j not in xrange(self.NF): - return angs - if type(self.theta[j]) != int and not self.theta[j].is_number: - angs.append((self.theta[j], j)) - if type(self.alpha[j]) != int and not self.alpha[j].is_number: - angs.append((self.alpha[j], 'A%s' % j)) - if type(self.gamma[j]) != int and not self.gamma[j].is_number: - angs.append((self.gamma[j], 'G%s' % j)) - return angs - - def chain(self, j, k=0): - """Chain of antecedent frames between j-th and k-th frames - - Parameters - ========== - j: int - Start frame index. - k: int - Final frame index. - - Returns - ======= - u: list of ints - List of antecedent frames. j is the first index in the list. - k is not included - """ - u = [] - while j != k and j != 0: - u.append(j) - j = self.ant[j] - return u - - def loop_chain(self, i, j): - k = self.common_root(i, j) - chain = self.chain(i, k) - chain.append(k) - if k != j: - chain.extend(reversed(self.chain(j, k))) - return chain - - def common_root(self, i, j): - """Common root j-th and i-th frames - - Parameters - ========== - j: int - Frame index. - i: int - Frame index. - - Returns - ======= - common_root: int - The highest index of the common frame in chains for i and j. - If they don't have common root, -1 - """ - u = self.chain(i) - while True: - if j in u or j == 0: - return j - j = self.ant[j] - - def get_inert_param(self, j): - """Returns 10-vector of inertia paremeters of link j. - - Parameters - ========== - j: int - Link index. - - Returns - ======= - get_dynam_param: Matrix 10x1 - """ - K = [self.J[j][0], self.J[j][1], self.J[j][2], self.J[j][4], - self.J[j][5], self.J[j][8], self.MS[j][0], self.MS[j][1], - self.MS[j][2], self.M[j]] - return Matrix(K) - - def put_inert_param(self, K, j): - """Write the inertia parameters of link j from 10-vector K. - - Parameters - ========== - K: Matrix 10x1 - Vector of inertia parameters - j: int - Link index. - """ - self.J[j] = Matrix([[K[0], K[1], K[2]], - [K[1], K[3], K[4]], - [K[2], K[4], K[5]]]) - self.MS[j] = Matrix(3, 1, K[6:9]) - self.M[j] = K[9] - - def get_ext_dynam_head(self): - """Returns header for external forces and torques, - friction parameters and joint speeds, accelerations. - Used for output generation. - - Returns - ======= - get_ext_dynam_head: list of strings - """ - return ['j', 'FX', 'FY', 'FZ', 'CX', 'CY', 'CZ', - 'FS', 'FV', 'QP', 'QDP', 'GAM'] - - def get_dynam_head(self): - """Returns header for inertia parameters. - Used for output generation. - - Returns - ======= - get_dynam_head: list of strings - """ - return ['j', 'XX', 'XY', 'XZ', 'YY', 'YZ', 'ZZ', - 'MX', 'MY', 'MZ', 'M', 'IA'] - - def get_geom_head(self): - """Returns header for geometric parameters. - Used for output generation. - - Returns - ======= - get_geom_head: list of strings - """ - return ['j', 'ant', 'sigma', 'mu', 'gamma', 'b', - 'alpha', 'd', 'theta', 'r'] - - def get_base_vel_head(self): - """Returns header for base velocities and gravity vector. - Used for output generation. - - Returns - ======= - get_base_vel_head: list of strings - """ - return ['axis', 'W0', 'WP0', 'V0', 'VP0', 'G'] - - def get_param_vec(self, head, j): - params = list() - axis_dict = {0: 'X', 1: 'Y', 2: 'Z'} - for h in head: - if h == 'j': - params.append(j) - elif h == 'axis': - params.append(axis_dict[j]) - else: - params.append(self.get_val(j, h)) - return params - - @classmethod - def CartPole(cls): - """Generates Robot instance of classical - CartPole dynamic system. - """ - #TODO: bring it to the new notation with 0-frame - robo = Robot() - robo.name = 'CartPole' - robo.ant = (-1, 0) - robo.sigma = (1, 0) - robo.alpha = (pi/2, pi/2) - robo.d = (0, 0) - robo.theta = (pi/2, var('Th2')) - robo.r = (var('R1'), 0) - robo.b = (0, 0) - robo.gamma = (0, 0) - robo.num = range(1, 3) - robo.NJ = 2 - robo.NL = 2 - robo.NF = 2 - robo.Nex = [zeros(3, 1) for i in robo.num] - robo.Fex = [zeros(3, 1) for i in robo.num] - robo.FS = [0 for i in robo.num] - robo.IA = [0 for i in robo.num] - robo.FV = [var('FV{0}'.format(i)) for i in robo.num] - robo.MS = [zeros(3, 1) for i in robo.num] - robo.MS[1][0] = var('MX2') - robo.M = [var('M{0}'.format(i)) for i in robo.num] - robo.GAM = [var('GAM{0}'.format(i)) for i in robo.num] - robo.J = [zeros(3) for i in robo.num] - robo.J[1][2, 2] = var('ZZ2') - robo.G = Matrix([0, 0, -var('G3')]) - robo.w0 = zeros(3, 1) - robo.wdot0 = zeros(3, 1) - robo.v0 = zeros(3, 1) - robo.vdot0 = zeros(3, 1) - robo.q = var('R1, Th2') - robo.qdot = var('R1d, Th2d') - robo.qddot = var('R1dd, Th2dd') - robo.num.append(0) - return robo - - @classmethod - def SR400(cls): - #TODO: bring it to the new notation with 0-frame - """Generates Robot instance of SR400""" - robo = Robot('SR400', 8, 9, 10, False) - robo.ant = [-1, 0, 1, 2, 3, 4, 5, 1, 7, 8, 3] - robo.sigma = [0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 2] - robo.mu = [0, 1, 1, 0, 1, 1, 1, 1, 0, 0, 0] - robo.alpha = [0, 0, -pi/2, 0, -pi/2, pi/2, -pi/2, -pi/2, 0, 0, 0] - d_var = var('D:9') - robo.d = [0, 0, d_var[2], d_var[3], d_var[4], 0, 0, - d_var[2], d_var[8], d_var[3], -d_var[8]] - robo.theta = [0] + list(var('th1:10')) + [0] - robo.r = [0, 0, 0, 0, var('RL4'), 0, 0, 0, 0, 0, 0] - robo.b = [0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0] - robo.gamma = [0, 0, 0, 0, 0, 0, 0, 0, 0, 0, pi/2] - robo.structure = CLOSED_LOOP - return robo - - @classmethod - def RX90(cls): - """Generates Robot instance of RX90""" - robo = Robot('RX90', 6, 6, 6, False) - # table of geometric parameters RX90 - robo.sigma = [2, 0, 0, 0, 0, 0, 0, 0] - robo.alpha = [0, 0, pi/2, 0, -pi/2, pi/2, -pi/2] - robo.d = [0, 0, 0, var('D3'), 0, 0, 0] - robo.theta = [0] + list(var('th1:7')) - robo.r = [0, 0, 0, 0, var('RL4'), 0, 0] - robo.b = [0, 0, 0, 0, 0, 0, 0] - robo.gamma = [0, 0, 0, 0, 0, 0, 0] - robo.mu = [0, 1, 1, 1, 1, 1, 1] - robo.structure = SIMPLE -# robo.w0 = zeros(3, 1) -# robo.wdot0 = zeros(3, 1) -# robo.v0 = zeros(3, 1) -# robo.vdot0 = zeros(3, 1) -# robo.qdot = [var('QP{0}'.format(i)) for i in num] -# robo.qddot = [var('QDP{0}'.format(i)) for i in num] -# robo.Nex= [zeros(3, 1) for i in num] -# robo.Nex[-1] = Matrix(var('CX{0}, CY{0}, CZ{0}'.format(robo.NJ))) -# robo.Fex = [zeros(3, 1) for i in num] -# robo.Fex[-1] = Matrix(var('FX{0}, FY{0}, FZ{0}'.format(robo.NJ))) -# robo.FS = [var('FS{0}'.format(i)) for i in num] -# robo.IA = [var('IA{0}'.format(i)) for i in num] -# robo.FV = [var('FV{0}'.format(i)) for i in num] -# robo.MS = [Matrix(var('MX{0}, MY{0}, MZ{0}'.format(i))) for i in num] -# robo.M = [var('M{0}'.format(i)) for i in num] -# robo.GAM = [var('GAM{0}'.format(i)) for i in num] -# robo.J = [Matrix(3, 3, var(('XX{0}, XY{0}, XZ{0}, ' -# 'XY{0}, YY{0}, YZ{0}, ' -# 'XZ{0}, YZ{0}, ZZ{0}').format(i))) for i in num] -# robo.G = Matrix([0, 0, var('G3')]) -# robo.num.append(0) - return robo - - -class Init: - @classmethod - def init_Jplus(cls, robo): - """Copies the inertia parameters. - Used for composed link inertia computation - - Returns - ======= - Jplus: list of Matrices 3x3 - MSplus: list of Matrices 3x1 - Mplus: list of var - """ - Jplus = copy(robo.J) - Jplus.append(zeros(3, 3)) - MSplus = copy(robo.MS) - MSplus.append(zeros(3, 1)) - Mplus = copy(robo.M) - Mplus.append(0) - return Jplus, MSplus, Mplus - - @classmethod - def init_mat(cls, robo, N=3): - """Generates a list of Matrices.Size of the - list is number of links. - - Parameters - ========== - robo: Robot - Instance of robot description container - N: int, optional - size of the matries, default is 3 - - Returns - ======= - list of Matrices NxN - """ - return [zeros(N, N) for i in xrange(robo.NL)] - - @classmethod - def init_vec(cls, robo, N=3, ext=0): - """Generates a list of vectors. - Size of the list is number of links. - - Parameters - ========== - robo: Robot - Instance of robot description container - N: int, optional - size of the vectors, default is 3 - ext: int, optional - additional vector instances over number of links - - Returns - ======= - list of Matrices Nx1 - """ - return [zeros(N, 1) for i in xrange(robo.NL+ext)] - - @classmethod - def init_scalar(cls, robo): - """Generates a list of vars. - Size of the list is number of links. - """ - return [0 for i in xrange(robo.NL)] - - @classmethod - def init_w(cls, robo): - """Generates a list of vectors for angular velocities. - Size of the list is number of links + 1. - The zero vector is the base angular velocity - """ - w = cls.init_vec(robo) - w[0] = robo.w0 - return w - - @classmethod - def init_v(cls, robo): - """Generates a list of vectors for linear velocities. - Size of the list is number of links + 1. - The zero vector is the base angular velocity - """ - v = cls.init_vec(robo) - v[0] = robo.v0 - return v - - @classmethod - def init_wv_dot(cls, robo, gravity=True): - """Generates lists of vectors for - angular and linear accelerations. - Size of the list is number of links + 1. - The zero vector is the base angular velocity - - Returns - ======= - vdot: list of Matrices 3x1 - wdot: list of Matrices 3x1 - """ - wdot = cls.init_vec(robo) - wdot[0] = robo.wdot0 - vdot = cls.init_vec(robo) - vdot[0] = robo.vdot0 - if gravity: - vdot[0] -= robo.G - return wdot, vdot - - @classmethod - def init_U(cls, robo): - """Generates a list of auxiliary U matrices""" - U = Init.init_mat(robo) - # the value for the -1th base frame - U.append(hat(robo.w0)**2 + hat(robo.wdot0)) - return U - - @classmethod - def product_combinations(cls, v): - """Generates 6-vector of different v elements' - product combinations - - Parameters - ========== - v: Matrix 3x1 - vector - - Returns - ======= - product_combinations: Matrix 6x1 - """ - return Matrix([v[0]*v[0], v[0]*v[1], v[0]*v[2], - v[1]*v[1], v[1]*v[2], v[2]*v[2]]) - - -def hat(v): - """Generates vectorial preproduct matrix - - Parameters - ========== - v: Matrix 3x1 - vector - - Returns - ======= - hat: Matrix 3x3 - """ - return Matrix([[0, -v[2], v[1]], - [v[2], 0, -v[0]], - [-v[1], v[0], 0]]) - - -def l2str(list_var, spacing=8): - """Converts a list into string, that will be - written into the text table. - - Parameters - ========== - list_var: list - List to be converted - spacing: int, optional - Defines the size of one cell of the table - - Returns - ======= - s: string - String representation - - Notes - ===== - l2str([1, 2, 3]) will be converted into '1 2 3 ' - """ - s = '' - for i in list_var: - s += str(i) + ' '*(spacing-len(str(i))) - return s - - -def get_trig_couple_names(sym): - names_s = find_trig_names(sym, r'S', 1) - names_c = find_trig_names(sym, r'C', 1) - return names_c & names_s - - -def find_trig_names(sym, pref=r'', pref_len=0, post=r'', post_len=0): - search_res = re.findall(pref + r'[AGm0-9]*' + post, str(sym)) - if post_len == 0: - return set([s[pref_len:] for s in search_res]) - else: - return set([s[pref_len:-post_len] for s in search_res]) - - -def get_max_coef_list(sym, x): - return [get_max_coef_mul(s, x) for s in Add.make_args(sym)] - - -def get_max_coef(sym, x): - return Add.fromiter(get_max_coef_mul(s, x) for s in Add.make_args(sym)) - - -def make_fname(robo, ext=None): - if ext is None: - fname = '%s.par' % robo.name - else: - fname = '%s_%s.txt' % (robo.name, ext) - full_name = '%s\\%s' % (robo.directory, fname) - if not os.path.exists(robo.directory): - os.makedirs(robo.directory) - return full_name - - -def get_max_coef_mul(sym, x): - """ - """ - k, ex = x.as_coeff_Mul() - coef = sym / k - pow_x = ex.as_powers_dict() - pow_c = coef.as_powers_dict() - pow_c[-1] = 0 - for a, pa in pow_x.iteritems(): - na = -a - if a in pow_c and pow_c[a] >= pa: - pow_c[a] -= pa - elif na in pow_c and pow_c[na] >= pa: - pow_c[na] -= pa - if pa % 2: - pow_c[-1] += 1 - else: - return ZERO - return Mul.fromiter(c**p for c, p in pow_c.iteritems()) - - -def ang_sum(np1, np2, nm1, nm2): - np2, nm1 = reduce_str(np2, nm1) - np1, nm2 = reduce_str(np1, nm2) - if len(nm1) + len(nm2) == 0: - return np1 + np2 - else: - return np1 + np2 + 'm' + nm1 + nm2 - - -def get_pos_neg(s): - if s.find('m') != -1: - s_split = s.split('m') - return s_split[0], s_split[1] - else: - return s, '' - - -def reduce_str(s1, s2): - while True: - for j, char in enumerate(s1): - if char in 'AG': - i = s2.find(s1[j:j+2]) - k = 2 - else: - i = s2.find(char) - k = 1 - if i != -1: - if i+k < len(s2): - s2_tail = s2[i+k:] - else: - s2_tail = '' - if j+k < len(s1): - s1_tail = s1[j+k:] - else: - s1_tail = '' - s2 = s2[:i] + s2_tail - s1 = s1[:j] + s1_tail - break - else: - break - return s1, s2 - - -def CS_syms(name): - if isinstance(name, str) and name[0] == 'm': - C, S = var('C{0}, S{0}'.format(name[1:])) - return C, -S - else: - return var('C{0}, S{0}'.format(name)) - - -def sym_less(A, B): - A_measure = A.count_ops() - B_measure = B.count_ops() - return A_measure < B_measure - - -def get_angles(expr): - angles_s = set() - for s in expr.atoms(sin): - angles_s |= set(s.args) - angles_c = set() - for c in expr.atoms(cos): - angles_c |= set(c.args) - return angles_s & angles_c - - -def cancel_terms(sym, X, coef): - if coef.is_Add: - for arg_c in coef.args: - sym = cancel_terms(sym, X, arg_c) - else: - terms = Add.make_args(sym) - return Add.fromiter(t for t in terms if t != X*coef) - - -def trigonometric_info(sym): - if not sym.has(sin) and not sym.has(cos): - short_form = True - names = get_trig_couple_names(sym) - else: - short_form = False - names = get_angles(sym) - return names, short_form - - -class Symoro: - """Symbol manager, responsible for symbol replacing, file writing.""" - def __init__(self, file_out='disp', sydi={}): - """Default values correspond to empty dictionary and screen output. - """ - self.file_out = file_out - """Output descriptor. Can be None, 'disp', file - defines the output destination""" - self.sydi = dict((k, sydi[k]) for k in sydi) - """Dictionary. All the substitutions are saved in it""" - self.revdi = dict((sydi[k], k) for k in sydi) - """Dictionary. Revers to the self.sydi""" - self.order_list = sydi.keys() - """keeps the order of variables to be compute""" - - def simp(self, sym): - sym = factor(sym) - new_sym = ONE - for e in Mul.make_args(sym): - if e.is_Pow: - e, p = e.args - else: - p = 1 - e = self.C2S2_simp(e) - e = self.CS12_simp(e, silent=True) - new_sym *= e**p - return new_sym - - def C2S2_simp(self, sym): - """ - Example - ======= - >> print C2S2_simp(sympify("-C**2*RL + S*(D - RL*S)")) - D*S - RL - """ - if not sym.is_Add: - repl_dict = {} - for term in sym.atoms(Add): - repl_dict[term] = self.C2S2_simp(term) - sym = sym.xreplace(repl_dict) - return sym - names, short_form = trigonometric_info(sym) - for name in names: - if short_form: - C, S = CS_syms(name) - else: - C, S = cos(name), sin(name) - sym = self.try_opt(ONE, None, S**2, C**2, sym) - return sym - - def CS12_simp(self, sym, silent=False): - """ - Example - ======= - >> print Symoro().CS12_simp(sympify("C2*C3 - S2*S3")) - C23 = C2*C3 - S2*S3 - C23 - >> print Symoro().CS12_simp(sympify("C2*S3*R + S2*C3*R")) - S23 = C2*S3 + S2*C3 - R*S23 - """ - if not sym.is_Add: - repl_dict = {} - for term in sym.atoms(Add): - repl_dict[term] = self.CS12_simp(term) - sym = sym.xreplace(repl_dict) - return sym - names, short_form = trigonometric_info(sym) - names = list(names) - names.sort() - sym2 = sym - for n1, n2 in combinations(names, 2): - if short_form: - C1, S1 = CS_syms(n1) - C2, S2 = CS_syms(n2) - np1, nm1 = get_pos_neg(n1) - np2, nm2 = get_pos_neg(n2) - n12 = ang_sum(np1, np2, nm1, nm2) - nm12 = ang_sum(np1, nm2, nm1, np2) - C12, S12 = CS_syms(n12) - C1m2, S1m2 = CS_syms(nm12) - else: - C1, S1 = cos(n1), sin(n1) - C2, S2 = cos(n2), sin(n2) - C12, S12 = cos(n1+n2), sin(n1+n2) - C1m2, S1m2 = cos(n1-n2), sin(n1-n2) - sym2 = self.try_opt(S12, S1m2, S1*C2, C1*S2, sym2, silent) - sym2 = self.try_opt(C12, C1m2, C1*C2, -S1*S2, sym2, silent) - if sym2 != sym: - return self.CS12_simp(sym2, silent) - else: - return sym - - def try_opt(self, A, Am, B, C, old_sym, silent=False): - """Replaces B + C by A or B - C by Am. - Chooses the best option. - """ - Bcfs = get_max_coef_list(old_sym, B) - Ccfs = get_max_coef_list(old_sym, C) - if Bcfs != [] and Ccfs != []: - Res = old_sym - Res_tmp = Res - for coef in Bcfs: - Res_tmp += A*coef - B*coef - C*coef - if sym_less(Res_tmp, Res): - Res = Res_tmp - if sym_less(Res, old_sym) and Am is None: - if not A.is_number and not silent: - self.add_to_dict(A, B + C) - return Res - elif Am is not None: - Res2 = old_sym - Res_tmp = Res2 - for coef in Bcfs: - Res_tmp += Am*coef - B*coef + C*coef - if sym_less(Res_tmp, Res2): - Res2 = Res_tmp - if sym_less(Res2, Res) and sym_less(Res2, old_sym): - if not Am.is_number and not silent: - self.add_to_dict(Am, B - C) - return Res2 - elif sym_less(Res, old_sym): - if not A.is_number and not silent: - self.add_to_dict(A, B + C) - return Res - return old_sym - - def add_to_dict(self, new_sym, old_sym): - """Internal function. - Extends symbol dictionary by (new_sym, old_sym) pair - """ - new_sym = sympify(new_sym) - if new_sym.as_coeff_Mul()[0] == -ONE: - new_sym = -new_sym - old_sym = -old_sym - if new_sym not in self.sydi: - self.sydi[new_sym] = old_sym - self.revdi[old_sym] = new_sym - self.order_list.append(new_sym) - self.write_equation(new_sym, old_sym) - - def trig_replace(self, M, angle, name): - """Replaces trigonometric expressions cos(x) - and sin(x) by CX and SX - - Parameters - ========== - M: var or Matrix - Object of substitution - angle: var - symbol that stands for the angle value - name: int or string - brief name X for the angle - - Notes - ===== - The cos(x) and sin(x) will be replaced by CX and SX, - where X is the name and x is the angle - """ - if not isinstance(angle, Expr) or angle.is_number: - return M - cos_sym, sin_sym = CS_syms(name) - sym_list = [(cos_sym, cos(angle)), (sin_sym, sin(angle))] - subs_dict = {} - for sym, sym_old in sym_list: - if sym_old.has(-1): - subs_dict[-sym_old] = -sym - else: - subs_dict[sym_old] = sym - self.add_to_dict(sym, sym_old) - for i1 in xrange(M.shape[0]): - for i2 in xrange(M.shape[1]): - M[i1, i2] = M[i1, i2].subs(subs_dict) - return M - - def replace(self, old_sym, name, index='', forced=False): - """Creates a new symbol for the symbolic expression old_sym. - - Parameters - ========== - old_sym: var - Symbolic expression to be substituted - name: string or var - denotion of the expression - index: int or string, optional - will be attached to the name. Usualy used for link or joint number. - Parameter exists for usage convenience - forced: bool, optional - If True, the new symbol will be created even if old symbol - is a simple expression - - Notes - ===== - Generaly only complex expressions, which contain + - * / ** operations - will be replaced by a new symbol - """ - inv_sym = -old_sym - is_simple = old_sym.is_Atom or inv_sym.is_Atom - if is_simple and not forced: - return old_sym - elif not forced: - for i in (1, -1): - if i * old_sym in self.revdi: - return i * self.revdi[i * old_sym] - new_sym = var(str(name) + str(index)) - self.add_to_dict(new_sym, old_sym) - if is_simple: - return old_sym - else: - return new_sym - - def mat_replace(self, M, name, index='', - forced=False, skip=0, symmet=False): - """Replaces each element in M by symbol - - Parameters - ========== - M: Matrix - Object of substitution - name: string - denotion of the expression - index: int or string, optional - will be attached to the name. Usualy used for link - or joint number. Parameter exists for usage convenience - forced: bool, optional - If True, the new symbol will be created even if old symbol - is a simple expression - skip: int, optional - Number of bottom rows of the matrix, which will be skipped. - Used in case of Transformation matrix and forced = True. - symmet: bool, optional - If true, only for upper triangle part of the matrix - symbols will be created. The bottom triangle part the - same symbols will be used - - - Returns - ======= - M: Matrix - Matrix with all the elements replaced - - Notes - ===== - -Each element M_ij will be replaced by - symbol name + i + j + index - -There are two ways to use this function (examples): - 1) >>> A = B+C+... - >>> symo.mat_replace(A, 'A') - # for the case when expression B+C+... is too big - 2) >>> A = symo.mat_replace(B+C+..., 'A') - # for the case when B+C+... is small enough - """ - for i2 in xrange(M.shape[1]): - for i1 in xrange(M.shape[0] - skip): - if symmet and i2 < i1: - M[i1, i2] = M[i2, i1] - continue - if M.shape[1] > 1: - name_index = name + str(i1 + 1) + str(i2 + 1) - else: - name_index = name + str(i1 + 1) - M[i1, i2] = self.replace(M[i1, i2], name_index, index, forced) - return M - - def unfold(self, expr): - """Unfold the expression using the dictionary. - - Parameters - ========== - expr: symbolic expression - Symbolic expression to be unfolded - - Returns - ======= - expr: symbolic expression - Unfolded expression - """ - while self.sydi.keys() & expr.atoms(): - expr = expr.subs(self.sydi) - return expr - - def write_param(self, name, header, robo, N): - """Low-level function for writing the parameters table - - Parameters - ========== - name: string - the name of the table - header: list - the table header - robo: Robot - Instance of parameter container - N: list of int - Indices for which parameter rows will be written - """ - self.write_line(name) - self.write_line(l2str(header)) - for j in N: - params = robo.get_param_vec(header, j) - self.write_line(l2str(params)) - self.write_line() - - #TODO: rewrite docstring - def write_params_table(self, robo, title='', geom=True, inert=False, - dynam=False, equations=True, - inert_name='Dynamic inertia parameters'): - """Writes the geometric parameters table - - Parameters - ========== - robo: Robot - Instance of the parameter container. - title: string - The document title. - - Notes - ===== - The synamic model generation program can be started with this function - """ - if title != '': - self.write_line(title) - self.write_line() - if geom: - self.write_param('Geometric parameters', robo.get_geom_head(), - robo, range(1, robo.NF)) - if inert: - if robo.is_mobile: - start_frame = 0 - else: - start_frame = 1 - - self.write_param(inert_name, robo.get_dynam_head(), - robo, range(start_frame, robo.NL)) - if dynam: - self.write_param('External forces and joint parameters', - robo.get_ext_dynam_head(), - robo, range(1, robo.NL)) - self.write_param('Base velicities parameters', - robo.get_base_vel_head(), - robo, [0, 1, 2]) - if equations: - self.write_line('Equations:') - - def unknown_sep(self, eq, known): - """If there is a sum inside trigonometric function and - the atoms are not the subset of 'known', - this function will replace the trigonometric symbol bu sum, - trying to separate known and unknown terms - """ - if not isinstance(eq, Expr) or eq.is_number: - return eq - while True: - res = False - trigs = eq.atoms(sin, cos) - for trig in trigs: - args = trig.args[0].atoms() - if args & known and not args <= known and trig in self.sydi: - eq = eq.subs(trig, self.sydi[trig]).expand() - res = True - if not res: - break - return eq - - def write_equation(self, A, B): - """Writes the equation A = B into the output - - Parameters - ========== - A: expression or var - left-hand side of the equation. - B: expression or var - right-hand side of the equation - """ - self.write_line(str(A) + ' = ' + str(B)) - - def write_line(self, line=''): - """Writes string data into tha output with new line symbol - - Parameters - ========== - line: string, optional - Data to be written. If empty, it adds an empty line - """ - if self.file_out == 'disp': - print line - elif self.file_out is not None: - self.file_out.write(str(line) + '\n') - - def file_open(self, robo, ext): - """ - Initialize file stream - - Parameters - ========== - robo: Robot instance - provides the robot's name - ext: string - provides the file name extention - """ - fname = make_fname(robo, ext) - self.file_out = open(fname, 'w') - - def file_close(self): - """ - Initialize file stream - - Parameters - ========== - robo: Robot instance - provides the robot's name - ext: string - provides the file name extention - """ - if self.file_out is not None: - self.write_line('*=*') - self.file_out.close() - - def gen_fheader(self, name, *args): - fun_head = [] - fun_head.append('def %s_func(*args):\n' % name) - imp_s_1 = 'from numpy import pi, sin, cos, sign\n' - imp_s_2 = 'from numpy import array, arctan2 as atan2, sqrt\n' - fun_head.append(' %s' % imp_s_1) - fun_head.append(' %s' % imp_s_2) - for i, var_list in enumerate(args): - v_str_list = self.convert_syms(args[i], True) - fun_head.append(' %s=args[%s]\n' % (v_str_list, i)) - return fun_head - - def convert_syms(self, syms, rpl_liter=False): - """Converts 'syms' structure to sintactically correct string - - Parameters - ========== - syms: list, Matrix or tuple of them - rpl_liter: bool - if true, all literals will be replaced with _ - It is done to evoid expression like [x, 0] = args[1] - Because it will cause exception of assigning to literal - """ - if isinstance(syms, tuple) or isinstance(syms, list): - syms = [self.convert_syms(item, rpl_liter) for item in syms] - res = '[' - for i, s in enumerate(syms): - res += s - if i < len(syms) - 1: - res += ',' - res += ']' - return res - elif isinstance(syms, Matrix): - res = '[' - for i in xrange(syms.shape[0]): - res += self.convert_syms(list(syms[i, :]), rpl_liter) - if i < syms.shape[0] - 1: - res += ',' - res += ']' - return res - else: - if rpl_liter and sympify(syms).is_number: - return '_' - else: - return str(syms) - - def extract_syms(self, syms): - """ returns set of all symbols from list or matrix - or tuple of them - """ - if isinstance(syms, tuple) or isinstance(syms, list): - atoms = (self.extract_syms(item) for item in syms) - return reduce(set.__or__, atoms, set()) - elif isinstance(syms, Matrix): - return self.extract_syms(list(syms)) - elif isinstance(syms, Expr): - return syms.atoms(Symbol) - else: - return set() - - def sift_syms(self, rq_syms, wr_syms): - """Returns ordered list of variables to be compute - """ - order_list = [] # vars that are defined in sydi - for s in reversed(self.order_list): - if s in rq_syms and not s in wr_syms: - order_list.insert(0, s) - s_val = self.sydi[s] - if isinstance(s_val, Expr): - atoms = s_val.atoms(Symbol) - rq_syms |= {s for s in atoms if not s.is_number} - rq_vals = [s for s in rq_syms if not (s in self.sydi or s in wr_syms)] - # required vars that are not defined in sydi - # will be set to '1.' - return rq_vals + order_list - - def gen_fbody(self, name, to_return, wr_syms, multival): - """Generates list of string statements of the function that - computes symbolf from to_return. wr_syms are considered to - be known - """ - # final symbols to be compute - syms = self.extract_syms(to_return) - # defines order of computation - order_list = self.sift_syms(syms, wr_syms) - # list of instructions in final function - fun_body = [] - # will be switched to true when branching detected - space = ' ' - folded = 1 # indentation = 1 + number of 'for' statements - - for s in order_list: - if s not in self.sydi: - item = '%s%s=1.\n' % (space * folded, s) - elif isinstance(self.sydi[s], tuple): - multival = True - item = '%sfor %s in %s:\n' % (space * folded, s, self.sydi[s]) - folded += 1 - else: - item = '%s%s=%s\n' % (space * folded, s, self.sydi[s]) - fun_body.append(item) - ret_expr = self.convert_syms(to_return) - if multival: - fun_body.insert(0, ' %s_result=[]\n' % (name)) - item = '%s%s_result.append(%s)\n' % (space*folded, name, ret_expr) - else: - item = ' %s_result=%s\n' % (name, ret_expr) - fun_body.append(item) - fun_body.append(' return %s_result\n' % (name)) - return fun_body - - def gen_func(self, name, to_return, args, multival=False): - """ Returns function that computes what is in to_return - using *args as arguments - - Parameters - ========== - name: string - Future function's name, must be different for - different fucntions - to_return: list, Matrix or tuple of them - Determins the shape of the output and symbols inside it - *args: any number of lists, Matrices or tuples of them - Determins the shape of the input and symbols - names to assigned - - Notes - ===== - -All unassigned used symbols will be set to '1.0'. - -This function must be called only after the model that - computes symbols in to_return have been generated. - """ - fun_head = self.gen_fheader(name, args) - wr_syms = self.extract_syms(args) # set of defined symbols - fun_body = self.gen_fbody(name, to_return, wr_syms, multival) - fun_string = "".join(fun_head + fun_body) - exec fun_string -# print fun_string -# TODO: print is for debug pupuses, to be removed - return eval('%s_func' % name) diff --git a/pysymoro/core/test.py b/pysymoro/core/test.py deleted file mode 100644 index 331f5d3..0000000 --- a/pysymoro/core/test.py +++ /dev/null @@ -1,347 +0,0 @@ -""" -Unit tests for SYMORO modules -""" -import unittest -from sympy import sympify, var, Matrix -from sympy.abc import A, B, C, X, Y, Z -from numpy import random, amax, matrix, eye, zeros -import symoro -import geometry -import kinematics -import invgeom -from geometry import Transform as trns -import dynamics -import parfile - - -class testMisc(unittest.TestCase): - def test_readwrite(self): - print "######## test_readwrite ##########" - original_robo = symoro.Robot.RX90() - parfile.writepar(original_robo) - d_name = original_robo.directory - new_robo, flag = parfile.readpar(d_name, - original_robo.name) - self.assertEqual(flag, symoro.OK) - l1 = original_robo.get_geom_head() - l2 = original_robo.get_dynam_head() - l3 = original_robo.get_ext_dynam_head() - for Name in l3[1:]+l2[1:]+l1[1:]: - for i in xrange(1, original_robo.NL): - self.assertEqual(original_robo.get_val(i, Name), - new_robo.get_val(i, Name)) - - def test_robo_misc(self): - print "######## test_robo_misc ##########" - self.robo = symoro.Robot.SR400() - q = list(var('th1:10')) - self.assertEqual(self.robo.q_vec, q) - self.assertEqual(self.robo.chain(6), [6, 5, 4, 3, 2, 1]) - self.assertEqual(self.robo.chain(6, 3), [6, 5, 4]) - self.assertEqual(self.robo.loop_chain(8, 9), [8, 9]) - self.assertEqual(self.robo.loop_chain(0, 6), [0, 1, 2, 3, 4, 5, 6]) - self.assertEqual(self.robo.loop_chain(6, 0), [6, 5, 4, 3, 2, 1, 0]) - self.assertEqual(self.robo.loop_chain(9, 10), [9, 8, 7, 1, 2, 3, 10]) - self.assertEqual(self.robo.loop_terminals, [(9, 10)]) - l1 = self.robo.get_geom_head() - l2 = self.robo.get_dynam_head() - l3 = self.robo.get_ext_dynam_head() - for Name in l1[1:] + l2[1:] + l3[1:]: - for i in xrange(self.robo.NL): - if Name in symoro.INT_KEYS: - self.assertEqual(self.robo.put_val(i, Name, i), symoro.OK) - else: - v = var(Name + str(i)) - self.assertEqual(self.robo.put_val(i, Name, v), symoro.OK) - for Name in l3[1:]+l2[1:]+l1[1:]: - for i in xrange(self.robo.NL): - if Name in symoro.INT_KEYS: - self.assertEqual(self.robo.get_val(i, Name), i) - else: - v = var(Name + str(i)) - self.assertEqual(self.robo.get_val(i, Name), v) - - -class testSymoroTrig(unittest.TestCase): - def setUp(self): - self.symo = symoro.Symoro() - - def test_GetMaxCoef(self): - print "######## test_GetMaxCoef ##########" - expr1 = A*B*X + C**2 - X - expr2 = Y*Z - B - self.assertEqual(symoro.get_max_coef(expr1*X + expr2, X), expr1) - expr3 = -A**3*B**2*X**5*(X-Y)**7 - expr3x = -A**3*B**2*X**5*(-X-Y)**7 - expr3y = -A**3*B**2*X**5*(-X+Y)**7 - expr4 = B*X**2*(X-Y)**3 - self.assertEqual(symoro.get_max_coef(expr3*expr4, expr4), expr3) - self.assertEqual(symoro.get_max_coef(expr3x, expr4), symoro.ZERO) - res = symoro.get_max_coef(expr3y, expr4)*expr4-expr3y - self.assertEqual(res.expand(), symoro.ZERO) - - def test_name_extraction(self): - print "######## test_name_extraction ##########" - expr1 = sympify("C2*S3*R + S2*C3*R") - self.assertEqual(symoro.get_trig_couple_names(expr1), {'2', '3'}) - expr2 = sympify("CG2*S3*R + SG2*C1*R") - self.assertEqual(symoro.get_trig_couple_names(expr2), {'G2'}) - expr2 = sympify("CA2*SA3*R + SG2*C3*R") - self.assertEqual(symoro.get_trig_couple_names(expr2), set()) - expr3 = sympify("C2*S3*R + S1*C4*R") - self.assertEqual(symoro.get_trig_couple_names(expr3), set()) - - def test_name_operations(self): - print "######## test_name_operations ##########" - self.assertEqual(symoro.reduce_str('12', '13'), ('2', '3')) - self.assertEqual(symoro.reduce_str('124', '123'), ('4', '3')) - self.assertEqual(symoro.reduce_str('124', '134'), ('2', '3')) - self.assertEqual(symoro.reduce_str('12', '124'), ('', '4')) - self.assertEqual(symoro.reduce_str('1G2', 'G24'), ('1', '4')) - self.assertEqual(symoro.reduce_str('1G2G4', '13G4'), ('G2', '3')) - - def test_try_opt(self): - print "######## test_try_opt ##########" - e1 = A*(B-C)*X**2 + B*X**3 + A*(B-C)*Y**2 + B*X*Y**2 - e2 = X**2 - e3 = Y**2 - e4 = symoro.ONE - e5 = symoro.ZERO - self.assertEqual(self.symo.try_opt(e4, e5, e2, e3, e1), A*(B-C) + B*X) - e6 = A*(B-C)*X**2 + B*X**3 - A*(B - C)*Y**2 - B*X*Y**2 - self.assertEqual(self.symo.try_opt(e4, e5, e2, e3, e6), e5) - e7 = A*B - self.assertEqual(self.symo.try_opt(e4, e7, e2, e3, e6), - e7*A*(B-C) + e7*B*X) - self.assertEqual(self.symo.try_opt(e7, e4, e2, e3, e1), - e7*A*(B-C) + e7*B*X) - - def test_trig_simp(self): - print "######## test_trig_simp ##########" - e1 = sympify("S2**2 + C2**2") - e1ans = sympify("1") - self.assertEqual(self.symo.C2S2_simp(e1), e1ans) - e1 = sympify("S1**2 + C2**2") - self.assertEqual(self.symo.C2S2_simp(e1), e1) - e1 = sympify("S2**3 + C2**2") - self.assertEqual(self.symo.C2S2_simp(e1), e1) - e1 = sympify("S2**2 + 2*C2**2") - e1ans = sympify("C2**2 + 1") - self.assertEqual(self.symo.C2S2_simp(e1), e1ans) - e1 = sympify("S1**2 + S1**2*C1 + C1**2 + C1**3 + C1**4") - e1ans = sympify("C1**4 + C1 + 1") - self.assertEqual(self.symo.C2S2_simp(e1), e1ans) - e2 = sympify("C1*S2 - C2*S1") - e2ans = sympify("-S1m2") - self.assertEqual(self.symo.CS12_simp(e2), e2ans) - e2 = sympify("(C1*S2 - C2*S1)*(C1*S2 + C2*S1)") - e2ans = sympify("-S1m2*S12") - self.assertEqual(self.symo.CS12_simp(e2), e2ans) - e2 = sympify("""C2*D3*S3m78 - C2m7*D8*S3 - - C3*D8*S2m7 - C3m78*D3*S2 + D2*S3""") - e2ans = sympify("D2*S3 - D3*S278m3 - D8*S23m7") - self.assertEqual(self.symo.CS12_simp(e2), e2ans) - e2 = sympify("sin(g+th2)*sin(th3+th8)-cos(g+th2)*cos(th3+th8)") - e2ans = sympify("-cos(g+th2+th3+th8)") - self.assertEqual(self.symo.CS12_simp(e2), e2ans) - e3 = sympify("""-a1*sin(th2+th1)*sin(th3)*cos(th1)- - a1*cos(th1)*cos(th2+th1)*cos(th3)""") - e3ans = sympify("-a1*cos(th1)*cos(th1 + th2 - th3)") - self.assertEqual(self.symo.CS12_simp(e3), e3ans) - e4 = sympify("""C2*C3*C4**2*C5**2*C6**4*D3**2*RL4*S5 + - 2*C2*C3*C4**2*C5**2*C6**2*D3**2*RL4*S5*S6**2 + - C2*C3*C4**2*C5**2*D3**2*RL4*S5*S6**4 + - C2*C3*C4**2*C6**4*D3**2*RL4*S5**3 + - 2*C2*C3*C4**2*C6**2*D3**2*RL4*S5**3*S6**2 + - C2*C3*C4**2*D3**2*RL4*S5**3*S6**4 + - C2*C3*C5**2*C6**4*D3**2*RL4*S4**2*S5 + - 2*C2*C3*C5**2*C6**2*D3**2*RL4*S4**2*S5*S6**2 + - C2*C3*C5**2*D3**2*RL4*S4**2*S5*S6**4 + - C2*C3*C6**4*D3**2*RL4*S4**2*S5**3 + - 2*C2*C3*C6**2*D3**2*RL4*S4**2*S5**3*S6**2 + - C2*C3*D3**2*RL4*S4**2*S5**3*S6**4 - - C3*C4**2*C5**2*C6**4*D3*RL4**2*S23*S5 - - 2*C3*C4**2*C5**2*C6**2*D3*RL4**2*S23*S5*S6**2 - - C3*C4**2*C5**2*D3*RL4**2*S23*S5*S6**4 - - C3*C4**2*C6**4*D3*RL4**2*S23*S5**3 - - 2*C3*C4**2*C6**2*D3*RL4**2*S23*S5**3*S6**2 - - C3*C4**2*D3*RL4**2*S23*S5**3*S6**4 - - C3*C5**2*C6**4*D3*RL4**2*S23*S4**2*S5 - - 2*C3*C5**2*C6**2*D3*RL4**2*S23*S4**2*S5*S6**2 - - C3*C5**2*D3*RL4**2*S23*S4**2*S5*S6**4 - - C3*C6**4*D3*RL4**2*S23*S4**2*S5**3 - - 2*C3*C6**2*D3*RL4**2*S23*S4**2*S5**3*S6**2 - - C3*D3*RL4**2*S23*S4**2*S5**3*S6**4""") - e4ans = sympify("C3*D3*RL4*S5*(C2*D3 - RL4*S23)") - self.assertEqual((self.symo.simp(e4)-e4ans).expand(), symoro.ZERO) - - -class testGeometry(unittest.TestCase): - - def setUp(self): - self.symo = symoro.Symoro() - self.robo = symoro.Robot.RX90() - -# def test_misc(self): -# self.assertEqual(self.robo.structure, symoro.SIMPLE) -# self.robo.ant[3] = 0 -# self.assertEqual(self.robo.type_of_structure, symoro.TREE) -# self.robo.ant[3] = 2 -# self.assertEqual(self.robo.type_of_structure, symoro.SIMPLE) -# robo2 = symoro.Robot.SR400() -# self.assertEqual(robo2.type_of_structure, symoro.CLOSED_LOOP) - - def test_dgm_RX90(self): - print "######## test_dgm_RX90 ##########" - T = geometry.dgm(self.robo, self.symo, 0, 6, - fast_form=True, trig_subs=True) - f06 = self.symo.gen_func('DGM_generated1', T, self.robo.q_vec) - T = geometry.dgm(self.robo, self.symo, 6, 0, - fast_form=True, trig_subs=True) - f60 = self.symo.gen_func('DGM_generated2', T, self.robo.q_vec) - for x in xrange(10): - arg = random.normal(size=6) - M = matrix(f06(arg))*matrix(f60(arg))-eye(4) - self.assertLess(amax(M), 1e-12) - t06 = matrix([[1, 0, 0, 1], [0, 1, 0, 0], - [0, 0, 1, 1], [0, 0, 0, 1]]) - self.assertLess(amax(matrix(f06(zeros(6)))-t06), 1e-12) - T46 = geometry.dgm(self.robo, self.symo, 4, 6, - fast_form=False, trig_subs=True) - C4, S4, C5, C6, S5, S6, RL4 = var("C4,S4,C5,C6,S5,S6,RL4") - T_true46 = Matrix([[C5*C6, -C5*S6, -S5, 0], [S6, C6, 0, 0], - [S5*C6, -S5*S6, C5, 0], [0, 0, 0, 1]]) - self.assertEqual(T46, T_true46) - T36 = geometry.dgm(self.robo, self.symo, 3, 6, - fast_form=False, trig_subs=True) - T_true36 = Matrix([[C4*C5*C6-S4*S6, -C4*C5*S6-S4*C6, -C4*S5, 0], - [S5*C6, -S5*S6, C5, RL4], - [-S4*C5*C6-C4*S6, S4*C5*S6-C4*C6, S4*S5, 0], - [0, 0, 0, 1]]) - self.assertEqual(T36, T_true36) - - def test_dgm_SR400(self): - print "######## test_dgm_SR400 ##########" - self.robo = symoro.Robot.SR400() - T = geometry.dgm(self.robo, self.symo, 0, 6, - fast_form=True, trig_subs=True) - f06 = self.symo.gen_func('DGM_generated1', T, self.robo.q_vec) - T = geometry.dgm(self.robo, self.symo, 6, 0, - fast_form=True, trig_subs=True) - f60 = self.symo.gen_func('DGM_generated2', T, self.robo.q_vec) - for x in xrange(10): - arg = random.normal(size=9) - M = matrix(f06(arg))*matrix(f60(arg))-eye(4) - self.assertLess(amax(M), 1e-12) - t06 = matrix([[1, 0, 0, 3], [0, -1, 0, 0], - [0, 0, -1, -1], [0, 0, 0, 1]]) - self.assertLess(amax(matrix(f06(zeros(9))) - t06), 1e-12) - - def test_igm(self): - print "######## test_igm ##########" - invgeom._paul_solve(self.robo, self.symo, invgeom.T_GENERAL, 0, 6) - igm_f = self.symo.gen_func('IGM_gen', self.robo.q_vec, - invgeom.T_GENERAL) - T = geometry.dgm(self.robo, self.symo, 0, 6, - fast_form=True, trig_subs=True) - f06 = self.symo.gen_func('DGM_generated1', T, self.robo.q_vec) - for x in xrange(100): - arg = random.normal(size=6) - Ttest = f06(arg) - solution = igm_f(Ttest) - for q in solution: - self.assertLess(amax(matrix(f06(q))-Ttest), 1e-12) - - def test_loop(self): - print "######## test_loop ##########" - self.robo = symoro.Robot.SR400() - invgeom.loop_solve(self.robo, self.symo) - l_solver = self.symo.gen_func('IGM_gen', self.robo.q_vec, - self.robo.q_active) - T = geometry.dgm(self.robo, self.symo, 9, 10, - fast_form=True, trig_subs=True) - t_loop = self.symo.gen_func('DGM_generated1', T, self.robo.q_vec) - for x in xrange(10): - arg = random.normal(size=6) - solution = l_solver(arg) - for q in solution: - self.assertLess(amax(matrix(t_loop(q))-eye(4)), 1e-12) - - -class testKinematics(unittest.TestCase): - def setUp(self): - self.symo = symoro.Symoro() - self.robo = symoro.Robot.RX90() - - def test_speeds(self): - print 'Speeds and accelerations' - kinematics.speeds_accelerations(self.robo) - - print 'Kinematic constraint equations' - kinematics.kinematic_constraints(symoro.Robot.SR400()) - - def test_jac(self): - print "######## test_jac ##########" - kinematics.jacobian(self.robo, 6, 3, 6) - for j in xrange(1, 7): - print "######## Jac validation through DGM ##########" - #compute Jac - J, l = kinematics._jac(self.robo, self.symo, j, 0, j) - jacj = self.symo.gen_func('JacRX90', J, self.robo.q_vec) - #compute DGM - T = geometry.dgm(self.robo, self.symo, 0, j, - fast_form=True, trig_subs=True) - T0j = self.symo.gen_func('DGM_generated1', T, self.robo.q_vec) - for i in xrange(10): - dq = random.normal(size=6, scale=1e-7) - q = random.normal(size=6) - dX = matrix(jacj(q)) * matrix(dq[:j]).T - T = (matrix(T0j(q+dq)) - T0j(q)) - self.assertLess(amax(dX[:3] - trns.P(T)), 1e-12) - - def test_jac2(self): - print "######## test_jac2 ##########" - J, L = kinematics._jac(self.robo, self.symo, 6, 3, 3) - jac63 = self.symo.gen_func('Jac1RX90', J, self.robo.q_vec) - L63 = self.symo.gen_func('LRX90', L, self.robo.q_vec) - J, L = kinematics._jac(self.robo, self.symo, 6, 3, 6) - jac66 = self.symo.gen_func('Jac2RX90', J, self.robo.q_vec) - for i in xrange(10): - q = random.normal(size=6) - j63 = matrix(jac63(q)) - l63 = matrix(L63(q)) - j66 = matrix(jac66(q)) - X = eye(6) - X[:3, 3:] = l63 - self.assertLess(amax(j66 - X*j63), 1e-12) - - -class testDynamics(unittest.TestCase): - def test_dynamics(self): - robo = symoro.Robot.RX90() - - print 'Inverse dynamic model using Newton - Euler Algorith' - dynamics.inverse_dynamic_NE(robo) - - print 'Direct dynamic model using Newton - Euler Algorith' - dynamics.direct_dynamic_NE(robo) - - print 'Dynamic identification model using Newton - Euler Algorith' - dynamics.dynamic_identification_NE(robo) - - print 'Inertia Matrix using composite links' - dynamics.inertia_matrix(robo) - - print 'Coriolis torques using Newton - Euler Algorith' - dynamics.pseudo_force_NE(robo) - - print 'Base parameters computation' - dynamics.base_paremeters(robo) - -if __name__ == '__main__': - unittest.main() -######################### - suite = unittest.TestSuite() -## suite.addTest(testSymoroTrig('test_trig_simp')) - suite.addTest(testKinematics('test_jac')) -# unittest.TextTestRunner().run(suite) diff --git a/pysymoro/dyniden.py b/pysymoro/dyniden.py new file mode 100644 index 0000000..d60abcb --- /dev/null +++ b/pysymoro/dyniden.py @@ -0,0 +1,197 @@ +# -*- coding: utf-8 -*- + + +# This file is part of the OpenSYMORO project. Please see +# https://github.com/symoro/symoro/blob/master/LICENCE for the licence. + + +""" +This module contains the functions for the computation of Dynamic +Identification model. +""" + + +import copy + +import sympy + +from pysymoro.geometry import compute_rot_trans +from pysymoro.kinematics import compute_vel_acc +from symoroutils.paramsinit import ParamsInit +from symoroutils import tools + + +def get_symbol(symbol, name, index, element=''): + return symbol + name.format(index=index, element=element) + + +def vec_replace_wrapper(symo, vec, symbol, name, index, forced=False): + new_vec = sympy.zeros(vec.rows, 1) + for idx in xrange(vec.rows): + sym = get_symbol(symbol, name, index, idx+1) + new_vec[idx] = symo.replace(vec[idx], sym, forced=forced) + return new_vec + + +def _compute_dynamic_wrench(robo, symo, name, j, w, wdot, U, vdot, F, N): + """ + Compute total wrench of link j (internal function). + + Note: + F, N are the output parameters + """ + F[j] = (robo.M[j] * vdot[j]) + (U[j] * robo.MS[j]) + F[j] = vec_replace_wrapper(symo, F[j], 'F', name, j) + Psi = robo.J[j] * w[j] + Psi = vec_replace_wrapper(symo, Psi, 'PSI', name, j) + N[j] = (robo.J[j] * wdot[j]) + (tools.skew(w[j]) * Psi) + N[j] = vec_replace_wrapper(symo, N[j], 'No', name, j) + + +def _compute_reaction_wrench( + robo, symo, name, j, antRj, antPj, vdot, F, N, Fjnt, Njnt, Fex, Nex +): + """ + Compute reaction wrench (for default Newton-Euler) of joint j + (internal function). + + Note: + Fjnt, Njnt, Fex, Nex are the output parameters + """ + i = robo.ant[j] + Fjnt[j] = F[j] + Fex[j] + Fjnt[j] = vec_replace_wrapper(symo, Fjnt[j], 'E', name, j) + Njnt[j] = N[j] + Nex[j] + (tools.skew(robo.MS[j]) * vdot[j]) + Njnt[j] = vec_replace_wrapper(symo, Njnt[j], 'N', name, j) + f_ant = antRj[j] * Fjnt[j] + f_ant = vec_replace_wrapper(symo, f_ant, 'FDI', name, j) + if i != -1: + Fex[i] = Fex[i] + f_ant + Nex[i] = Nex[i] + \ + (antRj[j] * Njnt[j]) + (tools.skew(antPj[j]) * f_ant) + + +def _compute_base_reaction_wrench( + robo, symo, name, antRj, antPj, vdot, F, N, Fex, Nex, Fjnt, Njnt +): + """ + Compute reaction wrench (for default Newton-Euler) on the base + (internal function). + + Note: + Fjnt, Njnt are the output parameters + """ + j = 0 + Fjnt[j] = F[j] + Fex[j] + Fjnt[j] = vec_replace_wrapper( + symo, Fjnt[j], 'DE', name, j, forced=True + ) + Njnt[j] = N[j] + Nex[j] + (tools.skew(robo.MS[j]) * vdot[j]) + Njnt[j] = vec_replace_wrapper( + symo, Njnt[j], 'DN', name, j, forced=True + ) + + +def _compute_joint_torque(robo, symo, name, j, Fjnt, Njnt): + """ + Compute actuator torques - projection of joint wrench on the joint + axis (internal function). + """ + if robo.sigma[j] == 2: + tau_total = 0 + else: + tau = (robo.sigma[j] * Fjnt[j]) + ((1 - robo.sigma[j]) * Njnt[j]) + fric_rotor = robo.fric_s(j) + robo.fric_v(j) + robo.tau_ia(j) + tau_total = tau[2] + fric_rotor + symo.replace(tau_total, get_symbol('DG', name, j), forced=True) + + +def _compute_joint_torque_deriv(symo, param, arg, index): + """Compute joint reactive torque if the parameter is 1 + + Parameters: + symo : symbolmgr.SymbolManager + symbol manager + param : var + Dynamic parameter + arg : var + The real torque is equal to arg*param + index : strig + identifies the parameter in the sybstituted symbol's name + """ + if param != tools.ZERO and arg != tools.ZERO: + index = str(index) + str(param) + symo.replace(arg, 'DG', index, forced=True) + + +def dynamic_identification_model(robo, symo): + """ + Compute the Dynamic Identification model of a robot using + Newton-Euler algorithm. + """ + # init forces vectors + Fjnt = ParamsInit.init_vec(robo) + Njnt = ParamsInit.init_vec(robo) + # init transformation + antRj, antPj = compute_rot_trans(robo, symo) + # init velocities and accelerations + w, wdot, vdot, U = compute_vel_acc( + robo, symo, antRj, antPj, floating=True + ) + # virtual robot with only one non-zero parameter at once + robo_tmp = copy.deepcopy(robo) + robo_tmp.IA = sympy.zeros(robo.NL, 1) + robo_tmp.FV = sympy.zeros(robo.NL, 1) + robo_tmp.FS = sympy.zeros(robo.NL, 1) + # start link number + is_fixed = False if robo.is_floating or robo.is_mobile else True + start_link = 0 + for k in xrange(start_link, robo.NL): + param_vec = robo.get_inert_param(k) + F = ParamsInit.init_vec(robo) + N = ParamsInit.init_vec(robo) + for i in xrange(10): + if param_vec[i] == tools.ZERO: + continue + # change link names according to current non-zero parameter + name = '{index}{element}' + str(param_vec[i]) + # set the parameter to 1 + mask = sympy.zeros(10, 1) + mask[i] = 1 + robo_tmp.put_inert_param(mask, k) + # compute the total forcec of the link k + _compute_dynamic_wrench( + robo_tmp, symo, name, k, w, wdot, U, vdot, F, N + ) + # init external forces + Fex = ParamsInit.init_vec(robo) + Nex = ParamsInit.init_vec(robo) + for j in reversed(xrange(1, k + 1)): + _compute_reaction_wrench( + robo_tmp, symo, name, j, antRj, antPj, + vdot, F, N, Fjnt, Njnt, Fex, Nex + ) + # reaction wrench for base + _compute_base_reaction_wrench( + robo_tmp, symo, name, antRj,antPj, + vdot, F, N, Fex, Nex, Fjnt, Njnt + ) + for j in xrange(1, k + 1): + _compute_joint_torque(robo_tmp, symo, name, j, Fjnt, Njnt) + # reset all the parameters to zero + robo_tmp.put_inert_param(sympy.zeros(10, 1), k) + # compute model for the joint parameters + # avoid these parameters for link 0 + if k == 0: continue + _compute_joint_torque_deriv( + symo, robo.IA[k], robo.qddot[k], k + ) + _compute_joint_torque_deriv( + symo, robo.FS[k], sympy.sign(robo.qdot[k]), k + ) + _compute_joint_torque_deriv( + symo, robo.FV[k], robo.qdot[k], k + ) + return symo + + diff --git a/pysymoro/dynmodel.py b/pysymoro/dynmodel.py new file mode 100644 index 0000000..28daf52 --- /dev/null +++ b/pysymoro/dynmodel.py @@ -0,0 +1,834 @@ +# -*- coding: utf-8 -*- + + +""" +This module contains the functions and classes for the computation of +dynamic models (inverse and direct). +""" + + +import copy + +from sympy import Matrix +from sympy import sign + +from pysymoro.screw import Screw +from pysymoro.screw6 import Screw6 +from symoroutils.tools import skew + + +class DynModel(object): + """ + Data structure: + Hold the various components obtained during the computation of + dynamic models (inverse and direct). + """ + def __init__(self, joints, is_symbolic, model_type='inverse'): + """ + Constructor period. + + Args: + joints: An iterative object containing all the joint + numbers. This is usually the `joint_nums` attribute in + `Robot` class. + model_type: A string indicating whether the model + corresponds to inverse or direct dynamic model. + `inverse` the default option is used to indicate the + inverse dynamic model. Similarly, `direct` is used to + indicate direct dynamic model. + """ + # model type - inverse or dynamic + self.model_type = model_type + # symbolic or numeric + self.is_symboilc = is_symbolic + # link velocity + self.vels = list(None for j in joints) + # gyroscopic acceleration + self.gammas = list(None for j in joints) + # relative acceleration + self.zetas = list(None for j in joints) + # wrench - external+coriolis+centrifugal + self.betas = list(None for j in joints) + # link acceleration + self.accels = list(None for j in joints) + # reaction wrench + self.wrenchs = list(None for j in joints) + if self.model_type is 'inverse': + # composite spatial inertia matrix + self.composite_inertias = list(None for j in joints) + # composite wrench + self.composite_betas = list(None for j in joints) + # joint torque + self.torques = list(None for j in joints) + elif self.model_type is 'direct': + # similar to composite spatial inertia matrix + self.star_inertias = list(None for j in joints) + # similar to composite wrench + self.star_betas = list(None for j in joints) + # joint torques removing the effect of friction + self.taus = list(None for j in joints) + # wrench as a function tau (torque) + self.alphas = list(None for j in joints) + # inertial element corresponding to the joint in the spatial + # inertia matrix + self.joint_inertias = list(None for j in joints) + # spatial inertia matrix after eliminating the element + # corresponding to qddot - joint acceleration + self.no_qddot_inertias = list(None for j in joints) + # joint accelerations + self.qddots = list(None for j in joints) + + def __str__(self): + str_format = "" + # add header + str_format = str_format + "DynModel ({0}):\n".format( + self.model_type + ) + str_format = str_format + "-------------------\n" + # get all the attributes currently in the class + attrs = [ + attr for attr in dir(self) \ + if not attr.startswith('_') + ] + # add each attribute + for attr in attrs: + items = getattr(self, attr) + if hasattr(items, '__iter__'): + attr_str = self._str_items(items) + else: + attr_str = '\t' + str(items) + '\n' + str_format = str_format + str(attr) + ": \n" + str_format = str_format + attr_str + return str_format + + def __repr__(self): + return str(self) + + def _str_items(self, items): + """ + Create a string representation for a given attribute. + + Args: + items: An attribute of the class which is a list + + Returns: + A string representation of the attribute. + """ + row_format = '\t' + ('{0:^6} : {1}') + '\n' + str_format = "" + for idx, item in enumerate(items): + str_format = str_format + row_format.format(*( + str(idx), str(item) + )) + return str_format + + +def _init_composite_inertia(model, robo, j): + """ + Initialise the composite spatial inertia matrix of link j. + + Args: + model: An instance of DynModel + robo: An instance of Robot + j: link number + + Returns: + An instance of DynModel that contains all the new values. + """ + model.composite_inertias[j] = robo.dyns[j].spatial_inertia + return model + + +def _init_composite_beta(model, robo, j): + """ + Initialise the composite beta wrench of link j. + + Args: + model: An instance of DynModel + robo: An instance of Robot + j: link number + + Returns: + An instance of DynModel that contains all the new values. + """ + model.composite_betas[j] = model.betas[j] + return model + + +def _init_star_inertia(model, robo, j): + """ + Initialise the star spatial inertia matrix of link j. + + Args: + model: An instance of DynModel + robo: An instance of Robot + j: link number + + Returns: + An instance of DynModel that contains all the new values. + """ + model.star_inertias[j] = robo.dyns[j].spatial_inertia + return model + + +def _init_star_beta(model, robo, j): + """ + Initialise the star beta wrench of link j. + + Args: + model: An instance of DynModel + robo: An instance of Robot + j: link number + + Returns: + An instance of DynModel that contains all the new values. + """ + model.star_betas[j] = model.betas[j] + return model + + +def _compute_link_velocity(model, robo, j, i): + """ + Compute the velocity of link j whose antecedent is i. + + Args: + model: An instance of DynModel + robo: An instance of Robot + j: link number + i: antecendent value + + Returns: + An instance of DynModel that contains all the new values. + """ + j_v_j = Screw() + if i == 0: model.vels[i] = robo.base_vel + # local variables + j_s_i = robo.geos[j].tmat.s_i_wrt_j + qdot_j = robo.qdots[j] + j_a_j = robo.geos[j].axisa + i_v_i = model.vels[i].val + # actual computation + j_v_j.val = (j_s_i * i_v_i) + (qdot_j * j_a_j) + # store computed velocity in model + model.vels[j] = j_v_j + return model + + +def _compute_link_acceleration(model, robo, j, i): + """ + Compute the acceleration of link j whose antecedent is i. + + Args: + model: An instance of DynModel + robo: An instance of Robot + j: link number + i: antecendent value + + Returns: + An instance of DynModel that contains all the new values. + """ + j_vdot_j = Screw() + # local variables + j_s_i = robo.geos[j].tmat.s_i_wrt_j + i_vdot_i = model.accels[i].val + j_zeta_j = model.zetas[j].val + # actual computation + j_vdot_j.val = (j_s_i * i_vdot_i) + j_zeta_j + # store computed velocity in model + model.accels[j] = j_vdot_j + return model + + +def _compute_gyroscopic_acceleration(model, robo, j, i): + """ + Compute the gyroscopic acceleration of link j. + + Args: + model: An instance of DynModel + robo: An instance of Robot + j: link number + i: antecendent value + + Returns: + An instance of DynModel that contains all the new values. + """ + j_gamma_j = Screw() + # local variables + j_rot_i = robo.geos[j].tmat.inv_rot + i_trans_j = robo.geos[j].tmat.trans + i_omega_i = model.vels[i].ang + sigma_j = robo.geos[j].sigma + sigma_dash_j = 1 - sigma_j + j_z_j = robo.geos[j].zunit + qdot_j = robo.qdots[j] + # actual computation + j_omega_i = j_rot_i * i_omega_i + # term1 = i_omega_i x (i_omega_i x i_trans_j) + term1 = skew(i_omega_i) * (skew(i_omega_i) * i_trans_j) + # term2 = j_omega_i x (qdot_j * j_z_j) + term2 = skew(j_omega_i) * (qdot_j * j_z_j) + j_gamma_j.lin = (j_rot_i * term1) + (2 * sigma_j * term2) + j_gamma_j.ang = sigma_dash_j * term2 + # store computed acceleration in model + model.gammas[j] = j_gamma_j + return model + + +def _compute_relative_acceleration(model, robo, j): + """ + Compute the relative acceleration of link j. + + Args: + model: An instance of DynModel + robo: An instance of Robot + j: link number + + Returns: + An instance of DynModel that contains all the new values. + """ + j_zeta_j = Screw() + # local variables + j_a_j = robo.geos[j].axisa + j_gamma_j = model.gammas[j].val + if model.model_type is 'inverse': + qddot_j = robo.qddots[j] + elif model.model_type is 'direct': + qddot_j = model.qddots[j] + # actual computation + j_zeta_j.val = j_gamma_j + (qddot_j * j_a_j) + # store computed relative acceleration in model + model.zetas[j] = j_zeta_j + return model + + +def _compute_beta_wrench(model, robo, j): + """ + Compute the wrench for link j which combines the external forces, + Coriolis forces and centrifugal forces. + + Args: + model: An instance of DynModel + robo: An instance of Robot + j: link number + + Returns: + An instance of DynModel that contains all the new values. + """ + j_beta_j = Screw() + # local variables + j_omega_j = model.vels[j].ang + j_fe_j = robo.dyns[j].wrench.val + j_ms_j = robo.dyns[j].mass_tensor + j_inertia_j = robo.dyns[j].inertia + # actual computation + # lin_term = j_omega_j x (j_omega_j x j_ms_j) + lin_term = skew(j_omega_j) * (skew(j_omega_j) * j_ms_j) + # ang_term = j_omega_j x (j_inertia_j * j_omega_j) + ang_term = skew(j_omega_j) * (j_inertia_j * j_omega_j) + term = Screw(lin=lin_term, ang=ang_term) + j_beta_j.val = - j_fe_j - term.val + # store computed wrench in model + model.betas[j] = j_beta_j + return model + + +def _compute_composite_inertia(model, robo, j, i): + """ + Compute the composite spatial inertia matrix for link i. + + Args: + model: An instance of DynModel + robo: An instance of Robot + j: link number + i: antecedent value + + Returns: + An instance of DynModel that contains all the new values. + """ + i_inertia_i_c = Screw6() + # local variables + j_s_i = robo.geos[j].tmat.s_i_wrt_j + i_inertia_i = model.composite_inertias[i].val + j_inertia_j_c = model.composite_inertias[j].val + # actual computation + i_inertia_i_c.val = i_inertia_i + \ + (j_s_i.transpose() * j_inertia_j_c * j_s_i) + # store computed matrix in model + model.composite_inertias[i] = i_inertia_i_c + return model + + +def _compute_composite_beta(model, robo, j, i): + """ + Compute the composite beta wrench for link i. + + Args: + model: An instance of DynModel + robo: An instance of Robot + j: link number + i: antecedent value + + Returns: + An instance of DynModel that contains all the new values. + """ + i_beta_i_c = Screw() + # local variables + j_s_i = robo.geos[j].tmat.s_i_wrt_j + i_beta_i = model.composite_betas[i].val + j_beta_j_c = model.composite_betas[j].val + j_inertia_j_c = model.composite_inertias[j].val + j_zeta_j = model.zetas[j].val + # actual computation + i_beta_i_c.val = i_beta_i + (j_s_i.transpose() * j_beta_j_c) - \ + (j_s_i.transpose() * j_inertia_j_c * j_zeta_j) + # store computed beta in model + model.composite_betas[i] = i_beta_i_c + return model + + +def _compute_reaction_wrench(model, robo, j): + """ + Compute the reaction wrench for link j. + + Args: + model: An instance of DynModel + robo: An instance of Robot + j: link number + + Returns: + An instance of DynModel that contains all the new values. + """ + j_f_j = Screw() + # local variables + j_vdot_j = model.accels[j].val + j_inertia_j_c = model.composite_inertias[j].val + j_beta_j_c = model.composite_betas[j].val + # actual computation + j_f_j.val = (j_inertia_j_c * j_vdot_j) - j_beta_j_c + # store computed reaction wrench in model + model.wrenchs[j] = j_f_j + return model + + +def _compute_joint_torque(model, robo, j): + """ + Compute the joint torque for joint j. + + Args: + model: An instance of DynModel + robo: An instance of Robot + j: joint number + + Returns: + An instance of DynModel that contains all the new values. + """ + # local variables + qdot_j = robo.qdots[j] + qddot_j = robo.qddots[j] + j_a_j = robo.geos[j].axisa + ia_j = robo.dyns[j].ia + f_cj = robo.dyns[j].frc + f_vj = robo.dyns[j].frv + j_f_j = model.wrenchs[j].val + # actual computation + wrench_term = j_f_j.transpose() * j_a_j + actuator_inertia_term = Matrix([ia_j * qddot_j]) + coriolis_friction_term = Matrix([f_cj * sign(qdot_j)]) + viscous_friction_term = Matrix([f_vj * qdot_j]) + gamma_j = wrench_term + actuator_inertia_term + \ + viscous_friction_term + coriolis_friction_term + # store computed torque in model + model.torques[j] = gamma_j[0, 0] + return model + + +def _compute_joint_inertia(model, robo, j): + """ + Compute the element in the spatial inertia matrix corresponding to + the joint axis along with the effect of rotor inertia. + + Args: + model: An instance of DynModel + robo: An instance of Robot + j: joint number + + Returns: + An instance of DynModel that contains all the new values. + """ + h_j = 0 + # local variables + j_a_j = robo.geos[j].axisa + ia_j = Matrix([robo.dyns[j].ia]) + j_inertia_j_s = model.star_inertias[j].val + # actual computation + h_j = (j_a_j.transpose() * j_inertia_j_s * j_a_j) + ia_j + # store in model + model.joint_inertias[j] = h_j[0, 0] + return model + + +def _compute_no_qddot_inertia(model, robo, j): + """ + Compute the spatial inertia by eliminating the joint acceleration + effect. + + Args: + model: An instance of DynModel + robo: An instance of Robot + j: joint number + + Returns: + An instance of DynModel that contains all the new values. + """ + j_k_j = Screw6() + # local variables + j_a_j = robo.geos[j].axisa + j_inertia_j_s = model.star_inertias[j].val + h_j = Matrix([model.joint_inertias[j]]) + # actual computation + j_k_j.val = j_inertia_j_s - (j_inertia_j_s * j_a_j * h_j.inv() * \ + j_a_j.transpose() * j_inertia_j_s) + # store in model + model.no_qddot_inertias[j] = j_k_j + return model + + +def _compute_tau(model, robo, j): + """ + Compute the joint torque by subtracting the effect of friction. + + Args: + model: An instance of DynModel + robo: An instance of Robot + j: joint number + + Returns: + An instance of DynModel that contains all the new values. + """ + tau_j = 0 + # local variables + qdot_j = robo.qdots[j] + gamma_j = robo.torques[j] + f_cj = robo.dyns[j].frc + f_vj = robo.dyns[j].frv + # actual computation + coriolis_friction_term = f_cj * sign(qdot_j) + viscous_friction_term = f_vj * qdot_j + tau_j = gamma_j - coriolis_friction_term - viscous_friction_term + # store in model + model.taus[j] = tau_j + return model + + +def _compute_alpha_wrench(model, robo, j): + """ + Compute the wrench as a function of tau - joint torque without + friction. + + Args: + model: An instance of DynModel + robo: An instance of Robot + j: joint number + + Returns: + An instance of DynModel that contains all the new values. + """ + j_alpha_j = Screw() + # local variables + j_a_j = robo.geos[j].axisa + j_k_j = model.no_qddot_inertias[j].val + j_gamma_j = model.gammas[j].val + j_inertia_j_s = model.star_inertias[j].val + j_beta_j_s = model.star_betas[j].val + h_j = Matrix([model.joint_inertias[j]]) + tau_j = Matrix([model.taus[j]]) + # actual computation + j_alpha_j.val = (j_k_j * j_gamma_j) + \ + (j_inertia_j_s * j_a_j * h_j.inv() * \ + (tau_j + j_a_j.transpose() * j_beta_j_s)) - j_beta_j_s + # store in model + model.alphas[j] = j_alpha_j + return model + + +def _compute_star_inertia(model, robo, j, i): + """ + Compute the star spatial inertia matrix for link i. This matrix is + similar to the composite spatial inertia matrix. + + Args: + model: An instance of DynModel + robo: An instance of Robot + j: link number + i: antecedent value + + Returns: + An instance of DynModel that contains all the new values. + """ + i_inertia_i_s = Screw6() + # local variables + j_s_i = robo.geos[j].tmat.s_j_wrt_i + i_inertia_i = model.star_inertias[i].val + j_k_j = model.no_qddot_inertias[j].val + # actual computation + i_inertia_i_s.val = i_inertia_i + (j_s_i.transpose() * j_k_j * j_s_i) + # store in model + model.star_inertias[i] = i_inertia_i_s + return model + + +def _compute_star_beta(model, robo, j, i): + """ + Compute the star beta wrench for link i. This is similar to the + composite beta wrench but is a function of `tau` instead of `qddot`. + + Args: + model: An instance of DynModel + robo: An instance of Robot + j: link number + i: antecedent value + + Returns: + An instance of DynModel that contains all the new values. + """ + i_beta_i_s = Screw() + # local variables + j_s_i = robo.geos[j].tmat.s_j_wrt_i + i_beta_i = model.star_betas[i].val + j_alpha_j = model.alphas[j].val + # actual computation + i_beta_i_s.val = i_beta_i - (j_s_i.transpose() * j_alpha_j) + # store in model + model.star_betas[i] = i_beta_i_s + return model + + +def _compute_joint_acceleration(model, robo, j, i): + """ + Compute the joint acceleration for joint j. + + Args: + model: An instance of DynModel + robo: An instance of Robot + j: link number + i: antecedent value + + Returns: + An instance of DynModel that contains all the new values. + """ + # local variables + j_a_j = robo.geos[j].axisa + j_s_i = robo.geos[j].tmat.s_j_wrt_i + j_gamma_j = model.gammas[j].val + j_inertia_j_s = model.star_inertias[j].val + j_beta_j_s = model.star_betas[j].val + h_j = Matrix([model.joint_inertias[j]]) + tau_j = Matrix([model.taus[j]]) + i_vdot_i = model.accels[i].val + # actual computation + qddot_j = h_j.inv() * (-j_a_j.transpose() * j_inertia_j_s * \ + ((j_s_i * i_vdot_i) + j_gamma_j) + tau_j + \ + (j_a_j.transpose() * j_beta_j_s)) + # store in model + model.qddots[j] = qddot_j[0, 0] + return model + + +def _compute_reaction_wrench_alpha(model, robo, j, i): + """ + Compute the reaction wrench for link j as a function of alpha wrench. + + Args: + model: An instance of DynModel + robo: An instance of Robot + j: link number + i: antecedent value + + Returns: + An instance of DynModel that contains all the new values. + """ + j_f_j = Screw() + # local variables + j_s_i = robo.geos[j].tmat.s_j_wrt_i + j_k_j = model.no_qddot_inertias[j].val + j_alpha_j = model.alphas[j].val + i_vdot_i = model.accels[i].val + # actual computation + j_f_j.val = (j_k_j * j_s_i * i_vdot_i) + j_alpha_j + # store in model + model.wrenchs[j] = j_f_j + return model + + +def _compute_base_acceleration(model, robo): + """ + Compute the base acceleration for a robot with floating base without + and with taking gravity into account. In the case of a robot with + fixed base, this function returns just the effect of gravity as the + base acceleration. + + Args: + model: An instance of DynModel + robo: An instance of Robot + + Returns: + An instance of DynModel that contains all the new values. + """ + o_vdot_o = Screw() + gravity = Screw() + # local variables + gravity.lin = robo.gravity + if robo.is_floating: + if model.model_type is 'inverse': + o_inertia_o_c = model.composite_inertias[0].val + o_beta_o_c = model.composite_betas[0].val + elif model.model_type is 'direct': + o_inertia_o_c = model.star_inertias[0].val + o_beta_o_c = model.star_betas[0].val + # actual computation + # TODO: replace sympy's matrix inversion with custom function + o_vdot_o.val = o_inertia_o_c.inv() * o_beta_o_c + # store computed base acceleration without gravity effect in model + model.base_accel_w_gravity = copy.copy(o_vdot_o) + # compute base acceleration removing gravity effect + o_vdot_o.val = o_vdot_o.val - gravity.val + # store in model + model.accels[0] = o_vdot_o + return model + + +def inverse_dynamic_model(robo): + """ + Compute the inverse dynamic model for the given robot by using the + recursive Newton-Euler algorithm. + + Args: + robo: An instance of the FloatingRobot class. + + Returns: + The inverse dynamic model of the robot. + """ + # some book keeping variables + model = DynModel(robo.joint_nums, robo.is_symbolic, 'inverse') + # first forward recursion + for j in robo.joint_nums: + if j == 0: continue + # antecedent index + i = robo.geos[j].ant + # compute j^V_j : link velocity (6x1) + model = _compute_link_velocity(model, robo, j, i) + # compute j^gamma_j : gyroscopic acceleration (6x1) + model = _compute_gyroscopic_acceleration(model, robo, j, i) + # compute j^beta_j : external+coriolis+centrifugal wrench (6x1) + model = _compute_beta_wrench(model, robo, j) + # compute j^zeta_j : relative acceleration (6x1) + # TODO: check joint flexibility + model = _compute_relative_acceleration(model, robo, j) + # first backward recursion - initialisation step + for j in reversed(robo.joint_nums): + if j == 0: + # compute 0^beta_0 + model = _compute_beta_wrench(model, robo, j) + # initialise j^I_j^c : composite spatial inertia matrix + model = _init_composite_inertia(model, robo, j) + # initialise j^beta_j^c : composite wrench + model = _init_composite_beta(model, robo, j) + # second backward recursion - compute composite terms + for j in reversed(robo.joint_nums): + if j == 0: + # compute 0^\dot{V}_0 : base acceleration + # for fixed base robots, the value returned is just the + # effect of gravity + model = _compute_base_acceleration(model, robo) + continue + # antecedent index + i = robo.geos[j].ant + # compute i^I_i^c : composite spatial inertia matrix + model = _compute_composite_inertia(model, robo, j, i) + # compute i^beta_i^c : composite wrench + model = _compute_composite_beta(model, robo, j, i) + # second forward recursion + for j in robo.joint_nums: + if j == 0: continue + # antecedent index + i = robo.geos[j].ant + # compute j^\dot{V}_j : link acceleration + model = _compute_link_acceleration(model, robo, j, i) + # compute j^F_j : reaction wrench + model = _compute_reaction_wrench(model, robo, j) + # compute gamma_j : joint torque + model = _compute_joint_torque(model, robo, j) + return model + + +def direct_dynamic_model(robo): + """ + Compute the direct dynamic model for the given robot by using the + recursive Newton-Euler algorithm. + + Args: + robo: An instance of the FloatingRobot class. + + Returns: + The direct dynamic model of the robot. + """ + # some book keeping variables + model = DynModel(robo.joint_nums, robo.is_symbolic, 'direct') + # first forward recursion + for j in robo.joint_nums: + if j == 0: continue + # antecedent index + i = robo.geos[j].ant + # compute j^V_j : link velocity (6x1) + model = _compute_link_velocity(model, robo, j, i) + # compute j^gamma_j : gyroscopic acceleration (6x1) + model = _compute_gyroscopic_acceleration(model, robo, j, i) + # compute j^beta_j : external+coriolis+centrifugal wrench (6x1) + model = _compute_beta_wrench(model, robo, j) + # first backward recursion - initialisation step + for j in reversed(robo.joint_nums): + if j == 0: + # compute 0^beta_0 + model = _compute_beta_wrench(model, robo, j) + # initialise j^I_j^* : star spatial inertia matrix + model = _init_star_inertia(model, robo, j) + # initialise j^beta_j^* : star beta wrench + model = _init_star_beta(model, robo, j) + # second backward recursion - compute star terms + for j in reversed(robo.joint_nums): + if j == 0: continue + # antecedent index + i = robo.geos[j].ant + # compute H_j : joint inertia (scalar term) + model = _compute_joint_inertia(model, robo, j) + # compute j^K_j : inertia without the effect of qddot + model = _compute_no_qddot_inertia(model, robo, j) + # compute tau_j : torque removing the effect of friction params + model = _compute_tau(model, robo, j) + # compute j^alpha_j : wrench as a function of tau + model = _compute_alpha_wrench(model, robo, j) + # compute i^I_i^* : star spatial inertia matrix + model = _compute_star_inertia(model, robo, j, i) + # compute i^beta_i^* : star beta wrench + model = _compute_star_beta(model, robo, j, i) + # second forward recursion + for j in robo.joint_nums: + if j == 0: + # compute 0^\dot{V}_0 : base acceleration + # for fixed base robots, the value returned is just the + # effect of gravity + model = _compute_base_acceleration(model, robo) + continue + # antecedent index + i = robo.geos[j].ant + # compute qddot_j : joint acceleration + model = _compute_joint_acceleration(model, robo, j, i) + # compute j^F_j : reaction wrench as a function of alpha wrench + model = _compute_reaction_wrench_alpha(model, robo, j, i) + # compute j^zeta_j : relative acceleration + model = _compute_relative_acceleration(model, robo, j) + # compute j^V_j : link acceleration + model = _compute_link_acceleration(model, robo, j, i) + return model + + diff --git a/pysymoro/dynparams.py b/pysymoro/dynparams.py new file mode 100644 index 0000000..9d11db5 --- /dev/null +++ b/pysymoro/dynparams.py @@ -0,0 +1,238 @@ +# -*- coding: utf-8 -*- + + +""" +This module contains the DynParams data structure. +""" + + +import re + +from sympy import eye, var +from sympy import Matrix + +from pysymoro.screw import Screw +from pysymoro.screw6 import Screw6 +from symoroutils import tools + + +class DynParams(object): + """ + Data structure: + Represent the data structure to hold the inertial parameters, + friction parameters and the external forces for a given link. An + instance of the inertia matrix, the spatial inertia matrix and + the mass tensor term are also maintained. + Note: + Mass tensor refers to the MS 3x1 matrix which is the first moments + of a link wrt its own frame of reference. MS = transpose([MX MY MZ]) + """ + def __init__(self, link, params=None): + """ + Constructor period. + + Usage: + DynParams(link=) + DynParams(link=, params=) + """ + self.link = link + # Inertia matrix terms""" + self.xx = None + self.xy = None + self.xz = None + self.yy = None + self.yz = None + self.zz = None + # Mass tensor terms""" + self.msx = None + self.msy = None + self.msz = None + # Link mass""" + self.mass = None + # Rotor inertia term""" + self.ia = None + # Coulomb friction parameter""" + self.frc = None + # Viscous friction parameter""" + self.frv = None + # External forces and moments""" + self.fx_ext = None + self.fy_ext = None + self.fz_ext = None + self.mx_ext = None + self.my_ext = None + self.mz_ext = None + # lists to hold the string representation for the prefix of + # different terms + self._inertial_terms = { + 'xx': 'XX', + 'xy': 'XY', + 'xz': 'XZ', + 'yy': 'YY', + 'yz': 'YZ', + 'zz': 'ZZ' + } + self._ms_terms = { + 'msx': 'MX', + 'msy': 'MY', + 'msz': 'MZ', + 'mass': 'M' + } + self._fr_terms = { + 'ia': 'IA', + 'frc': 'FS', + 'frv': 'FV' + } + self._ext_force_terms = { + 'fx_ext': 'FX', + 'fy_ext': 'FY', + 'fz_ext': 'FZ', + 'mx_ext': 'CX', + 'my_ext': 'CY', + 'mz_ext': 'CZ' + } + # initialise the different parameters + self._init_inertial_terms() + self._init_ms_terms() + self._init_fr_terms() + self._init_ext_force_terms() + # initialise with values if available + if params is not None: + self.update_params(params) + + def __str__(self): + row_format = '\t' + ('{:^7}' * 20) + str_format = row_format.format(*( + str(self.link), + str(self.xx), str(self.xy), str(self.xz), + str(self.yy), str(self.yz), str(self.zz), + str(self.msx), str(self.msy), str(self.msz), str(self.mass), + str(self.ia), str(self.frc), str(self.frv), + str(self.fx_ext), str(self.fy_ext), str(self.fz_ext), + str(self.mx_ext), str(self.my_ext), str(self.mz_ext) + )) + return str_format + + def __repr__(self): + repr_format = str(self).lstrip().rstrip() + repr_format = re.sub(r'\s+', ', ', repr_format) + repr_format = '(' + repr_format + ')' + return repr_format + + def update_params(self, params): + """ + Update the dynamic parameter values. + + Args: + params: A dict in which the keys correspond to the list of + parameters that are to be updated and the values + correspond to the values with which the parameters are + to be updated. + """ + for key, value in params.iteritems(): + if hasattr(self, key): + setattr(self, key, value) + else: + raise AttributeError( + "%s is not an attribute of DynParams" % key + ) + + def set_to_zero(self): + """ + Set all the dynamic parameter values to zero. + """ + # attributes that shouldn't be set to zero + outliers = [ + 'link', 'update_params', 'inertia', 'mass_tensor', + 'spatial_inertia', 'wrench', 'force', 'moment' + ] + # get all the attributes of the class ignoring system attributes + attrs = [ + attr for attr in dir(self) \ + if not attr.startswith('_') + ] + for attr in attrs: + # ignore attributes in outliers + if attr in outliers: + continue + setattr(self, attr, 0) + + @property + def inertia(self): + """Get inertia (3x3) matrix.""" + return Matrix([ + [self.xx, self.xy, self.xz], + [self.xy, self.yy, self.yz], + [self.xz, self.yz, self.zz] + ]) + + @property + def mass_tensor(self): + """Get mass tensor (3x1) column vector.""" + return Matrix([self.msx, self.msy, self.msz]) + + @property + def spatial_inertia(self): + """Get spatial inertia (6x6) matrix.""" + m_eye = self.mass * eye(3) + ms_skew = tools.skew(self.mass_tensor) + return Screw6( + tl=m_eye, tr=ms_skew.transpose(), + bl=ms_skew, br=self.inertia + ) + + @property + def wrench(self): + """Get external force (6x1) column vector (linear + angular).""" + return Screw( + lin=Matrix([self.fx_ext, self.fy_ext, self.fz_ext]), + ang=Matrix([self.mx_ext, self.my_ext, self.mz_ext]) + ) + + @property + def force(self): + """Get external force (3x1) column vector (linear).""" + return self.wrench.lin + + @property + def moment(self): + """Get external moment (3x1) column vector (angular).""" + return self.wrench.ang + + def _init_inertial_terms(self): + """Initialise inertial terms.""" + for key, term in self._inertial_terms.iteritems(): + if self.link != 0: + value = var(term + str(self.link)) + else: + value = 0 + setattr(self, key, value) + + def _init_ms_terms(self): + """Initialise mass tensor terms and mass of the link.""" + for key, term in self._ms_terms.iteritems(): + if self.link != 0: + value = var(term + str(self.link)) + else: + value = 0 + setattr(self, key, value) + + def _init_fr_terms(self): + """Initialise rotor inertia and friction parameters.""" + for key, term in self._fr_terms.iteritems(): + if self.link != 0: + value = var(term + str(self.link)) + else: + value = 0 + setattr(self, key, value) + + def _init_ext_force_terms(self): + """Initialise external force terms.""" + for key, term in self._ext_force_terms.iteritems(): + if self.link != 0: + value = var(term + str(self.link)) + else: + value = 0 + setattr(self, key, value) + + diff --git a/pysymoro/geometry.py b/pysymoro/geometry.py new file mode 100644 index 0000000..e9bd791 --- /dev/null +++ b/pysymoro/geometry.py @@ -0,0 +1,529 @@ +# -*- coding: utf-8 -*- + + +# This file is part of the OpenSYMORO project. Please see +# https://github.com/symoro/symoro/blob/master/LICENCE for the licence. + + +""" +This module of SYMORO package computes the geometric models. +""" + + +from sympy import Matrix, zeros, eye, sin, cos +from copy import copy + +from symoroutils import symbolmgr +from symoroutils import tools +from symoroutils.paramsinit import ParamsInit + + +Z_AXIS = Matrix([0, 0, 1]) + + +class Transform(): + @classmethod + def sna(self, T): + """Extracts the s, n, a vector basis of rotation 3x3 matrix + from 4x4 transformation matrix + + Parameters + ========== + T: Matrix 4x4 + Transformation matrix + + Returns + ======= + s: Matrix 3x1 + n: Matrix 3x1 + a: Matrix 3x1 + """ + R = Transform.R(T) + return R.col(0), R.col(1), R.col(2) + + @classmethod + def R(self, T): + """Extracts rotation 3x3 matrix from 4x4 transformation matrix + + Parameters + ========== + T: Matrix 4x4 + Transformation matrix + + Returns + ======= + get_r: Matrix 3x3 + """ + return T[:3, :3] + + @classmethod + def P(self, T): + """Extracts translation vector from 4x4 transformation matrix + + Parameters + ========== + T: Matrix 4x4 + Transformation matrix + + Returns + ======= + get_p: Matrix 3x1 + """ + return T[:3, 3] + + @classmethod + def kRj(self, robo, antRj, k, chainj): + T = eye(3) + all_paral = True + for i in chainj: + if i > k: + T = antRj[i]*T + if antRj[i].col(2) != Z_AXIS and robo.ant[i] != 0: + all_paral = False + return T, all_paral + + @classmethod + def find_r12(self, robo, chainj, antRj, j): + r1 = robo.NL + r2 = robo.NL + rot12 = eye(3) + orthog = False + for i in reversed(chainj): + if robo.sigma[i] == 0: + rot12 *= antRj[i] + if r1 == robo.NL: + r1 = i + elif r2 == robo.NL and rot12.col(2) != Matrix([0, 0, 1]): + r2 = i + if Matrix([0, 0, 1]).dot(rot12.col(2)) == 0: + orthog = True + break + return r1, r2, orthog + + @classmethod + def z_paral(self, T): + return T.col(2) == Z_AXIS + + +class CompTransf: + def __init__(self, transform_type, axis, val, i=0, j=0, name=''): + self.type = transform_type + self.axis = axis + self.val = val + self.i = i + self.j = j + self.name = "%s%s" % (name, max(i, j)) + + def __str__(self): + axes = ('x', 'y', 'z') + trans_type = ('rot', 'trans') + return "%s(%s, %s)" % (trans_type[self.type], + axes[self.axis], self.val) + + def __repr__(self): + return str(self) + + def matrix(self): + """ + Homogeneous transformation matrix + """ + if self.type == 0: + return _rot_trans(self.axis, th=self.val) + elif self.type == 1: + return _rot_trans(self.axis, p=self.val) + + def matrix_inv(self): + """ + Homogeneous transformation matrix (inverted) + """ + if self.type == 0: + return _rot_trans(self.axis, th=-self.val) + elif self.type == 1: + return _rot_trans(self.axis, p=-self.val) + + def rot(self): + if self.type == 0: + return _rot(self.axis, self.val) + elif self.type == 1: + return eye(3) + + def trans(self): + if self.type == 1: + return _trans_vec(self.axis, self.val) + else: + return zeros(3, 1) + + +class TransConvolve: + def __init__(self, symo=None, trig_subs=False, simplify=True): + self.rot = CompTransf(0, 0, 0) + self.rot_mat = eye(3) + self.trans = zeros(3, 1) + self.symo = symo + self.trig_subs = trig_subs and symo is not None + self.T_tmp = eye(4) + self.simplify = simplify + + def process(self, tr): + if tr.type == 0: # rotation + if self.rot.axis == tr.axis and self.simplify: + self.rot.val += tr.val + self.rot.name += tr.name + else: # translation + self.rot_mat = self.rot_mat * self.rot.rot() + if self.trig_subs: + self.symo.trig_replace(self.rot_mat, self.rot.val, + self.rot.name) + self.rot = copy(tr) + elif tr.type == 1: + self.trans += self.rot_mat * self.rot.rot() * tr.trans() + if self.trig_subs: + self.symo.trig_replace(self.trans, self.rot.val, + self.rot.name) + + def process_left(self, tr): + if tr.type == 0: # rotation + self.trans = tr.rot() * self.trans + if self.trig_subs: + self.symo.trig_replace(self.trans, tr.val, tr.name) + if self.rot.axis == tr.axis: + self.rot.val += tr.val + self.rot.name += tr.name + else: # translation + self.rot_mat = self.rot.rot() * self.rot_mat + if self.trig_subs: + self.symo.trig_replace(self.rot_mat, self.rot.val, + self.rot.name) + self.rot = copy(tr) + elif tr.type == 1: + self.trans += tr.trans() + + def result(self, direction='right'): + if direction == 'right': + r = self.rot_mat * self.rot.rot() + elif direction == 'left': + r = self.rot.rot() * self.rot_mat + if self.trig_subs: + self.symo.trig_replace(self.trans, self.rot.val, self.rot.name) + self.symo.trig_replace(r, self.rot.val, self.rot.name) + return Matrix([r.row_join(self.trans), + [0, 0, 0, 1]]) + + +def transform_list(robo, i, j): + """ + Computes the chain of transformations for iTj + """ + _x = 0 + _z = 2 + k = robo.common_root(i, j) + chain1 = robo.chain(i, k) + chain2 = robo.chain(j, k) + chain2.reverse() + tr_list = [] + for indx in chain1: + ant = robo.ant[indx] + tr_list.append(CompTransf(0, _z, -robo.theta[indx], indx, ant)) + tr_list.append(CompTransf(1, _z, -robo.r[indx], indx, ant)) + tr_list.append(CompTransf(0, _x, -robo.alpha[indx], indx, ant, 'A')) + tr_list.append(CompTransf(1, _x, -robo.d[indx], indx, ant)) + tr_list.append(CompTransf(0, _z, -robo.gamma[indx], indx, ant, 'G')) + tr_list.append(CompTransf(1, _z, -robo.b[indx], indx, ant)) + for indx in chain2: + ant = robo.ant[indx] + tr_list.append(CompTransf(0, _z, robo.gamma[indx], ant, indx, 'G')) + tr_list.append(CompTransf(1, _z, robo.b[indx], ant, indx)) + tr_list.append(CompTransf(0, _x, robo.alpha[indx], ant, indx, 'A')) + tr_list.append(CompTransf(1, _x, robo.d[indx], ant, indx)) + tr_list.append(CompTransf(0, _z, robo.theta[indx], ant, indx)) + tr_list.append(CompTransf(1, _z, robo.r[indx], ant, indx)) + return [tr for tr in tr_list if tr.val != 0] + + +def to_matrix_fast(symo, tr_list, forced=False): + conv = TransConvolve(symo, trig_subs=True) + T = eye(4) + i = tr_list[0].i + j = tr_list[0].j + for tr in tr_list: + if tr.j != j: + T *= conv.result() + symo.mat_replace(T, 'T%sT%s' % (i, j), skip=1) + j = tr.j + conv = TransConvolve(symo, trig_subs=True) + conv.process(tr) + T *= conv.result() + symo.mat_replace(T, 'T%sT%s' % (i, j), skip=1, forced=forced) + return T + + +def to_matrix(tr_list, symo=None, trig_subs=False, simplify=True): + conv = TransConvolve(symo, trig_subs, simplify) + for tr in tr_list: + conv.process(tr) + return conv.result() + + +def to_matrices_right(tr_list, symo=None, trig_subs=False): + conv = TransConvolve(symo, trig_subs) + j = tr_list[0].j + i = tr_list[0].i + res = {(i, i): eye(4)} + for tr in tr_list: + if tr.j != j: + res[i, j] = conv.result() + j = tr.j + conv.process(tr) + res[i, j] = conv.result() + return res + + +def to_matrices_left(tr_list, symo=None, trig_subs=False): + conv = TransConvolve(symo, trig_subs) + j = tr_list[-1].j + i = tr_list[-1].i + res = {(j, j): eye(4)} + for tr in reversed(tr_list): + if tr.i != i: + res[i, j] = conv.result('left') + i = tr.i + conv.process_left(tr) + res[i, j] = conv.result('left') + return res + + +def dgm(robo, symo, i, j, key='one', fast_form=True, + trig_subs=True, forced=False): + """must be the final DGM function + + Parameters + ========== + symo: symbolmgr.SymbolManager + Instance of symbolmgr.SymbolManager. All the substitutions will + be put into symo.sydi + i: int + To-frame index. + j: int + From-frame index. + key: {'one','left','right'} + Defines whether return just one transform or all the chain + with multiplication from left and right + fast_form: bool, optional + If False, result will be in unfolded mode (triginimetric + substitutions only) + trig_subs: bool, optional + If True, all the sin(x) and cos(x) will be replaced by symbols + SX and CX with adding them to the dictionary + + """ + if i == j: + if key == 'one': + return eye(4) + else: + return {(i, i): eye(4)} + tr_list = transform_list(robo, i, j) + if key == 'one' and fast_form: + return to_matrix_fast(symo, tr_list, forced) + else: + if key == 'left': + return to_matrices_left(tr_list, symo, trig_subs) + elif key == 'right': + return to_matrices_right(tr_list, symo, trig_subs) + elif key == 'one': + return to_matrix(tr_list, symo, trig_subs) + + +def _transform(robo, j, invert=False): + """Transform matrix between frames j and ant[j] + + Parameters + ========== + j: int + Frame index. + invert: bool, optional + Defines the transformation direction + + Returns + ======= + transform: Matrix 4x4 + Transformation matrix. If invert is True then j_T_ant, + else ant_T_j. + """ + if not invert: + R1 = _rot_trans(2, robo.gamma[j], robo.b[j]) + R2 = _rot_trans(0, robo.alpha[j], robo.d[j]) + R3 = _rot_trans(2, robo.theta[j], robo.r[j]) + return R1*R2*R3 + else: + R1 = _rot_trans(2, -robo.gamma[j], -robo.b[j]) + R2 = _rot_trans(0, -robo.alpha[j], -robo.d[j]) + R3 = _rot_trans(2, -robo.theta[j], -robo.r[j]) + return R3*R2*R1 + + +def compute_transform(robo, symo, j, antRj, antPj): + """Internal function. Computes rotation matrix and translation vector + of ant_T_j homogenuous transform. Does the trigonometric subsctitution + and saves the symbols into symo.sydi + + Notes + ===== + antPj and antRj are the output parameters + """ + antTj = _transform(robo, j) + for angle, name in robo.get_angles(j): + antTj = symo.trig_replace(antTj, angle, name) + antRj[j] = symo.mat_replace(Transform.R(antTj), 'A', j) + antPj[j] = symo.mat_replace(Transform.P(antTj), 'L', j) + + +def compute_screw_transform(robo, symo, j, antRj, antPj, jTant): + """Internal function. Computes the screw transformation matrix + between ant[j] and j frames. + + Notes + ===== + jTant is an output parameter + """ + jRant = antRj[j].T + ET = symo.mat_replace(-jRant*tools.skew(antPj[j]), 'JPR', j) + jTant[j] = (Matrix([jRant.row_join(ET), + zeros(3, 3).row_join(jRant)])) + + +def _rot(axis=2, th=0): + """Rotation matrix about axis + + Parameters + ========== + axis: {0, 1, 2} + Rotation axis + th: var + Rotation angle + + Returns + ======= + rot: Matrix 3x3 + """ + assert axis in {0, 1, 2} + if axis == 0: + return Matrix([[1, 0, 0], + [0, cos(th), - sin(th)], + [0, sin(th), cos(th)]]) + elif axis == 1: + return Matrix([[cos(th), 0, sin(th)], + [0, 1, 0], + [-sin(th), 0, cos(th)]]) + else: + return Matrix([[cos(th), - sin(th), 0], + [sin(th), cos(th), 0], + [0, 0, 1]]) + + +def _trans_vec(axis=2, p=0): + """Translation vector along axis + + Parameters + ========== + axis: {0, 1, 2} + Translation axis + p: var + Translation distance + + Returns + ======= + v: Matrix 3x1 + """ + assert axis in {0, 1, 2} + v = zeros(3, 1) + v[axis] = p + return v + + +def _rot_trans(axis=2, th=0, p=0): + """Transformation matrix with rotation about and + translation along axis + + Parameters + ========== + axis: {'x', 'y', 'z'} + Transformation axis + p: var + Translation distance + th: var + Rotation angle + + Returns + ======= + rot_trans: Matrix 4x4 + """ + assert axis in {0, 1, 2} + return Matrix([_rot(axis, th).row_join(_trans_vec(axis, p)), + [0, 0, 0, 1]]) + + +def compute_rot_trans(robo, symo): + #init transformation + antRj = ParamsInit.init_mat(robo) + antPj = ParamsInit.init_vec(robo) + for j in xrange(robo.NL): + compute_transform(robo, symo, j, antRj, antPj) + return antRj, antPj + + +#TODO: validate for different structures +def direct_geometric_fast(robo, i, j): + """Computes trensformation matrix iTj. + + Parameters + ========== + robo: Robot + Instance of robot description container + i: int + the to-frame + j: int + the from-frame + + Returns + ======= + symo: symbolmgr.SymbolManager + Instance that contains all the relations of the computed model + """ + symo = symbolmgr.SymbolManager() + symo.file_open(robo, 'fgm') + symo.write_params_table(robo, 'Direct Geometric model') + dgm(robo, symo, i, j, fast_form=True, forced=True) + symo.file_close() + return symo + + +def direct_geometric(robo, frames, trig_subs): + """Computes trensformation matrix iTj. + + Parameters + ========== + robo: Robot + Instance of robot description container + frames: list of tuples of type (i,j) + Defines list of required transformation matrices iTj + trig_subs: bool, optional + If True, all the sin(x) and cos(x) will be replaced by symbols + SX and CX with adding them to the dictionary + + Returns + ======= + symo: symbolmgr.SymbolManager + Instance that contains all the relations of the computed model + """ + symo = symbolmgr.SymbolManager() + symo.file_open(robo, 'trm') + symo.write_params_table(robo, 'Direct Geometric model') + for i, j in frames: + symo.write_line('Tramsformation matrix %s T %s' % (i, j)) + T = dgm(robo, symo, i, j, fast_form=False, trig_subs=trig_subs) + symo.mat_replace(T, 'T%sT%s' % (i, j), forced=True, skip=1) + symo.write_line() + symo.file_close() + return symo + + diff --git a/pysymoro/geoparams.py b/pysymoro/geoparams.py new file mode 100644 index 0000000..6ce5d0a --- /dev/null +++ b/pysymoro/geoparams.py @@ -0,0 +1,128 @@ +# -*- coding: utf-8 -*- + + +""" +This module contains the GeoParams data structure. +""" + + +import re + +from sympy import Matrix + +from pysymoro import transform + + +class GeoParams(object): + """ + Data structure: + Represent the data structure to hold the geometric parameters. + """ + def __init__(self, frame, params=None): + """ + Constructor period. + + Note: + By default the antecedent is selected as the previous frame + (j-1). This would be automatically updated when the update() + method is called with the correct parameters. + + Usage: + GeoParams(frame=) + GeoParams(frame=, params=) + """ + self.frame = frame + self.ant = frame - 1 + self.sigma = 0 if frame != 0 else 2 + self.mu = 0 + self.gamma = 0 + self.b = 0 + self.alpha = 0 + self.d = 0 + self.theta = 0 + self.r = 0 + self.tmat = transform.TransformationMatrix( + i=self.ant, j=self.frame + ) + # initialise with values if available + if params is not None: + self.update_params(params) + + def __str__(self): + row_format = '\t' + ('{:^8}' * 11) + str_format = row_format.format(*( + str(self.frame), str(self.ant), + str(self.sigma), str(self.mu), + str(self.gamma), str(self.b), + str(self.alpha), str(self.d), + str(self.theta), str(self.r), + str(self.q) + )) + return str_format + + def __repr__(self): + repr_format = str(self).lstrip().rstrip() + repr_format = re.sub(r'\s+', ', ', repr_format) + repr_format = '(' + repr_format + ')' + return repr_format + + def update_params(self, params): + """ + Update the geometric parameter values. + + Args: + params: A dict in which the keys correspond to the list of + parameters that are to be updated and the values + correspond to the values with which the parameters are + to be updated. + """ + for key, value in params.iteritems(): + if hasattr(self, key): + setattr(self, key, value) + else: + raise AttributeError( + "%s is not an attribute of GeoParams" % key + ) + self.tmat.update(params) + + @property + def zunit(self): + """ + Get the unit vector along the z-axis which is the joint axis. + + Returns: + A (3x1) Matrix. + """ + if self.sigma != 2: + return Matrix([0, 0, 1]) + else: + return Matrix([0, 0, 0]) + + @property + def axisa(self): + """ + Get the joint axis in screw form. + + Returns: + A (6x1) Matrix. + """ + if self.sigma != 2: + return Matrix([0, 0, self.sigma, 0, 0, (1 - self.sigma)]) + else: + return Matrix([0, 0, 0, 0, 0, 0]) + + @property + def q(self): + """ + Get the joint variable value. + + Returns: + The value of theta for a revolute joint and in the case of + prismatic joint the value of r. For a fixed joint 0 is + returned. + """ + if self.sigma == 2: + return 0 + return ((1 - self.sigma) * self.theta) + (self.sigma * self.r) + + diff --git a/pysymoro/gui/ui_definition.py b/pysymoro/gui/ui_definition.py deleted file mode 100644 index 8edc77c..0000000 --- a/pysymoro/gui/ui_definition.py +++ /dev/null @@ -1,185 +0,0 @@ -__author__ = 'Izzat' -import wx -import wx.lib.scrolledpanel as scrolled -from core.symoro import SIMPLE, TREE, CLOSED_LOOP -from sympy import Expr, Symbol -#TODO: PROG_NAME - - -class DialogConversion(wx.Dialog): - def __init__(self, prefix, robo, par_dict, parent=None): - super(DialogConversion, self).__init__(parent) - self.robo = robo - self.par_dict = par_dict - self.SetTitle(prefix + ": Enter numerical values") - self.construct_sym() - self.init_ui() - - def has_syms(self): - return len(self.syms) > 0 - - def construct_sym(self): - params = self.robo.get_geom_head()[4:] - q_vec = self.robo.q_vec - self.syms = set() - for par in params: - for i in range(1, self.robo.NF): - val = self.robo.get_val(i, par) - if val in q_vec: - continue - if isinstance(val, Expr): - for at in val.atoms(Symbol): - self.syms.add(at) - - def init_ui(self): - p = scrolled.ScrolledPanel(self, -1) - vbox = wx.BoxSizer(wx.VERTICAL) - self.widgets = {} - for par in self.syms: - if par in self.par_dict: - val = str(self.par_dict[par]) - else: - val = 1. - hor_sizer = wx.BoxSizer(wx.HORIZONTAL) - label = wx.StaticText(p, label=str(par), size=(60, -1), - style=wx.ALIGN_RIGHT) - hor_sizer.Add(label, 0, wx.ALIGN_CENTER_VERTICAL) - hor_sizer.AddSpacer(5) - txt_box = wx.TextCtrl(p, size=(120, -1), value=str(val)) - hor_sizer.Add(txt_box) - vbox.Add(hor_sizer, 0, wx.ALL | wx.ALIGN_CENTER_HORIZONTAL, 5) - self.widgets[str(par)] = txt_box - - p.SetSizer(vbox) - p.SetAutoLayout(1) - p.SetupScrolling() - - main_sizer = wx.BoxSizer(wx.VERTICAL) - main_sizer.Add(p, 1, wx.ALL | wx.EXPAND, 2) - hor_sizer = wx.BoxSizer(wx.HORIZONTAL) - ok_btn = wx.Button(self, wx.ID_OK, "OK") - ok_btn.Bind(wx.EVT_BUTTON, self.OnOK) - cancel_btn = wx.Button(self, wx.ID_CANCEL, "Cancel") - cancel_btn.Bind(wx.EVT_BUTTON, self.OnCancel) - hor_sizer.Add(ok_btn, 0, wx.ALL, 15) - hor_sizer.Add(cancel_btn, 0, wx.ALL, 15) - main_sizer.Add(hor_sizer, 0, wx.ALIGN_CENTER_HORIZONTAL, 0) - self.SetSizer(main_sizer) - #if n_syms == 0: - # self.EndModal(wx.ID_OK) - - def OnOK(self, _): - self.EndModal(wx.ID_OK) - - def OnCancel(self, _): - self.EndModal(wx.ID_CANCEL) - - def get_values(self): - result = {} - for par in self.syms: - result[par] = self.widgets[str(par)].Value - return result - - -class DialogDefinition(wx.Dialog): - def __init__(self, prefix, name, nl, nj, structure, is_mobile, parent=None): - super(DialogDefinition, self).__init__(parent, style=wx.SYSTEM_MENU | - wx.CAPTION | wx.CLOSE_BOX | wx.CLIP_CHILDREN) - self.init_ui(name, nl, nj, is_mobile, structure) - self.SetTitle(prefix + ": New robot definition") - - def init_ui(self, name, nl, nj, is_mobile, structure): - main_sizer = wx.BoxSizer(wx.VERTICAL) - - #title - label_main = wx.StaticText(self, label="Robot definition") - main_sizer.Add(label_main, 0, wx.ALL | wx.ALIGN_CENTER_HORIZONTAL, 25) - - #grid - grid = wx.GridBagSizer(15, 15) - grid.Add(wx.StaticText(self, label='Name of the robot:'), pos=(0, 0), - flag=wx.BOTTOM | wx.ALIGN_LEFT, border=2) - grid.Add(wx.TextCtrl(self, size=(92, -1), name='name', value=name), - pos=(0, 1)) - label = wx.StaticText(self, label='Number of moving links:') - grid.Add(label, pos=(1, 0), - flag=wx.BOTTOM | wx.TOP | wx.ALIGN_LEFT, border=2) - self.spin_links = wx.SpinCtrl(self, size=(92, -1), - value=str(nl), min=1) - self.spin_links.Bind(wx.EVT_SPINCTRL, self.OnSpinNL) - grid.Add(self.spin_links, pos=(1, 1)) - - label = wx.StaticText(self, label='Number of joints:') - grid.Add(label, pos=(2, 0), - flag=wx.BOTTOM | wx.TOP | wx.ALIGN_LEFT, border=2) - self.spin_joints = wx.SpinCtrl(self, size=(92, -1), - value=str(nj), min=0) - grid.Add(self.spin_joints, pos=(2, 1)) - - grid.Add(wx.StaticText(self, label='Type of structure'), pos=(3, 0), - flag=wx.BOTTOM | wx.TOP | wx.ALIGN_LEFT, border=2) - self.cmb_structure = wx.ComboBox(self, size=(92, -1), - name='structure', style=wx.CB_READONLY, - choices=[SIMPLE, TREE, CLOSED_LOOP], - value=structure) - grid.Add(self.cmb_structure, pos=(3, 1)) - self.cmb_structure.Bind(wx.EVT_COMBOBOX, self.OnTypeChanged) - self.OnTypeChanged(None) - - main_sizer.Add(grid, 0, wx.ALL | wx.ALIGN_CENTER_HORIZONTAL, 15) - self.ch_is_mobile = wx.CheckBox(self, label=' Is mobile') - self.ch_is_mobile.Value = is_mobile - self.ch_keep_geo = wx.CheckBox(self, label=' Keep geometric parameters') - self.ch_keep_geo.Value = True - self.ch_keep_dyn = wx.CheckBox(self, label=' Keep dynamic parameters') - self.ch_keep_dyn.Value = True - self.ch_keep_base = wx.CheckBox(self, label=' Keep base parameters') - self.ch_keep_base.Value = True - main_sizer.Add(self.ch_is_mobile, 0, - wx.LEFT | wx.RIGHT | wx.ALIGN_LEFT, 15) - main_sizer.Add(self.ch_keep_geo, 0, - wx.TOP | wx.LEFT | wx.ALIGN_LEFT, 15) - main_sizer.Add(self.ch_keep_dyn, 0, - wx.TOP | wx.LEFT | wx.ALIGN_LEFT, 15) - main_sizer.Add(self.ch_keep_base, 0, - wx.TOP | wx.LEFT | wx.ALIGN_LEFT, 15) - hor_sizer = wx.BoxSizer(wx.HORIZONTAL) - ok_btn = wx.Button(self, wx.ID_OK, "OK") - ok_btn.Bind(wx.EVT_BUTTON, self.OnOK) - cancel_btn = wx.Button(self, wx.ID_CANCEL, "Cancel") - cancel_btn.Bind(wx.EVT_BUTTON, self.OnCancel) - hor_sizer.Add(ok_btn, 0, wx.ALL, 25) - hor_sizer.Add(cancel_btn, 0, wx.ALL, 25) - main_sizer.Add(hor_sizer) - - self.SetSizerAndFit(main_sizer) - - def OnOK(self, _): - self.EndModal(wx.ID_OK) - - def OnCancel(self, _): - self.EndModal(wx.ID_CANCEL) - - def OnTypeChanged(self, _): - if self.cmb_structure.GetSelection() == 2: - self.spin_joints.Enable(True) - else: - self.spin_joints.Enable(False) - self.OnSpinNL(None) - - def OnSpinNL(self, _): - self.spin_joints.SetRange(int(self.spin_links.Value), 100) - if self.cmb_structure.GetSelection() != 2: - self.spin_joints.Value = self.spin_links.Value - - def get_values(self): - name = self.FindWindowByName('name').Value - nl = int(self.spin_links.Value) - nj = int(self.spin_joints.Value) - return {'init_pars': (name, nl, nj, 2*nj - nl, - self.ch_is_mobile.Value, - self.cmb_structure.Value), - 'keep_geo': self.ch_keep_geo.Value, - 'keep_dyn': self.ch_keep_dyn.Value, - 'keep_base': self.ch_keep_base.Value} - diff --git a/pysymoro/inertia.py b/pysymoro/inertia.py new file mode 100644 index 0000000..1ee0d0f --- /dev/null +++ b/pysymoro/inertia.py @@ -0,0 +1,240 @@ +# -*- coding: utf-8 -*- + + +# This file is part of the OpenSYMORO project. Please see +# https://github.com/symoro/symoro/blob/master/LICENCE for the licence. + + +""" +This module contains the functions for the computation of Inertia +matrix. +""" + + +import sympy +from sympy import Matrix + +from pysymoro.geometry import compute_rot_trans +from symoroutils.paramsinit import ParamsInit +from symoroutils import tools + + +CHARSYMS = ( + 'a', 'b', 'c', 'd', 'e', 'f', 'g', 'h', 'j', 'k', 'l', 'm', 'n', + 'p', 'q', 'r', 's', 't', 'u', 'v', 'w', 'x', 'y', 'z', 'aa', 'ab', + 'ac', 'ad', 'ae', 'af', 'ag', 'ah', 'aj', 'ak', 'al', 'am', 'an', + 'ap', 'aq', 'ar', 'as', 'at', 'au', 'av', 'aw', 'ax', 'ay', 'az', + 'ba', 'bb', 'bc', 'bd', 'be', 'bf', 'bg', 'bh', 'bj', 'bk', 'bl', + 'bm', 'bn', 'bp', 'bq', 'br', 'bs', 'bt', 'bu', 'bv', 'bw', 'bx', + 'by', 'bz' +) + + +def inertia_spatial(inertia, ms_tensor, mass): + """ + Setup spatial inertia matrix (internal function). + """ + return Matrix([ + (mass * sympy.eye(3)).row_join(tools.skew(ms_tensor).transpose()), + tools.skew(ms_tensor).row_join(inertia) + ]) + + +def replace_composite_terms( + symo, j, comp_inertia3, comp_ms, comp_mass +): + """ + Replace composite inertia terms (internal function). + + Note: + comp_inertia3, comp_ms, comp_mass are the output parameters. + """ + comp_inertia3[j] = symo.mat_replace(comp_inertia3[j], 'JP', j) + comp_ms[j] = symo.mat_replace(comp_ms[j], 'MSP', j) + comp_mass[j] = symo.replace(comp_mass[j], 'MP', j) + + +def compute_composite_inertia( + robo, symo, j, antRj, antPj, + aje1, comp_inertia3, comp_ms, comp_mass +): + """ + Compute composite inertia (internal function). + + Note: + aje1, comp_inertia3, comp_ms, comp_mass are the output + parameters. + """ + i = robo.ant[j] + i_ms_j_c = antRj[j] * comp_ms[j] + i_ms_j_c = symo.mat_replace(i_ms_j_c, 'AS', j) + expr1 = antRj[j] * comp_inertia3[j] + expr1 = symo.mat_replace(expr1, 'AJ', j) + aje1[j] = expr1[:, 2] + expr2 = expr1 * antRj[j].transpose() + expr2 = symo.mat_replace(expr2, 'AJA', j) + expr3 = tools.skew(antPj[j]) * tools.skew(i_ms_j_c) + expr3 = symo.mat_replace(expr3, 'PAS', j) + comp_inertia3[i] += expr2 - (expr3 + expr3.transpose()) + \ + (comp_mass[j] * tools.skew(antPj[j]) * \ + tools.skew(antPj[j]).transpose()) + comp_ms[i] = comp_ms[i] + i_ms_j_c + (antPj[j] * comp_mass[j]) + comp_mass[i] = comp_mass[i] + comp_mass[j] + + +def compute_diagonal_elements( + robo, symo, j, comp_inertia3, comp_ms, comp_mass, + forces, moments, inertia_a22 +): + """ + Compute diagonal elements of the inertia matrix (internal function). + + Note: + forces, moments, inertia_a22 are the output parameters + """ + if robo.sigma[j] == 0: + forces[j] = Matrix([-comp_ms[j][1], comp_ms[j][0], 0]) + moments[j] = comp_inertia3[j][:, 2] + inertia_a22[j-1, j-1] = comp_inertia3[j][2, 2] + robo.IA[j] + elif robo.sigma[j] == 1: + forces[j] = Matrix([0, 0, comp_mass[j]]) + moments[j] = Matrix([comp_ms[j][1], -comp_ms[j][0], 0]) + inertia_a22[j-1, j-1] = comp_mass[j] + robo.IA[j] + forces[j] = symo.mat_replace(forces[j], 'E' + CHARSYMS[j], j) + moments[j] = symo.mat_replace(moments[j], 'N' + CHARSYMS[j], j) + + +def compute_triangle_elements( + robo, symo, j, k, ka, antRj, antPj, aje1, + forces, moments, inertia_a12, inertia_a22 +): + """ + Compute elements below and above diagonal of the inertia matrix + (internal function). + + Note: + forces, moments, inertia_a12, inertia_a22 are the output + parameters + """ + forces[ka] = antRj[k] * forces[k] + if k == j and robo.sigma[j] == 0: + moments[ka] = aje1[k] + \ + (tools.skew(antPj[k]) * antRj[k] * forces[k]) + else: + moments[ka] = (antRj[k] * moments[k]) + \ + (tools.skew(antPj[k]) * antRj[k] * forces[k]) + if ka == 0: + inertia_a12[j][:3, 0] = symo.mat_replace( + forces[ka], 'AV0', j, forced=True + ) + inertia_a12[j][3:, 0] = symo.mat_replace( + moments[ka], 'AW0', j, forced=True + ) + else: + symo.mat_replace(forces[ka], 'E' + CHARSYMS[j], ka) + symo.mat_replace(moments[ka], 'N' + CHARSYMS[j], ka) + if robo.sigma[ka] == 0: + inertia_a22[j-1, ka-1] = moments[ka][2] + elif robo.sigma[ka] == 1: + inertia_a22[j-1, ka-1] = forces[ka][2] + inertia_a22[ka-1, j-1] = inertia_a22[j-1, ka-1] + + +def fixed_inertia_matrix(robo, symo): + """ + Compute Inertia Matrix for robots with fixed base. This function + computes just the A22 matrix when the inertia matrix + A = [A11, A12; A12.transpose(), A22]. + """ + # init terms + comp_inertia3, comp_ms, comp_mass = ParamsInit.init_jplus(robo) + aje1 = ParamsInit.init_vec(robo) + forces = ParamsInit.init_vec(robo, ext=1) + moments = ParamsInit.init_vec(robo, ext=1) + inertia_a12 = ParamsInit.init_vec(robo, num=6) + inertia_a22 = sympy.zeros(robo.nl, robo.nl) + # init transformation + antRj, antPj = compute_rot_trans(robo, symo) + for j in reversed(xrange(1, robo.NL)): + replace_composite_terms( + symo, j, comp_inertia3, comp_ms, comp_mass + ) + if j != 1: + compute_composite_inertia( + robo, symo, j, antRj, antPj, + aje1, comp_inertia3, comp_ms, comp_mass + ) + for j in xrange(1, robo.NL): + compute_diagonal_elements( + robo, symo, j, comp_inertia3, comp_ms, + comp_mass, forces, moments, inertia_a22 + ) + ka = j + while ka != 1: + k = ka + ka = robo.ant[ka] + compute_triangle_elements( + robo, symo, j, k, ka, antRj, antPj, aje1, + forces, moments, inertia_a12, inertia_a22 + ) + symo.mat_replace(inertia_a22, 'A', forced=True, symmet=True) + return inertia_a22 + + +def floating_inertia_matrix(robo, symo): + """ + Compute Inertia Matrix for robots with floating or mobile base. This + function computes the A11, A12 and A22 matrices when the inertia + matrix A = [A11, A12; A12.transpose(), A22] + """ + # init terms + comp_inertia3, comp_ms, comp_mass = ParamsInit.init_jplus(robo) + aje1 = ParamsInit.init_vec(robo) + forces = ParamsInit.init_vec(robo, ext=1) + moments = ParamsInit.init_vec(robo, ext=1) + inertia_a12 = ParamsInit.init_vec(robo, num=6) + inertia_a22 = sympy.zeros(robo.nl, robo.nl) + # init transformation + antRj, antPj = compute_rot_trans(robo, symo) + for j in reversed(xrange(0, robo.NL)): + replace_composite_terms( + symo, j, comp_inertia3, comp_ms, comp_mass + ) + if j != 0: + compute_composite_inertia( + robo, symo, j, antRj, antPj, + aje1, comp_inertia3, comp_ms, comp_mass + ) + for j in xrange(1, robo.NL): + compute_diagonal_elements( + robo, symo, j, comp_inertia3, comp_ms, + comp_mass, forces, moments, inertia_a22 + ) + ka = j + while ka != 0: + k = ka + ka = robo.ant[ka] + compute_triangle_elements( + robo, symo, j, k, ka, antRj, antPj, aje1, + forces, moments, inertia_a12, inertia_a22 + ) + symo.mat_replace(inertia_a22, 'A', forced=True, symmet=True) + inertia_a11 = inertia_spatial( + comp_inertia3[0], comp_ms[0], comp_mass[0] + ) + inertia_a11 = symo.mat_replace( + inertia_a11, 'Jcomp', 0, forced=True, symmet=True + ) + # setup inertia_a12 in Matrix form + a12mat = sympy.zeros(6, robo.NL) + for j in xrange(1, robo.NL): + a12mat[:, j] = inertia_a12[j] + a12mat = a12mat[:, 1:] + # setup the complete inertia matrix + inertia = Matrix([ + inertia_a11.row_join(a12mat), + a12mat.transpose().row_join(inertia_a22) + ]) + return inertia + + diff --git a/pysymoro/invdata.py b/pysymoro/invdata.py new file mode 100644 index 0000000..ee0bb4f --- /dev/null +++ b/pysymoro/invdata.py @@ -0,0 +1,1148 @@ +# -*- coding: utf-8 -*- + + +# This file is part of the OpenSYMORO project. Please see +# https://github.com/symoro/symoro/blob/master/LICENCE for the licence. + + +""" +This module of SYMORO package provides symbolic +solutions for inverse geometric problem using Pieper Method. +""" + + +from sympy import var, sin, cos, eye, atan2, atan, sqrt, pi +from sympy import Matrix, Symbol, Expr, trigsimp, zeros +from numpy import dot, array +from numpy.linalg import inv + +from pysymoro.geometry import dgm, _rot, _trans_vec, _rot_trans +from symoroutils import tools + +EMPTY = var("EMPTY") + +T_GENERAL = Matrix([var('sx,nx,ax,px'), + var('sy,ny,ay,py'), + var('sz,nz,az,pz'), + [0, 0, 0, 1]]) + + +def sin_alphaj_eq_0(robo, symo, X_joint, tc, ts, tr, t0, G, offset): + """ + Function that finds the symbolic solutions of the X joints. + (Case: Sin(alphaj) == 0) + Parameters: + =========== + 1) X_joints: Type of the X joints + 2) tc, ts, tr, t0: Coefficients of eq. 1.25 + 3) G: G = [Gx; Gy; Gz] -> Vector with constant values + """ + # eq 1.31-1.34 + # i and j revolute joints + [i,j,k] = X_joint + [x,y,z] = [0,1,2] + [offseti, offsetj, offsetk] = offset + symo.write_line("# X joints i and j are both revolute") + symo.write_line("# Case: sin(alpha{0}) = 0".format(j) + "\r\n") + + # Solve qk + if robo.sigma[k] == 0: + qk = robo.theta[k] + eq_type = 3 + t1 = tr*robo.r[k] + t0 + t1 = symo.replace(trigsimp(t1), 't1', qk) + + el1 = cos(robo.alpha[j])*tc[z] + el2 = cos(robo.alpha[j])*ts[z] + el3 = cos(robo.alpha[j])*t1[z] - G[z] + coef = [el1,el2,el3] + else: + qk = robo.r[k] + eq_type = 1 + t2 = tc*cos(robo.theta[k]) + ts*sin(robo.theta[k]) + t0 + t2 = symo.replace(trigsimp(t2), 't2', qk) + + el1 = cos(robo.alpha[j])*tr[z] + el2 = cos(robo.alpha[j])*t2[z] - G[z] + coef = [el1,el2] + _equation_solve(symo,coef,eq_type,qk,offsetk) + + if robo.sigma[k] == 0: + F = tc*cos(qk) + ts*sin(qk) + t1 + else: + F = tr*qk + t2 + F = symo.replace(trigsimp(F), 'F', qk) + + # Solve qj + qj = robo.theta[j] + eq_type = 3 + el1 = 2*robo.d[j]*F[x] + el2 = -2*robo.d[j]*F[y] + el3 = F[x]**2 + F[y]**2 + robo.d[j]**2 - (G[x]**2 + G[y]**2) + coef = [el1,el2,el3] + _equation_solve(symo,coef,eq_type,qj,offsetj) + + # Solve qi + qi = robo.theta[i] + eq_type = 4 + el1 = G[x] + el2 = G[y] + el3 = -robo.d[j] -cos(qj)*F[x] + sin(qj)*F[y] + el4 = G[y] + el5 = -G[x] + el6 = -F[x]*sin(qj)*cos(robo.alpha[j]) - F[y]*cos(qj)*cos(robo.alpha[j]) + coef = [el1,el2,el3,el4,el5,el6] + _equation_solve(symo,coef,eq_type,qi,offseti) + + return + +def dj_eq_0(robo, symo, X_joint, tc, ts, tr, t0, G, offset): + """ + Function that finds the symbolic solutions of the X joints. + (Case: dj == 0) + Parameters: + =========== + 1) X_joints: Type of the X joints + 2) tc, ts, tr, t0: Coefficients of eq. 1.25 + 3) G: G = [Gx; Gy; Gz] -> Vector with constant values + """ + # eq 1.35-1.38 + # i and j revolute joints + [i,j,k] = X_joint + [x,y,z] = [0,1,2] + [offseti, offsetj, offsetk] = offset + symo.write_line("# X joints i and j are both revolute") + symo.write_line("# Case: d{0} = 0".format(j) + "\r\n") + + # Solve qk + if robo.sigma[k] == 0: + qk = robo.theta[k] + eq_type = 3 + t1 = tr*robo.r[k] + t0 + t1 = symo.replace(trigsimp(t1), 't1', qk) + + a = 2*(tc[x]*t1[x] + tc[y]*t1[y] + tc[z]*t1[z]) + b = 2*(ts[x]*t1[x] + ts[y]*t1[y] + ts[z]*t1[z]) + c = t1[x]**2 + t1[y]**2 + t1[z]**2 + tc[x]**2 + tc[y]**2 + tc[z]**2 - (G[x]**2 + G[y]**2 + G[z]**2) + coef = [a,b,c] + else: + qk = robo.r[k] + eq_type = 2 + t2 = tc*cos(robo.theta[k]) + ts*sin(robo.theta[k]) + t0 + t2 = symo.replace(trigsimp(t2), 't2', qk) + + a = 1 + b = 2*(tr[x]*t2[x] + tr[y]*t2[y] + tr[z]*t2[z]) + c = t2[x]**2 + t2[y]**2 + t2[z]**2 - (G[x]**2 + G[y]**2 + G[z]**2) + coef = [a,b,c] + _equation_solve(symo,coef,eq_type,qk,offsetk) + + if robo.sigma[k] == 0: + F = tc*cos(qk) + ts*sin(qk) + t1 + else: + F = tr*qk + t2 + F = symo.replace(trigsimp(F), 'F', qk) + + # Solve qj + qj = robo.theta[j] + eq_type = 3 + el1 = sin(robo.alpha[j])*F[y] + el2 = sin(robo.alpha[j])*F[x] + el3 = cos(robo.alpha[j])*F[z] - G[z] + coef = [el1,el2,el3] + _equation_solve(symo,coef,eq_type,qj,offsetj) + + # Solve qi + qi = robo.theta[i] + eq_type = 4 + el1 = G[x] + el2 = G[y] + el3 = -cos(qj)*F[x] + sin(qj)*F[y] + el4 = G[y] + el5 = -G[x] + el6 = sin(robo.alpha[j])*F[z] - F[x]*sin(qj)*cos(robo.alpha[j]) - F[y]*cos(qj)*cos(robo.alpha[j]) + coef = [el1,el2,el3,el4,el5,el6] + _equation_solve(symo,coef,eq_type,qi,offseti) + + return + +def dj_and_sin_alpha_dif_0(robo, symo, X_joint, tc, ts, tr, t0, G, offset): + """ + Function that finds the symbolic solutions of the X joints. + (Case: Sin(alphaj) != 0 and dj != 0) + Parameters: + =========== + 1) X_joints: Type of the X joints + 2) tc, ts, tr, t0: Coefficients of eq. 1.25 + 3) G: G = [Gx; Gy; Gz] -> Vector with constant values + """ + # eq 1.39-1.48 + # i and j revolute joints + [i,j,k] = X_joint + [x,y,z] = [0,1,2] + [offseti, offsetj, offsetk] = offset + symo.write_line("# X joints i and j are both revolute") + symo.write_line("# Case: d{0} != 0".format(j) + "and sin(alpha{0}) != 0".format(j) + "\r\n") + + # Solve qk + if robo.sigma[k] == 0: + qk = robo.theta[k] + eq_type = 6 + t1 = tr*robo.r[k] + t0 + t1 = symo.replace(trigsimp(t1), 't1', qk) + + K0 = t1[x]**2 + t1[y]**2 + t1[z]**2 + tc[x]**2 + tc[y]**2 + tc[z]**2 - (G[x]**2 + G[y]**2 + G[z]**2) - robo.d[j]**2 + K0 = symo.replace(trigsimp(K0), 'K0', qk) + + K1 = 2*(ts[x]*t1[x] + ts[y]*t1[y] + ts[z]*t1[z]) + K1 = symo.replace(trigsimp(K1), 'K1', qk) + + K2 = 2*(tc[x]*t1[x] + tc[y]*t1[y] + tc[z]*t1[z]) + K2 = symo.replace(trigsimp(K2), 'K2', qk) + + K3 = t1[z] - G[z]*cos(robo.alpha[j]) + K3 = symo.replace(trigsimp(K3), 'K3', qk) + + K4 = ts[z] + K4 = symo.replace(trigsimp(K4), 'K4', qk) + + K5 = tc[z] + K5 = symo.replace(trigsimp(K5), 'K5', qk) + + K6 = G[x]**2 + G[y]**2 + K6 = symo.replace(trigsimp(K6), 'K6', qk) + + a4 = (sin(robo.alpha[j])**2)*(K1**2 - K2**2) + 4*(robo.d[j]**2)*(K4**2 - K5**2) + a3 = 2*(sin(robo.alpha[j])**2)*K1*K2 + 8*(robo.d[j]**2)*K4*K5 + a2 = 2*(sin(robo.alpha[j])**2)*K2*K0 + 8*(robo.d[j]**2)*K5*K3 + a1 = 2*(sin(robo.alpha[j])**2)*K1*K0 + 8*(robo.d[j]**2)*K4*K3 + a0 = (sin(robo.alpha[j])**2)*(K0**2 + K2**2) + 4*(robo.d[j]**2)*(K3**2 + K5**2) - 4*(robo.d[j]**2)*(sin(robo.alpha[j])**2)*K6 + coef = [a0,a1,a2,a3,a4] + else: + qk = robo.r[k] + eq_type = 5 + t2 = tc*cos(robo.theta[k]) + ts*sin(robo.theta[k]) + t0 + t2 = symo.replace(trigsimp(t2), 't2', qk) + + N0 = t2[x]**2 + t2[y]**2 + t2[z]**2 - (G[x]**2 + G[y]**2 + G[z]**2) - robo.d[j]**2 + N0 = symo.replace(trigsimp(N0), 'N0', qk) + + N1 = 2*(tr[x]*t2[x] + tr[y]*t2[y] + tr[z]*t2[z]) + N1 = symo.replace(trigsimp(N1), 'N1', qk) + + N2 = cos(robo.alpha[j])*G[z] + t2[z] + N2 = symo.replace(trigsimp(N2), 'N2', qk) + + N3 = tr[z] + N3 = symo.replace(trigsimp(N3), 'N3', qk) + + N4 = G[x]**2 + G[y]**2 + N4 = symo.replace(trigsimp(N4), 'N4', qk) + + a4 = sin(robo.alpha[j])**2 + a3 = 2*(sin(robo.alpha[j])**2)*N1 + a2 = (sin(robo.alpha[j])**2)*(2*N0 + N1**2) + 4*(robo.d[j]**2)*(N3**2) + a1 = 2*(sin(robo.alpha[j])**2)*N1*N0 + 8*(robo.d[j]**2)*N3*N2 + a0 = (sin(robo.alpha[j])**2)*(N0**2) + 4*(robo.d[j]**2)*(N2**2) - 4*(robo.d[j]**2)*(sin(robo.alpha[j])**2)*N4 + coef = [a0,a1,a2,a3,a4] + _equation_solve(symo,coef,eq_type,qk,offsetk) + + if robo.sigma[k] == 0: + F = tc*cos(qk) + ts*sin(qk) + t1 + else: + F = tr*qk + t2 + F = symo.replace(trigsimp(F), 'F', qk) + + # Solve qj + qj = robo.theta[j] + eq_type = 4 + el1 = sin(robo.alpha[j])*F[y] + el2 = sin(robo.alpha[j])*F[x] + el3 = cos(robo.alpha[j])*F[z] - G[z] + el4 = 2*robo.d[j]*F[x] + el5 = -2*robo.d[j]*F[y] + el6 = robo.d[j]**2 + F[x]**2 + F[y]**2 + F[z]**2 - (G[x]**2 + G[y]**2 + G[z]**2) + coef = [el1,el2,el3,el4,el5,el6] + _equation_solve(symo,coef,eq_type,qj,offsetj) + + # Solve qi + qi = robo.theta[i] + eq_type = 4 + el1 = G[x] + el2 = G[y] + el3 = -cos(qj)*F[x] + sin(qj)*F[y] - robo.d[j] + el4 = G[y] + el5 = -G[x] + el6 = sin(robo.alpha[j])*F[z] - F[x]*sin(qj)*cos(robo.alpha[j]) - F[y]*cos(qj)*cos(robo.alpha[j]) + coef = [el1,el2,el3,el4,el5,el6] + _equation_solve(symo,coef,eq_type,qi,offseti) + + return + +def cos_alpha_equal_0(robo, symo, X_joints, tc, ts, tr, t0, G, offset): + """ + Function that finds the symbolic solutions of the X joints. + (Case: Cos(alphaj) == 0) + Parameters: + =========== + 1) X_joints: Type of the X joints + 2) tc, ts, tr, t0: Coefficients of eq. 1.25 + 3) G: G = [Gx; Gy; Gz] -> Vector with constant values + """ + # eq 1.50-1.51 + # i is revolute and j is prismatic + [i,j,k] = X_joints + [x,y,z] = [0,1,2] + [offseti, offsetj, offsetk] = offset + symo.write_line("# X joints i is revolute and j is prismatic") + symo.write_line("# Case: cos(alpha{0}) = 0".format(j) + "\r\n") + + # Solve qk + if robo.sigma[k] == 0: + # Type 3 in rk + qk = robo.theta[k] + eq_type = 3 + t1 = tr*robo.r[k] + t0 + t1 = symo.replace(trigsimp(t1), 't1', qk) + + el1 = tc[y] + el2 = ts[y] + el3 = t1[y] - sin(robo.alpha[j])*G[z] + coef = [el1,el2,el3] + else: + # Type 1 in rk + qk = robo.r[k] + eq_type = 1 + t2 = tc*cos(robo.theta[k]) + ts*sin(robo.theta[k]) + t0 + t2 = symo.replace(trigsimp(t2), 't2', qk) + + el1 = tr[y] + el2 = t2[y] - sin(robo.alpha[j])*G[z] + coef = [el1,el2] + _equation_solve(symo,coef,eq_type,qk,offsetk) + + if robo.sigma[k] == 0: + F = tc*cos(qk) + ts*sin(qk) + t1 + else: + F = tr*qk + t2 + F = symo.replace(trigsimp(F), 'F', qk) + + # Solve qi + qi = robo.theta[i] + eq_type = 3 + el1 = G[x] + el2 = G[y] + el3 = -(F[x] + robo.d[j]) + coef = [el1,el2,el3] + _equation_solve(symo,coef,eq_type,qi,offseti) + + # Solve qj + qj = robo.r[j] + eq_type = 1 + el1 = 1 + el2 = F[z] + sin(robo.alpha[j])*(-sin(qi)*G[x] + cos(qi)*G[y]) + coef = [el1,el2] + _equation_solve(symo,coef,eq_type,qj,offsetj) + + return + +def cos_alpha_dif_0(robo, symo, X_joints, tc, ts, tr, t0, G, offset): + """ + Function that finds the symbolic solutions of the X joints. + (Case: Cos(alphaj) != 0) + Parameters: + =========== + 1) X_joints: Type of the X joints + 2) tc, ts, tr, t0: Coefficients of eq. 1.25 + 3) G: G = [Gx; Gy; Gz] -> Vector with constant values + """ + # eq 1.52-1.54 + # i is revolute and j is prismatic + [i,j,k] = X_joints + [x,y,z] = [0,1,2] + [offseti, offsetj, offsetk] = offset + symo.write_line("# X joints i is revolute and j is prismatic") + symo.write_line("# Case: cos(alpha{0}) != 0".format(j) + "\r\n") + + # Solve qk + if robo.sigma[k] == 0: + qk = robo.theta[k] + # eq 1.53 + eq_type = 6 + t1 = tr*robo.r[k] + t0 + t1 = symo.replace(trigsimp(t1), 't1', qk) + + el1 = (cos(robo.alpha[j])**2)*((t1[x] + robo.d[j])**2 + tc[x]**2 - (G[x]**2 + G[y]**2)) + (t1[y] - sin(robo.alpha[j])*G[z])**2 + tc[y]**2 + el2 = 2*(cos(robo.alpha[j])**2)*(t1[x] + robo.d[j])*ts[x] + 2*(t1[y] - sin(robo.alpha[j])*G[z])*ts[y] + el3 = 2*(cos(robo.alpha[j])**2)*(t1[x] + robo.d[j])*tc[x] + 2*(t1[y] - sin(robo.alpha[j])*G[z])*tc[y] + el4 = 2*(cos(robo.alpha[j])**2)*tc[x]*ts[x] + 2*ts[x]*tc[y] + el5 = (cos(robo.alpha[j])**2)*(ts[x]**2 - tc[x]**2) + ts[y]**2 - tc[y]**2 + coef = [el1,el2,el3,el4,el5] + else: + qk = robo.r[k] + # eq 1.54 + eq_type = 2 + t2 = tc*cos(robo.theta[k]) + ts*sin(robo.theta[k]) + t0 + t2 = symo.replace(trigsimp(t2), 't2', qk) + + el1 = (cos(robo.alpha[j])**2)*tr[x]**2 + tr[y]**2 + el2 = 2*(cos(robo.alpha[j])**2)*(t2[x] + robo.d[j])*tr[x] + 2*(t2[y] - sin(robo.alpha[j])*G[z])*tr[y] + el3 = (cos(robo.alpha[j])**2)*((t2[x] + robo.d[j])**2 - (G[x]**2 + G[y]**2)) + (t2[y] - sin(robo.alpha[j])*G[z])**2 + coef = [el1,el2,el3] + _equation_solve(symo,coef,eq_type,qk,offsetk) + + if robo.sigma[k] == 0: + F = tc*cos(qk) + ts*sin(qk) + t1 + else: + F = tr*qk + t2 + F = symo.replace(trigsimp(F), 'F', qk) + + # Solve qi + qi = robo.theta[i] + eq_type = 4 + el1 = G[x] + el2 = G[y] + el3 = -(F[x] + robo.d[j]) + el4 = cos(robo.alpha[j])*G[y] + el5 = -cos(robo.alpha[j])*G[x] + el6 = -F[y] + sin(robo.alpha[j])*G[z] + coef = [el1,el2,el3,el4,el5,el6] + _equation_solve(symo,coef,eq_type,qi,offseti) + + # Solve qj + qj = robo.r[j] + eq_type = 1 + el1 = 1 + el2 = F[z] + sin(robo.alpha[j])*(-sin(qi)*G[x] + cos(qi)*G[y]) - cos(robo.alpha[j])*G[z] + coef = [el1,el2] + _equation_solve(symo,coef,eq_type,qj,offsetj) + + return + +def cos_alpha_equal_zero(robo, symo, X_joints, tc, ts, tr, t0, G, offset): + """ + Function that finds the symbolic solutions of the X joints. + (Case: Cos(alphaj) == 0) + Parameters: + =========== + 1) X_joints: Type of the X joints + 2) tc, ts, tr, t0: Coefficients of eq. 1.25 + 3) G: G = [Gx; Gy; Gz] -> Vector with constant values + """ + # eq 1.56-1.57 + # i is prismatic and j is revolute + [i,j,k] = X_joints + [x,y,z] = [0,1,2] + [offseti, offsetj, offsetk] = offset + symo.write_line("# X joints i is prismatic and j is revolute") + symo.write_line("# Case: cos(alpha{0}) = 0".format(j) + "\r\n") + + # Solve qk + if robo.sigma[k] == 0: + qk = robo.theta[k] + eq_type = 3 + t1 = tr*robo.r[k] + t0 + t1 = symo.replace(trigsimp(t1), 't1', qk) + + el1 = sin(robo.alpha[j])*tc[z] + el2 = sin(robo.alpha[j])*ts[z] + el3 = sin(robo.alpha[j])*t1[z] + G[y] + coef = [el1,el2,el3] + else: + qk = robo.r[k] + eq_type = 1 + t2 = tc*cos(robo.theta[k]) + ts*sin(robo.theta[k]) + t0 + t2 = symo.replace(trigsimp(t2), 't2', qk) + + el1 = sin(robo.alpha[j])*tr[z] + el2 = sin(robo.alpha[j])*t2[z] + G[y] + coef = [el1,el2] + _equation_solve(symo,coef,eq_type,qk,offsetk) + + if (robo.sigma[k] == 0): + F = tc*cos(qk) + ts*sin(qk) + t1 + else: + F = tr*qk + t2 + F = symo.replace(trigsimp(F), 'F', qk) + + # Solve qj + qj = robo.theta[j] + eq_type = 3 + el1 = F[x] + el2 = -F[y] + el3 = -G[x] + robo.d[j] + coef = [el1,el2,el3] + _equation_solve(symo,coef,eq_type,qj,offsetj) + + # Solve qi + qi = robo.r[i] + eq_type = 1 + el1 = 1 + el2 = -G[z] + sin(robo.alpha[j])*(sin(qj)*F[x] + cos(qj)*F[y]) + coef = [el1,el2] + _equation_solve(symo,coef,eq_type,qi,offseti) + + return + +def cos_alpha_dif_zero(robo, symo, X_joints, tc, ts, tr, t0, G, offset): + """ + Function that finds the symbolic solutions of the X joints. + (Case: Cos(alphaj) != 0) + Parameters: + =========== + 1) X_joints: Type of the X joints + 2) tc, ts, tr, t0: Coefficients of eq. 1.25 + 3) G: G = [Gx; Gy; Gz] -> Vector with constant values + """ + # eq 1.58-1.61 + # i is prismatic and j is revolute + [i,j,k] = X_joints + [x,y,z] = [0,1,2] + [offseti, offsetj, offsetk] = offset + symo.write_line("# X joints i is prismatic and j is revolute") + symo.write_line("# Case: cos(alpha{0}) != 0".format(j) + "\r\n") + + # Solve qk + if robo.sigma[k] == 0: + eq_type = 6 + qk = robo.theta[k] + t1 = tr*robo.r[k] + t0 + t1 = symo.replace(trigsimp(t1), 't1', qk) + + K3 = sin(robo.alpha[j])*t1[z] + G[y] + K3 = symo.replace(trigsimp(K3), 'K3', qk) + + K4 = sin(robo.alpha[j])*ts[z] + K4 = symo.replace(trigsimp(K4), 'K4', qk) + + K5 = sin(robo.alpha[j])*tc[z] + K5 = symo.replace(trigsimp(K5), 'K5', qk) + + K6 = t1[x]**2 + t1[y]**2 + K6 = symo.replace(trigsimp(K6), 'K6', qk) + + K7 = 2*(ts[x]*t1[x] + ts[y]*t1[y]) + K7 = symo.replace(trigsimp(K7), 'K7', qk) + + K8 = 2*(tc[x]*t1[x] + tc[y]*t1[y]) + K8 = symo.replace(trigsimp(K8), 'K8', qk) + + K9 = 2*(tc[x]*ts[x] + tc[y]*ts[y]) + K9 = symo.replace(trigsimp(K9), 'K9', qk) + + K10 = ts[x]**2 + ts[y]**2 + K10 = symo.replace(trigsimp(K10), 'K10', qk) + + K11 = tc[x]**2 + tc[y]**2 + K11 = symo.replace(trigsimp(K11), 'K11', qk) + + el1 = K3**2 + K5**2 + (cos(robo.alpha[j])**2)*((G[x] - robo.d[j])**2 - (K6 + K11)) + el2 = 2*K4*K3 - (cos(robo.alpha[j])**2)*K7 + el3 = 2*K5*K3 - (cos(robo.alpha[j])**2)*K8 + el4 = 2*K4*K5 - (cos(robo.alpha[j])**2)*K9 + el5 = K4**2 - K5**2 - (cos(robo.alpha[j])**2)*(K10 - K11) + coef = [el1,el2,el3,el4,el5] + else: + qk = robo.r[k] + eq_type = 2 + t2 = tc*cos(robo.theta[k]) + ts*sin(robo.theta[k]) + t0 + t2 = symo.replace(trigsimp(t2), 't2', qk) + + el1 = tr[z]**2 - cos(robo.alpha[j])**2 + el2 = -2*(cos(robo.alpha[j])**2)*(tr[x]*t2[x] + tr[y]*t2[y]) + 2*sin(robo.alpha[j])*tr[z]*(sin(robo.alpha[j])*t2[z] + G[y]) + el3 = (cos(robo.alpha[j])**2)*((G[x] - robo.d[j])**2 - t2[x]**2 - t2[y]**2) + (sin(robo.alpha[j])*t2[z] + G[y])**2 + coef = [el1,el2,el3] + _equation_solve(symo,coef,eq_type,qk,offsetk) + + if robo.sigma[k] == 0: + F = tc*cos(qk) + ts*sin(qk) + t1 + else: + F = tr*qk + t2 + F = symo.replace(trigsimp(F), 'F', qk) + + # Solve qj + qj = robo.theta[j] + eq_type = 4 + el1 = F[x] + el2 = -F[y] + el3 = -G[x] + robo.d[j] + el4 = cos(robo.alpha[j])*F[y] + el5 = cos(robo.alpha[j])*F[x] + el6 = -(G[y] + sin(robo.alpha[j])*F[z]) + coef = [el1,el2,el3,el4,el5,el6] + _equation_solve(symo,coef,eq_type,qj,offsetj) + + # Solve qi + qi = robo.r[i] + eq_type = 1 + el1 = -1 + el2 = G[z] -cos(robo.alpha[j])*F[z] - sin(robo.alpha[j])*(sin(qj)*F[x] + cos(qj)*F[y]) + coef = [el1,el2] + _equation_solve(symo,coef,eq_type,qi,offseti) + + return + +def ij_prismatic(robo, symo, X_joint, tc, ts, tr, t0, G, offset): + """ + Function that finds the symbolic solutions of the X joints. + + Parameters: + =========== + 1) X_joints: Type of the X joints + 2) tc, ts, tr, t0: Coefficients of eq. 1.25 + 3) G: G = [Gx; Gy; Gz] -> Vector with constant values + """ + # eq 1.62-1.63 (Corrected version) + # i and j are prismatic joints + [i,j,k] = X_joint + [x,y,z] = [0,1,2] + [offseti, offsetj, offsetk] = offset + symo.write_line("# X joints i and j are both prismatic \r\n") + + # Solve qk + if robo.sigma[k] == 0: + qk = robo.theta[k] + eq_type = 3 + t1 = tr*robo.r[k] + t0 + t1 = symo.replace(trigsimp(t1), 't1', qk) + + el1 = tc[x] + el2 = ts[x] + el3 = t1[x] - G[x] + robo.d[j] + coef = [el1,el2,el3] + else: + qk = robo.r[k] + eq_type = 1 + t2 = tc*cos(robo.theta[k]) + ts*sin(robo.theta[k]) + t0 + t2 = symo.replace(trigsimp(t2), 't2', qk) + + el1 = tr[x] + el2 = t2[x] - G[x] + robo.d[j] + coef = [el1,el2] + _equation_solve(symo,coef,eq_type,qk,offsetk) + + if (robo.sigma[k] == 0): + F = tc*cos(qk) + ts*sin(qk) + t1 + else: + F = tr*qk + t2 + F = symo.replace(trigsimp(F), 'F', qk) + + # Solve qj + qj = robo.r[j] + eq_type = 1 + el1 = -sin(robo.alpha[j]) + el2 = cos(robo.alpha[j])*F[y] - sin(robo.alpha[j])*F[z] - G[y] + coef = [el1,el2] + _equation_solve(symo,coef,eq_type,qj,offsetj) + + # Solve qi + qi = robo.r[i] + eq_type = 1 + el1 = 1 + el2 = sin(robo.alpha[j])*F[y] + cos(robo.alpha[j])*F[z] - G[z] + cos(robo.alpha[j])*qj + coef = [el1,el2] + _equation_solve(symo,coef,eq_type,qi,offseti) + + return + +def _equation_solve(symo, coef, eq_type, unknown, offset): + """ + Function that solves the possible type of equations that we need to solve. + (One extra type is the system of equations for the prismatic case -> type 0) + + Parameters: + =========== + 1) Symo instance + 2) coef: Vector containing the coefficients of each equation + 3) eq_type: Type of equation for the unknown (System of equation is assigned as 0 type) + 4) unknown: The unknown(s) of this equation system + """ + + symo.write_line("\r\n\r\n") + if eq_type == 0: + """ + System of equations for the three prismatic joints case + """ + [ri,rj,rk] = unknown + [a1,a2,ct] = [0,1,2] + [x,y,z] = [0,1,2] + + symo.write_line("# System of equations in " + str(ri) + ", " + str(rj) + " and " + str(rk) + ":") + symo.write_line("#=================================================\r\n") + + a1x = symo.replace(coef[a1,x], 'a1x', rk) + a1y = symo.replace(coef[a1,y], 'a1y', rk) + a1z = symo.replace(coef[a1,z], 'a1z', rk) + a2x = symo.replace(coef[a2,x], 'a2x', ri) + a2y = symo.replace(coef[a2,y], 'a2y', ri) + a2z = symo.replace(coef[a2,z], 'a2z', ri) + ct1 = symo.replace(coef[ct,x], 'ct1') + ct2 = symo.replace(coef[ct,y], 'ct2') + ct3 = symo.replace(coef[ct,z], 'ct3') + + expr1 = array(a1x)*rk + array(a2x)*ri + expr2 = array(a1y)*rk + array(a2y)*ri + expr3 = array(a1z)*rk + array(a2z)*ri + rj + symo.write_line("\r\n# Equations: ") + symo.write_line("#========== \r\n") + symo.write_line("# {0}".format(expr1) + " = " + "{0}".format(-array(ct1))) + symo.write_line("# {0}".format(expr2) + " = " + "{0}".format(-array(ct2))) + symo.write_line("# {0}".format(expr3) + " = " + "{0}".format(-array(ct3)) + "\r\n") + symo.write_line("# Solution: ") + symo.write_line("#=========\r\n") + A = Matrix( [[a1x,a2x,0],[a1y,a2y,0],[a1z,a2z,1]] ) + B = -Matrix( [ct1,ct2,ct3] ) + C = Matrix( [offset[0], offset[1], offset[2]] ) + r = A.inv()*B - C + symo.write_line("\r\n\r\n# The solutions of the lengths for the given linear system, are:") + symo.write_line("#==============================================================\r\n\r\n") + symo.write_line("# {0}".format(rk) + " = " + "{0}".format(r[0])) + symo.write_line("# {0}".format(ri) + " = " + "{0}".format(r[1])) + symo.write_line("# {0}".format(rj) + " = " + "{0}".format(r[2])) + symo.write_line("\r\n") + + elif eq_type == 1: + """Solution for the system: + a*r + b = 0 + """ + r = unknown + symo.write_line("# Equation in {0}: ".format(r)) + symo.write_line("#=================") + symo.write_line("# Type {0}".format(eq_type)) + symo.write_line("# a*{0} + b = 0".format(r) + "\r\n") + a = symo.replace(coef[0], "a", r) + b = symo.replace(-coef[1], "b", r) + expr = a*r + symo.write_line("\r\n# Equation: ") + symo.write_line("#========== \r\n") + symo.write_line("# {0}".format(expr) + " = " + "{0}".format(b) + "\r\n") + symo.write_line("# Solution: ") + symo.write_line("#==============\r\n") + symo.add_to_dict(r, b/a - offset) + symo.write_line("\r\n") + + elif eq_type == 2: + """Solution for the system: + a*r^2 + b*r + c = 0 + """ + r = unknown + symo.write_line("# Equation in {0}: ".format(r)) + symo.write_line("#=================") + EPS = var('EPS'+str(r)) + symo.write_line("# Type {0}".format(eq_type)) + symo.write_line("# a*{0}**2 + b*{0} + c = 0".format(r) + "\r\n") + a = symo.replace(coef[0],"a",r) + b = symo.replace(coef[1],"b",r) + c = symo.replace(coef[2],"c",r) + expr = a*r**2 + b*r + c + symo.write_line("\r\n# Equation: ") + symo.write_line("#========== \r\n") + symo.write_line("# {0} = 0".format(expr) + "\r\n") + symo.add_to_dict(EPS, (tools.ONE, - tools.ONE)) + Delta = symo.replace(b**2 - 4*a*c, 'Delta', r) + symo.write_line("# Solution: ") + symo.write_line("#==============\r\n") + symo.add_to_dict(r, (-b + EPS*sqrt(Delta))/2*a - offset) + symo.write_line("\r\n") + + elif eq_type == 3: + """Solution for the system: + a*C(th) + b*S(th) + c = 0 + """ + th = unknown + symo.write_line("# Equation in {0}: ".format(th)) + symo.write_line("#=================") + EPS = var('EPS'+str(th)) + symo.write_line("# Type {0}".format(eq_type)) + symo.write_line("# a*C({0}) + b*S({0}) + c = 0".format(th) + "\r\n") + a = symo.replace(coef[0], "a", th) + b = symo.replace(coef[1], "b", th) + c = symo.replace(-coef[2], "c", th) + expr = a*cos(th) + b*sin(th) + symo.write_line("\r\n# Equation: ") + symo.write_line("#========== \r\n") + symo.write_line("# {0}".format(expr) + " = " + " {0}".format(c) + "\r\n") + symo.write_line("# Solution: ") + symo.write_line("#==============\r\n") + + # Type 3 + # case 1 + if a == tools.ZERO and b != tools.ZERO: + S = symo.replace(c/b, 'S', th) + symo.add_to_dict(EPS, (tools.ONE, - tools.ONE)) + symo.add_to_dict(th, atan2(S, EPS*sqrt(1-S**2)) - offset ) + # case 2 + elif a != tools.ZERO and b == tools.ZERO: + C = symo.replace(c/a, 'C', th) + symo.add_to_dict(EPS, (tools.ONE, - tools.ONE)) + symo.add_to_dict(th, atan2(EPS*sqrt(1-C**2), C) - offset ) + # case 3 + elif c == tools.ZERO: + symo.add_to_dict(EPS, (tools.ONE, tools.ZERO)) + symo.add_to_dict(th, atan2(-a, b) + EPS*pi - offset ) + # case 4 + else: + B = symo.replace(a**2 + b**2, 'B', th) + D = symo.replace(B - c**2, 'D', th) + symo.add_to_dict(EPS, (tools.ONE, - tools.ONE)) + S = symo.replace((b*c + EPS*a*sqrt(D))/B, 'S', th) + C = symo.replace((a*c - EPS*b*sqrt(D))/B, 'C', th) + symo.add_to_dict(th, atan2(S, C) - offset ) + symo.write_line("\r\n") + + elif eq_type == 4: + """Solution for the system: + a*C(th) + b*S(th) + c = 0 + a'*C(th) + b'*S(th) + c' = 0 + """ + th = unknown + symo.write_line("# Equation in {0}: ".format(th)) + symo.write_line("#=================") + symo.write_line("# Type {0}".format(eq_type)) + symo.write_line("# a*C({0}) + b*S({0}) + c = 0".format(th)) + symo.write_line("# a'*C({0}) + b'*S({0}) + c' = 0".format(th) + "\r\n") + a = symo.replace(coef[0], 'a', th) + b = symo.replace(coef[1], 'b', th) + c = symo.replace(coef[2], 'c', th) + ap = symo.replace(coef[3], 'ap', th) + bp = symo.replace(coef[4], 'bp', th) + cp = symo.replace(coef[5], 'cp', th) + expr1 = a*cos(th) + b*sin(th) + expr2 = ap*cos(th) + bp*sin(th) + symo.write_line("\r\n# Equations:".format(th)) + symo.write_line("#===============") + symo.write_line("# {0}".format(expr1) + " = " + "{0}".format(-c) ) + symo.write_line("# {0}".format(expr2) + " = " + "{0}".format(-cp) + "\r\n") + symo.write_line("# Solution: ") + symo.write_line("#==============\r\n") + if b == tools.ZERO and ap == tools.ZERO: + symo.add_to_dict(th, atan2(-cp/bp, -c/a) - offset ) + elif bp == tools.ZERO and a == tools.ZERO: + symo.add_to_dict(th, atan2(-c/b, -cp/ap) - offset ) + else: + D = symo.replace(b*ap - bp*a, 'D', th) + C = symo.replace(-(cp*b - c*bp)/D, 'C', th) + S = symo.replace(-(c*ap - cp*a)/D, 'S', th) + symo.add_to_dict(th, atan2(S, C) - offset) + symo.write_line("\r\n") + + elif eq_type == 5: + """Solution for the system: + a4*r^4 + a3*r^3 + a2*r^2 + a1*r + a0 = 0 + """ + r = unknown + EPS = var('EPS'+str(r)) + symo.write_line("\r\n# Equation in {0}: ".format(r)) + symo.write_line("#=================") + symo.write_line("# Type {0}".format(eq_type)) + symo.write_line("# a4*{0}**4 + a3*{0}**3 + a2*{0}**2 + a1*{0} + a0 = 0".format(r) + "\r\n") + a4 = symo.replace(coef[4], "a4", r) + a3 = symo.replace(coef[3], "a3", r) + a2 = symo.replace(coef[2], "a2", r) + a1 = symo.replace(coef[1], "a1", r) + a0 = symo.replace(coef[0], "a0", r) + expr = a4*r**4 + a3*r**3 + a2*r**2 + a1*r + a0 + symo.write_line("\r\n# Equations:".format(r)) + symo.write_line("#===============") + symo.write_line("# {0} = 0".format(expr) ) + symo.write_line("\r\n# Solution: ") + symo.write_line("#==============\r\n") + delta0 = symo.replace(trigsimp( a2**2 - 3*a3*a1 + 12*a4*a0 ), "delta0", r) + delta1 = symo.replace(trigsimp( 2*a2**3 - 9*a3*a2*a1 + 27*(a3**2)*a0 + 27*a4*a1**2 - 72*a4*a2*a0 ), "delta1", r) + Q = symo.replace(trigsimp( ((delta1 + sqrt(delta1**2 - 4*delta0**3))/2)**(1/3) ), "Q", r) + p = symo.replace(trigsimp( (8*a4*a2 - 3*a3**2)/(8*a4**2) ), "p", r) + q = symo.replace(trigsimp( (a3**3 - 4*a4*a3*a2 + 8*(a4**2)*a1)/(8*a4**3) ), "q", r) + S = symo.replace(trigsimp( sqrt(-(2*p)/3 + ((Q**2 + delta0)/(3*a4*Q)))/2 ), "S", r) + symo.add_to_dict(EPS, (tools.ONE, - tools.ONE)) + sol12 = -a3/(4*a4) - S + EPS*sqrt(-4*S**2 - 2*p + q/S)/2 - offset + sol34 = -a3/(4*a4) + S + EPS*sqrt(-4*S**2 - 2*p - q/S)/2 - offset + symo.write_line("\r\n# Solutions 1 and 2 are: ") + r_12 = var(str(r)+'_12') + symo.add_to_dict(r_12, sol12) + symo.write_line("\r\n\r\n# Solutions 3 and 4 are: \r\n") + r_34 = var(str(r)+'_34') + symo.add_to_dict(r_34, sol34) + + elif eq_type == 6: + """Solution for the system: + a4*S(th)^2 + a3*C(th)*S(th) + a2*C(th) + a1*S(th) + a0 = 0 + Equation in type 5: + (a0 - a2)*t^4 + 2*(a1 - a3)*t^3 + 2*(a0 + 2*a4)*t^2 + 2*(a1 + a3)*t + (a0 + a2) = 0 , with t = tan(th/2) + """ + th = unknown + t = var('t') + EPS = var('EPS'+str(th)) + symo.write_line("# Equation in {0}: ".format(th)) + symo.write_line("#=================") + symo.write_line("# Type {0}".format(eq_type)) + symo.write_line("# a4*S({0})**2 + a3*CS({0}) + a2*C({0}) + a1*S({0}) + a0 = 0".format(th) + "\r\n") + a4 = symo.replace(coef[4], "a4", th) + a3 = symo.replace(coef[3], "a3", th) + a2 = symo.replace(coef[2], "a2", th) + a1 = symo.replace(coef[1], "a1", th) + a0 = symo.replace(coef[0], "a0", th) + expr1 = a4*sin(th)**2 + a3*cos(th)*sin(th) + a2*cos(th) + a1*sin(th) + a0 + A4 = symo.replace(trigsimp(a0 - a2), "A4", th) + A3 = symo.replace(trigsimp(2*(a1 - a3)), "A3", th) + A2 = symo.replace(trigsimp(2*(a0 + 2*a4)), "A2", th) + A1 = symo.replace(trigsimp(2*(a1 + a3)), "A1", th) + A0 = symo.replace(trigsimp(a0 + a2), "A0", th) + expr2 = A4*t**4 + A3*t**3 + A2*t**2 + A1*t + A0 + symo.write_line("\r\n# Equation:".format(th)) + symo.write_line("#===============") + symo.write_line("# {0} = 0".format(expr1) ) + symo.write_line("\r\n# New Equation:") + symo.write_line("#=================") + symo.write_line("# {0} = 0".format(expr2) + " (with {0}".format(t) + " = " + "tan({0}/2) )".format(th) + "\r\n\r\n") + symo.write_line("\r\n# Solution: ") + symo.write_line("#==============\r\n") + delta0 = symo.replace(trigsimp( A2**2 - 3*A3*A1 + 12*A4*A0 ), "delta0", th) + delta1 = symo.replace(trigsimp( 2*A2**3 - 9*A3*A2*A1 + 27*(A3**2)*A0 + 27*A4*A1**2 - 72*A4*A2*A0 ), "delta1", th) + Q = symo.replace(trigsimp( ((delta1 + sqrt(delta1**2 - 4*delta0**3))/2)**(1/3) ), "Q", th) + p = symo.replace(trigsimp( (8*A4*A2 - 3*A3**2)/(8*A4**2) ), "p", th) + q = symo.replace(trigsimp( (A3**3 - 4*A4*A3*A2 + 8*(A4**2)*A1)/(8*A4**3) ), "q", th) + S = symo.replace(trigsimp( sqrt(-(2*p)/3 + ((Q**2 + delta0)/(3*A4*Q)))/2 ), "S", th) + symo.add_to_dict(EPS, (tools.ONE, - tools.ONE)) + sol12 = -A3/(4*A4) - S + EPS*sqrt(-4*S**2 - 2*p + q/S)/2 + sol34 = -A3/(4*A4) + S + EPS*sqrt(-4*S**2 - 2*p - q/S)/2 + symo.write_line("\r\n# Solutions 1 and 2 are: ") + t_12 = symo.replace(sol12, "t_12", th) + symo.add_to_dict("{0}_12".format(th), 2*atan(t_12) - offset ) + symo.write_line("\r\n# Solutions 3 and 4 are: \r\n") + t_34 = symo.replace(sol34, "t_34", th) + symo.add_to_dict("{0}_34".format(th), 2*atan(t_34) - offset ) + + symo.write_line("--------------------------------------------------------------------------------------------") + return + +def solve_position(robo, symo, com_key, X_joints, fc, fs, fr, f0, g): + """ + Function that solves the position equation for the four spherical cases. + + Parameters: + =========== + 1) com_key: X joints combination + 2) X_joints: Type of the X joints + 3) fc, fs, fr, f0: Coefficients of equation (1.23.b) + 4) g: g = [gx; gy; gz] -> Vector containing constant values + """ + [i,j,k] = X_joints + [x,y,z] = [0,1,2] # Book convention indexes + offset = zeros(1,len(X_joints)) + + if robo.sigma[k] == 0: + robo.r[k] = robo.r[k] + robo.b[robo.ant[k]] + offset[2] = robo.gamma[robo.ant[k]] + elif robo.sigma[k] == 1: + robo.theta[k] = robo.theta[k] + robo.gamma[robo.ant[k]] + offset[2] = robo.b[robo.ant[k]] + + if robo.sigma[j] == 0: # If the joint j is revolute + robo.r[j] = robo.r[j] + robo.b[robo.ant[j]] + offset[1] = robo.gamma[robo.ant[j]] + T = _rot_trans(axis=z, th=0, p=robo.r[j]) + elif robo.sigma[j] == 1: # If the joint j is prismatic + robo.theta[j] = robo.theta[j] + robo.gamma[robo.ant[j]] + offset[1] = robo.b[robo.ant[j]] + T = _rot_trans(axis=z, th=robo.theta[j], p=0) + + tc = T*fc + tc = symo.replace(trigsimp(tc), 'tc', k) + ts = T*fs + ts = symo.replace(trigsimp(ts), 'ts', k) + tr = T*fr + tr = symo.replace(trigsimp(tr), 'tr', k) + t0 = T*f0 + t0 = symo.replace(trigsimp(t0), 't0', k) + + if robo.sigma[i] == 0: # If joint i is revolute + robo.r[i] = robo.r[i] + robo.b[robo.ant[i]] + offset[0] = robo.gamma[robo.ant[i]] + T = _rot_trans(axis=z, th=0, p=-robo.r[i]) + else: # If the joint i is prismatic + robo.theta[i] = robo.theta[i] + robo.gamma[robo.ant[i]] + offset[0] = robo.b[robo.ant[i]] + T = _rot_trans(axis=z, th=-robo.theta[i], p=0) + + G = T*g + G = symo.replace(trigsimp(G), 'G') + + # Conditions to get the solution for its possible X joints combination + if (com_key == 0) or (com_key == 1): # X joints: RRX with X:either R or P joint + if sin(robo.alpha[j]) == 0: # sin(alphaj) equal 0 + sin_alphaj_eq_0(robo, symo, X_joints, tc, ts, tr, t0, G, offset) + elif robo.d[j] == 0: # dj equal 0 + dj_eq_0(robo, symo, X_joints, tc, ts, tr, t0, G, offset) + elif (sin(robo.alpha[j]) != 0) and (robo.d[j] != 0): # sin(alphaj) and dj not equal to 0 + dj_and_sin_alpha_dif_0(robo, symo, X_joints, tc, ts, tr, t0, G, offset) + elif (com_key == 2) or (com_key == 3): # X joints: RPX with X:either R or P joint + if cos(robo.alpha[j]) == 0: # cos(alphaj) equal 0 + cos_alpha_equal_0(robo, symo, X_joints, tc, ts, tr, t0, G, offset) + elif cos(robo.alpha[j]) != 0: # cos(alphaj) not equal to 0 + cos_alpha_dif_0(robo, symo, X_joints, tc, ts, tr, t0, G, offset) + elif (com_key == 4) or (com_key == 5): # X joints: PRX with X:either R or P joint + if cos(robo.alpha[j]) == 0: # cos(alphaj) equal 0 + cos_alpha_equal_zero(robo, symo, X_joints, tc, ts, tr, t0, G, offset) + elif cos(robo.alpha[j]) != 0: # cos(alphaj) not equal to 0 + cos_alpha_dif_zero(robo, symo, X_joints, tc, ts, tr, t0, G, offset) + else: # X joints: PPX with X:either R or P joint + ij_prismatic(robo, symo, X_joints, tc, ts, tr, t0, G, offset) + + return + +def solve_orientation(robo, symo, pieper_joints): + """ + Function that solves the orientation equation for the four spherical cases. + + Parameters: + =========== + 1) pieper_joints: Joints that form the spherical wrist + """ + m = pieper_joints[1] # Center of the spherical joint + [S,N,A] = [0,1,2] # Book convention indexes + [x,y,z] = [0,1,2] # Book convention indexes + + t1 = dgm(robo, symo, m-2, 0, fast_form=True, trig_subs=True) + t2 = dgm(robo, symo, 6, m+1, fast_form=True, trig_subs=True) + + Am2A0 = Matrix([ t1[:3,:3] ]) + A6Am1 = Matrix([ t2[:3,:3] ]) + + A0 = T_GENERAL[:3,:3] + + SNA = _rot(axis=x, th=-robo.alpha[m-1])*Am2A0*A0*A6Am1 + SNA = symo.replace(trigsimp(SNA), 'SNA') + + # calculate theta(m-1) (i) + # eq 1.68 + eq_type = 3 + offset = robo.gamma[robo.ant[m-1]] + robo.r[m-1] = robo.r[m-1] + robo.b[robo.ant[m-1]] + coef = [-SNA[y,A]*sin(robo.alpha[m]) , SNA[x,A]*sin(robo.alpha[m]) , SNA[z,A]*cos(robo.alpha[m])-cos(robo.alpha[m+1])] + _equation_solve(symo, coef, eq_type, robo.theta[m-1], offset) + + # calculate theta(m) (j) + # eq 1.72 + S1N1A1 = _rot(axis=x, th=-robo.alpha[m])*_rot(axis=z, th=-robo.theta[m-1])*SNA + eq_type = 4 + offset = robo.gamma[robo.ant[m]] + robo.r[m] = robo.r[m] + robo.b[robo.ant[m]] + symo.write_line("\r\n\r\n") + B1 = symo.replace(trigsimp(-S1N1A1[x,A]), 'B1', robo.theta[m]) + B2 = symo.replace(trigsimp(S1N1A1[y,A]), 'B2', robo.theta[m]) + coef = [0, sin(robo.alpha[m+1]), B1, sin(robo.alpha[m+1]), 0, B2] + _equation_solve(symo, coef, eq_type, robo.theta[m], offset) + + # calculate theta(m+1) (k) + # eq 1.73 + eq_type = 4 + offset = robo.gamma[robo.ant[m+1]] + robo.r[m+1] = robo.r[m+1] + robo.b[robo.ant[m+1]] + symo.write_line("\r\n\r\n") + B1 = symo.replace(trigsimp(-S1N1A1[z,S]), 'B1', robo.theta[m+1]) + B2 = symo.replace(trigsimp(-S1N1A1[z,N]), 'B2', robo.theta[m+1]) + coef = [0, sin(robo.alpha[m+1]), B1, sin(robo.alpha[m+1]), 0, B2] + _equation_solve(symo, coef, eq_type, robo.theta[m+1], offset) + + return + + +def solve_orientation_prismatic(robo, symo, X_joints): + """ + Function that solves the orientation equation of the three prismatic joints case. (to find the three angles) + + Parameters: + =========== + 1) X_joint: The three revolute joints for the prismatic case + """ + [i,j,k] = X_joints # X joints vector + [S,S1,S2,S3,x] = [0,0,0,0,0] # Book convention indexes + [N,N1,N2,N3,y] = [1,1,1,1,1] # Book convention indexes + [A,A1,A2,A3,z] = [2,2,2,2,2] # Book convention indexes + robo.theta[i] = robo.theta[i] + robo.gamma[robo.ant[i]] + robo.theta[j] = robo.theta[j] + robo.gamma[robo.ant[j]] + robo.theta[k] = robo.theta[k] + robo.gamma[robo.ant[k]] + + T6k = dgm(robo, symo, 6, k, fast_form=True,trig_subs=True) + Ti0 = dgm(robo, symo, robo.ant[i], 0, fast_form=True, trig_subs=True) + Tji = dgm(robo, symo, j-1, i, fast_form=True, trig_subs=True) + Tjk = dgm(robo, symo, j, k-1, fast_form=True, trig_subs=True) + + S3N3A3 = _rot_trans(axis=x, th=-robo.alpha[i], p=0)*Ti0*T_GENERAL*T6k # S3N3A3 = rot(x,-alphai)*rot(z,-gami)*aiTo*SNA + S2N2A2 = _rot_trans(axis=x, th=-robo.alpha[j], p=0)*Tji # S2N2A2 = iTa(j)*rot(x,-alphaj) + S1N1A1 = Tjk*_rot_trans(axis=x, th=-robo.alpha[k], p=0) # S1N1A1 = jTa(k)*rot(x,alphak) + + S3N3A3 = Matrix([ S3N3A3[:3, :3] ]) + S2N2A2 = Matrix([ S2N2A2[:3, :3] ]) + S1N1A1 = Matrix([ S1N1A1[:3, :3] ]) + SNA = Matrix([ T_GENERAL[:3, :3] ]) + SNA = symo.replace(trigsimp(SNA), 'SNA') + + # solve thetai + # eq 1.100 (page 49) + eq_type = 3 + offset = robo.gamma[robo.ant[i]] + robo.r[i] = robo.r[i] + robo.b[robo.ant[i]] + el1 = S2N2A2[z,S2]*S3N3A3[x,A3] + S2N2A2[z,N2]*S3N3A3[y,A3] + el2 = S2N2A2[z,S2]*S3N3A3[y,A3] - S2N2A2[z,N2]*S3N3A3[x,A3] + el3 = S2N2A2[z,A2]*S3N3A3[z,A3] - S1N1A1[z,A1] + coef = [el1,el2,el3] + _equation_solve(symo, coef, eq_type, robo.theta[i], offset) + + # solve thetaj + # eq 1.102 + eq_type = 4 + offset = robo.gamma[robo.ant[j]] + robo.r[j] = robo.r[j] + robo.b[robo.ant[j]] + coef = [S1N1A1[x,A1] , -S1N1A1[y,A1] , -SNA[x,A] , S1N1A1[y,A1] , S1N1A1[x,A1] , -SNA[y,A] ] + _equation_solve(symo, coef, eq_type, robo.theta[j], offset) + + # solve thetak + # eq 1.103 + eq_type = 4 + offset = robo.gamma[robo.ant[k]] + robo.r[k] = robo.r[k] + robo.b[robo.ant[k]] + coef = [S1N1A1[z,S1] , S1N1A1[z,N1] , -SNA[z,S] , S1N1A1[z,N1] , -S1N1A1[z,S1] , -SNA[z,N] ] + _equation_solve(symo, coef, eq_type, robo.theta[k], offset) + + return + + +def solve_position_prismatic(robo, symo, pieper_joints): + """ + Function that solves the position equation of the three prismatic joints case. (to find the three angles) + + Parameters: + =========== + 1) Pieper_joints: The three prismatic joints for the prismatic case + """ + eq_type = 0 # Type 0: Linear system + [i,j,k] = pieper_joints # Pieper joints vector + [x,y,z] = [0,1,2] # Book convention indexes + robo.theta[i] = robo.theta[i] + robo.gamma[robo.ant[i]] + robo.theta[j] = robo.theta[j] + robo.gamma[robo.ant[j]] + robo.theta[k] = robo.theta[k] + robo.gamma[robo.ant[k]] + + T6k = dgm(robo, symo, 6, k, fast_form=True, trig_subs=True) + Ti0 = dgm(robo, symo, robo.ant[i], 0, fast_form=True, trig_subs=True) + Tji = dgm(robo, symo, j-1, i, fast_form=True, trig_subs=True) + Tjk = dgm(robo, symo, j, k-1, fast_form=True, trig_subs=True) + + S3N3A3P3 = _rot_trans(axis=z, th=-robo.theta[i], p=0)*_rot_trans(axis=x, th=-robo.alpha[i], p=-robo.d[i])*Ti0*T_GENERAL*T6k # S3N3A3 = rot(z,-thetai)*Trans(x,-di)*rot(x,-alphai)*a(i)To*SNAP*6Tk + S2N2A2P2 = _rot_trans(axis=z, th=-robo.theta[j], p=0)*_rot_trans(axis=x, th=-robo.alpha[j], p=-robo.d[j])*Tji # S2N2A2 = rot(z,-thetaj)*Trans(x,-dj)*rot(x,-alphaj)*a(j)Ti + S1N1A1P1 = Tjk*_rot_trans(axis=x, th=robo.alpha[k], p=robo.d[k])*_rot_trans(axis=z, th=robo.theta[k], p=0) # S1N1A1 = jTa(k)*rot(x,alphak)*Trans(x,dk)*rot(z,thetak) + + S2N2A2 = array( S2N2A2P2[:3, :3] ) + P3 = array( S3N3A3P3[:3, 3] ) + P2 = array( S2N2A2P2[:3, 3] ) + P1 = array( S1N1A1P1[:3, 3] ) + + t2 = S2N2A2[:3, 2] + P4 = P1 - dot(S2N2A2, P3) - P2 + t1 = S1N1A1P1[:3,2] + coef = Matrix([[t1[0],t1[1],t1[2]],[t2[0],t2[1],t2[2]],[P4[0][0],P4[1][0],P4[2][0]]]) + rijk = [robo.r[i], robo.r[j], robo.r[k]] + offset = [robo.b[robo.ant[k]], robo.b[robo.ant[j]], robo.b[robo.ant[i]]] + _equation_solve(symo, coef, eq_type, rijk, offset) + + return diff --git a/pysymoro/core/invgeom.py b/pysymoro/invgeom.py similarity index 58% rename from pysymoro/core/invgeom.py rename to pysymoro/invgeom.py index 8ab9814..1a3598d 100644 --- a/pysymoro/core/invgeom.py +++ b/pysymoro/invgeom.py @@ -1,160 +1,208 @@ +# -*- coding: utf-8 -*- + + +# This file is part of the OpenSYMORO project. Please see +# https://github.com/symoro/symoro/blob/master/LICENCE for the licence. + + """ This module of SYMORO package provides symbolic solutions for inverse geompetric problem. - -The core symbolic library is sympy. -Needed modules : symoro.py, geometry.py - -ECN - ARIA1 2013 """ + from heapq import heapify, heappop from sympy import var, sin, cos, eye, atan2, sqrt, pi -from sympy import Matrix, Symbol, Expr -from symoro import Symoro, ZERO, ONE, get_max_coef -from geometry import dgm +from sympy import Matrix, Symbol, Expr, trigsimp + +from pysymoro.geometry import transform_list, to_matrix +from symoroutils import symbolmgr +from symoroutils import tools + EMPTY = var("EMPTY") T_GENERAL = Matrix([var("s1,n1,a1,p1"), var("s2,n2,a2,p2"), var("s3,n3,a3,p3"), [0, 0, 0, 1]]) -#dictionary for equation type classification +# Dictionary for equation type classification. eq_dict = {(1, 0, 0): 0, (0, 1, 0): 1, (1, 1, 0): 2, (0, 2, 0): 3, (0, 2, 1): 4} -def _paul_solve(robo, symo, nTm, n, m, known=set()): +def _paul_solve(robo, symo, nTm, n, m, known_vars=None): + if known_vars is None: + knowns = set() + else: + knowns = set(known_vars) chain = robo.loop_chain(m, n) - iTn = dgm(robo, symo, m, n, key='left', trig_subs=False) - iTm = dgm(robo, symo, n, m, key='left', trig_subs=False) -# mTi = dgm(robo, symo, m, n, key='right', trig_subs=False) -# nTi = dgm(robo, symo, n, m, key='right', trig_subs=False) th_all = set() r_all = set() + # Create the set of all knowns symbols for i in chain: if i >= 0: - if robo.sigma[i] == 0: + if robo.sigma[i] == 0 and isinstance(robo.theta[i], Expr): th_all.add(robo.theta[i]) - if robo.sigma[i] == 1: + if isinstance(robo.r[i], Expr): + knowns |= robo.r[i].atoms(Symbol) + if robo.sigma[i] == 1 and isinstance(robo.r[i], Expr): r_all.add(robo.r[i]) + if isinstance(robo.theta[i], Expr): + knowns |= robo.theta[i].atoms(Symbol) if isinstance(robo.gamma[i], Expr): - known |= robo.gamma[i].atoms(Symbol) + knowns |= robo.gamma[i].atoms(Symbol) if isinstance(robo.alpha[i], Expr): - known |= robo.alpha[i].atoms(Symbol) + knowns |= robo.alpha[i].atoms(Symbol) + if isinstance(robo.d[i], Expr): + knowns |= robo.d[i].atoms(Symbol) + if isinstance(robo.b[i], Expr): + knowns |= robo.b[i].atoms(Symbol) while True: repeat = False - for i in reversed(chain): - M_eq = iTn[(i, n)]*nTm - iTm[(i, m)] - while True: - found = _look_for_eq(symo, M_eq, known, th_all, r_all) - repeat |= found - if not found or th_all | r_all <= known: - break - if th_all | r_all <= known: + iTm = nTm.copy() + tr_list = transform_list(robo, n, m) + _replace_EMPTY(iTm, tr_list) + tr_list.reverse() + tr_const, tr_list = _extract_const_transforms(tr_list, knowns) + for trc in tr_const: + iTm = iTm * trc.matrix_inv() + tr_list.reverse() + while tr_list: + tr_const, tr_list = _extract_const_transforms(tr_list, knowns) + for trc in tr_const: + iTm = trc.matrix_inv() * iTm + tr = tr_list.pop(0) + if tr.val.atoms(Symbol) - knowns: + M_eq = tr.matrix() * to_matrix(tr_list, simplify=False) + while True: + found = _look_for_eq(symo, M_eq - iTm, + knowns, th_all, r_all) + repeat |= found + if not found or th_all | r_all <= knowns: + break + iTm = tr.matrix_inv() * iTm + if th_all | r_all <= knowns: break -# if th_all | r_all <= known: -# break -# for i in reversed(chain): -# while True: -# found = _look_for_eq(symo, M_eq, known, th_all, r_all) -# repeat |= found -# if not found or th_all | r_all <= known: -# break -# if th_all | r_all <= known: -# break - if not repeat or th_all | r_all <= known: + if not repeat or th_all | r_all <= knowns: break - return known + return knowns -def _look_for_eq(symo, M_eq, known, th_all, r_all): +def _replace_EMPTY(T, tr_list): + T_sym = to_matrix(tr_list, simplify=True) + for e1 in xrange(4): + for e2 in xrange(4): + if T[e1, e2].has(EMPTY): + T[e1, e2] = T_sym[e1, e2] + + +def _extract_const_transforms(tr_list, knowns): + var_idx = len(tr_list) + var_found = False + for i, tr in enumerate(tr_list): + if not var_found: + if tr.val.atoms(Symbol) - knowns: + var_found = True + var_idx = i + elif tr.axis == tr_list[var_idx].axis: + if not tr.val.atoms(Symbol) - knowns: + tr_list[i] = tr_list[var_idx] + tr_list[var_idx] = tr + var_idx = i + else: + break + return tr_list[:var_idx], tr_list[var_idx:] + + +def _look_for_eq(symo, M_eq, knowns, th_all, r_all): cont_search = False eq_candidates = [list() for list_index in xrange(5)] for e1 in xrange(3): for e2 in xrange(4): - if M_eq[e1, e2].has(EMPTY): + eq = M_eq[e1, e2] + if not isinstance(eq, Expr) or eq.is_Atom: continue - eq = symo.unknown_sep(M_eq[e1, e2], known) - th_vars = (eq.atoms(Symbol) & th_all) - known - if th_vars: - arg_sum = max(at.count_ops()-1 for at in eq.atoms(sin, cos) - if not at.atoms(Symbol) & known) + th_vars = (eq.atoms(Symbol) & th_all) - knowns + arg_ops = [at.count_ops()-1 for at in eq.atoms(sin, cos) + if not at.atoms(Symbol) & knowns] + if th_vars and arg_ops: + arg_sum = max(arg_ops) else: arg_sum = 0 - rs_s = (eq.atoms(Symbol) & r_all) - known + rs_s = (eq.atoms(Symbol) & r_all) - knowns eq_features = (len(rs_s), len(th_vars), arg_sum) if eq_features in eq_dict: eq_key = eq_dict[eq_features] eq_pack = (eq, list(rs_s), list(th_vars)) eq_candidates[eq_key].append(eq_pack) - cont_search |= _try_solve_0(symo, eq_candidates[0], known) - cont_search |= _try_solve_1(symo, eq_candidates[1], known) + cont_search |= _try_solve_0(symo, eq_candidates[0], knowns) + cont_search |= _try_solve_1(symo, eq_candidates[1], knowns) cont_search |= _try_solve_2(symo, eq_candidates[2] + - eq_candidates[1], known) - cont_search |= _try_solve_3(symo, eq_candidates[3], known) - cont_search |= _try_solve_4(symo, eq_candidates[4], known) + eq_candidates[1], knowns) + cont_search |= _try_solve_3(symo, eq_candidates[3], knowns) + cont_search |= _try_solve_4(symo, eq_candidates[4], knowns) return cont_search -def loop_solve(robo, symo, knowns=None): - #TODO: rewrite; Add parallelogram detection - q_vec = q_vec = [robo.get_q(i) for i in xrange(robo.NF)] +def loop_solve(robo, symo, know=None): + # TODO: rewrite; Add parallelogram detection + q_vec = [robo.get_q(i) for i in xrange(robo.NF)] loops = [] - if knowns is None: - knowns = robo.q_active + if know is None: + know = robo.q_active # set(q for i, q in enumerate(q_vec) if robo.mu[i] == 1) for i, j in robo.loop_terminals: chain = robo.loop_chain(i, j) - knowns_ij = set(q_vec[i] for i in chain if q_vec[i] in knowns) - unknowns_ij = set(q_vec[i] for i in chain if q_vec[i] not in knowns) - loops.append([len(unknowns_ij), i, j, knowns_ij, unknowns_ij]) + know_ij = set(q_vec[i] for i in chain if q_vec[i] in know) + unknow_ij = set(q_vec[i] for i in chain if q_vec[i] not in know) + loops.append([len(unknow_ij), i, j, know_ij, unknow_ij]) while loops: heapify(loops) loop = heappop(loops) - res_knowns = _paul_solve(robo, symo, eye(4), *loop[1:4]) + res_know = _paul_solve(robo, symo, eye(4), *loop[1:4]) for l in loops: - found = l[4] & res_knowns + found = l[4] & res_know l[3] |= found l[4] -= found l[0] = len(l[4]) -def igm_Paul(robo, T_ref, n): +def igm_paul(robo, T_ref, n): if isinstance(T_ref, list): T_ref = Matrix(4, 4, T_ref) - symo = Symoro() + symo = symbolmgr.SymbolManager() symo.file_open(robo, 'igm') - symo.write_params_table(robo, 'Inverse Geometrix Model for frame %s' % n) + symo.write_params_table(robo, 'Inverse Geometric Model for frame %s' % n) _paul_solve(robo, symo, T_ref, 0, n) symo.file_close() return symo -#TODO: think about smarter way of matching -def _try_solve_0(symo, eq_sys, known): +# TODO: think about smarter way of matching +def _try_solve_0(symo, eq_sys, knowns): res = False for eq, [r], th_names in eq_sys: - X = get_max_coef(eq, r) + X = tools.get_max_coef(eq, r) if X != 0: Y = X*r - eq - print "type 1" - X = symo.replace(symo.CS12_simp(X), 'X', r) - Y = symo.replace(symo.CS12_simp(Y), 'Y', r) + symo.write_line("# Solving type 1") + X = symo.replace(trigsimp(X), 'X', r) + Y = symo.replace(trigsimp(Y), 'Y', r) symo.add_to_dict(r, Y/X) - known.add(r) + knowns.add(r) res = True return res -def _try_solve_1(symo, eq_sys, known): +def _try_solve_1(symo, eq_sys, knowns): res = False for i in xrange(len(eq_sys)): eqi, rs_i, [th_i] = eq_sys[i] - if th_i in known: + if th_i in knowns: continue Xi, Yi, Zi, i_ok = _get_coefs(eqi, sin(th_i), cos(th_i), th_i) - i_ok &= sum([Xi == ZERO, Yi == ZERO, Zi == ZERO]) <= 1 + zero = tools.ZERO + i_ok &= sum([Xi == zero, Yi == zero, Zi == zero]) <= 1 if not i_ok: continue j_ok = False @@ -171,12 +219,12 @@ def _try_solve_1(symo, eq_sys, known): else: symo.write_line("# Solving type 2") _solve_type_2(symo, Xi, Yi, -Zi, th_i) - known.add(th_i) + knowns.add(th_i) res = True return res -def _try_solve_2(symo, eq_sys, known): +def _try_solve_2(symo, eq_sys, knowns): if all(len(rs) == 0 for eq, rs, ths in eq_sys): return False for i in xrange(len(eq_sys)): @@ -197,8 +245,8 @@ def _try_solve_2(symo, eq_sys, known): break X1, Y1, Z1, i_ok = _get_coefs(eqi, S, C, th, r) X2, Y2, Z2, j_ok = _get_coefs(eqj, C, S, th, r) - i_ok &= X1.has(r) and not Z1.has(r) and Y1 == ZERO - j_ok &= X2.has(r) and not Z2.has(r) and Y2 == ZERO + i_ok &= X1.has(r) and not Z1.has(r) and Y1 == tools.ZERO + j_ok &= X2.has(r) and not Z2.has(r) and Y2 == tools.ZERO all_ok = j_ok and i_ok if all_ok: eq_type = 4 @@ -211,10 +259,10 @@ def _try_solve_2(symo, eq_sys, known): continue symo.write_line("# Solving type %s" % eq_type) if eq_type == 4: - _solve_type_4(symo, X1, Y1, X2, Y2, th, r) + _solve_type_4(symo, X1, -Y1, X2, -Y2, th, r) else: - _solve_type_5(symo, X1, Y1, Z1, X2, Y2, Z2, th, r) - known |= {th, r} + _solve_type_5(symo, X1, -Y1, -Z1, X2, -Y2, -Z2, th, r) + knowns |= {th, r} return True return False @@ -223,7 +271,7 @@ def _match_coef(A1, A2, B1, B2): return A1 == A2 and B1 == B2 or A1 == -A2 and B1 == -B2 -def _try_solve_3(symo, eq_sys, known): +def _try_solve_3(symo, eq_sys, knowns): for i in xrange(len(eq_sys)): all_ok = False for j in xrange(len(eq_sys)): @@ -266,19 +314,19 @@ def _try_solve_3(symo, eq_sys, known): continue symo.write_line("# Solving type 6, 7") _solve_type_7(symo, V1, W1, -X1, -Y1, -Z1, -Z2, eps, th1, th2) - known |= {th1, th2} + knowns |= {th1, th2} return True return False -#TODO: make it with itertool -def _try_solve_4(symo, eq_sys, known): +# TODO: make it with itertool +def _try_solve_4(symo, eq_sys, knowns): res = False for i in xrange(len(eq_sys)): all_ok = False for j in xrange(len(eq_sys)): - eqj, rs_j, ths_i = eq_sys[j] - eqi, rs_i, ths_j = eq_sys[i] + eqj, rs_j, ths_j = eq_sys[j] + eqi, rs_i, ths_i = eq_sys[i] if i == j or set(ths_i) != set(ths_j): continue th12 = ths_i[0] + ths_i[1] @@ -293,6 +341,7 @@ def _try_solve_4(symo, eq_sys, known): X1, Y1, Z1, i_ok = _get_coefs(eqi, C1, C12, th1, th2) X2, Y2, Z2, j_ok = _get_coefs(eqj, S1, S12, th1, th2) all_ok = (X1*Y2 == Y1*X2 and i_ok and j_ok) + all_ok &= X1 != 0 and Y1 != 0 all_ok &= not eqi.has(S1) and not eqi.has(S12) all_ok &= not eqj.has(C1) and not eqj.has(C12) if all_ok: @@ -300,8 +349,8 @@ def _try_solve_4(symo, eq_sys, known): if not all_ok: continue symo.write_line("# Solving type 8") - _solve_type_8(symo, X1, Y1, Z1, Z2, th1, th2) - known |= {th1, th2} + _solve_type_8(symo, X1, Y1, -Z1, -Z2, th1, th2) + knowns |= {th1, th2} res = True return res @@ -311,25 +360,25 @@ def _solve_type_2(symo, X, Y, Z, th): X*S + Y*C = Z """ symo.write_line("# X*sin({0}) + Y*cos({0}) = Z".format(th)) - X = symo.replace(symo.CS12_simp(X), 'X', th) - Y = symo.replace(symo.CS12_simp(Y), 'Y', th) - Z = symo.replace(symo.CS12_simp(Z), 'Z', th) + X = symo.replace(trigsimp(X), 'X', th) + Y = symo.replace(trigsimp(Y), 'Y', th) + Z = symo.replace(trigsimp(Z), 'Z', th) YPS = var('YPS'+str(th)) - if X == ZERO and Y != ZERO: + if X == tools.ZERO and Y != tools.ZERO: C = symo.replace(Z/Y, 'C', th) - symo.add_to_dict(YPS, (ONE, - ONE)) + symo.add_to_dict(YPS, (tools.ONE, - tools.ONE)) symo.add_to_dict(th, atan2(YPS*sqrt(1-C**2), C)) - elif X != ZERO and Y == ZERO: + elif X != tools.ZERO and Y == tools.ZERO: S = symo.replace(Z/X, 'S', th) - symo.add_to_dict(YPS, (ONE, - ONE)) + symo.add_to_dict(YPS, (tools.ONE, - tools.ONE)) symo.add_to_dict(th, atan2(S, YPS*sqrt(1-S**2))) - elif Z == ZERO: - symo.add_to_dict(YPS, (ONE, ZERO)) + elif Z == tools.ZERO: + symo.add_to_dict(YPS, (tools.ONE, tools.ZERO)) symo.add_to_dict(th, atan2(-Y, X) + YPS*pi) else: B = symo.replace(X**2 + Y**2, 'B', th) D = symo.replace(B - Z**2, 'D', th) - symo.add_to_dict(YPS, (ONE, - ONE)) + symo.add_to_dict(YPS, (tools.ONE, - tools.ONE)) S = symo.replace((X*Z + YPS * Y * sqrt(D))/B, 'S', th) C = symo.replace((Y*Z - YPS * X * sqrt(D))/B, 'C', th) symo.add_to_dict(th, atan2(S, C)) @@ -342,15 +391,15 @@ def _solve_type_3(symo, X1, Y1, Z1, X2, Y2, Z2, th): """ symo.write_line("# X1*sin({0}) + Y1*cos({0}) = Z1".format(th)) symo.write_line("# X2*sin({0}) + Y2*cos({0}) = Z2".format(th)) - X1 = symo.replace(symo.CS12_simp(X1), 'X1', th) - Y1 = symo.replace(symo.CS12_simp(Y1), 'Y1', th) - Z1 = symo.replace(symo.CS12_simp(Z1), 'Z1', th) - X2 = symo.replace(symo.CS12_simp(X2), 'X2', th) - Y2 = symo.replace(symo.CS12_simp(Y2), 'Y2', th) - Z2 = symo.replace(symo.CS12_simp(Z2), 'Z2', th) - if X1 == ZERO and Y2 == ZERO: + X1 = symo.replace(trigsimp(X1), 'X1', th) + Y1 = symo.replace(trigsimp(Y1), 'Y1', th) + Z1 = symo.replace(trigsimp(Z1), 'Z1', th) + X2 = symo.replace(trigsimp(X2), 'X2', th) + Y2 = symo.replace(trigsimp(Y2), 'Y2', th) + Z2 = symo.replace(trigsimp(Z2), 'Z2', th) + if X1 == tools.ZERO and Y2 == tools.ZERO: symo.add_to_dict(th, atan2(Z2/X2, Z1/Y1)) - elif X2 == ZERO and Y1 == ZERO: + elif X2 == tools.ZERO and Y1 == tools.ZERO: symo.add_to_dict(th, atan2(Z1/X1, Z2/Y2)) else: D = symo.replace(X1*Y2-X2*Y1, 'D', th) @@ -366,12 +415,12 @@ def _solve_type_4(symo, X1, Y1, X2, Y2, th, r): """ symo.write_line("# X1*sin({0})*{1} = Y1".format(th, r)) symo.write_line("# X2*cos({0})*{1} = Y2".format(th, r)) - X1 = symo.replace(symo.CS12_simp(X1), 'X1', th) - Y1 = symo.replace(symo.CS12_simp(Y1), 'Y1', th) - X2 = symo.replace(symo.CS12_simp(X2), 'X2', th) - Y2 = symo.replace(symo.CS12_simp(Y2), 'Y2', th) + X1 = symo.replace(trigsimp(X1), 'X1', th) + Y1 = symo.replace(trigsimp(Y1), 'Y1', th) + X2 = symo.replace(trigsimp(X2), 'X2', th) + Y2 = symo.replace(trigsimp(Y2), 'Y2', th) YPS = var('YPS' + r) - symo.add_to_dict(YPS, (ONE, - ONE)) + symo.add_to_dict(YPS, (tools.ONE, - tools.ONE)) symo.add_to_dict(r, YPS*sqrt((Y1/X1)**2 + (Y2/X2)**2)) symo.add_to_dict(th, atan2(Y1/(X1*r), Y2/(X2*r))) @@ -383,18 +432,18 @@ def _solve_type_5(symo, X1, Y1, Z1, X2, Y2, Z2, th, r): """ symo.write_line("# X1*sin({0}) = Y1 + Z1*{1}".format(th, r)) symo.write_line("# X2*cos({0}) = Y2 + Z2*{1}".format(th, r)) - X1 = symo.replace(symo.CS12_simp(X1), 'X1', th) - Y1 = symo.replace(symo.CS12_simp(Y1), 'Y1', th) - Z1 = symo.replace(symo.CS12_simp(Z1), 'Z1', th) - X2 = symo.replace(symo.CS12_simp(X2), 'X2', th) - Y2 = symo.replace(symo.CS12_simp(Y2), 'Y2', th) - Z2 = symo.replace(symo.CS12_simp(Z2), 'Z2', th) + X1 = symo.replace(trigsimp(X1), 'X1', th) + Y1 = symo.replace(trigsimp(Y1), 'Y1', th) + Z1 = symo.replace(trigsimp(Z1), 'Z1', th) + X2 = symo.replace(trigsimp(X2), 'X2', th) + Y2 = symo.replace(trigsimp(Y2), 'Y2', th) + Z2 = symo.replace(trigsimp(Z2), 'Z2', th) V1 = symo.replace(Y1/X1, 'V1', r) W1 = symo.replace(Z1/X1, 'W1', r) V2 = symo.replace(Y2/X2, 'V2', r) W2 = symo.replace(Z2/X2, 'W2', r) _solve_square(W1**2 + W2**2, 2*(V1*W1 + V2*W2), V1**2 + V2**2, r) - _solve_type_3(X1, ZERO, Y1 + Z1*r, ZERO, X2, Y2 + Z2*r) + _solve_type_3(X1, tools.ZERO, Y1 + Z1*r, tools.ZERO, X2, Y2 + Z2*r) def _solve_type_7(symo, V, W, X, Y, Z1, Z2, eps, th_i, th_j): @@ -406,12 +455,12 @@ def _solve_type_7(symo, V, W, X, Y, Z1, Z2, eps, th_i, th_j): symo.write_line(s.format(th_j, th_i)) s = "# eps*(V*sin({0}) - W*cos({0})) = X*sin({1}) - Y*cos({1}) + Z2" symo.write_line(s.format(th_j, th_i)) - V = symo.replace(symo.CS12_simp(V), 'V', th_i) - W = symo.replace(symo.CS12_simp(W), 'W', th_i) - X = symo.replace(symo.CS12_simp(X), 'X', th_i) - Y = symo.replace(symo.CS12_simp(Y), 'Y', th_i) - Z1 = symo.replace(symo.CS12_simp(Z1), 'Z1', th_i) - Z2 = symo.replace(symo.CS12_simp(Z2), 'Z2', th_i) + V = symo.replace(trigsimp(V), 'V', th_i) + W = symo.replace(trigsimp(W), 'W', th_i) + X = symo.replace(trigsimp(X), 'X', th_i) + Y = symo.replace(trigsimp(Y), 'Y', th_i) + Z1 = symo.replace(trigsimp(Z1), 'Z1', th_i) + Z2 = symo.replace(trigsimp(Z2), 'Z2', th_i) B1 = symo.replace(2*(Z1*Y + Z2*X), 'B1', th_i) B2 = symo.replace(2*(Z1*X - Z2*Y), 'B2', th_i) B3 = symo.replace(V**2 + W**2 - X**2 - Y**2 - Z1**2 - Z2**2, 'B3', th_i) @@ -419,11 +468,6 @@ def _solve_type_7(symo, V, W, X, Y, Z1, Z2, eps, th_i, th_j): Zi1 = symo.replace(X*cos(th_i) + Y*sin(th_i) + Z1, 'Zi1', th_j) Zi2 = symo.replace(X*sin(th_i) - Y*cos(th_i) + Z2, 'Zi2', th_j) _solve_type_3(symo, W, V, Zi1, eps*V, -eps*W, Zi2, th_j) -# print_eq(symo, "V1", "X*sin({0}) + Y*cos({0}) + Z1".format(th_i)) -# print_eq(symo, "V2", "X*cos({0}) - Y*sin({0}) + Z2".format(th_i)) -# print_eq(symo, "C", "(V1 - V2)/(2*W2)") -# print_eq(symo, "S", "(V1 + V2)/(2*W1)") -# print_eq(symo, th_j, "atan2(S, C)") def _solve_type_8(symo, X, Y, Z1, Z2, th_i, th_j): @@ -433,17 +477,17 @@ def _solve_type_8(symo, X, Y, Z1, Z2, th_i, th_j): """ symo.write_line("# X*cos({0}) + Y*cos({0} + {1}) = Z1".format(th_i, th_j)) symo.write_line("# X*sin({0}) + Y*sin({0} + {1}) = Z2".format(th_i, th_j)) - X = symo.replace(symo.CS12_simp(X), 'X', th_j) - Y = symo.replace(symo.CS12_simp(Y), 'Y', th_j) - Z1 = symo.replace(symo.CS12_simp(Z1), 'Z1', th_j) - Z2 = symo.replace(symo.CS12_simp(Z2), 'Z2', th_j) + X = symo.replace(trigsimp(X), 'X', th_j) + Y = symo.replace(trigsimp(Y), 'Y', th_j) + Z1 = symo.replace(trigsimp(Z1), 'Z1', th_j) + Z2 = symo.replace(trigsimp(Z2), 'Z2', th_j) Cj = symo.replace((Z1**2 + Z2**2 - X**2 - Y**2) / (2*X*Y), 'C', th_j) YPS = var('YPS%s' % th_j) - symo.add_to_dict(YPS, (ONE, - ONE)) + symo.add_to_dict(YPS, (tools.ONE, -tools.ONE)) symo.add_to_dict(th_j, atan2(YPS*sqrt(1 - Cj**2), Cj)) Q1 = symo.replace(X + Y*cos(th_j), 'Q1', th_i) - Q2 = symo.replace(X + Y*sin(th_j), 'Q2', th_i) - Den = symo.replace(Q1**2+Q2**2, 'Den', th_i) + Q2 = symo.replace(Y*sin(th_j), 'Q2', th_i) + Den = symo.replace(Q1**2 + Q2**2, 'Den', th_i) Si = symo.replace((Q1*Z2 - Q2*Z1)/Den, 'S', th_i) Ci = symo.replace((Q1*Z1 + Q2*Z2)/Den, 'C', th_i) symo.add_to_dict(th_i, atan2(Si, Ci)) @@ -458,16 +502,10 @@ def _solve_square(symo, A, B, C, x): C = symo.replace(C, 'C', x) Delta = symo.repalce(B**2 - 4*A*C, 'Delta', x) YPS = var('YPS' + x) - symo.add_to_dict(YPS, (ONE, - ONE)) + symo.add_to_dict(YPS, (tools.ONE, - tools.ONE)) symo.add_to_dict(x, (-B + YPS*sqrt(Delta))/(2*A)) -def _is_parallelogram(robo, i, j): - k = robo.common_root(i, j) - chi = robo.chain(i,k) - chj = robo.chain(j,k) - - def _check_const(consts, *xs): is_ok = True for coef in consts: @@ -478,16 +516,12 @@ def _check_const(consts, *xs): def _get_coefs(eq, A1, A2, *xs): eqe = eq.expand() - X = get_max_coef(eqe, A1) - eqe = eqe.xreplace({A1: ZERO}) - Y = get_max_coef(eqe, A2) - Z = eqe.xreplace({A2: ZERO}) -# is_ok = not X.has(A2) and not X.has(A1) and not Y.has(A2) + X = tools.get_max_coef(eqe, A1) + eqe = eqe.xreplace({A1: tools.ZERO}) + Y = tools.get_max_coef(eqe, A2) + Z = eqe.xreplace({A2: tools.ZERO}) + # is_ok = not X.has(A2) and not X.has(A1) and not Y.has(A2) is_ok = True is_ok &= _check_const((X, Y, Z), *xs) -# if is_ok != is_ok2: -# print 'GET COEF333333333333333333333333333333333333333333333"' -# print X, Y, Z, is_ok -# print eq, 'i', A1, 'i', A2 -# print xs return X, Y, Z, is_ok + diff --git a/pysymoro/core/kinematics.py b/pysymoro/kinematics.py similarity index 69% rename from pysymoro/core/kinematics.py rename to pysymoro/kinematics.py index 2db935d..9c7402d 100644 --- a/pysymoro/core/kinematics.py +++ b/pysymoro/kinematics.py @@ -1,16 +1,23 @@ -""" -This module of SYMORO package provides kinematic models' computation. +# -*- coding: utf-8 -*- + -The core symbolic library is sympy. +# This file is part of the OpenSYMORO project. Please see +# https://github.com/symoro/symoro/blob/master/LICENCE for the licence. -Needed modules : symoro.py, geometry.py -ECN - ARIA1 2013 """ -from sympy import Matrix, zeros -from symoro import Symoro, Init, hat -from symoro import FAIL, ZERO -from geometry import dgm, Transform, compute_rot_trans, Z_AXIS +This module of SYMORO package computes the kinematic models. +""" + + +from sympy import Matrix, zeros, trigsimp + +from pysymoro.geometry import dgm, Transform +from pysymoro.geometry import compute_rot_trans, Z_AXIS +from symoroutils import symbolmgr +from symoroutils import tools +from symoroutils.paramsinit import ParamsInit + TERMINAL = 0 ROOT = 1 @@ -27,31 +34,33 @@ def _omega_ij(robo, j, jRant, w, qdj): def _omega_dot_j(robo, j, jRant, w, wi, wdot, qdj, qddj): wdot[j] = jRant*wdot[robo.ant[j]] if robo.sigma[j] == 0: # revolute joint - wdot[j] += (qddj + hat(wi)*qdj) + wdot[j] += (qddj + tools.skew(wi)*qdj) return wdot[j] def _v_j(robo, j, antPj, jRant, v, w, qdj, forced=False): ant = robo.ant[j] - v[j] = jRant*(hat(w[ant])*antPj[j] + v[ant]) + v[j] = jRant*(tools.skew(w[ant])*antPj[j] + v[ant]) if robo.sigma[j] == 1: # prismatic joint v[j] += qdj return v[j] -def _v_dot_j(robo, symo, j, jRant, antPj, w, wi, wdot, U, vdot, qdj, qddj): - DV = Init.product_combinations(w[j]) +def _v_dot_j( + robo, symo, j, jRant, antPj, w, wi, wdot, U, vdot, qdj, qddj +): + DV = ParamsInit.product_combinations(w[j]) symo.mat_replace(DV, 'DV', j) hatw_hatw = Matrix([[-DV[3]-DV[5], DV[1], DV[2]], [DV[1], -DV[5]-DV[0], DV[4]], [DV[2], DV[4], -DV[3]-DV[0]]]) - U[j] = hatw_hatw + hat(wdot[j]) + U[j] = hatw_hatw + tools.skew(wdot[j]) symo.mat_replace(U[j], 'U', j) vsp = vdot[robo.ant[j]] + U[robo.ant[j]]*antPj[j] symo.mat_replace(vsp, 'VSP', j) vdot[j] = jRant*vsp if robo.sigma[j] == 1: # prismatic joint - vdot[j] += qddj + 2*hat(wi)*qdj + vdot[j] += qddj + 2*tools.skew(wi)*qdj return vdot[j] @@ -74,17 +83,17 @@ def _jac(robo, symo, n, i, j, chain=None, forced=False, trig_subs=False): """ Computes jacobian of frame n (with origin On in Oj) projected to frame i """ -# symo.write_geom_param(robo, 'Jacobian') + # symo.write_geom_param(robo, 'Jacobian') # TODO: Check projection frames, rewrite DGM call for higher efficiency - M = [] + J_col_list = [] if chain is None: chain = robo.chain(n) chain.reverse() -# chain_ext = chain + [robo.ant[min(chain)]] -# if not i in chain_ext: -# i = min(chain_ext) -# if not j in chain_ext: -# j = max(chain_ext) + # chain_ext = chain + [robo.ant[min(chain)]] + # if not i in chain_ext: + # i = min(chain_ext) + # if not j in chain_ext: + # j = max(chain_ext) kTj_dict = dgm(robo, symo, chain[0], j, key='left', trig_subs=trig_subs) kTj_tmp = dgm(robo, symo, chain[-1], j, key='left', trig_subs=trig_subs) kTj_dict.update(kTj_tmp) @@ -104,13 +113,14 @@ def _jac(robo, symo, n, i, j, chain=None, forced=False, trig_subs=False): J_col = dvdq.col_join(iak) else: J_col = Matrix([0, 0, 0, 0, 0, 0]) - M.append(J_col.T) - Jac = Matrix(M).T + J_col_list.append(J_col.T) + Jac = Matrix(J_col_list).T Jac = Jac.applyfunc(symo.simp) iRj = Transform.R(iTk_dict[i, j]) jTn = dgm(robo, symo, j, n, fast_form=False, trig_subs=trig_subs) jPn = Transform.P(jTn) - L = -hat(iRj*jPn) + L = -tools.skew(iRj*jPn) + L = L.applyfunc(trigsimp) if forced: symo.mat_replace(Jac, 'J', '', forced) L = symo.mat_replace(L, 'L', '', forced) @@ -130,7 +140,7 @@ def _jac_inv(robo, symo, n, i, j): J = _make_square(J) det = _jac_det(robo, symo, J=J) Jinv = J.adjugate() - if det == ZERO: + if det == tools.ZERO: print 'Matrix is singular!' else: Jinv = Jinv/det @@ -161,7 +171,7 @@ def extend_W(J, r, W, indx, chain): def _kinematic_loop_constraints(robo, symo, proj=None): if robo.NJ == robo.NL: - return FAIL + return tools.FAIL indx_c = robo.indx_cut indx_a = robo.indx_active indx_p = robo.indx_passive @@ -180,9 +190,9 @@ def _kinematic_loop_constraints(robo, symo, proj=None): chi.extend(chj) J = Ji.row_join(-Jj) for row in xrange(6): - if all(J[row, col] == ZERO for col in xrange(len(chi))): + if all(J[row, col] == tools.ZERO for col in xrange(len(chi))): continue - elif J[row, chi.index(i)] == ZERO: + elif J[row, chi.index(i)] == tools.ZERO: extend_W(J, row, W_a, indx_a, chi) extend_W(J, row, W_p, indx_p, chi) else: @@ -191,49 +201,65 @@ def _kinematic_loop_constraints(robo, symo, proj=None): extend_W(J, row, W_c, indx_c, chi) W_a, W_p = Matrix(W_a), Matrix(W_p) W_ac, W_pc, W_c = Matrix(W_ac), Matrix(W_pc), Matrix(W_c) - # print is for debug purpose -# print W_a -# print W_p -# print W_ac, W_pc, W_c return W_a, W_p, W_ac, W_pc, W_c -def compute_vel_acc(robo, symo, antRj, antPj, forced=False, gravity=True): +def compute_vel_acc( + robo, symo, antRj, antPj, forced=False, gravity=True, floating=False +): """Internal function. Computes speeds and accelerations usitn Parameters ========== robo : Robot Instance of robot description container - symo : Symoro + symo : symbolmgr.SymbolManager Instance of symbolic manager """ #init velocities and accelerations - w = Init.init_w(robo) - wdot, vdot = Init.init_wv_dot(robo, gravity) + w = ParamsInit.init_w(robo) + wdot, vdot = ParamsInit.init_wv_dot(robo, gravity) + # decide first link + first_link = 1 + if floating or robo.is_floating or robo.is_mobile: + first_link = 0 #init auxilary matrix - U = Init.init_U(robo) - for j in xrange(1, robo.NL): - jRant = antRj[j].T - qdj = Z_AXIS * robo.qdot[j] - qddj = Z_AXIS * robo.qddot[j] - wi, w[j] = _omega_ij(robo, j, jRant, w, qdj) - symo.mat_replace(w[j], 'W', j) - symo.mat_replace(wi, 'WI', j) - _omega_dot_j(robo, j, jRant, w, wi, wdot, qdj, qddj) - symo.mat_replace(wdot[j], 'WP', j, forced) - _v_dot_j(robo, symo, j, jRant, antPj, w, wi, wdot, U, vdot, qdj, qddj) - symo.mat_replace(vdot[j], 'VP', j, forced) + U = ParamsInit.init_u(robo) + for j in xrange(first_link, robo.NL): + if j == 0: + w[j] = symo.mat_replace(w[j], 'W', j) + wdot[j] = symo.mat_replace(wdot[j], 'WP', j) + vdot[j] = symo.mat_replace(vdot[j], 'VP', j) + dv0 = ParamsInit.product_combinations(w[j]) + symo.mat_replace(dv0, 'DV', j) + hatw_hatw = Matrix([ + [-dv0[3]-dv0[5], dv0[1], dv0[2]], + [dv0[1], -dv0[5]-dv0[0], dv0[4]], + [dv0[2], dv0[4], -dv0[3]-dv0[0]] + ]) + U[j] = hatw_hatw + tools.skew(wdot[j]) + symo.mat_replace(U[j], 'U', j) + else: + jRant = antRj[j].T + qdj = Z_AXIS * robo.qdot[j] + qddj = Z_AXIS * robo.qddot[j] + wi, w[j] = _omega_ij(robo, j, jRant, w, qdj) + symo.mat_replace(w[j], 'W', j) + symo.mat_replace(wi, 'WI', j) + _omega_dot_j(robo, j, jRant, w, wi, wdot, qdj, qddj) + symo.mat_replace(wdot[j], 'WP', j, forced) + _v_dot_j(robo, symo, j, jRant, antPj, w, wi, wdot, U, vdot, qdj, qddj) + symo.mat_replace(vdot[j], 'VP', j, forced) return w, wdot, vdot, U def velocities(robo): - symo = Symoro(None) + symo = symbolmgr.SymbolManager(None) symo.file_open(robo, 'vel') symo.write_params_table(robo, 'Link velocities') antRj, antPj = compute_rot_trans(robo, symo) - w = Init.init_w(robo) - v = Init.init_v(robo) + w = ParamsInit.init_w(robo) + v = ParamsInit.init_v(robo) for j in xrange(1, robo.NL): jRant = antRj[j].T qdj = Z_AXIS * robo.qdot[j] @@ -246,7 +272,7 @@ def velocities(robo): def accelerations(robo): - symo = Symoro(None) + symo = symbolmgr.SymbolManager(None) symo.file_open(robo, 'acc') symo.write_params_table(robo, 'Link accelerations') antRj, antPj = compute_rot_trans(robo, symo) @@ -255,19 +281,21 @@ def accelerations(robo): return symo -#very simial to comute_vel_acc def jdot_qdot(robo): - symo = Symoro(None) + """ + Similar to compute_vel_acc. + """ + symo = symbolmgr.SymbolManager(None) symo.file_open(robo, 'jpqp') symo.write_params_table(robo, 'JdotQdot') antRj, antPj = compute_rot_trans(robo, symo) - w = Init.init_w(robo) - wdot, vdot = Init.init_wv_dot(robo, gravity=False) - U = Init.init_U(robo) + w = ParamsInit.init_w(robo) + wdot, vdot = ParamsInit.init_wv_dot(robo, gravity=False) + U = ParamsInit.init_u(robo) for j in xrange(1, robo.NL): jRant = antRj[j].T qdj = Z_AXIS * robo.qdot[j] - qddj = Z_AXIS * ZERO + qddj = Z_AXIS * tools.ZERO wi, w[j] = _omega_ij(robo, j, jRant, w, qdj) symo.mat_replace(w[j], 'W', j) symo.mat_replace(wi, 'WI', j) @@ -280,18 +308,18 @@ def jdot_qdot(robo): def jacobian(robo, n, i, j): - symo = Symoro() + symo = symbolmgr.SymbolManager() symo.file_open(robo, 'jac') - title = "Jacobian matrix for frame %s\n" - title += "Projection frame %s, intermediat frame %s" - symo.write_params_table(robo, title % (n, i, j)) + title = "Jacobian matrix for frame {}\n" + title += "Projection frame {}, intermediate frame {}" + symo.write_params_table(robo, title.format(n, i, j)) _jac(robo, symo, n, i, j, forced=True) symo.file_close() return symo def jacobian_determinant(robo, n, i, j, rows, cols): - symo = Symoro(None) + symo = symbolmgr.SymbolManager(None) J, L = _jac(robo, symo, n, i, j, trig_subs=False) J_reduced = zeros(len(rows), len(cols)) for i, i_old in enumerate(rows): @@ -305,10 +333,10 @@ def jacobian_determinant(robo, n, i, j, rows, cols): def kinematic_constraints(robo): - symo = Symoro(None) + symo = symbolmgr.SymbolManager(None) res = _kinematic_loop_constraints(robo, symo) - if res == FAIL: - return FAIL + if res == tools.FAIL: + return tools.FAIL W_a, W_p, W_ac, W_pc, W_c = res symo.file_open(robo, 'ckel') symo.write_params_table(robo, 'Constraint kinematic equations of loop', @@ -331,28 +359,3 @@ def kinematic_constraints(robo): return symo -#symo = Symoro() -#from symoro import Symoro, Robot -#kinematic_constraints(Robot.SR400()) -##jacobian_determinant(robo, 6, range(6), range(6)) -###print _jac(robo, symo, 2, 5, 5) -###print _jac_det(robo, symo, 5) -###W = kinematic_loop_constraints(robo, symo) -###print W[0] -###print W[1] -###speeds_accelerations(robo, symo) -###print _jac_inv(Robot.RX90(), symo, 2, 5, 5) -## -#def b(): -# symo = Symoro() -# print _jac_inv(Robot.RX90(), symo, 6, 3, 3) -####from timeit import timeit -#####print timeit(a, number=10) -#####print timeit(b, number=10) -#### -#import profile -### -##profile.run('b()', sort = 'cumtime') -##profile.run('b()') -#from timeit import timeit -#print timeit(b, number=1) diff --git a/pysymoro/main.py b/pysymoro/main.py deleted file mode 100644 index 901e509..0000000 --- a/pysymoro/main.py +++ /dev/null @@ -1,667 +0,0 @@ -__author__ = 'Izzat' -import os -import wx -from core.symoro import Robot, FAIL -from core import geometry, kinematics, dynamics, invgeom -from visualize import graphics -from gui import ui_definition, ui_geometry, ui_kinematics -from core.parfile import readpar, writepar - - -PROG_NAME = 'SYMORO-Python' - - -class MainFrame(wx.Frame): - def __init__(self): - main_title = PROG_NAME + ": SYmbolic MOdeling of RObots" - style = wx.DEFAULT_FRAME_STYLE ^ wx.RESIZE_BORDER ^ wx.MAXIMIZE_BOX - wx.Frame.__init__(self, None, title=main_title, size=(0, 0), style=style) - self.Bind(wx.EVT_CLOSE, self.OnClose) - - self.create_mnu() - self.robo = Robot.RX90() - self.widgets = {} - self.par_dict = {} - - self.statusbar = self.CreateStatusBar() - self.p = wx.Panel(self) - self.mainSizer = wx.BoxSizer(wx.VERTICAL) - - m_text = wx.StaticText(self.p, -1, "SYmbolic MOdelling of RObots") - m_text.SetFont(wx.Font(14, wx.SWISS, wx.NORMAL, wx.BOLD)) - m_text.SetSize(m_text.GetBestSize()) - self.mainSizer.Add(m_text, 0, wx.TOP | wx.ALIGN_CENTER_HORIZONTAL, 15) - - self.create_ui() - self.p.SetSizerAndFit(self.mainSizer) - self.Fit() - self.feed_data() - - def params_grid(self, grid, rows, elems, handler=None, start_i=0, size=60): - for i, name in enumerate(elems): - horBox = wx.BoxSizer(wx.HORIZONTAL) - horBox.Add(wx.StaticText(self.p, label=name, - size=(40, -1), style=wx.ALIGN_RIGHT), - 0, wx.ALL | wx.ALIGN_RIGHT, 5) - textBox = wx.TextCtrl(self.p, size=(size, -1), name=name, id=i%rows) - self.widgets[name] = textBox - textBox.Bind(wx.EVT_KILL_FOCUS, handler) - horBox.Add(textBox, 0, wx.ALL | wx.ALIGN_LEFT, 1) - grid.Add(horBox, pos=(i/rows, i % rows + start_i), - flag=wx.ALL, border=2) - - def create_ui(self): - descr_sizer = wx.StaticBoxSizer( - wx.StaticBox(self.p, label='Robot Description'), wx.HORIZONTAL) - sizer1 = wx.BoxSizer(wx.VERTICAL) - descr_sizer.AddSpacer(3) - descr_sizer.Add(sizer1, 0, wx.ALL | wx.EXPAND, 5) - sizer2 = wx.BoxSizer(wx.VERTICAL) - descr_sizer.Add(sizer2, 0, wx.ALL | wx.EXPAND, 5) - self.mainSizer.Add(descr_sizer, 0, wx.ALL, 10) - - # Left Side of main window - robot_type_sizer = wx.StaticBoxSizer( - wx.StaticBox(self.p, label='Robot Type'), wx.HORIZONTAL) - grid = wx.GridBagSizer(12, 10) - - gen_info = [('Name of the robot:', 'name'), - ('Number of moving links:', 'NL'), - ('Number of joints:', 'NJ'), ('Number of frames:', 'NF'), - ('Type of structure:', 'type'), ('Is Mobile:', 'mobile'), - ('Number of closed loops:', 'loops')] - - for i, (lab, name) in enumerate(gen_info): - label = wx.StaticText(self.p, label=lab) - grid.Add(label, pos=(i, 0), flag=wx.LEFT, border=10) - label = wx.StaticText(self.p, size=(125, -1), name=name) - self.widgets[name] = label - grid.Add(label, pos=(i, 1), flag=wx.LEFT | wx.RIGHT, border=10) - - robot_type_sizer.Add(grid, 0, wx.TOP | wx.BOTTOM | wx.EXPAND, 6) - sizer1.Add(robot_type_sizer) - - ##### Gravity components - sizer1.AddSpacer(8) - sbs_gravity = wx.StaticBoxSizer( - wx.StaticBox(self.p, label='Gravity components'), wx.HORIZONTAL) - for iden, name in enumerate(['GX', 'GY', 'GZ']): - sbs_gravity.AddSpacer(5) - sbs_gravity.Add(wx.StaticText(self.p, label=name), 0, wx.ALL, 4) - text_box = wx.TextCtrl(self.p, name=name, size=(60, -1), id=iden) - self.widgets[name] = text_box - text_box.Bind(wx.EVT_KILL_FOCUS, self.OnBaseTwistChanged) - sbs_gravity.Add(text_box, 0, wx.ALL | wx.ALIGN_LEFT, 2) - sizer1.Add(sbs_gravity, 0, wx.ALL | wx.EXPAND, 0) - - ##### Location of the robot - sizer1.AddSpacer(8) - sbs_location = wx.StaticBoxSizer( - wx.StaticBox(self.p, label='Location of the robot'), wx.HORIZONTAL) - sizer1.Add(sbs_location, 0, wx.ALL | wx.EXPAND, 0) - loc_tbl = wx.GridBagSizer(1, 1) - for i in range(4): - ver_lbl = wx.StaticText(self.p, label='Z' + str(i + 1) + ': ') - hor_lbl = wx.StaticText(self.p, label='Z - ' + str(i + 1)) - botLab = wx.StaticText(self.p, label=' ' + str(0 if i < 3 else 1)) - loc_tbl.Add(ver_lbl, pos=(i + 1, 0), flag=wx.RIGHT, border=3) - loc_tbl.Add(hor_lbl, pos=(0, i + 1), - flag=wx.ALIGN_CENTER_HORIZONTAL, border=3) - loc_tbl.Add(botLab, pos=(4, i+1), flag=wx.ALIGN_LEFT, border=3) - for j in range(3): - index = j*4+i - name = 'Z'+str(index) - text_box = wx.TextCtrl(self.p, name=name, - size=(60, -1), id=index) - self.widgets[name] = text_box - text_box.Bind(wx.EVT_KILL_FOCUS, self.OnZParamChanged) - loc_tbl.Add(text_box, pos=(j + 1, i + 1), - flag=wx.ALIGN_LEFT, border=5) - sbs_location.Add(loc_tbl, 0, wx.ALL | wx.EXPAND, 5) - - ##### Geometric Params - sbs = wx.StaticBoxSizer( - wx.StaticBox(self.p, label='Geometric Params'), wx.HORIZONTAL) - grid = wx.GridBagSizer(0, 5) - ver_sizer = wx.BoxSizer(wx.VERTICAL) - ver_sizer.Add(wx.StaticText(self.p, label='Frame'), - 0, wx.ALL | wx.ALIGN_CENTER_HORIZONTAL, 5) - combo_box = wx.ComboBox(self.p, style=wx.CB_READONLY, - size=(60, -1), name='frame') - self.widgets['frame'] = combo_box - combo_box.Bind(wx.EVT_COMBOBOX, self.OnFrameChanged) - ver_sizer.Add(combo_box) - for i, name in enumerate(self.robo.get_geom_head()[1:4]): - hor_box = wx.BoxSizer(wx.HORIZONTAL) - label = wx.StaticText(self.p, label=name, size=(40, -1), - style=wx.ALIGN_RIGHT) - self.widgets[name] = label - hor_box.Add(label, 0, wx.ALL | wx.ALIGN_RIGHT, 5) - combo_box = wx.ComboBox(self.p, style=wx.CB_READONLY, - size=(60, -1), name=name) - self.widgets[name] = combo_box - combo_box.Bind(wx.EVT_COMBOBOX, self.OnGeoParamChanged) - hor_box.Add(combo_box, 0, wx.ALL | wx.ALIGN_LEFT, 1) - grid.Add(hor_box, pos=(i, 1), flag=wx.ALL, border=2) - - grid.Add(ver_sizer, pos=(0, 0), flag=wx.ALL | wx.ALIGN_CENTER_VERTICAL, - span=(3, 1), border=5) - - self.params_grid(grid, 2, self.robo.get_geom_head()[4:], - self.OnGeoParamChanged, 2, 111) - - sbs.Add(grid) - sizer2.Add(sbs, 0, wx.ALL | wx.EXPAND, 0) - - ##### Dynamic Params and external forces - lbl = 'Dynamic Params and external forces' - sbs = wx.StaticBoxSizer(wx.StaticBox(self.p, label=lbl), wx.HORIZONTAL) - grid = wx.GridBagSizer(0, 0) - ver_sizer = wx.BoxSizer(wx.VERTICAL) - ver_sizer.Add(wx.StaticText(self.p, label='Link'), - 0, wx.ALL | wx.ALIGN_CENTER_HORIZONTAL, 5) - combo_box = wx.ComboBox(self.p, style=wx.CB_READONLY, - size=(60, -1), name='link') - combo_box.Bind(wx.EVT_COMBOBOX, self.OnLinkChanged) - self.widgets['link'] = combo_box - ver_sizer.Add(combo_box) - grid.Add(ver_sizer, pos=(0, 0), flag=wx.ALL | wx.ALIGN_CENTER_VERTICAL, - span=(5, 1), border=5) - - params = self.robo.get_dynam_head()[1:] - params += self.robo.get_ext_dynam_head()[1:-3] - self.params_grid(grid, 4, params, self.OnDynParamChanged, 1) - - sbs.Add(grid) - sbs.AddSpacer(4) - sizer2.AddSpacer(8) - sizer2.Add(sbs, 0, wx.ALL | wx.EXPAND, 0) - - ##### Speed and acceleration of the base - lbl = 'Speed and acceleration of the base' - sbs = wx.StaticBoxSizer(wx.StaticBox(self.p, label=lbl), wx.HORIZONTAL) - grid = wx.GridBagSizer(0, 0) - sbs.Add(grid) - hor_sizer = wx.BoxSizer(wx.HORIZONTAL) - hor_sizer.Add(sbs) - hor_sizer.AddSpacer(8) - - params = [] - for name in self.robo.get_base_vel_head()[1:-1]: - for c in ['X', 'Y', 'Z']: - params.append(name + c) - - self.params_grid(grid, 3, params, self.OnBaseTwistChanged, 0) - - ##### Joints velocity and acceleration - lbl = 'Joint velocity and acceleration' - sbs2 = wx.StaticBoxSizer(wx.StaticBox(self.p, label=lbl), wx.VERTICAL) - sbs2.AddSpacer(5) - grid = wx.GridBagSizer(5, 5) - sbs2.Add(grid) - combo_box = wx.ComboBox(self.p, size=(70, -1), - style=wx.CB_READONLY, name='joint') - self.widgets['joint'] = combo_box - combo_box.Bind(wx.EVT_COMBOBOX, self.OnJointChanged) - grid.Add(combo_box, pos=(0, 1), - flag=wx.ALIGN_CENTER_HORIZONTAL, border=0) - names = ['Joint'] + self.robo.get_ext_dynam_head()[-3:] - for i, name in enumerate(names): - label = wx.StaticText(self.p, label=name, - size=(55, -1), style=wx.ALIGN_RIGHT) - grid.Add(label, pos=(i, 0), flag=wx.TOP | wx.RIGHT, border=3) - if i > 0: - text_box = wx.TextCtrl(self.p, name=name, size=(70, -1)) - self.widgets[name] = text_box - text_box.Bind(wx.EVT_KILL_FOCUS, self.OnSpeedChanged) - grid.Add(text_box, pos=(i, 1)) - - hor_sizer.Add(sbs2, 1, wx.ALL | wx.EXPAND, 0) - sizer2.AddSpacer(8) - sizer2.Add(hor_sizer, 1, wx.ALL | wx.EXPAND, 0) - self.mainSizer.AddSpacer(10) - #sizer2.Add(sbs, 0, wx.ALL | wx.EXPAND, 0) - - def Change(self, index, name, evtObject): - prev_value = str(self.robo.get_val(index, name)) - if evtObject.Value != prev_value: - if self.robo.put_val(index, name, evtObject.Value) == FAIL: - message = "Unacceptable value '%s' has been input in %s%s" \ - % (evtObject.Value, name, index) - self.message_error(message) - evtObject.Value = prev_value - else: - self.changed = True - - def OnGeoParamChanged(self, evt): - frame_index = int(self.widgets['frame'].Value) - self.Change(frame_index, evt.EventObject.Name, evt.EventObject) - if evt.EventObject.Name == 'ant': - self.widgets['type'].SetLabel(self.robo.structure) - if evt.EventObject.Name == 'sigma': - self.update_geo_params() - - def OnDynParamChanged(self, evt): - link_index = int(self.widgets['link'].Value) - self.Change(link_index, evt.EventObject.Name, evt.EventObject) - # print type(self.robo.get_val(link_index, evt.EventObject.Name)) - - def OnSpeedChanged(self, evt): - joint_index = int(self.widgets['joint'].Value) - self.Change(joint_index, evt.EventObject.Name, evt.EventObject) - - def OnBaseTwistChanged(self, evt): - index = int(evt.EventObject.Id) - name = evt.EventObject.Name[:-1] - self.Change(index, name, evt.EventObject) - - def OnZParamChanged(self, evt): - index = int(evt.EventObject.Id) - self.Change(index, 'Z', evt.EventObject) - - def OnFrameChanged(self, evt): - frame_index = int(evt.EventObject.Value) - cmb = self.widgets['ant'] - cmb.SetItems([str(i) for i in range(frame_index)]) - self.update_geo_params() - - def OnLinkChanged(self, _): - self.update_dyn_params() - - def OnJointChanged(self, _): - self.update_vel_params() - - def update_params(self, index, pars): - for par in pars: - widget = self.widgets[par] - widget.ChangeValue(str(self.robo.get_val(index, par))) - - def update_geo_params(self): - index = int(self.widgets['frame'].Value) - for par in self.robo.get_geom_head()[1:4]: - self.widgets[par].SetValue(str(self.robo.get_val(index, par))) - self.update_params(index, self.robo.get_geom_head()[4:]) - - def update_dyn_params(self): - pars = self.robo.get_dynam_head()[1:] - # cut first and last 3 elements - pars += self.robo.get_ext_dynam_head()[1:-3] - - index = int(self.widgets['link'].Value) - self.update_params(index, pars) - - def update_vel_params(self): - pars = self.robo.get_ext_dynam_head()[-3:] - index = int(self.widgets['joint'].Value) - self.update_params(index, pars) - - def update_base_twist_params(self): - for name in self.robo.get_base_vel_head()[1:]: - for i, c in enumerate(['X', 'Y', 'Z']): - widget = self.widgets[name + c] - widget.ChangeValue(str(self.robo.get_val(i, name))) - - def update_z_params(self): - T = self.robo.Z - for i in range(12): - widget = self.widgets['Z' + str(i)] - widget.ChangeValue(str(T[i])) - - def feed_data(self): - # Robot Type - names = [('name', self.robo.name), ('NF', self.robo.nf), - ('NL', self.robo.nl), ('NJ', self.robo.nj), - ('type', self.robo.structure), - ('mobile', self.robo.is_mobile), - ('loops', self.robo.nj-self.robo.nl)] - for name, info in names: - label = self.widgets[name] - label.SetLabel(str(info)) - - lsts = [('frame', [str(i) for i in range(1, self.robo.NF)]), - ('link', [str(i) for i in range(int(not self.robo.is_mobile), - self.robo.NL)]), - ('joint', [str(i) for i in range(1, self.robo.NJ)]), - ('ant', ['0']), ('sigma', ['0', '1', '2']), ('mu', ['0', '1'])] - for name, lst in lsts: - cmb = self.widgets[name] - cmb.SetItems(lst) - cmb.SetSelection(0) - - self.update_geo_params() - self.update_dyn_params() - self.update_vel_params() - self.update_base_twist_params() - self.update_z_params() - - self.changed = False - self.par_dict = {} - - def create_mnu(self): - mnu_bar = wx.MenuBar() - - ##### FILE - file_mnu = wx.Menu() - m_new = file_mnu.Append(wx.ID_NEW, '&New...') - self.Bind(wx.EVT_MENU, self.OnNew, m_new) - m_open = file_mnu.Append(wx.ID_OPEN, '&Open...') - self.Bind(wx.EVT_MENU, self.OnOpen, m_open) - m_save = file_mnu.Append(wx.ID_SAVE, '&Save...') - self.Bind(wx.EVT_MENU, self.OnSave, m_save) - m_save_as = file_mnu.Append(wx.ID_SAVEAS, '&Save As...') - self.Bind(wx.EVT_MENU, self.OnSaveAs, m_save_as) - file_mnu.Append(wx.ID_PREFERENCES, '&Preferences...') - file_mnu.AppendSeparator() - - m_exit = file_mnu.Append(wx.ID_EXIT, "E&xit\tAlt-X", - "Close window and exit program.") - self.Bind(wx.EVT_MENU, self.OnClose, m_exit) - mnu_bar.Append(file_mnu, "&File") - - ##### GEOMETRIC - geo_mnu = wx.Menu() - m_trans_matrix = wx.MenuItem(geo_mnu, wx.ID_ANY, - "Transformation matrix...") - self.Bind(wx.EVT_MENU, self.OnTransformationMatrix, m_trans_matrix) - geo_mnu.AppendItem(m_trans_matrix) - fast_dgm = wx.MenuItem(geo_mnu, wx.ID_ANY, "Fast geometric model...") - self.Bind(wx.EVT_MENU, self.OnFastGeometricModel, fast_dgm) - geo_mnu.AppendItem(fast_dgm) - igm_paul = wx.MenuItem(geo_mnu, wx.ID_ANY, "I.G.M. Paul method...") - self.Bind(wx.EVT_MENU, self.OnIGMPaul, igm_paul) - geo_mnu.AppendItem(igm_paul) - constr_geom = wx.MenuItem(geo_mnu, wx.ID_ANY, - "Constraint geometric equations of loops") - self.Bind(wx.EVT_MENU, self.OnConstraintGeoEq, constr_geom) - geo_mnu.AppendItem(constr_geom) - - mnu_bar.Append(geo_mnu, "&Geometric") - - ##### KINEMATIC - kin_mnu = wx.Menu() - jac_matrix = wx.MenuItem(kin_mnu, wx.ID_ANY, "Jacobian matrix...") - self.Bind(wx.EVT_MENU, self.OnJacobianMatrix, jac_matrix) - kin_mnu.AppendItem(jac_matrix) - determ = wx.MenuItem(kin_mnu, wx.ID_ANY, "Determinant of a Jacobian...") - self.Bind(wx.EVT_MENU, self.OnDeterminant, determ) - kin_mnu.AppendItem(determ) - #TODO: add the dialog, ask for projection frame - ckel = wx.MenuItem(kin_mnu, wx.ID_ANY, "Kinematic constraints") - self.Bind(wx.EVT_MENU, self.OnCkel, ckel) - kin_mnu.AppendItem(ckel) - vels = wx.MenuItem(kin_mnu, wx.ID_ANY, "Velocities") - self.Bind(wx.EVT_MENU, self.OnVelocities, vels) - kin_mnu.AppendItem(vels) - accel = wx.MenuItem(kin_mnu, wx.ID_ANY, "Accelerations") - self.Bind(wx.EVT_MENU, self.OnAccelerations, accel) - kin_mnu.AppendItem(accel) - jpqp = wx.MenuItem(kin_mnu, wx.ID_ANY, "Jpqp") - self.Bind(wx.EVT_MENU, self.OnJpqp, jpqp) - kin_mnu.AppendItem(jpqp) - - mnu_bar.Append(kin_mnu, "&Kinematic") - - ##### DYNAMIC - dyn_mnu = wx.Menu() - dyn_submnu = wx.MenuItem(dyn_mnu, wx.ID_ANY, 'Inverse dynamic model') - self.Bind(wx.EVT_MENU, self.OnInverseDynamic, dyn_submnu) - dyn_mnu.AppendItem(dyn_submnu) - dyn_submnu = wx.MenuItem(dyn_mnu, wx.ID_ANY, 'Inertia Matrix') - self.Bind(wx.EVT_MENU, self.OnInertiaMatrix, dyn_submnu) - dyn_mnu.AppendItem(dyn_submnu) - dyn_submnu = wx.MenuItem(dyn_mnu, wx.ID_ANY, - 'Centrifugal, Coriolis & Gravity torques') - self.Bind(wx.EVT_MENU, self.OnCentrCoriolGravTorq, dyn_submnu) - dyn_mnu.AppendItem(dyn_submnu) - dyn_submnu = wx.MenuItem(dyn_mnu, wx.ID_ANY, 'Direct Dynamic Model') - self.Bind(wx.EVT_MENU, self.OnDirectDynamicModel, dyn_submnu) - dyn_mnu.AppendItem(dyn_submnu) - - mnu_bar.Append(dyn_mnu, "&Dynamic") - - ##### IDENTIFICATION - iden = wx.Menu() - base_inert = wx.MenuItem( - iden, wx.ID_ANY, 'Base inertial parameters (symbolic or numeric)') - self.Bind(wx.EVT_MENU, self.OnBaseInertialParams, base_inert) - iden.AppendItem(base_inert) - dyn_iden_model = wx.MenuItem(iden, wx.ID_ANY, - 'Dynamic identification model') - self.Bind(wx.EVT_MENU, self.OnDynIdentifModel, dyn_iden_model) - iden.AppendItem(dyn_iden_model) - - mnu_bar.Append(iden, "&Identification") - - ##### OPTIMIZER - # optMenu = wx.Menu() - # jac_matrix = wx.MenuItem(optMenu, wx.ID_ANY, "Jacobian matrix...") - # self.Bind(wx.EVT_MENU, self.OnJacobianMatrix, jac_matrix) - # optMenu.AppendItem(jac_matrix) - # - # menuBar.Append(optMenu, "&Optimizer") - - ##### VISUALIZATION - vis_mnu = wx.Menu() - vis_menu = wx.MenuItem(vis_mnu, wx.ID_ANY, "Visualisation") - self.Bind(wx.EVT_MENU, self.OnVisualisation, vis_menu) - vis_mnu.AppendItem(vis_menu) - - mnu_bar.Append(vis_mnu, "&Visualization") - - self.SetMenuBar(mnu_bar) - - def OnNew(self, _): - dialog = ui_definition.DialogDefinition( - PROG_NAME, self.robo.name, self.robo.nl, - self.robo.nj, self.robo.structure, self.robo.is_mobile) - if dialog.ShowModal() == wx.ID_OK: - result = dialog.get_values() - new_robo = Robot(*result['init_pars']) - if result['keep_geo']: - nf = min(self.robo.NF, new_robo.NF) - new_robo.ant[:nf] = self.robo.ant[:nf] - new_robo.sigma[:nf] = self.robo.sigma[:nf] - new_robo.mu[:nf] = self.robo.mu[:nf] - new_robo.gamma[:nf] = self.robo.gamma[:nf] - new_robo.alpha[:nf] = self.robo.alpha[:nf] - new_robo.theta[:nf] = self.robo.theta[:nf] - new_robo.b[:nf] = self.robo.b[:nf] - new_robo.d[:nf] = self.robo.d[:nf] - new_robo.r[:nf] = self.robo.r[:nf] - if result['keep_dyn']: - nl = min(self.robo.NL, new_robo.NL) - new_robo.Nex[:nl] = self.robo.Nex[:nl] - new_robo.Fex[:nl] = self.robo.Fex[:nl] - new_robo.FS[:nl] = self.robo.FS[:nl] - new_robo.IA[:nl] = self.robo.IA[:nl] - new_robo.FV[:nl] = self.robo.FV[:nl] - new_robo.MS[:nl] = self.robo.MS[:nl] - new_robo.M[:nl] = self.robo.M[:nl] - new_robo.J[:nl] = self.robo.J[:nl] - if result['keep_base']: - new_robo.Z = self.robo.Z - new_robo.w0 = self.robo.w0 - new_robo.wdot0 = self.robo.wdot0 - new_robo.v0 = self.robo.v0 - new_robo.vdot0 = self.robo.vdot0 - new_robo.G = self.robo.G - self.robo = new_robo - directory = os.path.join('robots', self.robo.name) - if not os.path.exists(directory): - os.makedirs(directory) - self.robo.directory = directory - self.feed_data() - dialog.Destroy() - - def message_error(self, message): - wx.MessageDialog(None, message, - 'Error', wx.OK | wx.ICON_ERROR).ShowModal() - - def message_warning(self, message): - wx.MessageDialog(None, message, - 'Error', wx.OK | wx.ICON_WARNING).ShowModal() - - def message_info(self, message): - wx.MessageDialog(None, message, 'Information', - wx.OK | wx.ICON_INFORMATION).ShowModal() - - def model_success(self, model_name): - msg = 'The model has been saved in %s\\%s_%s.txt' % \ - (self.robo.directory, self.robo.name, model_name) - self.message_info(msg) - - def OnOpen(self, _): - if self.changed: - dialog_res = wx.MessageBox('Do you want to save changes?', - 'Please confirm', - wx.ICON_QUESTION | - wx.YES_NO | wx.CANCEL, - self) - if dialog_res == wx.CANCEL: - return - elif dialog_res == wx.YES: - if self.OnSave(None) == FAIL: - return - dialog = wx.FileDialog(self, message="Choose PAR file", style=wx.OPEN, - wildcard='*.par', defaultFile='*.par') - if dialog.ShowModal() == wx.ID_OK: - new_robo, flag = readpar(dialog.GetDirectory(), - dialog.GetFilename()[:-4]) - if new_robo is None: - self.message_error('File could not be read!') - else: - if flag == FAIL: - self.message_warning('While reading file an error occured.') - self.robo = new_robo - self.feed_data() - - def OnSave(self, _): - writepar(self.robo) - self.changed = False - - def OnSaveAs(self, _): - dialog = wx.FileDialog(self, message="Save PAR file", - defaultFile=self.robo.name+'.par', - defaultDir=self.robo.directory, - wildcard='*.par') - if dialog.ShowModal() == wx.ID_CANCEL: - return FAIL - - self.robo.directory = dialog.GetDirectory() - self.robo.name = dialog.GetFilename()[:-4] - writepar(self.robo) - self.widgets['name'].SetLabel(self.robo.name) - self.changed = False - - def OnTransformationMatrix(self, _): - dialog = ui_geometry.DialogTrans(PROG_NAME, self.robo.NF) - if dialog.ShowModal() == wx.ID_OK: - frames, trig_subs = dialog.GetValues() - geometry.direct_geometric(self.robo, frames, trig_subs) - self.model_success('trm') - dialog.Destroy() - - def OnFastGeometricModel(self, _): - dialog = ui_geometry.DialogFast(PROG_NAME, self.robo.NF) - if dialog.ShowModal() == wx.ID_OK: - i, j = dialog.GetValues() - geometry.direct_geometric_fast(self.robo, i, j) - self.model_success('fgm') - dialog.Destroy() - - def OnIGMPaul(self, _): - dialog = ui_geometry.DialogPaul(PROG_NAME, self.robo.endeffectors, - str(invgeom.EMPTY)) - if dialog.ShowModal() == wx.ID_OK: - lst_T, n = dialog.get_values() - invgeom.igm_Paul(self.robo, lst_T, n) - self.model_success('igm') - dialog.Destroy() - - def OnConstraintGeoEq(self, _): - pass - - def OnJacobianMatrix(self, _): - dialog = ui_kinematics.DialogJacobian(PROG_NAME, self.robo) - if dialog.ShowModal() == wx.ID_OK: - n, i, j = dialog.get_values() - kinematics.jacobian(self.robo, n, i, j) - self.model_success('jac') - dialog.Destroy() - - def OnDeterminant(self, _): - dialog = ui_kinematics.DialogDeterminant(PROG_NAME, self.robo) - if dialog.ShowModal() == wx.ID_OK: - kinematics.jacobian_determinant(self.robo, *dialog.get_values()) - self.model_success('det') - dialog.Destroy() - - def OnCkel(self, _): - if kinematics.kinematic_constraints(self.robo) == FAIL: - self.message_warning('There are no loops') - else: - self.model_success('ckel') - - def OnVelocities(self, _): - kinematics.velocities(self.robo) - self.model_success('vlct') - - def OnAccelerations(self, _): - kinematics.accelerations(self.robo) - self.model_success('aclr') - - def OnJpqp(self, _): - kinematics.jdot_qdot(self.robo) - self.model_success('jpqp') - - def OnInverseDynamic(self, _): - dynamics.inverse_dynamic_NE(self.robo) - self.model_success('idm') - - def OnInertiaMatrix(self, _): - dynamics.inertia_matrix(self.robo) - self.model_success('inm') - - def OnCentrCoriolGravTorq(self, _): - dynamics.pseudo_force_NE(self.robo) - self.model_success('ccg') - - def OnDirectDynamicModel(self, _): - dynamics.direct_dynamic_NE(self.robo) - self.model_success('ddm') - - def OnBaseInertialParams(self, _): - dynamics.base_paremeters(self.robo) - self.model_success('regp') - - def OnDynIdentifModel(self, _): - dynamics.dynamic_identification_NE(self.robo) - self.model_success('dim') - - def OnVisualisation(self, _): - dialog = ui_definition.DialogConversion(PROG_NAME, - self.robo, self.par_dict) - if dialog.has_syms(): - if dialog.ShowModal() == wx.ID_OK: - self.par_dict = dialog.get_values() - graphics.MainWindow(PROG_NAME, self.robo, self.par_dict, self) - else: - graphics.MainWindow(PROG_NAME, self.robo, self.par_dict, self) - - def OnClose(self, _): - if self.changed: - result = wx.MessageBox( - 'Do you want to save changes?', 'Please confirm', - wx.ICON_QUESTION | wx.YES_NO | wx.CANCEL, self) - if result == wx.YES: - if self.OnSave(_) == FAIL: - return - elif result == wx.CANCEL: - return - self.Destroy() - -app = wx.App(redirect=False) -main = MainFrame() -main.Show() -app.MainLoop() - diff --git a/pysymoro/nealgos.py b/pysymoro/nealgos.py new file mode 100644 index 0000000..60084d4 --- /dev/null +++ b/pysymoro/nealgos.py @@ -0,0 +1,884 @@ +# -*- coding: utf-8 -*- + + +from copy import copy + +import sympy +from sympy import Matrix + +from pysymoro.geometry import compute_screw_transform +from pysymoro.geometry import compute_rot_trans +from pysymoro.kinematics import compute_vel_acc +from pysymoro.kinematics import compute_omega +from symoroutils import tools +from symoroutils.paramsinit import ParamsInit + + +def inertia_spatial(inertia, ms_tensor, mass): + """ + Compute spatial inertia matrix (internal function). + """ + return Matrix([ + (mass * sympy.eye(3)).row_join(tools.skew(ms_tensor).transpose()), + tools.skew(ms_tensor).row_join(inertia) + ]) + + +def compute_torque(robo, symo, j, jaj, react_wrench, torque): + """ + Compute torque (internal function). + + Note: + torque is the output parameter. + """ + if robo.sigma[j] == 2: + tau_total = 0 + else: + tau = react_wrench[j].transpose() * jaj[j] + fric_rotor = robo.fric_s(j) + robo.fric_v(j) + robo.tau_ia(j) + tau_total = tau[0, 0] + fric_rotor + torque[j] = symo.replace(tau_total, 'GAM', j, forced=True) + + +def compute_joint_torque(robo, symo, j, Fjnt, Njnt, torque): + """ + Compute actuator torques - projection of joint wrench on the joint + axis (internal function). + + Note: + torque is the output parameter. + """ + if robo.sigma[j] == 2: + tau_total = 0 + else: + tau = (robo.sigma[j] * Fjnt[j]) + ((1 - robo.sigma[j]) * Njnt[j]) + fric_rotor = robo.fric_s(j) + robo.fric_v(j) + robo.tau_ia(j) + tau_total = tau[2] + fric_rotor + torque[j] = symo.replace(tau_total, 'GAM', j, forced=True) + + +def compute_dynamic_wrench(robo, symo, j, w, wdot, U, vdot, F, N): + """ + Compute total wrench of link j (internal function). + + Note: + F, N are the output parameters + """ + F[j] = (robo.M[j] * vdot[j]) + (U[j] * robo.MS[j]) + F[j] = symo.mat_replace(F[j], 'F', j) + Psi = robo.J[j] * w[j] + Psi = symo.mat_replace(Psi, 'PSI', j) + N[j] = (robo.J[j] * wdot[j]) + (tools.skew(w[j]) * Psi) + N[j] = symo.mat_replace(N[j], 'No', j) + + +def compute_joint_wrench( + robo, symo, j, antRj, antPj, vdot, F, N, Fjnt, Njnt, Fex, Nex +): + """ + Compute reaction wrench (for default Newton-Euler) of joint j + (internal function). + + Note: + Fjnt, Njnt, Fex, Nex are the output parameters + """ + forced = True if j == 0 else False + i = robo.ant[j] + Fjnt[j] = F[j] + Fex[j] + Fjnt[j] = symo.mat_replace(Fjnt[j], 'E', j, forced=forced) + Njnt[j] = N[j] + Nex[j] + (tools.skew(robo.MS[j]) * vdot[j]) + Njnt[j] = symo.mat_replace(Njnt[j], 'N', j, forced=forced) + f_ant = antRj[j] * Fjnt[j] + f_ant = symo.mat_replace(f_ant, 'FDI', j) + if i != -1: + Fex[i] = Fex[i] + f_ant + Nex[i] = Nex[i] + \ + (antRj[j] * Njnt[j]) + (tools.skew(antPj[j]) * f_ant) + + +def compute_beta(robo, symo, j, w, beta): + """ + Compute beta wrench which is a combination of coriolis forces, + centrifugal forces and external forces (internal function). + + Note: + beta is the output parameter + """ + expr1 = robo.J[j] * w[j] + expr1 = symo.mat_replace(expr1, 'JW', j) + expr2 = tools.skew(w[j]) * expr1 + expr2 = symo.mat_replace(expr2, 'KW', j) + expr3 = tools.skew(w[j]) * robo.MS[j] + expr4 = tools.skew(w[j]) * expr3 + expr4 = symo.mat_replace(expr4, 'SW', j) + expr5 = -robo.Nex[j] - expr2 + expr6 = -robo.Fex[j] - expr4 + beta[j] = Matrix([expr6, expr5]) + beta[j] = symo.mat_replace(beta[j], 'BETA', j) + + +def compute_gamma(robo, symo, j, antRj, antPj, w, wi, gamma): + """ + Compute gyroscopic acceleration (internal function). + + Note: + gamma is the output parameter + """ + i = robo.ant[j] + expr1 = tools.skew(wi[j]) * Matrix([0, 0, robo.qdot[j]]) + expr1 = symo.mat_replace(expr1, 'WQ', j) + expr2 = (1 - robo.sigma[j]) * expr1 + expr3 = 2 * robo.sigma[j] * expr1 + expr4 = tools.skew(w[i]) * antPj[j] + expr5 = tools.skew(w[i]) * expr4 + expr6 = antRj[j].transpose() * expr5 + expr7 = expr6 + expr3 + expr7 = symo.mat_replace(expr7, 'LW', j) + gamma[j] = Matrix([expr7, expr2]) + gamma[j] = symo.mat_replace(gamma[j], 'GYACC', j) + + +def compute_zeta(robo, symo, j, gamma, jaj, zeta, qddot=None): + """ + Compute relative acceleration (internal function). + + Note: + zeta is the output parameter + """ + if qddot == None: + qddot = robo.qddot + expr = gamma[j] + (qddot[j] * jaj[j]) + zeta[j] = symo.mat_replace(expr, 'ZETA', j) + + +def compute_composite_inertia( + robo, symo, j, antRj, antPj, + comp_inertia3, comp_ms, comp_mass, composite_inertia +): + """ + Compute composite inertia (internal function). + + Note: + comp_inertia3, comp_ms, comp_mass, composite_inertia are the + output parameters. + """ + i = robo.ant[j] + i_ms_j_c = antRj[j] * comp_ms[j] + i_ms_j_c = symo.mat_replace(i_ms_j_c, 'AS', j) + expr1 = antRj[j] * comp_inertia3[j] + expr1 = symo.mat_replace(expr1, 'AJ', j) + expr2 = expr1 * antRj[j].transpose() + expr2 = symo.mat_replace(expr2, 'AJA', j) + expr3 = tools.skew(antPj[j]) * tools.skew(i_ms_j_c) + expr3 = symo.mat_replace(expr3, 'PAS', j) + comp_inertia3[i] += expr2 - (expr3 + expr3.transpose()) + \ + (comp_mass[j] * tools.skew(antPj[j]) * \ + tools.skew(antPj[j]).transpose()) + comp_ms[i] = comp_ms[i] + i_ms_j_c + (antPj[j] * comp_mass[j]) + comp_mass[i] = comp_mass[i] + comp_mass[j] + composite_inertia[i] = inertia_spatial( + comp_inertia3[i], comp_ms[i], comp_mass[i] + ) + + +def compute_composite_beta( + robo, symo, j, jTant, zeta, composite_inertia, composite_beta +): + """ + Compute composite beta (internal function). + + Note: + composite_beta is the output parameter + """ + i = robo.ant[j] + expr1 = composite_inertia[j] * zeta[j] + expr1 = symo.mat_replace(expr1, 'IZ', j) + expr2 = jTant[j].transpose() * expr1 + expr2 = symo.mat_replace(expr2, 'SIZ', j) + expr3 = jTant[j].transpose() * composite_beta[j] + expr3 = symo.mat_replace(expr3, 'SBE', j) + composite_beta[i] = composite_beta[i] + expr3 - expr2 + + +def replace_composite_terms( + symo, grandJ, beta, j, composite_inertia, + composite_beta, replace=False +): + """ + Replace composite inertia and beta (internal function). + + Note: + composite_inertia are composite_beta are the output parameters + """ + forced = False + if replace and j == 0: forced = False + composite_inertia[j] = symo.mat_replace( + grandJ[j], 'MJE', j, symmet=True, forced=forced + ) + composite_beta[j] = symo.mat_replace( + beta[j], 'VBE', j, forced=forced + ) + + +def replace_star_terms( + symo, grandJ, beta, j, star_inertia, star_beta, replace=False +): + """ + Replace star inertia and beta (internal function). + + Note: + star_inertia are star_beta are the output parameters + """ + forced = False + if replace and j == 0: forced = False + star_inertia[j] = symo.mat_replace( + grandJ[j], 'MJE', j, symmet=True, forced=forced + ) + star_beta[j] = symo.mat_replace(beta[j], 'VBE', j, forced=forced) + + +def compute_composite_terms( + robo, symo, j, jTant, zeta, + composite_inertia, composite_beta +): + """ + Compute composite inertia and beta (internal function). + + Note: + composite_inertia are composite_beta are the output parameters + """ + i = robo.ant[j] + expr1 = jTant[j].transpose() * composite_inertia[j] + expr1 = symo.mat_replace(expr1, 'GX', j) + expr2 = expr1 * jTant[j] + expr2 = symo.mat_replace(expr2, 'TKT', j, symmet=True) + expr3 = expr1 * zeta[j] + expr3 = symo.mat_replace(expr3, 'SIZ', j) + expr4 = jTant[j].transpose() * composite_beta[j] + expr4 = symo.mat_replace(expr4, 'SBE', j) + composite_inertia[i] = composite_inertia[i] + expr2 + composite_beta[i] = composite_beta[i] + expr4 - expr3 + + +def compute_hinv( + robo, symo, j, jaj, star_inertia, jah, h_inv, flex=False +): + """ + Note: + h_inv and jah are the output parameters + """ + inertia_jaj = star_inertia[j] * jaj[j] + inertia_jaj = symo.mat_replace(inertia_jaj, 'JA', j) + h = jaj[j].dot(inertia_jaj) + if not flex: + h = h + robo.IA[j] + h_inv[j] = 1 / h + h_inv[j] = symo.replace(h_inv[j], 'JD', j) + jah[j] = inertia_jaj * h_inv[j] + jah[j] = symo.mat_replace(jah[j], 'JU', j) + + +def compute_tau(robo, symo, j, jaj, star_beta, tau, flex=False): + """ + Note: + tau is the output parameter + """ + if robo.sigma[j] == 2: + tau[j] = 0 + else: + if flex: + joint_friction = 0 + else: + joint_friction = robo.fric_s(j) + robo.fric_v(j) + tau[j] = jaj[j].dot(star_beta[j]) + robo.GAM[j] - joint_friction + tau[j] = symo.replace(tau[j], 'GW', j) + + +def compute_star_terms( + robo, symo, j, jaj, jTant, gamma, tau, + h_inv, jah, star_inertia, star_beta, flex=False +): + """ + Note: + h_inv, jah, star_inertia, star_beta are the output parameters + """ + i = robo.ant[j] + inertia_jaj = star_inertia[j] * jaj[j] + inertia_jaj = symo.mat_replace(inertia_jaj, 'JA', j) + h = jaj[j].dot(inertia_jaj) + if not flex: + h = h + robo.IA[j] + if not flex or robo.eta[j]: + h_inv[j] = 1 / h + h_inv[j] = symo.replace(h_inv[j], 'JD', j) + jah[j] = inertia_jaj * h_inv[j] + jah[j] = symo.mat_replace(jah[j], 'JU', j) + k_inertia = star_inertia[j] - (jah[j] * inertia_jaj.transpose()) + k_inertia = symo.mat_replace(k_inertia, 'GK', j) + else: + k_inertia = star_inertia[j] + expr1 = k_inertia * gamma[j] + expr1 = symo.mat_replace(expr1, 'NG', j) + if not flex or robo.eta[j]: + expr2 = expr1 + (jah[j] * tau[j]) + else: + expr2 = expr1 + (star_inertia[j] * jaj[j] * robo.qddot[j]) + expr2 = symo.mat_replace(expr2, 'VS', j) + alpha = expr2 - star_beta[j] + alpha = symo.mat_replace(alpha, 'AP', j) + expr3 = jTant[j].transpose() * k_inertia + expr3 = symo.mat_replace(expr3, 'GX', j) + expr4 = expr3 * jTant[j] + expr4 = symo.mat_replace(expr4, 'TKT', j, symmet=True) + star_inertia[i] = star_inertia[i] + expr4 + star_beta[i] = star_beta[i] - (jTant[j].transpose() * alpha) + + +def compute_joint_accel( + robo, symo, j, jaj, jTant, h_inv, jah, gamma, + tau, grandVp, star_beta, star_inertia, qddot +): + """ + Compute joint acceleration (internal function) + + Note: + qddot is the output parameter + """ + i = robo.ant[j] + expr1 = (jTant[j] * grandVp[i]) + gamma[j] + expr1 = symo.mat_replace(expr1, 'VR', j) + expr2 = jah[j].dot(expr1) + expr2 = symo.replace(expr2, 'GU', j) + if robo.sigma[j] == 2: + qddot[j] = 0 + else: + qddot[j] = (h_inv[j] * tau[j]) - expr2 + qddot[j] = symo.replace(qddot[j], 'QDP', j, forced=True) + + +def compute_link_accel(robo, symo, j, jTant, zeta, grandVp): + """ + Compute link acceleration (internal function). + + Note: + grandVp is the output parameter + """ + i = robo.ant[j] + grandVp[j] = (jTant[j] * grandVp[i]) + zeta[j] + grandVp[j][:3, 0] = symo.mat_replace(grandVp[j][:3, 0], 'VP', j) + grandVp[j][3:, 0] = symo.mat_replace(grandVp[j][3:, 0], 'WP', j) + + +def write_numerical_inverse(symo, inertia, symmet=False): + """ + Write the inverse for the inertia matrix (6x6) to be computed + numerically using numpy in the output file. + """ + # write strating comments + symo.write_line("# NUMERICAL INVERSION OF INERTIA MATRIX - START") + symo.write_line("# REQUIRES numpy") + # setup matrix numMJE0 + symo.write_line("# setup matrix in numpy format") + symo.write_equation('numMJE0', 'numpy.zeros((6, 6))') + for i in xrange(inertia.rows): + for j in xrange(inertia.cols): + if inertia[i, j] != 0: + symo.write_equation( + 'numMJE0[{row}, {col}]'.format(row=i, col=j), + str(inertia[i, j]) + ) + # numInvMJE0 = numpy.linalg.inv(numMJE0) + symo.write_line("# invert matrix") + symo.write_line( + "# In Matlab this can be performed without matrix inverse" + ) + symo.write_line("# VP0 = numMJE0 \ numBETA0") + symo.write_equation('numInvMJE0', 'numpy.linalg.inv(numMJE0)') + # assign elements of the inverted matrix + symo.write_line("# assign each element of the inverted (symmetric)") + symo.write_line("# matrix to be compatible with future computation") + for i in xrange(inertia.rows): + for j in xrange(inertia.cols): + if symmet and i < j: + continue + symo.write_equation( + 'InvMJE{row}{col}0'.format(row=i+1, col=j+1), + 'numInvMJE0[{row}, {col}]'.format(row=i, col=j) + ) + # write ending comments + symo.write_line("# NUMERICAL INVERSION OF INERTIA MATRIX - END") + + +def get_numerical_inverse_out(inertia, symmet=False): + """ + Return the inverse of the matrix as formed by strings. + """ + inv_inertia = sympy.zeros(inertia.rows, inertia.cols) + for j in xrange(inertia.cols): + for i in xrange(inertia.rows): + if symmet and i < j: + inv_inertia[i, j] = inv_inertia[j, i] + continue + inv_inertia[i, j] = sympy.var( + 'InvMJE{row}{col}0'.format(row=i+1, col=j+1) + ) + return inv_inertia + + +def compute_base_accel(robo, symo, star_inertia, star_beta, grandVp): + """ + Compute base acceleration (internal function). + + Note: + grandVp is the output parameter + """ + forced = False + grandVp[0] = Matrix([robo.vdot0 - robo.G, robo.w0]) + if robo.is_floating: + forced = True + symo.flushout() + write_numerical_inverse(symo, star_inertia[0], symmet=True) + inv_base_star_inertia = get_numerical_inverse_out( + star_inertia[0], symmet=True + ) + grandVp[0] = inv_base_star_inertia * star_beta[0] + grandVp[0][:3, 0] = symo.mat_replace( + grandVp[0][:3, 0], 'VP', 0, forced=forced + ) + grandVp[0][3:, 0] = symo.mat_replace( + grandVp[0][3:, 0], 'WP', 0, forced=forced + ) + + +def compute_base_accel_composite( + robo, symo, composite_inertia, composite_beta, grandVp +): + """ + Compute base acceleration when using composite inertia matrix + (internal function). + + Note: + grandVp is the output parameter + """ + forced = False + grandVp[0] = Matrix([robo.vdot0 - robo.G, robo.w0]) + if robo.is_floating: + forced = True + symo.flushout() + write_numerical_inverse(symo, composite_inertia[0], symmet=True) + inv_base_comp_inertia = get_numerical_inverse_out( + composite_inertia[0], symmet=True + ) + grandVp[0] = inv_base_comp_inertia * composite_beta[0] + grandVp[0][:3, 0] = symo.mat_replace( + grandVp[0][:3, 0], 'VP', 0, forced=forced + ) + grandVp[0][3:, 0] = symo.mat_replace( + grandVp[0][3:, 0], 'WP', 0, forced=forced + ) + + +def compute_reaction_wrench( + robo, symo, j, grandVp, inertia, beta_wrench, react_wrench +): + """ + Compute reaction wrench (internal function). + + Note: + react_wrench is the output parameter + """ + expr = inertia[j] * grandVp[j] + expr = symo.mat_replace(expr, 'DY', j) + wrench = expr - beta_wrench[j] + react_wrench[j][:3, 0] = symo.mat_replace(wrench[:3, 0], 'E', j) + react_wrench[j][3:, 0] = symo.mat_replace(wrench[3:, 0], 'N', j) + + +def fixed_inverse_dynmodel(robo, symo): + """ + Compute the Inverse Dynamic Model using Newton-Euler algorithm for + tree structure robots with fixed base. + + Parameters: + robo: Robot - instance of robot description container + symo: symbolmgr.SymbolManager - instance of symbolic manager + """ + # init external forces + Fex = copy(robo.Fex) + Nex = copy(robo.Nex) + # init transformation + antRj, antPj = compute_rot_trans(robo, symo) + # init velocities and accelerations + w, wdot, vdot, U = compute_vel_acc(robo, symo, antRj, antPj) + # init forces vectors + F = ParamsInit.init_vec(robo) + N = ParamsInit.init_vec(robo) + Fjnt = ParamsInit.init_vec(robo) + Njnt = ParamsInit.init_vec(robo) + # init torque list + torque = ParamsInit.init_scalar(robo) + for j in xrange(1, robo.NL): + compute_dynamic_wrench(robo, symo, j, w, wdot, U, vdot, F, N) + for j in reversed(xrange(1, robo.NL)): + compute_joint_wrench( + robo, symo, j, antRj, antPj, vdot, + F, N, Fjnt, Njnt, Fex, Nex + ) + for j in xrange(1, robo.NL): + compute_joint_torque(robo, symo, j, Fjnt, Njnt, torque) + + +def mobile_inverse_dynmodel(robo, symo): + """ + Compute the Inverse Dynamic Model using Newton-Euler algorithm for + mobile robots. + + Parameters: + robo: Robot - instance of robot description container + symo: symbolmgr.SymbolManager - instance of symbol manager + """ + # init external forces + Fex = copy(robo.Fex) + Nex = copy(robo.Nex) + # init transformation + antRj, antPj = compute_rot_trans(robo, symo) + # init velocities and accelerations + w, wdot, vdot, U = compute_vel_acc(robo, symo, antRj, antPj) + # init forces vectors + F = ParamsInit.init_vec(robo) + N = ParamsInit.init_vec(robo) + Fjnt = ParamsInit.init_vec(robo) + Njnt = ParamsInit.init_vec(robo) + # init torque list + torque = ParamsInit.init_scalar(robo) + for j in xrange(0, robo.NL): + compute_dynamic_wrench(robo, symo, j, w, wdot, U, vdot, F, N) + for j in reversed(xrange(0, robo.NL)): + compute_joint_wrench( + robo, symo, j, antRj, antPj, vdot, + F, N, Fjnt, Njnt, Fex, Nex + ) + for j in xrange(1, robo.NL): + compute_joint_torque(robo, symo, j, Fjnt, Njnt, torque) + + +def composite_inverse_dynmodel(robo, symo): + """ + Compute the Inverse Dynamic Model using Composite link Newton-Euler + algorithm for tree structure robots with fixed and floating base. + + Parameters: + robo: Robot - instance of robot description container + symo: symbolmgr.SymbolManager - instance of symbol manager + """ + # antecedent angular velocity, projected into jth frame + # j^omega_i + wi = ParamsInit.init_vec(robo) + # j^omega_j + w = ParamsInit.init_w(robo) + # j^a_j -- joint axis in screw form + jaj = ParamsInit.init_vec(robo, 6) + # Twist transform list of Matrices 6x6 + grandJ = ParamsInit.init_mat(robo, 6) + jTant = ParamsInit.init_mat(robo, 6) + gamma = ParamsInit.init_vec(robo, 6) + beta = ParamsInit.init_vec(robo, 6) + zeta = ParamsInit.init_vec(robo, 6) + composite_inertia = ParamsInit.init_mat(robo, 6) + composite_beta = ParamsInit.init_vec(robo, 6) + comp_inertia3, comp_ms, comp_mass = ParamsInit.init_jplus(robo) + grandVp = ParamsInit.init_vec(robo, 6) + react_wrench = ParamsInit.init_vec(robo, 6) + torque = ParamsInit.init_scalar(robo) + # init transformation + antRj, antPj = compute_rot_trans(robo, symo) + # first forward recursion + for j in xrange(1, robo.NL): + # compute spatial inertia matrix for use in backward recursion + grandJ[j] = inertia_spatial(robo.J[j], robo.MS[j], robo.M[j]) + # set jaj vector + if robo.sigma[j] == 0: + jaj[j] = Matrix([0, 0, 0, 0, 0, 1]) + elif robo.sigma[j] == 1: + jaj[j] = Matrix([0, 0, 1, 0, 0, 0]) + # compute j^omega_j and j^omega_i + compute_omega(robo, symo, j, antRj, w, wi) + # compute j^S_i : screw transformation matrix + compute_screw_transform(robo, symo, j, antRj, antPj, jTant) + # first forward recursion (still) + for j in xrange(1, robo.NL): + # compute j^gamma_j : gyroscopic acceleration (6x1) + compute_gamma(robo, symo, j, antRj, antPj, w, wi, gamma) + # compute j^beta_j : external+coriolis+centrifugal wrench (6x1) + compute_beta(robo, symo, j, w, beta) + # compute j^zeta_j : relative acceleration (6x1) + compute_zeta(robo, symo, j, gamma, jaj, zeta) + # first backward recursion - initialisation step + for j in reversed(xrange(0, robo.NL)): + if j == 0: + # compute spatial inertia matrix for base + grandJ[j] = inertia_spatial(robo.J[j], robo.MS[j], robo.M[j]) + # compute 0^beta_0 + compute_beta(robo, symo, j, w, beta) + replace_composite_terms( + symo, grandJ, beta, j, composite_inertia, composite_beta + ) + # second backward recursion - compute composite term + for j in reversed(xrange(0, robo.NL)): + if j == 0: + continue + compute_composite_inertia( + robo, symo, j, antRj, antPj, + comp_inertia3, comp_ms, comp_mass, composite_inertia + ) + compute_composite_beta( + robo, symo, j, jTant, zeta, composite_inertia, composite_beta + ) + replace_composite_terms( + symo, composite_inertia, composite_beta, robo.ant[j], + composite_inertia, composite_beta, replace=True + ) + # compute base acceleration : this returns the correct value for + # fixed base and floating base robots + compute_base_accel_composite( + robo, symo, composite_inertia, composite_beta, grandVp + ) + # second forward recursion + for j in xrange(1, robo.NL): + # compute j^Vdot_j : link acceleration + compute_link_accel(robo, symo, j, jTant, zeta, grandVp) + # compute j^F_j : reaction wrench + compute_reaction_wrench( + robo, symo, j, grandVp, + composite_inertia, composite_beta, react_wrench + ) + # compute torque + compute_torque(robo, symo, j, jaj, react_wrench, torque) + + +def flexible_inverse_dynmodel(robo, symo): + """ + Compute the Inverse Dynamic Model using Newton-Euler algorithm for + robots with flexible joints (fixed and floating base). + + Parameters: + robo: Robot - instance of robot description container + symo: symbolmgr.SymbolManager - instance of symbol manager + """ + # antecedent angular velocity, projected into jth frame + # j^omega_i + wi = ParamsInit.init_vec(robo) + # j^omega_j + w = ParamsInit.init_w(robo) + # j^a_j -- joint axis in screw form + jaj = ParamsInit.init_vec(robo, 6) + # Twist transform list of Matrices 6x6 + grandJ = ParamsInit.init_mat(robo, 6) + jTant = ParamsInit.init_mat(robo, 6) + gamma = ParamsInit.init_vec(robo, 6) + beta = ParamsInit.init_vec(robo, 6) + zeta = ParamsInit.init_vec(robo, 6) + h_inv = ParamsInit.init_scalar(robo) + jah = ParamsInit.init_vec(robo, 6) # Jj*aj*Hinv_j + tau = ParamsInit.init_scalar(robo) + star_inertia = ParamsInit.init_mat(robo, 6) + star_beta = ParamsInit.init_vec(robo, 6) + comp_inertia3, comp_ms, comp_mass = ParamsInit.init_jplus(robo) + qddot = ParamsInit.init_scalar(robo) + grandVp = ParamsInit.init_vec(robo, 6) + react_wrench = ParamsInit.init_vec(robo, 6) + torque = ParamsInit.init_scalar(robo) + # flag variables + use_composite = True + # init transformation + antRj, antPj = compute_rot_trans(robo, symo) + # first forward recursion + for j in xrange(1, robo.NL): + # compute spatial inertia matrix for use in backward recursion + grandJ[j] = inertia_spatial(robo.J[j], robo.MS[j], robo.M[j]) + # set jaj vector + if robo.sigma[j] == 0: + jaj[j] = Matrix([0, 0, 0, 0, 0, 1]) + elif robo.sigma[j] == 1: + jaj[j] = Matrix([0, 0, 1, 0, 0, 0]) + # compute j^omega_j and j^omega_i + compute_omega(robo, symo, j, antRj, w, wi) + # compute j^S_i : screw transformation matrix + compute_screw_transform(robo, symo, j, antRj, antPj, jTant) + # compute j^gamma_j : gyroscopic acceleration (6x1) + compute_gamma(robo, symo, j, antRj, antPj, w, wi, gamma) + # compute j^beta_j : external+coriolis+centrifugal wrench (6x1) + compute_beta(robo, symo, j, w, beta) + if not robo.eta[j]: + # when rigid + # compute j^zeta_j : relative acceleration (6x1) + compute_zeta(robo, symo, j, gamma, jaj, zeta) + # decide first link + first_link = 0 if robo.is_floating else 1 + # first backward recursion - initialisation step + for j in reversed(xrange(first_link, robo.NL)): + if j == first_link and robo.is_floating: + # compute spatial inertia matrix for base + grandJ[j] = inertia_spatial(robo.J[j], robo.MS[j], robo.M[j]) + # compute 0^beta_0 + compute_beta(robo, symo, j, w, beta) + replace_star_terms( + symo, grandJ, beta, j, star_inertia, star_beta + ) + # second backward recursion - compute star terms + for j in reversed(xrange(first_link, robo.NL)): + if j == first_link: + continue + # set composite flag to false when flexible + if robo.eta[j]: use_composite = False + if use_composite: + # use composite + compute_composite_inertia( + robo, symo, j, antRj, antPj, + comp_inertia3, comp_ms, comp_mass, star_inertia + ) + compute_composite_beta( + robo, symo, j, jTant, zeta, star_inertia, star_beta + ) + else: + # use star + if robo.eta[j]: + compute_tau( + robo, symo, j, jaj, star_beta, tau, flex=True + ) + compute_star_terms( + robo, symo, j, jaj, jTant, gamma, tau, + h_inv, jah, star_inertia, star_beta, flex=True + ) + replace_star_terms( + symo, star_inertia, star_beta, robo.ant[j], + star_inertia, star_beta + ) + # compute base acceleration : this returns the correct value for + # fixed base and floating base robots + compute_base_accel( + robo, symo, star_inertia, star_beta, grandVp + ) + # second forward recursion + for j in xrange(1, robo.NL): + if robo.eta[j]: + # when flexible + # compute qddot_j : joint acceleration + compute_joint_accel( + robo, symo, j, jaj, jTant, h_inv, jah, gamma, + tau, grandVp, star_beta, star_inertia, qddot + ) + # compute j^zeta_j : relative acceleration (6x1) + compute_zeta(robo, symo, j, gamma, jaj, zeta, qddot) + # compute j^Vdot_j : link acceleration + compute_link_accel(robo, symo, j, jTant, zeta, grandVp) + # compute j^F_j : reaction wrench + compute_reaction_wrench( + robo, symo, j, grandVp, + star_inertia, star_beta, react_wrench + ) + if not robo.eta[j]: + # when rigid compute torque + compute_torque(robo, symo, j, jaj, react_wrench, torque) + + +def direct_dynmodel(robo, symo): + """ + Compute the Direct Dynamic Model using Newton-Euler algorithm for + robots with floating and fixed base. + + Parameters: + robo: Robot - instance of robot description container + symo: symbolmgr.SymbolManager - instance of symbol manager + """ + # antecedent angular velocity, projected into jth frame + # j^omega_i + wi = ParamsInit.init_vec(robo) + # j^omega_j + w = ParamsInit.init_w(robo) + # j^a_j -- joint axis in screw form + jaj = ParamsInit.init_vec(robo, 6) + # Twist transform list of Matrices 6x6 + grandJ = ParamsInit.init_mat(robo, 6) + jTant = ParamsInit.init_mat(robo, 6) + gamma = ParamsInit.init_vec(robo, 6) + beta = ParamsInit.init_vec(robo, 6) + zeta = ParamsInit.init_vec(robo, 6) + h_inv = ParamsInit.init_scalar(robo) + jah = ParamsInit.init_vec(robo, 6) # Jj*aj*Hinv_j + tau = ParamsInit.init_scalar(robo) + star_inertia = ParamsInit.init_mat(robo, 6) + star_beta = ParamsInit.init_vec(robo, 6) + qddot = ParamsInit.init_scalar(robo) + grandVp = ParamsInit.init_vec(robo, 6) + react_wrench = ParamsInit.init_vec(robo, 6) + torque = ParamsInit.init_scalar(robo) + # init transformation + antRj, antPj = compute_rot_trans(robo, symo) + # first forward recursion + for j in xrange(1, robo.NL): + # compute spatial inertia matrix for use in backward recursion + grandJ[j] = inertia_spatial(robo.J[j], robo.MS[j], robo.M[j]) + # set jaj vector + if robo.sigma[j] == 0: + jaj[j] = Matrix([0, 0, 0, 0, 0, 1]) + elif robo.sigma[j] == 1: + jaj[j] = Matrix([0, 0, 1, 0, 0, 0]) + # compute j^omega_j and j^omega_i + compute_omega(robo, symo, j, antRj, w, wi) + # compute j^S_i : screw transformation matrix + compute_screw_transform(robo, symo, j, antRj, antPj, jTant) + # compute j^gamma_j : gyroscopic acceleration (6x1) + compute_gamma(robo, symo, j, antRj, antPj, w, wi, gamma) + # compute j^beta_j : external+coriolis+centrifugal wrench (6x1) + compute_beta(robo, symo, j, w, beta) + # decide first link + first_link = 0 if robo.is_floating else 1 + # first backward recursion - initialisation step + for j in reversed(xrange(first_link, robo.NL)): + if j == first_link and robo.is_floating: + # compute spatial inertia matrix for base + grandJ[j] = inertia_spatial(robo.J[j], robo.MS[j], robo.M[j]) + # compute 0^beta_0 + compute_beta(robo, symo, j, w, beta) + replace_star_terms( + symo, grandJ, beta, j, star_inertia, star_beta + ) + # second backward recursion - compute star terms + for j in reversed(xrange(first_link, robo.NL)): + if j == 0: + continue + compute_tau(robo, symo, j, jaj, star_beta, tau) + compute_star_terms( + robo, symo, j, jaj, jTant, gamma, tau, + h_inv, jah, star_inertia, star_beta + ) + if j == first_link: + continue + replace_star_terms( + symo, star_inertia, star_beta, robo.ant[j], + star_inertia, star_beta, replace=True + ) + # compute base acceleration : this returns the correct value for + # fixed base and floating base robots + compute_base_accel( + robo, symo, star_inertia, star_beta, grandVp + ) + # second forward recursion + for j in xrange(1, robo.NL): + # compute qddot_j : joint acceleration + compute_joint_accel( + robo, symo, j, jaj, jTant, h_inv, jah, gamma, + tau, grandVp, star_beta, star_inertia, qddot + ) + # compute j^zeta_j : relative acceleration (6x1) + compute_zeta(robo, symo, j, gamma, jaj, zeta, qddot) + # compute j^Vdot_j : link acceleration + compute_link_accel(robo, symo, j, jTant, zeta, grandVp) + # compute j^F_j : reaction wrench + compute_reaction_wrench( + robo, symo, j, grandVp, + star_inertia, star_beta, react_wrench + ) + + diff --git a/pysymoro/pieper.py b/pysymoro/pieper.py new file mode 100644 index 0000000..5b081e4 --- /dev/null +++ b/pysymoro/pieper.py @@ -0,0 +1,468 @@ +# -*- coding: utf-8 -*- + + +# This file is part of the OpenSYMORO project. Please see +# https://github.com/symoro/symoro/blob/master/LICENCE for the licence. + + +""" +This module of SYMORO package provides symbolic +solutions for inverse geometric problem using Pieper Method. +""" + + +from sympy import var, sin, cos, eye, atan2, sqrt, pi +from sympy import Matrix, Symbol, Expr, trigsimp, zeros, ones +from numpy import array, dot + +from symoroutils import symbolmgr, tools +from pysymoro import invdata +from pysymoro.invdata import solve_position +from pysymoro.invdata import solve_orientation +from pysymoro.invdata import solve_orientation_prismatic +from pysymoro.invdata import solve_position_prismatic + + +# dictionary for joint combinations out of the pieper ones +# coding: (i,j,k):type +joint_com = { + (0, 0, 0): 0, (0, 0, 1): 1, (0, 1, 0): 2, + (0, 1, 1): 3, (1, 0, 0): 4, (1, 0, 1): 5, + (1, 1, 0): 6, (1, 1, 1): 7 +} + + +def _pieper_solve(robo, symo): + """ + Main function of the Pieper solutions. + + Parameters: + =========== + 1) robo: Parameter that gives us access to the parameters of the robot. + 2) symo: Instance that gives us access for the symbolic handling. + """ + if robo.structure == tools.SIMPLE: + [pieper_joints, bools] = _look_for_case_simple(robo,symo) + [bool_fail,bool_prism,bool_spherical] = bools + if bool_fail == 0: + [com_key, X_joints] = _X_joints(robo,symo,pieper_joints) + else: + return + if bool_spherical == 1: + m = pieper_joints[1] + if m == 2: + RRRXXX(robo,symo,com_key,X_joints,pieper_joints) + elif m == 3: + XRRRXX(robo,symo,com_key,X_joints,pieper_joints) + elif m == 4: + XXRRRX(robo,symo,com_key,X_joints,pieper_joints) + elif m == 5: + XXXRRR(robo,symo,com_key,X_joints,pieper_joints) + elif bool_prism == 1: + Prismatic(robo,symo,X_joints,pieper_joints) + elif (robo.structure == tools.TREE) or (robo.structure == tools.CLOSED_LOOP): + [bools, pieper_branches, pieper_joints_all, X_joints_all, com_key] = _look_for_case_tree(robo,symo) + [bool_fail, bool_prism, bool_spherical] = bools + if sum(bool_fail) == len(bool_fail): + return + X_joints = [X_joints_all[x:x+3] for x in xrange(0, len(X_joints_all), 3)] + pieper_joints = [pieper_joints_all[x:x+3] for x in xrange(0, len(pieper_joints_all), 3)] + for i in range(len(pieper_branches)): + symo.write_line("# Solution for branch {0} :".format(i+1)) + symo.write_line("#============================= \r\n\r\n") + for j in range(len(bool_spherical)): + if bool_spherical[j] == 1: + m = pieper_joints[i][1] + bool_spherical[j] = 0 + if m == 2: + RRRXXX(robo,symo,com_key[i],X_joints[i],pieper_joints[i]) + break + elif m == 3: + XRRRXX(robo,symo,com_key[i],X_joints[i],pieper_joints[i]) + break + elif m == 4: + XXRRRX(robo,symo,com_key[i],X_joints[i],pieper_joints[i]) + break + elif m == 5: + XXXRRR(robo,symo,com_key[i],X_joints[i],pieper_joints[i]) + break + elif bool_prism[j] == 1: + bool_prism[j] = 0 + Prismatic(robo,symo,X_joints[i],pieper_joints[i]) + break + return + + +def _look_for_case_simple(robo, symo): + """ + Function that locates if a serial robot can be solved by PIEPER METHOD. + + Parameters: + =========== + """ + try_paul_str = ("\r\n\r\n# This robot cannot be ") + \ + ("solved by PIEPER METHOD. Try Paul method. \r\n\r\n") + symo.write_line("# Pieper Method data for the given robot: ") + [bool_fail, bool_prism, bool_spherical] = [0,0,0] + pieper_joints = [] + if robo.nj > 6: + symo.write_line( + ("\r\n\r\n# This robot cannot be solved by PIEPER METHOD.") + \ + (" The robot is redundant. \r\n\r\n") + ) + bool_fail = 1 + elif robo.nj < 6: + symo.write_line(try_paul_str) + symo.write_line("# --The robot has less than 6-DOF.--") + bool_fail = 1 + else: + if sum(robo.sigma[1:len(robo.sigma)]) > 3: + symo.write_line( + ("\r\n\r\n# This robot cannot be solved by ") + \ + ("PIEPER METHOD, because it is redundant. ") + \ + ("(more than three prismatic joints) \r\n\r\n") + ) + bool_fail = 1 + elif sum(robo.sigma[1:len(robo.sigma)]) == 3: + bool_prism = 1 + for joint in range(1, len(robo.sigma)): + if robo.sigma[joint] == 1: + pieper_joints.append(joint) + symo.write_line( + ("\r\n\r\n# PIEPER METHOD: Decoupled 6-DOF robot ") + \ + ("with 3 prismatic joints positioned at:") + \ + ("{0}".format(pieper_joints)) + \ + ("\r\n\r\n") + ) + else: + for m in range(2, len(robo.sigma)-1): + if (robo.sigma[m-1] == 0) and (robo.sigma[m] == 0) and (robo.sigma[m+1] == 0): + if (robo.d[m] == 0) and (robo.d[m+1] == 0) and (robo.r[m] == 0): + if (sin(robo.alpha[m]) != 0) and (sin(robo.alpha[m+1]) != 0): + pieper_joints = [m-1,m,m+1] + symo.write_line( + ("\r\n\r\n# PIEPER METHOD: Decoupled ") + \ + ("6-DOF robot with a spherical joint ") + \ + ("composed by joints: ") + \ + ("{0}".format(pieper_joints)) + \ + ("\r\n\r\n") + ) + bool_spherical = 1 + break + elif m == 5: + bool_fail = 1 + symo.write_line(try_paul_str) + elif m == 5: + bool_fail = 1 + symo.write_line(try_paul_str) + elif m == 5: + bool_fail = 1 + symo.write_line(try_paul_str) + bools = [bool_fail, bool_prism, bool_spherical] + return pieper_joints, bools + + +def _look_for_case_tree(robo,symo): + try_paul_str = ("\r\n\r\n# Branch {0}") + \ + ("of the robot cannot be solved by PIEPER METHOD. ") + \ + ("This branch has less than 6 joints. ") + \ + ("Try Paul Method \r\n\r\n") + End_joints = [] + X_joints = [] + pieper_joints = [] + j = [] + for i in range(robo.NJ+1): + j.append(i) + for joint in range(1, len(robo.ant)): + if j[joint] not in robo.ant and robo.sigma[joint] != 2: + End_joints.append(j[joint]) + branches = len(End_joints) + symo.write_line("# The tree structure robot has {0} branches.".format(branches)) + [bool_fail, bool_prism, bool_spherical] = [zeros(1, branches), zeros(1, branches), zeros(1, branches)] + [pieper_branches, com_key] = [999*ones(1, branches), 999*ones(1, branches)] + num_prism = zeros(1, branches) + for i in range(branches): + f_joint = End_joints[i] + c = 0 + globals()["bran"+str(i)] = [0]*max(robo.ant) + while f_joint != 0: + globals()["bran"+str(i)][c] = f_joint + f_joint = robo.ant[f_joint] + c += 1 + globals()["bran"+str(i)] = [x for x in globals()["bran"+str(i)] if x != 0] + globals()["bran"+str(i)] = globals()["bran"+str(i)][::-1] + if len(globals()["bran"+str(i)]) > 6: + symo.write_line( + ("\r\n\r\n# Branch {0} ".format(i+1)) + \ + ("of the robot cannot be solved by PIEPER METHOD. ") + \ + ("This branch is redundant. \r\n\r\n") + ) + bool_fail[i] = 1 + elif len(globals()["bran"+str(i)]) < 6: + symo.write_line(try_paul_str.format(i+1)) + bool_fail[i] = 1 + else: + bool_fail[i] = 0 + for k in range(len(globals()["bran"+str(i)])): + num_prism[i] = num_prism[i] + robo.sigma[globals()["bran"+str(i)][k]] + if num_prism[i] > 3: + symo.write_line( + ("\r\n\r\n# Branch {0} ".format(i+1)) + \ + ("of the robot cannot be solved by PIEPER METHOD. ") + \ + ("It is redundant (more than 3 prismatic joints). \r\n\r\n") + ) + bool_fail[i] = 1 + elif num_prism[i] == 3: + pieper_branches[i] = i + bool_prism[i] = 1 + p_joints = [] + joints = [] + for joint in range(len(globals()["bran"+str(i)])): + if robo.sigma[globals()["bran"+str(i)][joint]] == 1: + pieper_joints.append(globals()["bran"+str(i)][joint]) + p_joints.append(globals()["bran"+str(i)][joint]) + else: + X_joints.append(globals()["bran"+str(i)][joint]) + joints.append(robo.sigma[globals()["bran"+str(i)][joint]]) + joint_type = tuple(joints) + if joint_type in joint_com: + com_key[i] = joint_com[joint_type] + symo.write_line( + ("\r\n\r\n# PIEPER METHOD: Branch {0}".format(i+1)) + \ + (" is decoupled with 3 prismatic joints ") + \ + ("positioned at {0} \r\n\r\n".format(p_joints)) + ) + else: + for m in range(2, len(globals()["bran"+str(i)])): + if (robo.sigma[globals()["bran"+str(i)][m-1]] == 0) \ + and (robo.sigma[globals()["bran"+str(i)][m]] == 0) \ + and (robo.sigma[globals()["bran"+str(i)][m+1]] == 0): + if (robo.d[globals()["bran"+str(i)][m]] == 0) \ + and (robo.d[globals()["bran"+str(i)][m+1]] == 0) \ + and (robo.r[globals()["bran"+str(i)][m]] == 0): + if (sin(robo.alpha[globals()["bran"+str(i)][m]]) != 0) \ + and (sin(robo.alpha[globals()["bran"+str(i)][m+1]]) != 0): + pieper_branches[i] = i + pieper_joints = [ + globals()["bran"+str(i)][m-1], + globals()["bran"+str(i)][m], + globals()["bran"+str(i)][m+1] + ] + joints = [] + joint = globals()["bran"+str(i)] + for ji in range(len(joint)): + if joint[ji] not in pieper_joints: + X_joints.append(globals()["bran"+str(i)][ji]) + joints.append(robo.sigma[globals()["bran"+str(i)][ji]]) + joint_type = tuple(joints) + if joint_type in joint_com: + com_key[i] = joint_com[joint_type] + symo.write_line( + ("\r\n\r\n# PIEPER METHOD: ") + \ + ("Branch{0}".format(i+1)) + \ + (" is decoupled with a ") + \ + ("spherical joint composed by ") + \ + ("joints {0} \r\n\r\n".format(pieper_joints)) + ) + bool_spherical[i] = 1 + break + elif m == 5: + bool_fail[i] = 1 + symo.write_line(try_paul_str.format(i+1)) + elif m == 5: + bool_fail[i] = 1 + symo.write_line(try_paul_str.format(i+1)) + elif m == 5: + bool_fail[i] = 1 + symo.write_line(try_paul_str.format(i+1)) + bools = [bool_fail, bool_prism, bool_spherical] + pieper_branches = [x for x in pieper_branches if x != 999] + com_key = [x for x in com_key if x != 999] + return bools, pieper_branches, pieper_joints, X_joints, com_key + + +def igm_pieper(robo, T_ref, n): + symo = symbolmgr.SymbolManager() + symo.file_open(robo, 'pieper') + symo.write_params_table(robo, 'Inverse Geometrix Model for frame %s' % n) + _pieper_solve(robo, symo) + symo.file_close() + return symo + + +def _X_joints(robo, symo, pieper_joints): + """ + Function that takes the already identified pieper_joints and returns the other X_joints. + + Parameters: + =========== + """ + # pieper_joints will be a vector containing the position of the + # joints in the parameters table [pieper_joints = (q1,q2,q3)] + joints = [] # Empty vector to append the type of the joints + X_joints = [] + for joint in range(1, len(robo.sigma)): + if joint not in pieper_joints: + X_joints.append(joint) + joints.append(robo.sigma[joint]) + joint_type = tuple(joints) # Convert from list to tuple in order to use the vector + # Identify the combination of the X joints + if joint_type in joint_com: com_key = joint_com[joint_type] + + return com_key, X_joints + + +def XXXRRR(robo, symo, com_key, X_joints, pieper_joints): + """ + Function that prints the symbolic solution of this decoupled robot case using PIEPER METHOD. + + Parameters: + =========== + 1) X_joints: Joints that are out of spherical wrist + 2) pieper_joints: Joints that the spherical wrist is composed of. + """ + ## P36_1 = d4 + P36_1 = robo.d[4] + ## P36_2 = -r4*Sa4 + P36_2 = -sin(robo.alpha[4])*robo.r[4] + ## P36_3 = r4*Ca4 + P36_3 = cos(robo.alpha[4])*robo.r[4] + [px,py,pz] = invdata.T_GENERAL[:3,3] + g = Matrix([px,py,pz,1]) + ## fc = [d4; -Ca3*Sa4*r4; -Sa3*Sa4*r4] + fc = Matrix([P36_1, cos(robo.alpha[3])*P36_2, sin(robo.alpha[3])*P36_2, 0]) + ## fs = [Sa4*r4; Ca3*d4; Sa3*d4] + fs = Matrix([-P36_2, cos(robo.alpha[3])*P36_1, sin(robo.alpha[3])*P36_1, 0]) + ## fr = [0; -Sa3; Ca3] + fr = Matrix([0, -sin(robo.alpha[3]), cos(robo.alpha[3]), 0]) + ## f0 = [d3; -Sa3*Ca4*r4; Ca3*Ca4*r4] + f0 = Matrix([robo.d[3], -sin(robo.alpha[3])*P36_3, cos(robo.alpha[3])*P36_3, 1]) + # Position Equations First + solve_position(robo, symo, com_key, X_joints, fc, fs, fr, f0, g) + # Then Orientation Equations + solve_orientation(robo, symo, pieper_joints) + return + + +def XRRRXX(robo, symo, com_key, X_joints, pieper_joints): + """ + Function that prints the symbolic solution of this decoupled robot case using PIEPER METHOD. + + Parameters: + =========== + 1) X_joints: Joints that are out of spherical wrist + 2) pieper_joints: Joints that the spherical wrist is composed of. + """ + g = Matrix([robo.d[2], -sin(robo.alpha[2])*robo.r[2], cos(robo.alpha[2])*robo.r[2], 1]) + A0 = array(invdata.T_GENERAL[:3, :3]) + P0 = array(invdata.T_GENERAL[:3, 3]) + ## fc = A0*[d5-d6; r5*(Ca5*Sa6-Ca6*Sa5); 0] + fc = dot( + A0, + [robo.d[5]-robo.d[6], + robo.r[5]*(cos(robo.alpha[5])*sin(robo.alpha[6])-cos(robo.alpha[6])*sin(robo.alpha[5])), + 0] + ) + fc = Matrix([fc[0], fc[1], fc[2], 0]) + ## fs = A0*[r5*(Ca5*Sa6-Ca6*Sa5); d6-d5; 0] + fs = dot( + A0, + [robo.r[5]*(cos(robo.alpha[5])*sin(robo.alpha[6])-cos(robo.alpha[6])*sin(robo.alpha[5])), + robo.d[6]-robo.d[5], 0] + ) + fs = Matrix([fs[0], fs[1], fs[2], 0]) + ## fr = A0*[0; 0; -1] + fr = dot(A0, [0, 0, -1]) + fr = Matrix([fr[0], fr[1], fr[2], 0]) + ## f0 = A0*[0; 0; r5*C(a5-a6)] + P0 + f0 = sum(dot(A0, [0, 0, robo.r[5]*cos(robo.alpha[5]-robo.alpha[6])]), P0) + f0 = Matrix([f0[0][0], f0[1][0], f0[2][0], 1]) + # Position Equations First + solve_position(robo, symo, com_key, X_joints, fc, fs, fr, f0, g) + # Then Orientation Equations + solve_orientation(robo, symo, pieper_joints) + return + + +def XXRRRX(robo, symo, com_key, X_joints, pieper_joints): + """ + Function that prints the symbolic solution of this decoupled robot case using PIEPER METHOD. + + Parameters: + =========== + 1) X_joints: Joints that are out of spherical wrist + 2) pieper_joints: Joints that the spherical wrist is composed of. + """ + g = Matrix([robo.d[3], -sin(robo.alpha[3])*robo.r[3], cos(robo.alpha[3])*robo.r[3], 1]) + A0 = array(invdata.T_GENERAL[:3, :3]) + P0 = array(invdata.T_GENERAL[:3, 3]) + fc = dot(A0, [-robo.d[6], -robo.r[5]*sin(robo.alpha[6]), 0]) + ## fc = A0*[-d6; -Sa6*r5; 0] + fc = Matrix([fc[0], fc[1], fc[2], 0]) + symo.write_line(fc) + ## fs = A0*[-Sa6*r5; d6; 0] + fs = dot(A0, [-robo.r[5]*sin(robo.alpha[6]), robo.d[6], 0]) + fs = Matrix([fs[0], fs[1], fs[2], 0]) + ## fr = -A0*[0; 0; 1] + fr = -dot(A0, [0, 0, 1]) + fr = Matrix([fr[0], fr[1], fr[2], 0]) + ## f0 = A0*[0; 0; -Ca6*r5] + P0 + f0 = sum(dot(A0, [0, 0, -robo.r[5]*cos(robo.alpha[6])]), P0) + f0 = Matrix([f0[0][0], f0[1][0], f0[2][0], 1]) + # Position Equations First + solve_position(robo, symo, com_key, X_joints, fc, fs, fr, f0, g) + # Then Orientation Equations + solve_orientation(robo, symo, pieper_joints) + return + + +def RRRXXX(robo, symo, com_key, X_joints, pieper_joints): + """ + Function that prints the symbolic solution of this decoupled robot case using PIEPER METHOD. + + Parameters: + =========== + 1) X_joints: Joints that are out of spherical wrist + 2) pieper_joints: Joints that the spherical wrist is composed of. + """ + ## P63_1 = -d4 + P63_1 = -robo.d[4] + ## P63_2 = -r3*Sa4 + P63_2 = -sin(robo.alpha[4])*robo.r[3] + ## P63_3 = -r3*Ca4 + P63_3 = -cos(robo.alpha[4])*robo.r[3] + A0 = array(invdata.T_GENERAL[:3, :3]) + P0 = array(invdata.T_GENERAL[:3, 3]) + g = -dot(A0, P0) + g = Matrix([g[0][0], g[1][0], g[2][0], 1]) + ## fc = [-d4; -Ca5*Sa4*r3; Sa5*Sa4*r3] + fc = Matrix([P63_1, cos(robo.alpha[5])*P63_2, -sin(robo.alpha[5])*P63_2, 0]) + ## fs = [Sa4*r3; -Ca5*d4; Sa5*d4] + fs = Matrix([-P63_2, cos(robo.alpha[5])*P63_1, -sin(robo.alpha[5])*P63_1, 0]) + ## fr = [0; Sa5; Ca5] + fr = Matrix([0, sin(robo.alpha[5]), cos(robo.alpha[5]), 0]) + ## f0 = [-d5; -Sa5*Ca4*r3; -Ca5*Ca4*r3] + f0 = Matrix([-robo.d[5], sin(robo.alpha[5])*P63_3, cos(robo.alpha[5])*P63_3, 1]) + # Position Equations First + solve_position(robo, symo, com_key, X_joints, fc, fs, fr, f0, g) + # Then Orientation Equations + solve_orientation(robo, symo, pieper_joints) + return + + +def Prismatic(robo, symo, X_joints, pieper_joints): + """ + Function that prints the symbolic solution of this decoupled robot case using PIEPER METHOD. + + Parameters: + =========== + 1) X_joints: The three revolute joints. + 2) pieper_joints: The three prismatic joints. + """ + # Orientation Equations First + solve_orientation_prismatic(robo, symo, X_joints) + # Then Position Equations + solve_position_prismatic(robo, symo, pieper_joints) + return + + diff --git a/pysymoro/robot.py b/pysymoro/robot.py new file mode 100644 index 0000000..8853ab4 --- /dev/null +++ b/pysymoro/robot.py @@ -0,0 +1,757 @@ +# -*- coding: utf-8 -*- + + +# This file is part of the OpenSYMORO project. Please see +# https://github.com/symoro/symoro/blob/master/LICENCE for the licence. + + +""" +This module of SYMORO package provides description +of the robot parametrizaion container and symbol replacer class. +""" + + +import re +import os +import copy +from itertools import combinations + +from sympy import sin, cos, sign, pi +from sympy import Symbol, Matrix, Expr, Integer +from sympy import Mul, Add, factor, zeros, var, sympify, eye + +from pysymoro import baseparams +from pysymoro import dyniden +from pysymoro import inertia +from pysymoro import nealgos +from symoroutils import filemgr +from symoroutils import symbolmgr +from symoroutils import tools +from symoroutils.tools import ZERO, ONE, FAIL, OK +from symoroutils.tools import CLOSED_LOOP, SIMPLE, TREE, TYPES, INT_KEYS + + +class Robot(object): + """Container of the robot parametric description. + Responsible for low-level geometric transformation + and direct geometric model generation. + Also provides different representations of parameters.""" + def __init__( + self, name, NL=0, NJ=0, NF=0, is_floating=False, + structure=TREE, is_mobile=False, directory=None, + par_file_path=None + ): + # member variables: + """ name of the robot: string""" + self.name = name + """ directory name""" + self.directory = self.set_directory(directory) + """ PAR file path""" + self.par_file_path = self.set_par_file_path(par_file_path) + """ whether the base frame is floating: bool""" + self.is_floating = is_floating + """ whether the robot is a mobile robot""" + self.is_mobile = is_mobile + """ number of links: int""" + self.nl = NL + """ number of joints: int""" + self.nj = NJ + """ number of frames: int""" + self.nf = NF + """ type of robot's structure""" + self.structure = structure + """ joint type: list of int""" + self.sigma = [0 for i in xrange(NF + 1)] + """ index of antecedent joint: list of int""" + self.ant = range(-1, self.NF - 1) + """actuated, if 1, then the joint is actuated""" + self.mu = [1 for i in xrange(NF + 1)] + """ geometrical parameter: list of var""" + self.theta = [0] + [var('th%s' % (i+1)) for i in xrange(NF)] + """ geometrical parameter: list of var""" + self.r = [0 for i in xrange(NF + 1)] + """ geometrical parameter: list of var""" + self.alpha = [0 for i in xrange(NF + 1)] + """ geometrical parameter: list of var""" + self.d = [0 for i in xrange(NF + 1)] + """ geometrical parameter: list of var""" + self.gamma = [0 for i in xrange(NF + 1)] + """ geometrical parameter: list of var""" + self.b = [0 for i in xrange(NF + 1)] + """ transformation from reference frame to zero frame""" + self.Z = eye(4) + num = range(self.NL) + numj = range(self.NJ) + """ base angular velocity: 3x1 matrix""" + self.w0 = zeros(3, 1) + """ base angular acceleration: 3x1 matrix""" + self.wdot0 = zeros(3, 1) + """ base linear velocity: 3x1 matrix""" + self.v0 = zeros(3, 1) + """ base linear acceleration: 3x1 matrix""" + self.vdot0 = zeros(3, 1) + """ joint speed: list of var""" + self.qdot = [var('QP{0}'.format(i)) for i in numj] + """ joint acceleration: list of var""" + self.qddot = [var('QDP{0}'.format(i)) for i in numj] + """ external moment of link: list of 3x1 matrix""" + self.Nex = [zeros(3, 1) for i in num] + self.Nex[-1] = Matrix(var('CX{0}, CY{0}, CZ{0}'.format(self.NL - 1))) + """ external force of link: list of 3x1 matrix""" + self.Fex = [zeros(3, 1) for i in num] + self.Fex[-1] = Matrix(var('FX{0}, FY{0}, FZ{0}'.format(self.NL - 1))) + """ dry friction coefficient: list of ver""" + self.FS = [var('FS{0}'.format(i)) for i in num] + """ joint actuator inertia: list of var""" + self.IA = [var('IA{0}'.format(i)) for i in num] + """ viscous friction coefficient: list of var""" + self.FV = [var('FV{0}'.format(i)) for i in num] + """ first momentum of link: list of 3x1 matrix""" + self.MS = [Matrix(var('MX{0}, MY{0}, MZ{0}'.format(i))) for i in num] + """ mass of link: list of var""" + self.M = [var('M{0}'.format(i)) for i in num] + """ joint torques: list of var""" + self.GAM = [var('GAM{0}'.format(i)) for i in numj] + """ inertia tensor of link: list of 3x3 matrix""" + J_str = 'XX{0},XY{0},XZ{0},XY{0},YY{0},YZ{0},XZ{0},YZ{0},ZZ{0}' + self.J = [Matrix(3, 3, var(J_str.format(i))) for i in num] + """ gravity vector: 3x1 matrix""" + self.G = Matrix([0, 0, var('GZ')]) + """ eta - rigid or flexible""" + self.eta = [0 for j in numj] + """ k - joint stiffness""" + self.k = [0 for j in numj] + + def set_par_file_path(self, path): + if path is None or not os.path.isabs(path): + par_file_path = filemgr.get_file_path(self) + else: + file_path = path + self.par_file_path = file_path + return file_path + + def set_directory(self, path): + if path is None or not os.path.isdir(path): + directory = filemgr.get_folder_path(self.name) + else: + directory = path + self.directory = directory + return directory + + def set_defaults(self, joint=False, geom=False, base=False): + # joint params + if joint: + self._set_joint_defaults() + # geometric params + if geom: + self._set_geom_defaults() + # base params + if base: + self._set_base_defaults() + + def put_val(self, j, name, val): + try: + if isinstance(val, str) or isinstance(val, unicode): + val = sympify(val) + assert isinstance(val, Expr) + geom_head = self.get_geom_head() + if name in INT_KEYS: + val = int(val) + except: + return FAIL + base_vel_head = self.get_base_vel_head() + ext_dynam_head = self.get_ext_dynam_head() + dynam_head = self.get_dynam_head() + ext_head = ext_dynam_head[7:] + ['IA'] + f_ex_head = ext_dynam_head[1:4] + n_ex_head = ext_dynam_head[4:7] + if name in ext_head + geom_head + base_vel_head: + X = getattr(self, name) + X[j] = val + elif name in f_ex_head: + self.Fex[j][f_ex_head.index(name)] = val + elif name in n_ex_head: + self.Nex[j][n_ex_head.index(name)] = val + elif name in dynam_head: + params = self.get_inert_param(j) + i = dynam_head.index(name) + params[i-1] = val + self.put_inert_param(params, j) + elif name == 'eta': + self.eta[j] = int(val) + elif name == 'k': + self.k[j] = val + elif name == 'Z': + self.Z[j] = val + return OK + + def get_val(self, j, name): + geom_head = self.get_geom_head() + base_vel_head = self.get_base_vel_head() + ext_dynam_head = self.get_ext_dynam_head() + dynam_head = self.get_dynam_head() + ext_head = ext_dynam_head[7:] + ['IA'] + f_ex_head = ext_dynam_head[1:4] + n_ex_head = ext_dynam_head[4:7] + if name in ext_head + geom_head + base_vel_head: + X = getattr(self, name) + return X[j] + elif name in f_ex_head: + return self.Fex[j][f_ex_head.index(name)] + elif name in n_ex_head: + return self.Nex[j][n_ex_head.index(name)] + elif name in self.get_dynam_head(): + params = self.get_inert_param(j) + i = dynam_head.index(name) + return params[i-1] + elif name == 'eta': + return self.eta[j] + elif name == 'k': + return self.k[j] + elif name == 'Z': + return self.Z[j] + + def get_q_chain(self, j, k=0): + """Generates vector of joint variables in chain + between j, k (zero by default) + """ + chain = self.chain(j, k) + q = [] + for i in reversed(chain): + if int(self.sigma[i]) == 0: + q.append(self.theta[i]) + elif int(self.sigma[i]) == 1: + q.append(self.r[i]) + return q + + def get_q(self, i): + """ Returns symbol of joint variable + or 0 if joint is fixed + """ + if self.sigma[i] == 0: + return self.theta[i] + elif self.sigma[i] == 1: + return self.r[i] + else: + return 0 + + def compute_idym(self): + """ + Compute the Inverse Dynamic Model of the robot using the + recursive Newton-Euler algorithm. Also choose the Newton-Euler + algorithm based on the robot type. + """ + symo = symbolmgr.SymbolManager() + symo.file_open(self, 'idm') + title = "Inverse Dynamic Model using Newton-Euler Algorithm\n" + if 1 in self.eta: + # with flexible joints + title = title + "Robot with flexible joints\n" + symo.write_params_table(self, title, inert=True, dynam=True) + nealgos.flexible_inverse_dynmodel(self, symo) + elif self.is_floating: + # with rigid joints and floating base + title = title + "Robot with rigid joints and floating base\n" + symo.write_params_table(self, title, inert=True, dynam=True) + nealgos.composite_inverse_dynmodel(self, symo) + elif self.is_mobile: + # mobile robot with rigid joints - known base acceleration + title = title + "Robot with mobile base (Vdot0 is known)\n" + symo.write_params_table(self, title, inert=True, dynam=True) + nealgos.mobile_inverse_dynmodel(self, symo) + else: + # with rigid joints and fixed base + title = title + "Robot with rigid joints and fixed base\n" + symo.write_params_table(self, title, inert=True, dynam=True) + nealgos.fixed_inverse_dynmodel(self, symo) + symo.file_close() + return symo + + def compute_inertiamatrix(self): + """ + Compute the Inertia Matrix of the robot using the Composite link + algorithm. + """ + symo = symbolmgr.SymbolManager() + symo.file_open(self, 'inm') + title = "Inertia matrix using Composite links algorithm\n" + if self.is_floating or self.is_mobile: + # with floating or mobile base + title = title + "Robot with floating/mobile base\n" + symo.write_params_table(self, title, inert=True, dynam=True) + inertia.floating_inertia_matrix(self, symo) + else: + # with fixed base + title = title + "Robot with fixed base\n" + symo.write_params_table(self, title, inert=True, dynam=True) + inertia.fixed_inertia_matrix(self, symo) + symo.file_close() + return symo + + def compute_ddym(self): + """ + Compute the Direct Dynamic Model of the robot using the + recursive Newton-Euler algorithm. + """ + symo = symbolmgr.SymbolManager() + symo.file_open(self, 'ddm') + title = "Direct Dynamic Model using Newton-Euler Algorithm\n" + if self.is_floating: + # with floating base + title = title + "Robot with floating base\n" + else: + # with fixed base + title = title + "Robot with fixed base\n" + symo.write_params_table(self, title, inert=True, dynam=True) + nealgos.direct_dynmodel(self, symo) + symo.file_close() + return symo + + def compute_pseudotorques(self): + """ + Compute Coriolis, Centrifugal, Gravity, Friction and external + torques using Newton-Euler algortihm. + """ + pseudo_robo = copy.deepcopy(self) + pseudo_robo.qddot = zeros(pseudo_robo.NL, 1) + symo = symbolmgr.SymbolManager() + symo.file_open(self, 'ccg') + title = "Pseudo forces using Newton-Euler Algorithm\n" + if 1 in pseudo_robo.eta: + # with flexible joints + title = title + "Robot with flexible joints\n" + symo.write_params_table(self, title, inert=True, dynam=True) + nealgos.flexible_inverse_dynmodel(pseudo_robo, symo) + elif pseudo_robo.is_floating: + # with rigid joints and floating base + title = title + "Robot with rigid joints and floating base\n" + symo.write_params_table(self, title, inert=True, dynam=True) + nealgos.composite_inverse_dynmodel(pseudo_robo, symo) + elif pseudo_robo.is_mobile: + # mobile robot with rigid joints - known base acceleration + title = title + "Robot with mobile base (Vdot0 is known)\n" + symo.write_params_table(self, title, inert=True, dynam=True) + nealgos.mobile_inverse_dynmodel(pseudo_robo, symo) + else: + # with rigid joints and fixed base + title = title + "Robot with rigid joints and fixed base\n" + symo.write_params_table(self, title, inert=True, dynam=True) + nealgos.fixed_inverse_dynmodel(pseudo_robo, symo) + symo.file_close() + return symo + + def compute_baseparams(self): + """ + Compute the Base Inertial Parameters of the robot. + """ + base_robo = copy.deepcopy(self) + symo = symbolmgr.SymbolManager() + symo.file_open(base_robo, 'regp') + title = "Base Inertial Parameters equations" + symo.write_params_table( + base_robo, title, inert=True, dynam=True + ) + # compute base inertial params + baseparams.base_inertial_parameters(base_robo, symo) + symo.write_line() + title = "Grouped inertial parameters" + symo.write_params_table( + base_robo, title, inert=True, equations=False + ) + symo.file_close() + # set new name for robot with base params + base_robo.name = base_robo.name + "_base" + file_path = filemgr.get_file_path(base_robo) + base_robo.set_par_file_path(file_path) + return symo, base_robo + + def compute_dynidenmodel(self): + """ + Compute the Dynamic Identification model of the robot. + """ + symo = symbolmgr.SymbolManager() + symo.file_open(self, 'dim') + title = "Dynamic Identification Model (Newton-Euler method)" + symo.write_params_table(self, title, inert=True, dynam=True) + dyniden.dynamic_identification_model(self, symo) + symo.file_close() + return symo + + @property + def q_vec(self): + """Generates vector of joint variables + """ + qs = [] + for i in xrange(1, self.NJ): + if self.sigma[i] != 2: + qs.append(self.get_q(i)) + return qs + + @property + def endeffectors(self): + return set(range(1, self.NJ + 1)) - set(self.ant) + + @property + def q_passive(self): + """Generates vector of passive joint variables (including cut!) + """ + q = list() + for i in xrange(1, self.NJ): + if self.mu[i] == 0: + q.append(self.get_q(i)) + return q + + @property + def q_active(self): + """Generates vector of active joint variables (including cut!) + """ + q = list() + for i in xrange(1, self.NJ): + if self.mu[i] == 1: + q.append(self.get_q(i)) + return q + + @property + def indx_passive(self): + """Generates vector of passive joint indices + """ + return [i for i in xrange(1, self.NL) if self.mu[i] == 0] + + @property + def indx_active(self): + """Generates vector of active joint indices + """ + return [i for i in xrange(1, self.NL) if self.mu[i] == 1] + + @property + def indx_cut(self): + """Generates vector of cut joint indices + """ + return range(self.NL, self.NJ) + + def fric_v(self, j): + """Fluid friction torque + + Parameters + ========== + j: int + Joint index. + + Returns + ======= + fric_v: sympy expression + Expression for fluid friction torque of joint j + """ + return self.FV[j] * self.qdot[j] + + def fric_s(self, j): + """Dry friction torque + + Parameters + ========== + j: int + Joint index. + + Returns + ======= + fric_s: sympy expression + Expression for dry friction torque of joint j + """ + return self.FS[j] * sign(self.qdot[j]) + + @property + def W0(self): + return self.w0 + + @property + def WP0(self): + return self.wdot0 + + @property + def V0(self): + return self.v0 + + @property + def VP0(self): + return self.vdot0 + + @property + def QP(self): + return self.qdot + + @property + def QDP(self): + return self.qddot + + @property + def NJ(self): + """ Actual number of joints counting 0 + """ + return self.nj + 1 + + @property + def NL(self): + """ Actual number of links counting 0 + """ + return self.nl + 1 + + @property + def NF(self): + """ Actual number of frames counting 0 + """ + return self.nf + 1 + + @property + def loop_terminals(self): + B = self.NJ - self.NL + return [(i, i+B) for i in xrange(self.NL, self.NJ)] + + def paral(self, i, j): + if j is None: + return False + elif self.ant[i] == j: + return sin(self.alpha[i]) == 0 + elif self.ant[j] == i: + return sin(self.alpha[j]) == 0 + elif self.ant[j] == self.ant[i]: + return sin(self.alpha[j] - self.alpha[i]) == 0 + else: + return False + + def tau_ia(self, j): + """Actuator inertia torque + + Parameters + ========== + j: int + Joint index. + + Returns + ======= + fric_v: sympy expression + Expression for actuator inertia torque of joint j + """ + return self.IA[j] * self.qddot[j] + + def get_angles(self, j): + """List of non-constant angles of frame j + + Parameters + ========== + j: int + Frame index. + + Returns + ======= + get_angles: list of touples (var, name) + Returns list of touples, where: + var - the angle symbol, + name - brief name for cos and sin abbreviation + """ + angs = [] + if j not in xrange(self.NF): + return angs + if type(self.theta[j]) != int and not self.theta[j].is_number: + angs.append((self.theta[j], j)) + if type(self.alpha[j]) != int and not self.alpha[j].is_number: + angs.append((self.alpha[j], 'A%s' % j)) + if type(self.gamma[j]) != int and not self.gamma[j].is_number: + angs.append((self.gamma[j], 'G%s' % j)) + return angs + + def chain(self, j, k=0): + """Chain of antecedent frames between j-th and k-th frames + + Parameters + ========== + j: int + Start frame index. + k: int + Final frame index. + + Returns + ======= + u: list of ints + List of antecedent frames. j is the first index in the list. + k is not included + """ + u = [] + while j != k and j != 0: + u.append(j) + j = self.ant[j] + return u + + def loop_chain(self, i, j): + k = self.common_root(i, j) + chain = self.chain(i, k) + chain.append(k) + if k != j: + chain.extend(reversed(self.chain(j, k))) + return chain + + def common_root(self, i, j): + """Common root j-th and i-th frames + + Parameters + ========== + j: int + Frame index. + i: int + Frame index. + + Returns + ======= + common_root: int + The highest index of the common frame in chains for i and j. + If they don't have common root, -1 + """ + u = self.chain(i) + while True: + if j in u or j == 0: + return j + j = self.ant[j] + + def get_inert_param(self, j): + """Returns 10-vector of inertia paremeters of link j. + + Parameters + ========== + j: int + Link index. + + Returns + ======= + get_dynam_param: Matrix 10x1 + """ + K = [self.J[j][0], self.J[j][1], self.J[j][2], self.J[j][4], + self.J[j][5], self.J[j][8], self.MS[j][0], self.MS[j][1], + self.MS[j][2], self.M[j]] + return Matrix(K) + + def put_inert_param(self, K, j): + """Write the inertia parameters of link j from 10-vector K. + + Parameters + ========== + K: Matrix 10x1 + Vector of inertia parameters + j: int + Link index. + """ + self.J[j] = Matrix([[K[0], K[1], K[2]], + [K[1], K[3], K[4]], + [K[2], K[4], K[5]]]) + self.MS[j] = Matrix(3, 1, K[6:9]) + self.M[j] = K[9] + + def get_ext_dynam_head(self): + """Returns header for external forces and torques, + friction parameters and joint speeds, accelerations. + Used for output generation. + + Returns + ======= + get_ext_dynam_head: list of strings + """ + return ['j', 'FX', 'FY', 'FZ', 'CX', 'CY', 'CZ', + 'FS', 'FV', 'QP', 'QDP', 'GAM', 'eta', 'k'] + + def get_dynam_head(self): + """Returns header for inertia parameters. + Used for output generation. + + Returns + ======= + get_dynam_head: list of strings + """ + return ['j', 'XX', 'XY', 'XZ', 'YY', 'YZ', 'ZZ', + 'MX', 'MY', 'MZ', 'M', 'IA'] + + def get_geom_head(self): + """Returns header for geometric parameters. + Used for output generation. + + Returns + ======= + get_geom_head: list of strings + """ + return ['j', 'ant', 'sigma', 'mu', 'gamma', 'b', + 'alpha', 'd', 'theta', 'r'] + + def get_base_vel_head(self): + """Returns header for base velocities and gravity vector. + Used for output generation. + + Returns + ======= + get_base_vel_head: list of strings + """ + return ['axis', 'W0', 'WP0', 'V0', 'VP0', 'G'] + + def get_param_vec(self, head, j): + params = list() + axis_dict = {0: 'X', 1: 'Y', 2: 'Z'} + for h in head: + if h == 'j': + params.append(j) + elif h == 'axis': + params.append(axis_dict[j]) + else: + params.append(self.get_val(j, h)) + return params + + def _set_joint_defaults(self): + """ + Set default values for joint parameters for those exceptional + from the ones set in the ctor. + """ + for j in xrange(1, self.NJ): + try: + if self.sigma[j] == 2: + self.qdot[j] = 0 + self.qddot[j] = 0 + self.GAM[j] = 0 + else: + self.qdot[j] = var('QP{0}'.format(j)) + self.qddot[j] = var('QDP{0}'.format(j)) + self.GAM[j] = var('GAM{0}'.format(j)) + except IndexError: + # just ignore exception + pass + if self.eta[j] == 1: + self.k[j] = var('k{0}'.format(j)) + else: + self.k[j] = 0 + + def _set_geom_defaults(self): + """ + Set default values for geometric parameters for those + exceptional from the ones set in the ctor. + """ + for j in xrange(1, self.NF): + if self.sigma[j] == 0: + self.theta[j] = var('th{0}'.format(j)) + elif self.sigma[j] == 1: + self.r[j] = var('r{0}'.format(j)) + elif self.sigma[j] == 2: + self.mu[j] = 0 + + def _set_base_defaults(self): + """ + Set default values for base parameters for those exceptional + from the ones set in the ctor. + """ + if self.is_floating or self.is_mobile: + self.G = Matrix([var('GX'), var('GY'), var('GZ')]) + self.v0 = Matrix([var('VXb'), var('VYb'), var('VZb')]) + self.w0 = Matrix([var('WXb'), var('WYb'), var('WZb')]) + self.vdot0 = Matrix([var('VPXb'), var('VPYb'), var('VPZb')]) + self.wdot0 = Matrix([var('WPXb'), var('WPYb'), var('WPZb')]) + # Z matrix + for i in range(0, 3): + for j in range(0, 3): + self.Z[i, j] = var('Zr{0}{1}'.format(i+1, j+1)) + for j in range(0, 3): + self.Z[j, 3] = var('Zt{0}'.format(j+1)) + + diff --git a/pysymoro/robotf.py b/pysymoro/robotf.py new file mode 100644 index 0000000..1c217fa --- /dev/null +++ b/pysymoro/robotf.py @@ -0,0 +1,500 @@ +# -*- coding: utf-8 -*- + + +""" +This module contains the FloatingRobot data structure. Additionally as a +temporary measure some other data structures are included in this module +as well. +""" + + +from sympy import eye, var +from sympy import Matrix + +from pysymoro.screw import Screw +from pysymoro.dynparams import DynParams +from pysymoro.geoparams import GeoParams +from pysymoro import dynmodel +from symoroutils import filemgr +from symoroutils import tools + + +class FloatingRobot(object): + """ + Represent the FloatingRobot data structure and provide methods that + act as the gateway for robot modelling. + """ + def __init__( + self, name, links=0, joints=0, frames=0, + is_floating=True, structure=tools.TREE, is_mobile=False, + is_symbolic=True + ): + """ + Constructor period. + + Usage: + """ + """Name of the robot.""" + self.name = name + """Folder to store the files related to the robot.""" + self.directory = filemgr.get_folder_path(name) + """Total number of links in the robot.""" + self.num_links = links + """Total number of joints in the robot.""" + self.num_joints = joints + """Total number of frames in the robot.""" + self.num_frames = frames + """To indicate if the base is floating or not.""" + self.is_floating = is_floating + """Type of the robot structure - simple, tree, closed-loop""" + self.structure = structure + """To indicate if the robot is a wheeled mobile robot""" + self.is_mobile = is_mobile + """To indicate if computation should be symbolic or numeric""" + self.is_symbolic = is_symbolic + # properties dependent on number of links + """ + List to hold the dynamic parameters. The indices of the list + start with 0 and it corresponds to parameters of link 0 (virtual + link of the base). + """ + self.dyns = [DynParams(j) for j in self.link_nums] + # properties dependent on number of joints + """ + To indicate if a joint is rigid or flexible. 0 for rigid and 1 + for flexible. The indices of the list start with 0 and + corresponds to a virtual joint of the base. This joint is + usually rigid. + """ + self.etas = [0 for j in self.joint_nums] + """Joint stiffness usually indicated by k.""" + self.stiffness = [0 for j in self.joint_nums] + """Joint velocities.""" + self.qdots = [var('QP{0}'.format(j)) for j in self.joint_nums] + """Joint accelerations.""" + self.qddots = [var('QDP{0}'.format(j)) for j in self.joint_nums] + """Joint torques.""" + self.torques = [var('GAM{0}'.format(j)) for j in self.joint_nums] + # properties dependent on number of frames + """ + List to hold the geometric parameters. NOTE: This might be moved + to a separate function. + The indices of the list start with 0 and the first object + corresponds to parameters of frame 0 (base) wrt its antecedent + (some arbitary reference frame). + """ + self.geos = [GeoParams(j) for j in self.frame_nums] + # properties independent of number of links, joints and frames + """Gravity vector a 3x1 Matrix.""" + self.gravity = Matrix([0, 0, var('G3')]) + # the values of properties below would be modified during + # the computation of dynamic models. + """Base velocity 6x1 column vector - a Screw.""" + self.base_vel = Screw() + """Base acceleration 6x1 column vector - a Screw.""" + self.base_accel = Screw() + """Transformation matrix of base wrt a reference frame at time 0.""" + self.base_tmat = eye(4) + # call init methods + self._init_maps() + + def __str__(self): + str_format = "" + # add robot description + str_format = str_format + "Robot Description:\n" + str_format = str_format + "------------------\n" + str_format = str_format + ("\tName: %s\n" % str(self.name)) + str_format = str_format + ("\tLinks: %s\n" % str(self.num_links)) + str_format = str_format + ("\tJoints: %s\n" % str(self.num_joints)) + str_format = str_format + ("\tFrames: %s\n" % str(self.num_frames)) + str_format = str_format + ("\tFloating: %s\n" % str(self.is_floating)) + str_format = str_format + ("\tMobile: %s\n" % str(self.is_mobile)) + str_format = str_format + ("\tStructure: %s\n" % str(self.structure)) + str_format = str_format + '\n' + # add geometric params + str_format = str_format + "Geometric Parameters:\n" + str_format = str_format + "---------------------\n" + str_format = str_format + ('\t' + ('{:^8}' * 11) + '\n').format(*( + 'frame', 'ant', 'sigma', 'mu', 'gamma', 'b', + 'alpha', 'd', 'theta', 'r', 'q' + )) + for geo in self.geos: + str_format = str_format + str(geo) + '\n' + str_format = str_format + '\n' + # add dynamic params + str_format = str_format + "Dynamic Parameters:\n" + str_format = str_format + "-------------------\n" + str_format = str_format + ('\t' + ('{:^7}' * 20) + '\n').format(*( + 'link', 'XX', 'XY', 'XZ', 'YY', 'YZ', 'ZZ', 'MX', 'MY', 'MZ', + 'M', 'IA', 'Fc', 'Fv', 'FX', 'FY', 'FZ', 'CX', 'CY', 'CZ' + )) + for dyn in self.dyns: + str_format = str_format + str(dyn) + '\n' + str_format = str_format + '\n' + # add joint params + str_format = str_format + "Joint Parameters:\n" + str_format = str_format + "-----------------\n" + str_format = str_format + ('\t' + ('{:^9}' * 6) + '\n').format(*( + 'joint', 'eta', 'stiffness', 'qdot', 'qddot', 'torque' + )) + for jnt in self.joint_nums: + jnt_str = ('\t' + ('{:^9}' * 6) + '\n').format(*( + str(jnt), str(self.etas[jnt]), str(self.stiffness[jnt]), + str(self.qdots[jnt]), str(self.qddots[jnt]), + str(self.torques[jnt]) + )) + str_format = str_format + jnt_str + str_format = str_format + '\n' + # end + str_format = str_format + ('=*' * 60) + '=' + return str_format + + def __repr__(self): + repr_format = ( + "(name=%s, links=%d, joints=%d, frames=%d, floating=%s, type=%s)" + ) % ( + str(self.name), self.num_links, self.num_joints, + self.num_frames, str(self.is_floating), str(self.structure) + ) + return repr_format + + def get_val(self, idx, name): + """ + Get the robot parameter values. The method is maninly to + communicate with the UI. + + Args: + idx: The joint/link/frame (index) value. + name: The parameter name. + + Returns: + The value corresponding to the name and index. + """ + if name is 'Z': + return 0 + elif name in self._dyn_params_map: + attr = getattr(self, 'dyns') + value = getattr(attr[idx], self._dyn_params_map[name]) + return value + elif name in self._geo_params_map: + attr = getattr(self, 'geos') + value = getattr(attr[idx], self._geo_params_map[name]) + return value + elif name in self._misc_params_map: + attr = getattr(self, self._misc_params_map[name]) + value = attr[idx] + return value + elif name in self._base_params_map: + return None + + def put_val(self, idx, name, value): + """ + Modify the robot parameter values. This method is mainly to + communicate with the UI. For other purposes, see update_params() + method. + + Args: + idx: The joint/link/frame (index) value. + name: The parameter name. + value: The value with which the parameter is to be modified. + + Returns: + A `OK` if successful and `FAIL` otherwise. + """ + if name is 'Z': + return tools.OK + elif name in self._dyn_params_map: + param = {int(idx): {self._dyn_params_map[name]: value}} + return self.update_params('dyns', param) + elif name in self._geo_params_map: + if name in ['frame', 'ant', 'sigma', 'mu']: + value = int(value) + param = {int(idx): {self._geo_params_map[name]: value}} + return self.update_params('geos', param) + elif name in self._misc_params_map: + key = self._misc_params_map[name] + param = {int(idx): {key: value}} + return self.update_params('misc', param) + elif name in self._base_params_map: + return self.update_params('base', param) + return tools.FAIL + + def update_params(self, kind, params): + """ + Update the parameter values of the robot. + + Args: + kind: A string metioning the paramter type. More specifically + indicating whether the parameters correspond to geometric + (`geos`), dynamic (`dyns`), base velocity (`base`), + base acceleration (`base`) or others (`misc`). + params: A nested dict containing the index values as the key + and another dict containing the parameter names and + values as the value. See Examples for a valid dict + argument that is accepted by this method. + + Example 1: + params = { + 2: {'ant': 0, 'sigma': 1, 'mu': 1}, + 4: {'ant': 2, 'alpha': 0, 'd': 1, 'r': 0}, + 7: {'ant': 5} + } + Example 2: + params = { + 3: {'xx': 'XX3', 'msx': 'MX3', 'mass': 'M3'}, + 5: {'yz': 0, 'xz': 0, 'xy': 0}, + 2: {'fx_ext': 0, 'fy_ext': 0, 'mz_ext': 0}, + 4: {'ia': 'IA4', 'frv': 0, 'frc': 0} + } + Example 3: + params = { + 1: {'qdots': 'QP1', 'qddots': 'QDP1', 'torques': 'GAM1'}, + 2: {'etas': 1, 'stiffness': 'k2'} + } + Example 4: + params = { + 0: {'gravity': 'GX'}, + 1: {'gravity': 'GY'}, + 2: {'gravity': 'GZ'} + } + """ + if kind in ['dyns', 'geos']: + self._update_dyns_geos(kind, params) + elif kind is 'misc': + self._update_misc(params) + elif kind is 'base': + raise NotImplementedError( + "Yet to be supported." + ) + else: + errmsg = "`kind` can be ['dyns', 'geos', 'misc', 'base']. " + errmsg = errmsg + ("Current input: {0}").format(kind) + raise ValueError(errmsg) + return tools.OK + + def set_dyns_to_zero(self, links=None): + """ + Set all the dynamic parameter values to zero for a specified + list of links. If no link is specified, dynamic parameters for + all links are set to zero. + + Args: + links: An iterable object with the link numbers. + """ + if links == None: + links = self.link_nums + for link in links: + if link in self.link_nums: + self.dyns[link].set_to_zero() + else: + err_msg = "Link number {} does not belong to the robot." + raise IndexError(err_msg.format(link)) + + def compute_idym(self): + """ + Compute the Inverse Dynamic Model of the robot using the + recursive Newton-Euler algorithm. + """ + self.idym = dynmodel.inverse_dynamic_model(self) + + def compute_ddym(self): + """ + Compute the Direct Dynamic Model of the robot using the + recursive Newton-Euler algorithm. + """ + self.ddym = dynmodel.direct_dynamic_model(self) + + @property + def link_nums(self): + """ + Get the link numbers. + + Returns: + An iteratable object with the link numbers. + Note: + Add 1 to number of links. This serves two purposes - + one, it indicates a virtual link - to represent the base and + two, it makes sure list index 1 corresponds to link 1 and so + on. + """ + return xrange(self.num_links + 1) + + @property + def joint_nums(self): + """ + Get the joint numbers. + + Returns: + An iteratable object with the joint numbers. + Note: + Add 1 to number of joints. This serves two purposes - + one, it indicates a virtual joint 0 to represent the base and + two, it makes sure list index 1 corresponds to joint 1, list + index 2 to joint 2 and so on. + """ + return xrange(self.num_joints + 1) + + @property + def frame_nums(self): + """ + Get the frame numbers. + + Returns: + An iteratable object with the frame numbers. + Note: + Add 1 to number of frames in order to include the base + frame as well. This also makes the list index 1 correspond + to frame 1 and so on. + """ + return xrange(self.num_frames + 1) + + @property + def q_vec(self): + """ + Get the list of joint variables. + + Returns: + A list containing the joint variables. + """ + q = list() + for j in self.joint_nums: + if j == 0: + continue + q.append(self.geos[j].q) + return q + + @property + def q_passive(self): + """ + Get the list of passive joint variables. + + Returns: + A list containing the passive joint variables. + """ + q = list() + for j in self.joint_nums: + if j == 0: continue + if self.geos[j].mu == 0 and self.geos[j].sigma != 2: + q.append(self.geos[j].q) + return q + + @property + def q_active(self): + """ + Get the list of active joint variables. + + Returns: + A list containing the active joint variables. + """ + q = list() + for j in self.joint_nums: + if j == 0: continue + if self.geos[j].mu == 1 and self.geos[j].sigma != 2: + q.append(self.geos[j].q) + return q + + @property + def passive_joints(self): + """ + Get the list of joint numbers (indices) corresponding to passive + joints. + + Returns: + A list containing the passive joint numbers (indices). + """ + joints = list() + for j in self.joint_nums: + if j == 0: continue + if self.geos[j].mu == 0 and self.geos[j].sigma != 2: + joints.append(j) + return joints + + @property + def active_joints(self): + """ + Get the list of joint numbers (indices) corresponding to active + joints. + + Returns: + A list containing the active joint numbers (indices). + """ + joints = list() + for j in self.joint_nums: + if j == 0: continue + if self.geos[j].mu == 1 and self.geos[j].sigma != 2: + joints.append(j) + return joints + + def _update_base(self, params): + """ + Update the base velocity and acceleration values of the robot. + """ + pass + + def _update_misc(self, params): + """ + Update the miscellaneous parameter values of the robot. + """ + for key in params: + idx = int(key) + curr_params = params[key] + for attr in curr_params: + if not hasattr(self, attr): + raise AttributeError(( + "{0} is not an attribute of Robot." + ).format(str(attr))) + else: + try: + param = getattr(self, attr) + param[idx] = curr_params[attr] + except IndexError: + raise IndexError(( + "`{0}` doesnt have {1} index value." + ).format(attr, str(idx))) + + def _update_dyns_geos(self, attr, params): + """ + Update the geometric and dynamic parameter values of the robot. + """ + param = getattr(self, attr) + for key in params: + idx = int(key) + try: + param[idx].update_params(params[key]) + except IndexError: + raise IndexError(( + "`{0}` doesnt have {1} index value." + ).format(attr, str(idx))) + + def _init_maps(self): + """ + Initialise maps (dict) that will be used to read and write the + different parameter values of the robot. The main purpose of + these maps is to talk easily with the user interface. + """ + self._dyn_params_map = dict([ + ('XX', 'xx'), ('XY', 'xy'), ('XZ', 'xz'), + ('YY', 'yy'), ('YZ', 'yz'), ('ZZ', 'zz'), + ('MX', 'msx'), ('MY', 'msy'), ('MZ', 'msz'), ('M', 'mass'), + ('IA', 'ia'), ('FS', 'frc'), ('FV', 'frv'), + ('FX', 'fx_ext'), ('FY', 'fy_ext'), ('FZ', 'fz_ext'), + ('CX', 'mx_ext'), ('CY', 'my_ext'), ('CZ', 'mz_ext') + ]) + self._geo_params_map = dict([ + ('j', 'frame'), ('ant', 'ant'), ('sigma', 'sigma'), + ('mu', 'mu'), ('gamma', 'gamma'), ('b', 'b'), + ('alpha', 'alpha'), ('d', 'd'), ('theta', 'theta'), ('r', 'r') + ]) + self._base_params_map = dict([ + ('V0', 'base_vel'), ('VP0', 'base_accel'), + ('W0', 'base_vel'), ('WP0', 'base_accel') + ]) + self._misc_params_map = dict([ + ('QP', 'qdots'), ('QDP', 'qddots'), ('GAM', 'torques'), + ('eta', 'etas'), ('k', 'stiffness'), ('G', 'gravity'), + ]) + + # methods for backward-compatability with the old Robot (for fixed + # base) class. + + diff --git a/pysymoro/screw.py b/pysymoro/screw.py new file mode 100644 index 0000000..2466605 --- /dev/null +++ b/pysymoro/screw.py @@ -0,0 +1,128 @@ +# -*- coding: utf-8 -*- + + +""" +This module contains the Screw data structure. +""" + + +from sympy import zeros +from sympy import ShapeError + + +class Screw(object): + """ + Data structure: + Represent the screw notation - a 6x1 vector in which the first + three rows represent the linear term and the last three rows + represent the angular term. + This class basically adds a lot of constraints to the Matrix + class of sympy - atleast tries to :) + """ + def __init__(self, lin=zeros(3, 1), ang=zeros(3, 1)): + """ + Constructor period. + + Args: + lin: A 3x1 matrix - linear term set to 0 by default. + ang: A 3x1 matrix - angular term set to 0 by default. + """ + self._val = zeros(6, 1) + if lin.rows != 3 or lin.cols != 1: + raise ShapeError("Linear term matrix size has to be 3x1.") + elif ang.rows != 3 or ang.cols != 1: + raise ShapeError("Angular term matrix size has to be 3x1.") + self._val[0:3, 0] = lin + self._val[3:6, 0] = ang + + def __str__(self): + row_format = '[' + ('{:} ; ' * 5) + ('{:}') + ']' + str_format = row_format.format(*( + str(self._val[0]), str(self._val[1]), str(self._val[2]), + str(self._val[3]), str(self._val[4]), str(self._val[5]) + )) + return str_format + + def __repr__(self): + repr_format = 'Screw({0})'.format(str(self)) + return repr_format + + @property + def val(self): + """ + Get the current value. + + Returns: + A 6x1 Matrix (column vector) with the current value. + """ + return self._val + + @val.setter + def val(self, value): + """ + Set the current value. + + Args: + value: A 6x1 Matrix + """ + if value.rows != 6 or value.cols != 1: + raise ShapeError("Matrix size has to be 6x1.") + self._val = value + + @property + def lin(self): + """ + Get the linear term value. + + Returns: + A 3x1 Matrix with the current linear term value. + """ + return self._val[0:3, 0] + + @lin.setter + def lin(self, value): + """ + Set the linear term value. + + Args: + value: A 3x1 Matrix - linear term + """ + if value.rows != 3 or value.cols != 1: + raise ShapeError("Linear term matrix size has to be 3x1.") + self._val[0:3, 0] = value + + @property + def ang(self): + """ + Get the angular term value. + + Returns: + A 3x1 Matrix with the current angular term value. + """ + return self._val[3:6, 0] + + @ang.setter + def ang(self, value): + """ + Set the linear term value. + + Args: + value: A 3x1 Matrix - angular term + """ + if value.rows != 3 or value.cols != 1: + raise ShapeError("Angular term matrix size has to be 3x1.") + self._val[3:6, 0] = value + + def __eq__(self, other): + """Check equality between two instances of Screw.""" + if type(self) != type(other): + raise ValueError( + "Unable to compare %s with Screw type." % str(type(other)) + ) + return self.val == other.val + + def __ne__(self, other): + """Check non-equality between two instances of Screw.""" + return not self == other + + diff --git a/pysymoro/screw6.py b/pysymoro/screw6.py new file mode 100644 index 0000000..e91d937 --- /dev/null +++ b/pysymoro/screw6.py @@ -0,0 +1,198 @@ +# -*- coding: utf-8 -*- + + +""" +This module contains the Screw6 data structure. +""" + + +from sympy import zeros +from sympy import ShapeError + + +class Screw6(object): + """ + Data structure: + Represent the data structure (base class) to hold a 6x6 matrix + which in turn contains four 3x3 matrices. + """ + def __init__(self, *args, **kwargs): + """ + Constructor period. + + Usage: + >>> # initialise to 0 by default + Screw6() + >>> # initialise to a given 6x6 matrix + Screw6() + >>> # intiialise each of the 4 sub-matrices individually + Screw6(, , , ) + >>> # initialise using keywords + Screw6(value=) + Screw6( + tl=, tr=, + bl=, br= + ) + """ + self._val = zeros(6, 6) + if len(args) == 1: + self.val = args[0] + elif len(args) == 4: + self.topleft = args[0] + self.topright = args[1] + self.botleft = args[2] + self.botright = args[3] + elif len(args) > 0: + raise NotImplementedError( + """Screw6 Constructor does not accept %s positional + arguments. See Usage.""" % (str(len(args))) + ) + if len(kwargs) == 4: + self.topleft = kwargs['tl'] + self.topright = kwargs['tr'] + self.botleft = kwargs['bl'] + self.botright = kwargs['br'] + elif len(kwargs) == 1: + self.val = kwargs['value'] + elif len(kwargs) > 0: + raise NotImplementedError( + """Screw6 Constructor does not accept %s keyword + arguments. See Usage.""" % (str(len(kwargs))) + ) + + def __str__(self): + row_format = '[' + ((('{},' * 6) + ';') * 6) + ']' + elements = list() + for i in range(self._val.rows): + for j in range(self._val.cols): + elements.append(str(self._val[i, j])) + str_format = row_format.format(*elements) + return str_format + + def __repr__(self): + repr_format = 'Screw6()' + return repr_format + + @property + def val(self): + """ + Get current value. + + Returns: + A 6x6 Matrix with the current value + """ + return self._val + + @val.setter + def val(self, value): + """ + Set the current value. + + Args: + value: A 6x6 Matrix + """ + if value.rows != 6 or value.cols != 6: + raise ShapeError("Matrix size has to be 6x6.") + self._val = value + + @property + def topleft(self): + """ + Get the top-left part of the 6x6 matrix. + + Returns: + A 3x3 Matrix. + """ + return self._val[0:3, 0:3] + + @property + def topright(self): + """ + Get the top-right part of the 6x6 matrix. + + Returns: + A 3x3 Matrix. + """ + return self._val[0:3, 3:6] + + @property + def botleft(self): + """ + Get the bottom-left part of the 6x6 matrix. + + Returns: + A 3x3 Matrix. + """ + return self._val[3:6, 0:3] + + @property + def botright(self): + """ + Get the bottom-right part of the 6x6 matrix. + + Returns: + A 3x3 Matrix. + """ + return self._val[3:6, 3:6] + + @topleft.setter + def topleft(self, value): + """ + Set the top-left part of the 6x6 matrix. + + Args: + value: A 3x3 Matrix - top-left value. + """ + if value.rows != 3 or value.cols != 3: + raise ShapeError("Top-left value size has to be 3x3.") + self._val[0:3, 0:3] = value + + @topright.setter + def topright(self, value): + """ + Set the top-right part of the 6x6 matrix. + + Args: + value: A 3x3 Matrix - top-right value. + """ + if value.rows != 3 or value.cols != 3: + raise ShapeError("Top-right value size has to be 3x3.") + self._val[0:3, 3:6] = value + + @botleft.setter + def botleft(self, value): + """ + Set the bottom-left part of the 6x6 matrix. + + Args: + value: A 3x3 Matrix - bottom-left value. + """ + if value.rows != 3 or value.cols != 3: + raise ShapeError("Bottom-left value size has to be 3x3.") + self._val[3:6, 0:3] = value + + @botright.setter + def botright(self, value): + """ + Set the bottom-right part of the 6x6 matrix. + + Args: + value: A 3x3 Matrix - bottom-right value. + """ + if value.rows != 3 or value.cols != 3: + raise ShapeError("Bottom-right value size has to be 3x3.") + self._val[3:6, 3:6] = value + + def __eq__(self, other): + """Check equality between two instances of Screw6.""" + if type(self) != type(other): + raise ValueError( + "Unable to compare %s with Screw6 type." % str(type(other)) + ) + return self.val == other.val + + def __ne__(self, other): + """Check non-equality between two instances of Screw6.""" + return not self == other + + diff --git a/pysymoro/tests/check_dynmodel.py b/pysymoro/tests/check_dynmodel.py new file mode 100644 index 0000000..3302c46 --- /dev/null +++ b/pysymoro/tests/check_dynmodel.py @@ -0,0 +1,309 @@ +#!/usr/bin/env python +# -*- coding: utf-8 -*- + + +import math +import random +import unittest + +from sympy import var + +from symoroutils import tools +from pysymoro.robotf import FloatingRobot as Robot + + +def planar2r(): + robo = Robot('planar2r', 2, 2, 3, False, tools.SIMPLE) + # update geometric params + params = { + 1: {'sigma': 0, 'mu': 1, 'theta': var('th1')}, + 2: {'sigma': 0, 'mu': 1, 'alpha': 0, 'd': var('L1'), 'theta': var('th2')}, + 3: {'sigma': 2, 'd': var('L2')} + } + robo.update_params('geos', params) + # update dynamic params + params = { + 1: {'xx': 0, 'xy': 0, 'xz': 0, 'yy': 0, 'yz': 0, 'ia': 0} + } + robo.update_params('dyns', params) + # update joint params + params = { + 0: {'qdots': 0, 'qddots': 0, 'torques': 0} + } + robo.update_params('misc', params) + # update gravity vector + params = { + 0: {'gravity': 0}, + 1: {'gravity': 0}, + 2: {'gravity': var('G3')}, + } + robo.update_params('misc', params) + return robo + + +def planar2r_numerical(is_floating=False): + # numerical values (random in some case) + L1 = 0.5 + L2 = 0.4 + ZZ1 = 3.7 + ZZ2 = 0.35 + MX2 = 0.4 + MY2 = 0.15 + M1 = 1.2 + M2 = 0.8 + FC1 = 0.3 + FC2 = 0.25 + FV1 = 0.3 + FV2 = 0.18 + IA1 = 0.0 + IA2 = 0.0 + G3 = -9.81 + # create robot + robo = Robot( + 'planar2r', 2, 2, 3, is_floating, + tools.SIMPLE, is_symbolic=False + ) + robo.set_dyns_to_zero() + # update geometric params + params = { + 1: {'sigma': 0, 'mu': 1}, + 2: {'sigma': 0, 'mu': 1, 'd': L1}, + 3: {'sigma': 2, 'd': L2} + } + robo.update_params('geos', params) + # update dynamic params + params = { + 1: { + 'zz': ZZ1, 'frc': FC1, 'frv': FV1, 'ia': IA1, 'mass': M1 + }, + 2: { + 'zz': ZZ2, 'frc': FC2, 'frv': FV2, 'ia': IA2, 'mass': M2, + 'msx': MX2, 'msy': MY2 + } + } + robo.update_params('dyns', params) + # update joint params + params = {0: {'qdots': 0, 'qddots': 0, 'torques': 0}} + robo.update_params('misc', params) + # update gravity vector + params = { + 0: {'gravity': 0}, + 1: {'gravity': 0}, + 2: {'gravity': G3}, + } + robo.update_params('misc', params) + return robo + + +def set_planar2r_joint_state(robo, q, qdot, qddot): + """ + Set the joint states for a 2R planar robot. + + Args: + robo: An instance of Robot class. + q: Joint positions. A list of size 2. + qdot: Joint velocities. A list of size 2. + qddot: Joint accelerations. A list of size 2. + + Returns: + The modified instance of Robot class. + """ + q1 = q[0] + q2 = q[1] + q1dot = qdot[0] + q2dot = qdot[1] + q1ddot = qddot[0] + q2ddot = qddot[1] + # update joint variables + params = {1: {'theta': q1}, 2: {'theta': q2}} + robo.update_params('geos', params) + # update joint params + params = { + 1: {'qdots': q1dot, 'qddots': q1ddot}, + 2: {'qdots': q2dot, 'qddots': q2ddot} + } + robo.update_params('misc', params) + return robo + + +def set_planar2r_joint_torque(robo, qtorque): + """ + Set the joint torques for a 2R planar robot. + + Args: + robo: An instance of Robot class. + qtorque: Joint torques. A list of size 2. + + Returns: + The modified instance of Robot class. + """ + gam1 = qtorque[0] + gam2 = qtorque[1] + # update torque values + params = {1: {'torques': gam1}, 2: {'torques': gam2}} + robo.update_params('misc', params) + return robo + + +class TestDynModelPlanar2rFixed(unittest.TestCase): + """ + Unit test for testing the inverse and direct dynamic model + computation for floating base robots algorithm. This testing is done + numerically. + """ + def setUp(self): + pass + + def test_when_zero(self): + """ + Test the dynamic model computation when joint position, + velocity and acceleration are set to zero. + """ + robo = planar2r_numerical() + # set joint state + robo = set_planar2r_joint_state( + robo, [0.0, 0.0], [0.0, 0.0], [0.0, 0.0] + ) + # compute IDyM + robo.compute_idym() + # set torque values for DDyM + robo = set_planar2r_joint_torque( + robo, [robo.idym.torques[1], robo.idym.torques[2]] + ) + # compute DDyM + robo.compute_ddym() + # check if the result of IDyM (computed torques) are zero + self.assertEqual(robo.idym.torques[1], 0.0) + self.assertEqual(robo.idym.torques[2], 0.0) + # check if the result of DDyM (computed qddots) are zero + self.assertEqual(robo.ddym.qddots[1], 0.0) + self.assertEqual(robo.ddym.qddots[2], 0.0) + # check if input to IDyM is same as output of DDyM + self.assertEqual(robo.ddym.qddots[1], robo.qddots[1]) + self.assertEqual(robo.ddym.qddots[2], robo.qddots[2]) + + def test_when_random(self): + """ + Test the dynamic model computation when joint position, + velocity and acceleration are set to random meaningful values. + """ + robo = planar2r_numerical() + # initialise joint position, velocity, acceleration to random + # values + random.seed(math.pi) + q = list(random.uniform(-math.pi, math.pi) for j in range(2)) + qdot = list(random.uniform(-math.pi, math.pi) for j in range(2)) + qddot = list(random.uniform(-math.pi, math.pi) for j in range(2)) + # set joint state + robo = set_planar2r_joint_state(robo, q, qdot, qddot) + # compute IDyM + robo.compute_idym() + print(robo.idym) + # set torque values for DDyM + robo = set_planar2r_joint_torque( + robo, [robo.idym.torques[1], robo.idym.torques[2]] + ) + # compute DDyM + robo.compute_ddym() + # + print('\n') + print(q) + print(qdot) + print(qddot) + print(robo.idym.torques) + print(robo.ddym.qddots) + # check if input to IDyM is same as output of DDyM + self.assertEqual(robo.ddym.qddots[1], robo.qddots[1]) + self.assertEqual(robo.ddym.qddots[2], robo.qddots[2]) + + +class TestDynModelPlanar2rFloating(unittest.TestCase): + """ + Unit test for testing the inverse and direct dynamic model + computation for floating base robots algorithm. This testing is done + numerically. + """ + def setUp(self): + pass + + def test_when_zero(self): + """ + Test the dynamic model computation when joint position, + velocity and acceleration are set to zero. + """ + robo = planar2r_numerical(is_floating=True) + # set joint state + robo = set_planar2r_joint_state( + robo, [0.0, 0.0], [0.0, 0.0], [0.0, 0.0] + ) + # compute IDyM + robo.compute_idym() + # set torque values for DDyM + robo = set_planar2r_joint_torque( + robo, [robo.idym.torques[1], robo.idym.torques[2]] + ) + # compute DDyM + robo.compute_ddym() + # assertions + # check if the result of IDyM (computed torques) are zero + self.assertEqual(robo.idym.torques[1], 0.0) + self.assertEqual(robo.idym.torques[2], 0.0) + # check if the result of DDyM (computed qddots) are zero + self.assertEqual(robo.ddym.qddots[1], 0.0) + self.assertEqual(robo.ddym.qddots[2], 0.0) + # check if input to IDyM is same as output of DDyM + self.assertEqual(robo.ddym.qddots[1], robo.qddots[1]) + self.assertEqual(robo.ddym.qddots[2], robo.qddots[2]) + + def test_when_random(self): + """ + Test the dynamic model computation when joint position, + velocity and acceleration are set to random meaningful values. + """ + robo = planar2r_numerical(is_floating=True) + # initialise joint position, velocity, acceleration to random + # values + random.seed(math.pi) + q = list(random.uniform(-math.pi, math.pi) for j in range(2)) + qdot = list(random.uniform(-math.pi, math.pi) for j in range(2)) + qddot = list(random.uniform(-math.pi, math.pi) for j in range(2)) + # set joint state + robo = set_planar2r_joint_state(robo, q, qdot, qddot) + # compute IDyM + robo.compute_idym() + # set torque values for DDyM + robo = set_planar2r_joint_torque( + robo, [robo.idym.torques[1], robo.idym.torques[2]] + ) + # compute DDyM + robo.compute_ddym() + # + print('\n') + print(q) + print(qdot) + print(qddot) + print(robo.idym.torques) + print(robo.ddym.qddots) + # assertions + # check if input to IDyM is same as output of DDyM + self.assertEqual(robo.ddym.qddots[1], robo.qddots[1]) + self.assertEqual(robo.ddym.qddots[2], robo.qddots[2]) + + +def run_tests(): + """Load and run the unittests""" + unit_suite = unittest.TestLoader().loadTestsFromTestCase( + TestDynModelPlanar2rFixed + ) + unittest.TextTestRunner(verbosity=2).run(unit_suite) + + +def main(): + """Main function.""" + run_tests() + + +if __name__ == '__main__': + main() + + diff --git a/pysymoro/tests/check_pieper.py b/pysymoro/tests/check_pieper.py new file mode 100644 index 0000000..976c3bb --- /dev/null +++ b/pysymoro/tests/check_pieper.py @@ -0,0 +1,47 @@ +#!/usr/bin/env python +# -*- coding: utf-8 -*- + +import unittest + +from sympy import var, Matrix +from numpy import random, amax, matrix, eye, zeros + +from pysymoro import geometry, pieper, invdata +from symoroutils import samplerobots, symbolmgr + +class TestIGM(unittest.TestCase): + """Unit test for GeoParams class.""" + def setUp(self): + self.symo = symbolmgr.SymbolManager() + + def test_igm_rx90(self): + robo = samplerobots.rx90() + nTm = invdata.T_GENERAL + self.symo.write_line("\r\n*******************PIEPER METHOD STARTS********************** \r\n") + pieper._pieper_solve(robo, self.symo, nTm, 0, robo.nf) +## self.symo.gen_func_string('IGM_gen', robo.q_vec, +## invgeom.T_GENERAL, syntax='matlab') + igm_f = self.symo.gen_func('IGM_gen', robo.q_vec, + invdata.T_GENERAL) + self.symo.write_line("\r\n*******************PIEPER METHOD FINISHED********************") +## T = geometry.dgm(robo, self.symo, 0, robo.nf, +## fast_form=True, trig_subs=True) +## f06 = self.symo.gen_func('DGM_generated1', T, robo.q_vec) +## for x in xrange(100): +## arg = random.normal(size=robo.nj) +## Ttest = f06(arg) +## solution = igm_f(Ttest) + +def run_tests(): + """Load and run the unittests""" + unit_suite = unittest.TestLoader().loadTestsFromTestCase(TestIGM) + unittest.TextTestRunner(verbosity=2).run(unit_suite) + + +def main(): + """Main function.""" + run_tests() + + +if __name__ == '__main__': + main() diff --git a/pysymoro/tests/oldtest.py b/pysymoro/tests/oldtest.py new file mode 100644 index 0000000..c74f777 --- /dev/null +++ b/pysymoro/tests/oldtest.py @@ -0,0 +1,226 @@ +#!/usr/bin/env python +# -*- coding: utf-8 -*- + + +# This file is part of the OpenSYMORO project. Please see +# https://github.com/symoro/symoro/blob/master/LICENCE for the licence. + + +""" +Unit tests for SYMORO modules +""" + + +import os +import unittest + +from sympy import sympify, var, Matrix +from sympy.abc import A, B, C, X, Y, Z +from numpy import random, amax, matrix, eye, zeros + +from pysymoro import robot +from pysymoro import geometry +from pysymoro.geometry import Transform as trns +from pysymoro import kinematics +from pysymoro import invgeom +from symoroutils import filemgr +from symoroutils import parfile +from symoroutils import samplerobots +from symoroutils import symbolmgr +from symoroutils import tools + + +class testMisc(unittest.TestCase): + def test_robo_misc(self): + print "######## test_robo_misc ##########" + self.robo = samplerobots.sr400() + q = list(var('th1:10')) + self.assertEqual(self.robo.q_vec, q) + self.assertEqual(self.robo.chain(6), [6, 5, 4, 3, 2, 1]) + self.assertEqual(self.robo.chain(6, 3), [6, 5, 4]) + self.assertEqual(self.robo.loop_chain(8, 9), [8, 9]) + self.assertEqual(self.robo.loop_chain(0, 6), [0, 1, 2, 3, 4, 5, 6]) + self.assertEqual(self.robo.loop_chain(6, 0), [6, 5, 4, 3, 2, 1, 0]) + self.assertEqual(self.robo.loop_chain(9, 10), [9, 8, 7, 1, 2, 3, 10]) + self.assertEqual(self.robo.loop_terminals, [(9, 10)]) + l1 = self.robo.get_geom_head() + l2 = self.robo.get_dynam_head() + l3 = self.robo.get_ext_dynam_head() + for name in l1[1:] + l2[1:] + l3[1:]: + for i in xrange(self.robo.NL): + if name in tools.INT_KEYS: + self.assertEqual(self.robo.put_val(i, name, i), tools.OK) + else: + v = var(name + str(i)) + self.assertEqual(self.robo.put_val(i, name, v), tools.OK) + for name in l3[1:]+l2[1:]+l1[1:]: + for i in xrange(self.robo.NL): + if name in tools.INT_KEYS: + self.assertEqual(self.robo.get_val(i, name), i) + else: + v = var(name + str(i)) + self.assertEqual(self.robo.get_val(i, name), v) + + +class testGeometry(unittest.TestCase): + + def setUp(self): + self.symo = symbolmgr.SymbolManager() + self.robo = samplerobots.rx90() + +# def test_misc(self): +# self.assertEqual(self.robo.structure, tools.SIMPLE) +# self.robo.ant[3] = 0 +# self.assertEqual(self.robo.type_of_structure, tools.TREE) +# self.robo.ant[3] = 2 +# self.assertEqual(self.robo.type_of_structure, tools.SIMPLE) +# robo2 = samplerobots.sr400() +# self.assertEqual(robo2.type_of_structure, tools.CLOSED_LOOP) + + def test_dgm_rx90(self): + print "######## test_dgm_rx90 ##########" + T = geometry.dgm(self.robo, self.symo, 0, 6, + fast_form=True, trig_subs=True) + self.symo.gen_func_string('DGM_generated1', T, self.robo.q_vec, + syntax = 'matlab') + f06 = self.symo.gen_func('DGM_generated1', T, self.robo.q_vec) + T = geometry.dgm(self.robo, self.symo, 6, 0, + fast_form=True, trig_subs=True) + f60 = self.symo.gen_func('DGM_generated2', T, self.robo.q_vec) + for x in xrange(10): + arg = random.normal(size=6) + M = matrix(f06(arg))*matrix(f60(arg))-eye(4) + self.assertLess(amax(M), 1e-12) + t06 = matrix([[1, 0, 0, 1], [0, 1, 0, 0], + [0, 0, 1, 1], [0, 0, 0, 1]]) + self.assertLess(amax(matrix(f06(zeros(6)))-t06), 1e-12) + T46 = geometry.dgm(self.robo, self.symo, 4, 6, + fast_form=False, trig_subs=True) + C4, S4, C5, C6, S5, S6, RL4 = var("C4,S4,C5,C6,S5,S6,RL4") + T_true46 = Matrix([[C5*C6, -C5*S6, -S5, 0], [S6, C6, 0, 0], + [S5*C6, -S5*S6, C5, 0], [0, 0, 0, 1]]) + self.assertEqual(T46, T_true46) + T36 = geometry.dgm(self.robo, self.symo, 3, 6, + fast_form=False, trig_subs=True) + T_true36 = Matrix([[C4*C5*C6-S4*S6, -C4*C5*S6-S4*C6, -C4*S5, 0], + [S5*C6, -S5*S6, C5, RL4], + [-S4*C5*C6-C4*S6, S4*C5*S6-C4*C6, S4*S5, 0], + [0, 0, 0, 1]]) + self.assertEqual(T36, T_true36) + + def test_dgm_sr400(self): + print "######## test_dgm_sr400 ##########" + self.robo = samplerobots.sr400() + T = geometry.dgm(self.robo, self.symo, 0, 6, + fast_form=True, trig_subs=True) + f06 = self.symo.gen_func('DGM_generated1', T, self.robo.q_vec) + T = geometry.dgm(self.robo, self.symo, 6, 0, + fast_form=True, trig_subs=True) + f60 = self.symo.gen_func('DGM_generated2', T, self.robo.q_vec) + for x in xrange(10): + arg = random.normal(size=9) + M = matrix(f06(arg))*matrix(f60(arg))-eye(4) + self.assertLess(amax(M), 1e-12) + t06 = matrix([[1, 0, 0, 3], [0, -1, 0, 0], + [0, 0, -1, -1], [0, 0, 0, 1]]) + self.assertLess(amax(matrix(f06(zeros(9))) - t06), 1e-12) + + def test_igm(self): + print "######## test_igm ##########" + self.robo.r[6] = var('R6') + self.robo.gamma[6] = var('G6') + invgeom._paul_solve(self.robo, self.symo, invgeom.T_GENERAL, 0, 6) + self.symo.gen_func_string('IGM_gen', self.robo.q_vec, + invgeom.T_GENERAL, syntax = 'matlab') + igm_f = self.symo.gen_func('IGM_gen', self.robo.q_vec, + invgeom.T_GENERAL) + T = geometry.dgm(self.robo, self.symo, 0, 6, + fast_form=True, trig_subs=True) + f06 = self.symo.gen_func('DGM_generated1', T, self.robo.q_vec) + for x in xrange(100): + arg = random.normal(size=6) + Ttest = f06(arg) + solution = igm_f(Ttest) + for q in solution: + self.assertLess(amax(matrix(f06(q))-Ttest), 1e-12) + + def test_loop(self): + print "######## test_loop ##########" + self.robo = samplerobots.sr400() + invgeom.loop_solve(self.robo, self.symo) + self.symo.gen_func_string('IGM_gen', self.robo.q_vec, + self.robo.q_active, syntax = 'matlab') + l_solver = self.symo.gen_func('IGM_gen', self.robo.q_vec, + self.robo.q_active) + T = geometry.dgm(self.robo, self.symo, 9, 10, + fast_form=True, trig_subs=True) + t_loop = self.symo.gen_func('DGM_generated1', T, self.robo.q_vec) + for x in xrange(10): + arg = random.normal(size=6) + solution = l_solver(arg) + for q in solution: + self.assertLess(amax(matrix(t_loop(q))-eye(4)), 1e-12) + + +class testKinematics(unittest.TestCase): + def setUp(self): + self.symo = symbolmgr.SymbolManager() + self.robo = samplerobots.rx90() + + def test_speeds(self): + print 'Speeds and accelerations' + kinematics.velocities(self.robo) + kinematics.accelerations(self.robo) + kinematics.jdot_qdot(self.robo) + print 'Kinematic constraint equations' + kinematics.kinematic_constraints(samplerobots.sr400()) + + def test_jac(self): + print "######## test_jac ##########" + kinematics.jacobian(self.robo, 6, 3, 6) + for j in xrange(1, 7): + print "######## Jac validation through DGM ##########" + #compute Jac + J, l = kinematics._jac(self.robo, self.symo, j, 0, j) + jacj = self.symo.gen_func('JacRX90', J, self.robo.q_vec) + #compute DGM + T = geometry.dgm(self.robo, self.symo, 0, j, + fast_form=True, trig_subs=True) + T0j = self.symo.gen_func('DGM_generated1', T, self.robo.q_vec) + for i in xrange(10): + dq = random.normal(size=6, scale=1e-7) + q = random.normal(size=6) + dX = matrix(jacj(q)) * matrix(dq[:j]).T + T = (matrix(T0j(q+dq)) - T0j(q)) + self.assertLess(amax(dX[:3] - trns.P(T)), 1e-12) + + def test_jac2(self): + print "######## test_jac2 ##########" + J, L = kinematics._jac(self.robo, self.symo, 6, 3, 3) + jac63 = self.symo.gen_func('Jac1RX90', J, self.robo.q_vec) + L63 = self.symo.gen_func('LRX90', L, self.robo.q_vec) + J, L = kinematics._jac(self.robo, self.symo, 6, 3, 6) + jac66 = self.symo.gen_func('Jac2RX90', J, self.robo.q_vec) + for i in xrange(10): + q = random.normal(size=6) + j63 = matrix(jac63(q)) + l63 = matrix(L63(q)) + j66 = matrix(jac66(q)) + X = eye(6) + X[:3, 3:] = l63 + self.assertLess(amax(j66 - X*j63), 1e-12) + + +if __name__ == '__main__': +# suite = unittest.TestSuite() +# suite.addTest(testMisc('test_robo_misc')) +# suite.addTest(testGeometry('test_dgm_rx90')) +# suite.addTest(testGeometry('test_dgm_sr400')) +# suite.addTest(testGeometry('test_igm')) +# suite.addTest(testGeometry('test_loop')) +# suite.addTest(testKinematics('test_jac')) +# suite.addTest(testKinematics('test_jac2')) +# unittest.TextTestRunner(verbosity=2).run(suite) +# unittest.main() + + diff --git a/pysymoro/tests/run_all_tests.py b/pysymoro/tests/run_all_tests.py new file mode 100644 index 0000000..9739990 --- /dev/null +++ b/pysymoro/tests/run_all_tests.py @@ -0,0 +1,29 @@ +#!/usr/bin/env python +# -*- coding: utf-8 -*- + + +""" +This module discovers all the unittest code in the folder and runs them. +""" + + +import inspect +import os +import unittest + + +def main(): + """Main function.""" + file_search_path = os.path.dirname( + os.path.abspath(inspect.getfile(inspect.currentframe())) + ) + all_tests = unittest.TestLoader().discover( + file_search_path, pattern='test_*.py' + ) + unittest.TextTestRunner(verbosity=2).run(all_tests) + + +if __name__ == '__main__': + main() + + diff --git a/pysymoro/tests/test_dynparams.py b/pysymoro/tests/test_dynparams.py new file mode 100644 index 0000000..dca180c --- /dev/null +++ b/pysymoro/tests/test_dynparams.py @@ -0,0 +1,164 @@ +#!/usr/bin/env python +# -*- coding: utf-8 -*- + + +"""Unit test module for DynParams class.""" + + +import unittest + +from sympy import eye, var +from sympy import Matrix + +from pysymoro import screw +from pysymoro import screw6 +from symoroutils import tools + +from pysymoro.dynparams import DynParams + + +class TestDynParams(unittest.TestCase): + """Unit test for DynParams class.""" + def setUp(self): + link = 33 + # setup an instance of DynParams + self.data = DynParams(link) + # setup data to compare against + self.link = link + # inertia matrix terms + self.xx = var('XX33') + self.xy = var('XY33') + self.xz = var('XZ33') + self.yy = var('YY33') + self.yz = var('YZ33') + self.zz = var('ZZ33') + # mass tensor terms + self.msx = var('MX33') + self.msy = var('MY33') + self.msz = var('MZ33') + # link mass + self.mass = var('M33') + # rotor inertia term + self.ia = var('IA33') + # coulomb friction parameter + self.frc = var('FS33') + # viscous friction parameter + self.frv = var('FV33') + # external forces and moments + self.fx_ext = var('FX33') + self.fy_ext = var('FY33') + self.fz_ext = var('FZ33') + self.mx_ext = var('CX33') + self.my_ext = var('CY33') + self.mz_ext = var('CZ33') + # setup matrix terms to compare against + self.inertia = Matrix([ + [self.xx, self.xy, self.xz], + [self.xy, self.yy, self.yz], + [self.xz, self.yz, self.zz] + ]) + self.mass_tensor = Matrix([self.msx, self.msy, self.msz]) + m_eye = self.mass * eye(3) + ms_skew = tools.skew(self.mass_tensor) + self.spatial_inertia = screw6.Screw6( + tl=m_eye, tr=ms_skew.transpose(), + bl=ms_skew, br=self.inertia + ) + self.wrench = screw.Screw( + lin=Matrix([self.fx_ext, self.fy_ext, self.fz_ext]), + ang=Matrix([self.mx_ext, self.my_ext, self.mz_ext]) + ) + # setup params to compare against + self.param_xx = var('XX20') + self.param_yy = var('YY20') + self.param_zz = var('ZZ20') + self.params = { + 'xx': self.param_xx, + 'yy': self.param_yy, + 'zz': self.param_zz + } + self.wrong_param = {'rand': 'some-value'} + + def test_init(self): + """Test constructor""" + # test instance type + self.assertIsInstance(DynParams(self.link), DynParams) + + def test_inertia(self): + """Test get of inertia()""" + # test get + self.assertEqual(self.data.inertia, self.inertia) + + def test_mass_tensor(self): + """Test get of mass_tensor()""" + # test get + self.assertEqual(self.data.mass_tensor, self.mass_tensor) + + def test_spatial_inertia(self): + """Test get of spatial_inertia()""" + # test get + self.assertEqual(self.data.spatial_inertia, self.spatial_inertia) + + def test_wrench(self): + """Test get of wrench()""" + # test get + self.assertEqual(self.data.wrench, self.wrench) + + def test_force(self): + """Test get and set of force()""" + # test get + self.assertEqual(self.data.force, self.wrench.lin) + + def test_moment(self): + """Test get of moment()""" + # test get + self.assertEqual(self.data.moment, self.wrench.ang) + + def test_update_params(self): + """Test update_params()""" + # test raise AttributeError + self.assertRaises( + AttributeError, self.data.update_params, self.wrong_param + ) + # test update values + self.data.update_params(self.params) + self.assertEqual(self.data.xx, self.param_xx) + self.assertEqual(self.data.yy, self.param_yy) + self.assertEqual(self.data.zz, self.param_zz) + + def test_set_to_zero(self): + """Test set_to_zero()""" + link = 3 + data = DynParams(link) + data.set_to_zero() + # check link value remains unchanged + self.assertEqual(data.link, link) + # check other values are zero (just a few) + self.assertEqual(data.xx, 0) + self.assertEqual(data.yy, 0) + self.assertEqual(data.zz, 0) + self.assertEqual(data.msx, 0) + self.assertEqual(data.mass, 0) + self.assertEqual(data.ia, 0) + self.assertEqual(data.frv, 0) + self.assertEqual(data.fx_ext, 0) + self.assertEqual(data.mz_ext, 0) + + +def run_tests(): + """Load and run the unittests""" + unit_suite = unittest.TestLoader().loadTestsFromTestCase( + TestDynParams + ) + unittest.TextTestRunner(verbosity=2).run(unit_suite) + + +def main(): + """Main function.""" + run_tests() + + +if __name__ == '__main__': + main() + + diff --git a/pysymoro/tests/test_geoparams.py b/pysymoro/tests/test_geoparams.py new file mode 100644 index 0000000..ff1b2a0 --- /dev/null +++ b/pysymoro/tests/test_geoparams.py @@ -0,0 +1,86 @@ +#!/usr/bin/env python +# -*- coding: utf-8 -*- + + +"""Unit test module for GeoParams class.""" + + +import unittest + +from sympy import var + +from pysymoro.geoparams import GeoParams + + +class TestGeoParams(unittest.TestCase): + """Unit test for GeoParams class.""" + def setUp(self): + # setup an instance of GeoParams + self.data_revol = GeoParams(22, {'sigma': 0}) + self.data_prism = GeoParams(20, {'sigma': 1}) + self.data_fixed = GeoParams(10, {'sigma': 2}) + # setup data to compare against + self.frame = 33 + # setup values to compare against + self.revol_theta = var('pi') + self.prism_r = 5 + self.data_revol.theta = self.revol_theta + self.data_prism.r = self.prism_r + # setup params to compare against + self.param_sigma = 0 + self.param_gamma = var('XX20') + self.param_alpha = var('YY20') + self.param_theta = var('ZZ20') + self.params = { + 'sigma': self.param_sigma, + 'gamma': self.param_gamma, + 'alpha': self.param_alpha, + 'theta': self.param_theta + } + self.wrong_param = {'rand': 'some-value'} + + def test_init(self): + """Test constructor""" + # test instance type + self.assertIsInstance(GeoParams(self.frame), GeoParams) + + def test_update_params(self): + """Test update_params()""" + # NOTE: the state of TransformationMatrix instance is not tested + # here. + # test raise AttributeError + self.assertRaises( + AttributeError, self.data_fixed.update_params, + self.wrong_param + ) + # test update values + self.data_prism.update_params(self.params) + self.assertEqual(self.data_prism.sigma, self.param_sigma) + self.assertEqual(self.data_prism.gamma, self.param_gamma) + self.assertEqual(self.data_prism.alpha, self.param_alpha) + self.assertEqual(self.data_prism.theta, self.param_theta) + + def test_q(self): + """Test get of q()""" + self.assertEqual(self.data_revol.q, self.revol_theta) + self.assertEqual(self.data_prism.q, self.prism_r) + self.assertEqual(self.data_fixed.q, 0) + + +def run_tests(): + """Load and run the unittests""" + unit_suite = unittest.TestLoader().loadTestsFromTestCase( + TestGeoParams + ) + unittest.TextTestRunner(verbosity=2).run(unit_suite) + + +def main(): + """Main function.""" + run_tests() + + +if __name__ == '__main__': + main() + + diff --git a/pysymoro/tests/test_invgeom.py b/pysymoro/tests/test_invgeom.py new file mode 100644 index 0000000..471e826 --- /dev/null +++ b/pysymoro/tests/test_invgeom.py @@ -0,0 +1,99 @@ +#!/usr/bin/env python +# -*- coding: utf-8 -*- + + +# This file is part of the OpenSYMORO project. Please see +# https://github.com/symoro/symoro/blob/master/LICENCE for the licence. + + +"""Unit test module for GeoParams class.""" + + +import unittest + +from sympy import var, Matrix +from numpy import random, amax, matrix, eye, zeros + +from pysymoro import invgeom +from symoroutils import samplerobots +from symoroutils import symbolmgr +from pysymoro import geometry + + +class TestIGM(unittest.TestCase): + """Unit test for invgeom module.""" + def setUp(self): + self.symo = symbolmgr.SymbolManager() + + def test_igm_2r(self): + robo = samplerobots.planar2r() + nTm = Matrix(4, 4, 12 * [invgeom.EMPTY] + [0, 0, 0, 1]) + nTm[0, 3], nTm[1, 3] = var('p1, p2') + invgeom._paul_solve(robo, self.symo, nTm, 0, robo.nf) + self.symo.gen_func_string('IGM_gen', robo.q_vec, + var('p1, p2'), syntax='matlab') + igm_f = self.symo.gen_func('IGM_gen', robo.q_vec, + var('p1, p2')) + T = geometry.dgm(robo, self.symo, 0, robo.nf, + fast_form=True, trig_subs=True) + f06 = self.symo.gen_func('DGM_generated1', (T[0, 3], T[1, 3]), + robo.q_vec) + for x in xrange(100): + arg = random.normal(size=robo.nj) + Ttest = f06(arg) + solution = igm_f(Ttest) + for q in solution: + self.assertLess(amax(matrix(f06(q))-Ttest), 1e-12) + + def test_igm_rx90(self): + robo = samplerobots.rx90() + #robo.r[6] = var('R6') + #robo.gamma[6] = var('G6') # invgeom.T_GENERAL + nTm = invgeom.T_GENERAL + invgeom._paul_solve(robo, self.symo, nTm, 0, robo.nf) + self.symo.gen_func_string('IGM_gen', robo.q_vec, + invgeom.T_GENERAL, syntax='matlab') + igm_f = self.symo.gen_func('IGM_gen', robo.q_vec, + invgeom.T_GENERAL) + T = geometry.dgm(robo, self.symo, 0, robo.nf, + fast_form=True, trig_subs=True) + f06 = self.symo.gen_func('DGM_generated1', T, robo.q_vec) + for x in xrange(100): + arg = random.normal(size=robo.nj) + Ttest = f06(arg) + solution = igm_f(Ttest) + for q in solution: + self.assertLess(amax(matrix(f06(q))-Ttest), 1e-12) + + def test_loop(self): + self.robo = samplerobots.sr400() + invgeom.loop_solve(self.robo, self.symo) + self.symo.gen_func_string('IGM_gen', self.robo.q_vec, + self.robo.q_active, syntax='matlab') + l_solver = self.symo.gen_func('IGM_gen', self.robo.q_vec, + self.robo.q_active) + T = geometry.dgm(self.robo, self.symo, 9, 10, + fast_form=True, trig_subs=True) + t_loop = self.symo.gen_func('DGM_generated1', T, self.robo.q_vec) + for x in xrange(10): + arg = random.normal(size=6) + solution = l_solver(arg) + for q in solution: + self.assertLess(amax(matrix(t_loop(q))-eye(4)), 1e-12) + + +def run_tests(): + """Load and run the unittests""" + unit_suite = unittest.TestLoader().loadTestsFromTestCase(TestIGM) + unittest.TextTestRunner(verbosity=2).run(unit_suite) + + +def main(): + """Main function.""" + run_tests() + + +if __name__ == '__main__': + main() + + diff --git a/pysymoro/tests/test_screw.py b/pysymoro/tests/test_screw.py new file mode 100644 index 0000000..73039c1 --- /dev/null +++ b/pysymoro/tests/test_screw.py @@ -0,0 +1,98 @@ +#!/usr/bin/env python +# -*- coding: utf-8 -*- + + +"""Unit test module for Screw class.""" + + +import unittest + +from sympy import Matrix +from sympy import ShapeError +from sympy import zeros + +from pysymoro.screw import Screw + + +class TestScrew(unittest.TestCase): + """Unit test for Screw class.""" + def setUp(self): + # screw with 0s + self.empty = Screw() + # screw created by separate linear and angular terms + self.indiv = Screw(lin=Matrix([1, 2, 3]), ang=Matrix([4, 5, 6])) + # screw created by 6x1 matrix + #self.full = Screw(Matrix([6, 5, 4, 3, 2, 1])) + + def test_init(self): + """Test constructors.""" + # test instance type + self.assertIsInstance(self.empty, Screw) + self.assertIsInstance(self.indiv, Screw) + # test raise appropriate exception + self.assertRaises( + ShapeError, Screw, Matrix([1, 2]), Matrix([7, 8]) + ) + self.assertRaises( + ShapeError, Screw, Matrix([1, 2, 3, 4, 5, 6, 7, 8]) + ) + + def test_val(self): + """Test get and set of val()""" + # test get + self.assertEqual(self.empty.val, zeros(6, 1)) + self.assertEqual(self.indiv.val, Matrix([1, 2, 3, 4, 5, 6])) + # test set + self.indiv.val = Matrix([6, 5, 4, 3, 2, 1]) + self.assertEqual(self.indiv.val, Matrix([6, 5, 4, 3, 2, 1])) + with self.assertRaises(ShapeError): + self.empty.val = Matrix([3, 3]) + + def test_lin(self): + """Test get and set of lin()""" + # test get + self.assertEqual(self.empty.lin, zeros(3, 1)) + # test set + self.indiv.lin = Matrix([1, 2, 3]) + self.assertEqual(self.indiv.lin, Matrix([1, 2, 3])) + with self.assertRaises(ShapeError): + self.empty.lin = Matrix([3, 3]) + + def test_ang(self): + """Test get and set of ang()""" + # test get + self.assertEqual(self.empty.ang, zeros(3, 1)) + # test set + self.indiv.ang = Matrix([4, 5, 6]) + self.assertEqual(self.indiv.ang, Matrix([4, 5, 6])) + with self.assertRaises(ShapeError): + self.empty.ang = Matrix([3, 3]) + + def test_equality(self): + """Test __eq__() and __ne__()""" + self.assertEqual(self.empty, Screw()) + self.assertNotEqual(self.indiv, Screw()) + self.assertRaises( + ValueError, self.empty.__eq__, + Matrix([1, 2, 3, 4, 5, 6, 7, 8]) + ) + self.assertRaises( + ValueError, self.empty.__ne__, + Matrix([1, 2, 3, 4, 5, 6, 7, 8]) + ) + +def run_tests(): + """Load and run the unittests""" + unit_suite = unittest.TestLoader().loadTestsFromTestCase(TestScrew) + unittest.TextTestRunner(verbosity=2).run(unit_suite) + + +def main(): + """Main function.""" + run_tests() + + +if __name__ == '__main__': + main() + + diff --git a/pysymoro/tests/test_screw6.py b/pysymoro/tests/test_screw6.py new file mode 100644 index 0000000..088b293 --- /dev/null +++ b/pysymoro/tests/test_screw6.py @@ -0,0 +1,155 @@ +#!/usr/bin/env python +# -*- coding: utf-8 -*- + + +"""Unit test module for Screw6 class.""" + + +import unittest + +from sympy import Matrix +from sympy import ShapeError +from sympy import zeros + +from pysymoro.screw6 import Screw6 + + +class TestScrew6(unittest.TestCase): + """Unit test for Screw6 class.""" + def setUp(self): + # setup data matrices to compare against + self.data = Matrix([ + [1, 2, 3, 4, 5, 6], + [7, 8, 9, 10, 11, 12], + [13, 14, 15, 16, 17, 18], + [19, 20, 21, 22, 23, 24], + [25, 26, 27, 28, 29, 30], + [31, 32, 33, 34, 35, 36] + ]) + self.data_tl = Matrix([ + [1, 2, 3], + [7, 8, 9], + [13, 14, 15] + ]) + self.data_tr = Matrix([ + [4, 5, 6], + [10, 11, 12], + [16, 17, 18] + ]) + self.data_bl = Matrix([ + [19, 20, 21], + [25, 26, 27], + [31, 32, 33] + ]) + self.data_br = Matrix([ + [22, 23, 24], + [28, 29, 30], + [34, 35, 36] + ]) + # setup Screw6 instances + self.empty = Screw6() + self.indiv = Screw6() + self.indiv.val = self.data + + def test_init(self): + """Test constructor.""" + # test instance type + self.assertIsInstance(self.empty, Screw6) + self.assertIsInstance(self.indiv, Screw6) + self.assertIsInstance(Screw6(self.data), Screw6) + self.assertIsInstance(Screw6(value=self.data), Screw6) + self.assertIsInstance( + Screw6( + self.data_tl, self.data_tr, + self.data_bl, self.data_br + ), Screw6 + ) + self.assertIsInstance( + Screw6( + tl=self.data_tl, bl=self.data_bl, + tr=self.data_tr, br=self.data_br + ), Screw6 + ) + # test raise appropriate exception + self.assertRaises(NotImplementedError, Screw6, 3, 4) + self.assertRaises(NotImplementedError, Screw6, tl=5, tr=6) + + def test_val(self): + """Test get and set of val()""" + # test get + self.assertEqual(self.empty.val, zeros(6, 6)) + self.assertEqual(self.indiv.val, self.data) + # test set + self.indiv.val = self.data.transpose() + self.assertEqual(self.indiv.val, self.data.transpose()) + with self.assertRaises(ShapeError): + self.empty.val = Matrix([3, 3]) + + def test_topleft(self): + """Test get and set of topleft()""" + # test get + self.assertEqual(self.empty.topleft, zeros(3, 3)) + # test set + self.indiv.topleft = self.data_tl + self.assertEqual(self.indiv.topleft, self.data_tl) + with self.assertRaises(ShapeError): + self.empty.topleft = Matrix([3, 3]) + + def test_topright(self): + """Test get and set of topright()""" + # test get + self.assertEqual(self.empty.topright, zeros(3, 3)) + # test set + self.indiv.topright = self.data_tr + self.assertEqual(self.indiv.topright, self.data_tr) + with self.assertRaises(ShapeError): + self.empty.topright = Matrix([3, 3]) + + def test_botleft(self): + """Test get and set of botleft()""" + # test get + self.assertEqual(self.empty.botleft, zeros(3, 3)) + # test set + self.indiv.botleft = self.data_bl + self.assertEqual(self.indiv.botleft, self.data_bl) + with self.assertRaises(ShapeError): + self.empty.botleft = Matrix([3, 3]) + + def test_botright(self): + """Test get and set of botright()""" + # test get + self.assertEqual(self.empty.botright, zeros(3, 3)) + # test set + self.indiv.botright = self.data_br + self.assertEqual(self.indiv.botright, self.data_br) + with self.assertRaises(ShapeError): + self.empty.botright = Matrix([3, 3]) + + def test_equality(self): + """Test __eq__() and __ne__()""" + self.assertEqual(self.empty, Screw6()) + self.assertNotEqual(self.indiv, Screw6()) + self.assertRaises( + ValueError, self.empty.__eq__, + Matrix([1, 2, 3, 4, 5, 6, 7, 8]) + ) + self.assertRaises( + ValueError, self.empty.__ne__, + Matrix([1, 2, 3, 4, 5, 6, 7, 8]) + ) + +def run_tests(): + """Load and run the unittests""" + unit_suite = unittest.TestLoader().loadTestsFromTestCase(TestScrew6) + unittest.TextTestRunner(verbosity=2).run(unit_suite) + + +def main(): + """Main function.""" + run_tests() + + +if __name__ == '__main__': + main() + + diff --git a/pysymoro/tests/test_transform.py b/pysymoro/tests/test_transform.py new file mode 100644 index 0000000..4e22004 --- /dev/null +++ b/pysymoro/tests/test_transform.py @@ -0,0 +1,101 @@ +#!/usr/bin/env python +# -*- coding: utf-8 -*- + + +"""Unit test module for the functions in transform module.""" + + +import unittest + +from sympy import pi, var +from sympy import cos, sin +from sympy import Matrix + +from pysymoro import transform + + +class TestTransform(unittest.TestCase): + """Unit test for functions in transform module.""" + def setUp(self): + # setup params for symbolic computation + th1 = var('th1') + l1 = var('L1') + self.sym_gamma = 0 + self.sym_b = 0 + self.sym_alpha = 0 + self.sym_d = l1 + self.sym_theta = th1 + self.sym_r = 0 + self.sym_tmat = Matrix([ + [cos(th1), -sin(th1), 0, l1], + [sin(th1), cos(th1), 0, 0], + [0, 0, 1, 0], + [0, 0, 0, 1] + ]) + # setup params for numeric computation + th1 = pi/2 + l1 = 1 + self.num_gamma = 0 + self.num_b = 0 + self.num_alpha = 0 + self.num_d = l1 + self.num_theta = th1 + self.num_r = 0 + self.num_tmat = Matrix([ + [cos(th1), -sin(th1), 0, l1], + [sin(th1), cos(th1), 0, 0], + [0, 0, 1, 0], + [0, 0, 0, 1] + ]) + + def test_get_transformation_matrix_sym(self): + """Test get_transformation_matrix() symbolically""" + self.assertEqual( + transform.get_transformation_matrix( + self.sym_gamma, self.sym_b, + self.sym_alpha, self.sym_d, + self.sym_theta, self.sym_r + ), self.sym_tmat + ) + + def test_get_transformation_matrix_num(self): + """Test get_transformation_matrix() numerically""" + self.assertEqual( + transform.get_transformation_matrix( + self.num_gamma, self.num_b, + self.num_alpha, self.num_d, + self.num_theta, self.num_r + ), self.num_tmat + ) + + def test_get_transformation_matrix_sub(self): + """Test get_transformation_matrix() by substitution""" + t_in_sym = transform.get_transformation_matrix( + self.sym_gamma, self.sym_b, + self.sym_alpha, self.sym_d, + self.sym_theta, self.sym_r + ) + t_in_num = t_in_sym.subs({ + self.sym_d: self.num_d, + self.sym_theta: self.num_theta + }) + self.assertEqual(t_in_num, self.num_tmat) + + +def run_tests(): + """Load and run the unittests""" + unit_suite = unittest.TestLoader().loadTestsFromTestCase( + TestTransform + ) + unittest.TextTestRunner(verbosity=2).run(unit_suite) + + +def main(): + """Main function.""" + run_tests() + + +if __name__ == '__main__': + main() + + diff --git a/pysymoro/tests/test_transformationmatrix.py b/pysymoro/tests/test_transformationmatrix.py new file mode 100644 index 0000000..d29df97 --- /dev/null +++ b/pysymoro/tests/test_transformationmatrix.py @@ -0,0 +1,178 @@ +#!/usr/bin/env python +# -*- coding: utf-8 -*- + + +"""Unit test module for TransformationMatrix class.""" + + +import unittest + +from sympy import eye, var, zeros +from sympy import cos, sin +from sympy import Matrix + +from pysymoro.screw6 import Screw6 +from symoroutils import tools + +from pysymoro.transform import TransformationMatrix + + +class TestTransformationMatrix(unittest.TestCase): + """Unit test for TransformationMatrix class.""" + def setUp(self): + # parameter values + th1 = var('th1') + l1 = var('L1') + self.gamma = 0 + self.b = 0 + self.alpha = 0 + self.d = l1 + self.theta = th1 + self.r = 0 + self.t_val = Matrix([ + [cos(th1), -sin(th1), 0, l1], + [sin(th1), cos(th1), 0, 0], + [0, 0, 1, 0], + [0, 0, 0, 1] + ]) + self.rot_val = Matrix([ + [cos(th1), -sin(th1), 0], + [sin(th1), cos(th1), 0], + [0, 0, 1] + ]) + self.trans_val = Matrix([l1, 0, 0]) + self.t_inv = Matrix([ + [cos(th1), sin(th1), 0, -l1*cos(th1)], + [-sin(th1), cos(th1), 0, l1*sin(th1)], + [0, 0, 1, 0], + [0, 0, 0, 1] + ]) + self.inv_rot = Matrix([ + [cos(th1), sin(th1), 0], + [-sin(th1), cos(th1), 0], + [0, 0, 1] + ]) + self.inv_trans = Matrix([-l1*cos(th1), l1*sin(th1), 0]) + self.sji = Screw6( + tl=self.rot_val, tr=tools.skew(self.trans_val), + bl=zeros(3, 3), br=self.rot_val + ).val + self.sij = Screw6( + tl=self.inv_rot, + tr=-(self.inv_rot * tools.skew(self.trans_val)), + bl=zeros(3, 3), br=self.inv_rot + ).val + # params dict + self.params = { + 'gamma': self.gamma, + 'b': self.b, + 'alpha': self.alpha, + 'd': self.d, + 'theta': self.theta, + 'r': self.r + } + self.sigmu_param = {'sigma': 2, 'mu': 0} + self.wrong_param = {'rand': 'some-value'} + # frames + self.frame_i = 5 + self.frame_j = 6 + # setup instances for use + self.t_empty = TransformationMatrix(i=0, j=0) + self.t_data = TransformationMatrix( + i=self.frame_i, j=self.frame_j, params=self.params + ) + + def test_init(self): + """Test contructor""" + params = self.params + # test raise NotImplementedError + self.assertRaises(NotImplementedError, TransformationMatrix) + # test raise AttributeError + self.assertRaises( + AttributeError, TransformationMatrix, params=params + ) + # test different forms of the constructor + self.assertIsInstance( + TransformationMatrix(i=0, j=0), TransformationMatrix + ) + self.assertIsInstance( + TransformationMatrix(i=0, j=0, params=params), + TransformationMatrix + ) + params['frame'] = 1 + params['ant'] = 0 + self.assertIsInstance( + TransformationMatrix(params=params), TransformationMatrix + ) + + def test_val(self): + """Test get of val()""" + self.assertEqual(self.t_empty.val, eye(4)) + self.assertEqual(self.t_data.val, self.t_val) + + def test_rot(self): + """Test get of rot()""" + self.assertEqual(self.t_empty.rot, eye(3)) + self.assertEqual(self.t_data.rot, self.rot_val) + + def test_trans(self): + """Test get of trans()""" + self.assertEqual(self.t_empty.trans, zeros(3, 1)) + self.assertEqual(self.t_data.trans, self.trans_val) + + def test_inv(self): + """Test get of inv()""" + self.assertEqual(self.t_empty.inv, eye(4)) + self.assertEqual(self.t_data.inv, self.t_inv) + + def test_inv_rot(self): + """Test get of inv_rot()""" + self.assertEqual(self.t_empty.inv_rot, eye(3)) + self.assertEqual(self.t_data.inv_rot, self.inv_rot) + + def test_inv_trans(self): + """Test get of inv_trans()""" + self.assertEqual(self.t_empty.inv_trans, zeros(3, 1)) + self.assertEqual(self.t_data.inv_trans, self.inv_trans) + + def test_s_j_wrt_i(self): + """Test get of s_j_wrt_i()""" + self.assertEqual(self.t_empty.s_j_wrt_i, eye(6)) + self.assertEqual(self.t_data.s_j_wrt_i, self.sji) + + def test_s_i_wrt_j(self): + """Test get of s_i_wrt_j()""" + self.assertEqual(self.t_empty.s_i_wrt_j, eye(6)) + self.assertEqual(self.t_data.s_i_wrt_j, self.sij) + + def test_update(self): + """Test update()""" + # test raise AttributeError + self.assertRaises( + AttributeError, self.t_empty.update, self.wrong_param + ) + # test sigma, mu + self.t_empty.update(self.sigmu_param) + self.assertEqual(self.t_empty.val, eye(4)) + # test with correct params + self.t_empty.update(self.params) + self.assertEqual(self.t_empty.val, self.t_val) + + +def run_tests(): + """Load and run the unittests""" + unit_suite = unittest.TestLoader().loadTestsFromTestCase( + TestTransformationMatrix + ) + unittest.TextTestRunner(verbosity=2).run(unit_suite) + + +def main(): + """Main function.""" + run_tests() + + +if __name__ == '__main__': + main() + + diff --git a/pysymoro/transform.py b/pysymoro/transform.py new file mode 100644 index 0000000..84ae937 --- /dev/null +++ b/pysymoro/transform.py @@ -0,0 +1,332 @@ +# -*- coding: utf-8 -*- + + +""" +This module contains the TransformationMatrix data structure. +""" + + +import sympy + +from pysymoro.screw6 import Screw6 +from symoroutils import tools + + +def get_transformation_matrix(gamma, b, alpha, d, theta, r): + """ + Compute and get transformation matrix between any two frames. + + Args: + gamma, b, alpha, d, theta, r: The geometric parameter values + that give the relationship between any two frames that are + defined using the modified DH-method. + Returns: + A 4x4 Matrix that is the homogenous transformation matrix + between the frames. + """ + # simplify computation + c_gamma = sympy.cos(gamma) + s_gamma = sympy.sin(gamma) + c_alpha = sympy.cos(alpha) + s_alpha = sympy.sin(alpha) + c_theta = sympy.cos(theta) + s_theta = sympy.sin(theta) + # intermediate terms + sg_ca = s_gamma * c_alpha + sg_sa = s_gamma * s_alpha + cg_ca = c_gamma * c_alpha + cg_sa = c_gamma * s_alpha + # t matrix elements + t11 = (c_gamma * c_theta) - (sg_ca * s_theta) + t12 = -(c_gamma * s_theta) - (sg_ca * c_theta) + t13 = sg_sa + t14 = (d * c_gamma) + (r * sg_sa) + t21 = (s_gamma * c_theta) + (cg_ca * s_theta) + t22 = -(s_gamma * s_theta) + (cg_ca * c_theta) + t23 = -cg_sa + t24 = (d * s_gamma) - (r * cg_sa) + t31 = s_alpha * s_theta + t32 = s_alpha * c_theta + t33 = c_alpha + t34 = (r * c_alpha) + b + # t matrix + tmat = sympy.Matrix([ + [t11, t12, t13, t14], + [t21, t22, t23, t24], + [t31, t32, t33, t34], + [0, 0, 0, 1] + ]) + return tmat + + +class TransformationMatrix(object): + """ + Data structure: + Represent the data structure to hold a transformation matrix + between any two frames. + """ + def __init__(self, **kwargs): + """ + Constructor period. + + Usage: + >>> # Positional arguments are not allowed. + >>> # Only frames are specified. i, j are of type int. + TransformationMatrix(i=, j=) + >>> # Frames and parameter values are specified. i, j are + >>> # of type int. + TransformationMatrix( + i=, j=, params= + ) + >>> # Only parameter values are specified. In this case the + >>> # must contain the `frame` and `ant` keys + >>> # with relevant values. + TransformationMatrix(params=) + """ + if len(kwargs) >= 1 and len(kwargs) <= 3: + self._init_default() + self._compute_tmat() + self._compute_tinv() + self._compute_smat() + self._compute_sinv() + if len(kwargs) > 1: + self._frame_i = int(kwargs['i']) + self._frame_j = int(kwargs['j']) + if kwargs.has_key('params'): + self.update(kwargs['params']) + else: + if self._has_frames(kwargs['params']): + self.update(kwargs['params']) + else: + self._cleanup() + raise AttributeError( + """Cannot setup TransformationMatrix without + specifying the two frames - frame, ant. Input + was: %s""" % str(kwargs['params']) + ) + else: + raise NotImplementedError( + """Wrong use of TransformationMatrix constructor. See + Usage.""" + ) + + def __str__(self): + str_format = ( + "T matrix of frame %d wrt frame %d:\n" + "----------------------------------\n" + "gamma=%s, b=%s, alpha=%s, d=%s, theta=%s, r=%s\n" + "%s\n" + "**********************************\n" + ) % ( + self._frame_j, self._frame_i, + str(self._gamma), str(self._b), + str(self._alpha), str(self._d), + str(self._theta), str(self._r), + sympy.pretty(self._tmat) + ) + return str_format + + def __repr__(self): + return "T of %d wrt %d" % (self._frame_j, self._frame_i) + + @property + def val(self): + """ + Get the value of the transformation matrix. + + Returns: + A 4x4 Matrix. + """ + if not hasattr(self, '_tmat'): + raise AttributeError("tmat is yet to be computed") + return self._tmat + + @property + def rot(self): + """ + Get the value of the rotation matrix. + + Returns: + A 3x3 Matrix. + """ + if not hasattr(self, '_tmat'): + raise AttributeError("tmat is yet to be computed") + return self._tmat[0:3, 0:3] + + @property + def trans(self): + """ + Get the value of the translation vector. + + Returns: + A 3x1 Matrix. + """ + if not hasattr(self, '_tmat'): + raise AttributeError("tmat is yet to be computed") + return self._tmat[0:3, 3:4] + + @property + def inv(self): + """ + Get the inverse of the transformation matrix. + + Returns: + A 4x4 Matrix. + """ + if not hasattr(self, '_tinv'): + raise AttributeError("tinv is yet to be computed") + return self._tinv + + @property + def inv_rot(self): + """ + Get the inverse of the rotation matrix. + + Returns: + A 3x3 Matrix. + """ + if not hasattr(self, '_tinv'): + raise AttributeError("tinv is yet to be computed") + return self._tinv[0:3, 0:3] + + @property + def inv_trans(self): + """ + Get the inverse of the translation vector. + + Returns: + A 3x1 Matrix. + """ + if not hasattr(self, '_tinv'): + raise AttributeError("tinv is yet to be computed") + return self._tinv[0:3, 3:4] + + @property + def s_j_wrt_i(self): + """ + Get the screw form of the transformation matrix. + + Returns: + A 6x6 Matrix. + """ + return self._smat.val + + @property + def s_i_wrt_j(self): + """ + Get the screw form of the inverse transformation matrix. + + Returns: + A 6x6 Matrix. + """ + return self._sinv.val + + def update(self, params): + """ + Update the parameter values and all the matrices accordingly. + + Args: + params: A dict in which the keys correspond to the list of + parameters that are to be updated and the values + correspond to the values with which the parameters are + to be updated. + """ + for key, value in params.iteritems(): + attr = '_' + key + if hasattr(self, attr): + setattr(self, attr, value) + elif key in ['sigma', 'mu']: + continue + elif key is 'frame': + self._frame_j = int(value) + elif key is 'ant': + self._frame_i = int(value) + else: + raise AttributeError( + "%s is not a geometric parameter" % key + ) + self._compute_tmat() + self._compute_tinv() + self._compute_smat() + self._compute_sinv() + + def _compute_tmat(self): + """ + Call (proxy method) the actual function that computes the + transformation matrix between any two frames. + """ + self._tmat = get_transformation_matrix( + self._gamma, self._b, + self._alpha, self._d, + self._theta, self._r + ) + + def _compute_tinv(self): + """ + Compute inverse of the transformation matrix. + """ + if not hasattr(self, '_tmat'): + raise AttributeError("tmat is yet to be computed") + self._tinv = sympy.eye(4) + rot_inv = self.rot.transpose() + trans_inv = -rot_inv * self.trans + self._tinv[0:3, 0:3] = rot_inv + self._tinv[0:3, 3:4] = trans_inv + + def _compute_smat(self): + """ + Compute the transformation matrix in Screw (6x6) matrix form. + """ + self._smat = Screw6( + tl=self.rot, tr=tools.skew(self.trans), + bl=sympy.zeros(3, 3), br=self.rot + ) + + def _compute_sinv(self): + """ + Compute the inverse transformation matrix in Screw + (6x6) matrix form. + """ + self._sinv = Screw6( + tl=self.inv_rot, tr=-(self.inv_rot * tools.skew(self.trans)), + bl=sympy.zeros(3, 3), br=self.inv_rot + ) + + def _init_default(self): + """Initialise to 0 by default.""" + self._gamma = 0 + self._b = 0 + self._alpha = 0 + self._d = 0 + self._theta = 0 + self._r = 0 + + def _has_frames(self, params): + """ + Check if the frame and its antecedent are specified in a + given dict. + + Args: + params: A dict containing the geometric parameters. + + Returns: + True if `frame` and `ant` keys are present in params. False + otherwise. + """ + if params.has_key('frame') and params.has_key('ant'): + return True + else: + return False + + def _cleanup(self): + """Remove attributes of the data structure.""" + del self._tinv + del self._tmat + del self._gamma + del self._b + del self._alpha + del self._d + del self._theta + del self._r + + diff --git a/pysymoro/visualize/objects.py b/pysymoro/visualize/objects.py deleted file mode 100644 index ad3c815..0000000 --- a/pysymoro/visualize/objects.py +++ /dev/null @@ -1,232 +0,0 @@ -__author__ = 'Izzat' -import OpenGL.GL as gL -from numpy import degrees, identity, array -from primitives import Primitives - - -class Frame(object): - def __init__(self, index=0, T=identity(4), show_frame=True): - self.children = [] - self.T = T - self.show_frame = show_frame - self.index = index - - def draw_frame(self): - if self.show_frame: - gL.glPushMatrix() - gL.glColor3f(1, 0, 0) - self.draw_arrow() - gL.glRotatef(90, 0, 1, 0) - gL.glColor3f(0, 1, 0) - self.draw_arrow() - gL.glPopMatrix() - - def draw_arrow(self): - gL.glVertexPointer(3, gL.GL_FLOAT, 0, self.arr_vertices) - gL.glNormalPointer(gL.GL_FLOAT, 0, self.arr_normals) - gL.glDrawElements(gL.GL_TRIANGLES, len(self.arr_indices), - gL.GL_UNSIGNED_INT, self.arr_indices) - - def set_show_frame(self, show=True): - self.show_frame = show - - def add_child(self, child): - self.children.append(child) - - def draw_frames(self): - gL.glPushMatrix() - gL.glMultMatrixf(self.T) - self.draw_frame() - for child in self.children: - child.draw_frames() - gL.glPopMatrix() - - def draw(self): - gL.glPushMatrix() - gL.glMultMatrixf(self.T) - for child in self.children: - child.draw() - gL.glPopMatrix() - - def __str__(self): - return '{0} Children: {1}'.format(self.index, self.children) - - def set_length(self, new_length): - self.arr_vertices, self.arr_indices, self.arr_normals = \ - Primitives.arr_array(new_length) - - -class JointObject(Frame): - - def __init__(self, index, theta=0., r=0., alpha=0., d=0., gamma=0., b=0.): - super(JointObject, self).__init__(index) - self.theta = theta - self.r = r - self.alpha = alpha - self.d = d - self.gamma = gamma - self.b = b - self.shift = 0. - self.init_length = 0. - - def draw_rod(self, length): - gL.glPushMatrix() - gL.glMultMatrixf(array([[1., 0., 0., 0.], [0., 1., 0., 0.], - [0., 0., length, 0.], [0., 0., 0., 1.]])) - gL.glColor3f(0.8, 0.51, 0.25) - gL.glVertexPointer(3, gL.GL_FLOAT, 0, self.rod_vertices) - gL.glNormalPointer(gL.GL_FLOAT, 0, self.rod_normals) - gL.glDrawElements(gL.GL_TRIANGLES, len(self.rod_indices), - gL.GL_UNSIGNED_INT, self.rod_indices) - gL.glPopMatrix() - - def draw_frames(self): - gL.glPushMatrix() - gL.glRotatef(degrees(self.gamma), 0, 0, 1) - gL.glTranslatef(0, 0, self.b) - gL.glRotatef(degrees(self.alpha), 1, 0, 0) - gL.glTranslatef(self.d, 0, 0) - gL.glRotatef(degrees(self.theta), 0, 0, 1) - gL.glTranslatef(0, 0, self.r) - self.draw_frame() - for child in self.children: - child.draw_frames() - gL.glPopMatrix() - - def draw(self): - gL.glPushMatrix() - if self.b: - self.draw_rod(self.b) - gL.glTranslatef(0, 0, self.b) - gL.glRotatef(degrees(self.gamma), 0, 0, 1) - if self.d: - gL.glPushMatrix() - gL.glRotatef(90, 0, 1, 0) - self.draw_rod(self.d) - gL.glPopMatrix() - gL.glTranslatef(self.d, 0, 0) - gL.glRotatef(degrees(self.alpha), 1, 0, 0) - if self.r: - self.draw_rod(self.r) - gL.glTranslatef(0, 0, self.r) - gL.glRotatef(degrees(self.theta), 0, 0, 1) - if self.shift: - gL.glPushMatrix() - shift = self.shift*self.length - self.draw_rod(shift) - gL.glTranslatef(0, 0, shift) - self.draw_joint() - gL.glPopMatrix() - else: - self.draw_joint() - for child in self.children: - child.draw() - gL.glPopMatrix() - - def set_length(self, new_length): - if not self.init_length: - self.rod_vertices, self.rod_indices, self.rod_normals = \ - Primitives.rod_array(new_length) - self.init_length = new_length - self.length = new_length - super(JointObject, self).set_length(new_length) - - -class RevoluteJoint(JointObject): - - def __init__(self, *args): - super(RevoluteJoint, self).__init__(*args) - self.q_init = self.theta - - def draw_joint(self): - gL.glColor3f(1., 1., 0.) - gL.glVertexPointer(3, gL.GL_FLOAT, 0, self.cyl_vertices) - gL.glNormalPointer(gL.GL_FLOAT, 0, self.cyl_normals) - gL.glDrawElements(gL.GL_TRIANGLES, len(self.cyl_indices), - gL.GL_UNSIGNED_INT, self.cyl_indices) - - @property - def q(self): - return self.theta - - @q.setter - def q(self, theta): - self.theta = theta - - def set_length(self, new_length): - self.cyl_vertices, self.cyl_indices, self.cyl_normals = \ - Primitives.cyl_array(new_length) - super(RevoluteJoint, self).set_length(new_length) - - -class PrismaticJoint(JointObject): - - def __init__(self, *args): - super(PrismaticJoint, self).__init__(*args) - self.q_init = self.r - - def draw_joint(self): - gL.glColor3f(1., 0.6, 0.) - gL.glVertexPointer(3, gL.GL_FLOAT, 0, self.box_vertices) - gL.glNormalPointer(gL.GL_FLOAT, 0, self.box_normals) - gL.glDrawArrays(gL.GL_QUADS, 0, 24) - - @property - def q(self): - return self.r - - @q.setter - def q(self, r): - self.r = r - - def set_length(self, new_length): - self.box_vertices, self.box_normals = Primitives.box_array(new_length) - super(PrismaticJoint, self).set_length(new_length) - - def draw(self): - gL.glPushMatrix() - if self.b: - self.draw_rod(self.b) - gL.glTranslatef(0, 0, self.b) - gL.glRotatef(degrees(self.gamma), 0, 0, 1) - if self.d: - gL.glPushMatrix() - gL.glRotatef(90, 0, 1, 0) - self.draw_rod(self.d) - gL.glPopMatrix() - gL.glTranslatef(self.d, 0, 0) - gL.glRotatef(degrees(self.alpha), 1, 0, 0) - if self.shift: - gL.glPushMatrix() - shift = self.shift*self.length - self.draw_rod(shift) - gL.glTranslatef(0, 0, shift) - self.draw_joint() - gL.glPopMatrix() - else: - self.draw_joint() - if self.r: - self.draw_rod(self.r) - gL.glTranslatef(0, 0, self.r) - gL.glRotatef(degrees(self.theta), 0, 0, 1) - for child in self.children: - child.draw() - gL.glPopMatrix() - - -class FixedJoint(JointObject): - - def __init__(self, *args): - super(FixedJoint, self).__init__(*args) - - def draw_joint(self): - gL.glColor3f(1., 0., 1.) - gL.glVertexPointer(3, gL.GL_FLOAT, 0, self.sph_vertices) - gL.glNormalPointer(gL.GL_FLOAT, 0, self.sph_normals) - gL.glDrawElements(gL.GL_TRIANGLES, len(self.sph_indices), - gL.GL_UNSIGNED_INT, self.sph_indices) - - def set_length(self, new_length): - self.sph_vertices, self.sph_indices, self.sph_normals = \ - Primitives.sph_array(new_length) - super(FixedJoint, self).set_length(new_length) \ No newline at end of file diff --git a/robots/AKR3000/AKR3000.par b/robots/AKR3000/AKR3000.par deleted file mode 100644 index 3e296d9..0000000 --- a/robots/AKR3000/AKR3000.par +++ /dev/null @@ -1,57 +0,0 @@ -(* Robotname = 'AKR3000' *) -NL = 10 -NJ = 12 -NF = 14 -Type = 2 -is_mobile = 0 - -(* Geometric parameters *) -ant = {0,1,1,1,3,4,2,7,8,9,5,6,7,2} -sigma = {0,0,0,0,1,1,0,0,0,0,0,0,2,2} -b = {0,0,0,0,0,0,0,0,0,0,0,0,0,0} -d = {0,0,D3,D4,0,0,D7,0,0,0,0,0,D13,D14} -r = {0,0,0,0,r5,r6,0,RL8,0,0,0,0,0,0} -gamma = {0,0,0,0,0,0,0,0,0,0,0,0,gam13,gam14} -alpha = {0,pi/2,pi/2,pi/2,-pi/2,-pi/2,0,-pi/2,pi/2,-pi/2,pi/2,pi/2,0,0} -mu = {1,0,0,0,1,1,0,1,1,1,0,0,0,0} -theta = {t1,t2,t3,t4,0,0,t7,t8,t9,t10,t11,t12,0,0} - -(* Dynamic parameters and external forces *) -XX = {XX1,XX2,XX3,XX4,XX5,XX6,XX7,XX8,XX9,XX10} -XY = {XY1,XY2,XY3,XY4,XY5,XY6,XY7,XY8,XY9,XY10} -XZ = {XZ1,XZ2,XZ3,XZ4,XZ5,XZ6,XZ7,XZ8,XZ9,XZ10} -YY = {YY1,YY2,YY3,YY4,YY5,YY6,YY7,YY8,YY9,YY10} -YZ = {YZ1,YZ2,YZ3,YZ4,YZ5,YZ6,YZ7,YZ8,YZ9,YZ10} -ZZ = {ZZ1,ZZ2,ZZ3,ZZ4,ZZ5,ZZ6,ZZ7,ZZ8,ZZ9,ZZ10} -MX = {MX1,MX2,MX3,MX4,MX5,MX6,MX7,MX8,MX9,MX10} -MY = {MY1,MY2,MY3,MY4,MY5,MY6,MY7,MY8,MY9,MY10} -MZ = {MZ1,MZ2,MZ3,MZ4,MZ5,MZ6,MZ7,MZ8,MZ9,MZ10} -M = {M1,M2,M3,M4,M5,M6,M7,M8,M9,M10} -IA = {IA1,IA2,IA3,IA4,IA5,IA6,IA7,IA8,IA9,IA10} -FV = {FV1,FV2,FV3,FV4,FV5,FV6,FV7,FV8,FV9,FV10} -FS = {FS1,FS2,FS3,FS4,FS5,FS6,FS7,FS8,FS9,FS10} -FX = {0,0,0,0,0,FX6,FX7,FX8,FX9,FX10} -FY = {0,0,0,0,0,FY6,FY7,FY8,FY9,FY10} -FZ = {0,0,0,0,0,FZ6,FZ7,FZ8,FZ9,FZ10} -CX = {0,0,0,0,0,CX6,CX7,CX8,CX9,CX10} -CY = {0,0,0,0,0,CY6,CY7,CY8,CY9,CY10} -CZ = {0,0,0,0,0,CZ6,CZ7,CZ8,CZ9,CZ10} - -(* Joint parameters *) -QP = {QP1,QP2,QP3,QP4,QP5,QP6,QP7,QP8,QP9,QP10,QP11,QP12} -QDP = {QDP1,QDP2,QDP3,QDP4,QDP5,QDP6,QDP7,QDP8,QDP9,QDP10,QDP11,QDP12} -GAM = {GAM1,GAM2,GAM3,GAM4,GAM5,GAM6,GAM7,GAM8,GAM9,GAM10,GAM11,GAM12} - -(* Speed and acceleration of the base *) -W0 = {0,0,0} -WP0 = {0,0,0} -V0 = {0,0,0} -VP0 = {0,0,0} - -(* Acceleration of gravity *) -G = {0,0,G3} - -(* Transformation of 0 frame position fT0 *) -Z = {1,0,0,0,0,1,0,0,0,0,1,0,0,0,0,1} - -(* End of definition *) diff --git a/robots/RX90/RX90.par b/robots/RX90/RX90.par deleted file mode 100644 index aef7411..0000000 --- a/robots/RX90/RX90.par +++ /dev/null @@ -1,57 +0,0 @@ -(* Robotname = 'RX90' *) -NL = 6 -NJ = 6 -NF = 6 -Type = 0 -is_mobile = 0 - -(* Geometric parameters *) -ant = {0,1,2,3,4,5} -sigma = {0,0,0,0,0,0} -b = {0,0,0,0,0,0} -d = {0,0,D3,0,0,0} -r = {0,0,0,RL4,0,0} -gamma = {0,0,0,0,0,0} -alpha = {0,pi/2,0,-pi/2,pi/2,-pi/2} -mu = {1,1,1,1,1,1} -theta = {th1,th2,th3,th4,th5,th6} - -(* Dynamic parameters and external forces *) -XX = {XX1,XX2,XX3,XX4,XX5,XX6} -XY = {XY1,XY2,XY3,XY4,XY5,XY6} -XZ = {XZ1,XZ2,XZ3,XZ4,XZ5,XZ6} -YY = {YY1,YY2,YY3,YY4,YY5,YY6} -YZ = {YZ1,YZ2,YZ3,YZ4,YZ5,YZ6} -ZZ = {ZZ1,ZZ2,ZZ3,ZZ4,ZZ5,ZZ6} -MX = {MX1,MX2,MX3,MX4,MX5,MX6} -MY = {MY1,MY2,MY3,MY4,MY5,MY6} -MZ = {MZ1,MZ2,MZ3,MZ4,MZ5,MZ6} -M = {M1,M2,M3,M4,M5,M6} -IA = {IA1,IA2,IA3,IA4,IA5,IA6} -FV = {FV1,FV2,FV3,FV4,FV5,FV6} -FS = {FS1,FS2,FS3,FS4,FS5,FS6} -FX = {0,0,0,0,0,FX6} -FY = {0,0,0,0,0,FY6} -FZ = {0,0,0,0,0,FZ6} -CX = {0,0,0,0,0,CX6} -CY = {0,0,0,0,0,CY6} -CZ = {0,0,0,0,0,CZ6} - -(* Joint parameters *) -QP = {QP1,QP2,QP3,QP4,QP5,QP6} -QDP = {QDP1,QDP2,QDP3,QDP4,QDP5,QDP6} -GAM = {GAM1,GAM2,GAM3,GAM4,GAM5,GAM6} - -(* Speed and acceleration of the base *) -W0 = {0,0,0} -WP0 = {0,0,0} -V0 = {0,0,0} -VP0 = {0,0,0} - -(* Acceleration of gravity *) -G = {0,0,G3} - -(* Transformation of 0 frame position fT0 *) -Z = {1,0,0,0,0,1,0,0,0,0,1,0,0,0,0,1} - -(* End of definition *) diff --git a/robots/RX90/RX90_ccg.txt b/robots/RX90/RX90_ccg.txt deleted file mode 100644 index 8a87aa1..0000000 --- a/robots/RX90/RX90_ccg.txt +++ /dev/null @@ -1,260 +0,0 @@ -Pseudo forces using Newton - Euler Algorith - -Geometric parameters -j ant sigma mu gamma b alpha d theta r -1 0 0 1 0 0 0 0 th1 0 -2 1 0 1 0 0 pi/2 0 th2 0 -3 2 0 1 0 0 0 D3 th3 0 -4 3 0 1 0 0 -pi/2 0 th4 RL4 -5 4 0 1 0 0 pi/2 0 th5 0 -6 5 0 1 0 0 -pi/2 0 th6 0 - -Dynamic inertia parameters -j XX XY XZ YY YZ ZZ MX MY MZ M IA -1 XX1 XY1 XZ1 YY1 YZ1 ZZ1 MX1 MY1 MZ1 M1 IA1 -2 XX2 XY2 XZ2 YY2 YZ2 ZZ2 MX2 MY2 MZ2 M2 IA2 -3 XX3 XY3 XZ3 YY3 YZ3 ZZ3 MX3 MY3 MZ3 M3 IA3 -4 XX4 XY4 XZ4 YY4 YZ4 ZZ4 MX4 MY4 MZ4 M4 IA4 -5 XX5 XY5 XZ5 YY5 YZ5 ZZ5 MX5 MY5 MZ5 M5 IA5 -6 XX6 XY6 XZ6 YY6 YZ6 ZZ6 MX6 MY6 MZ6 M6 IA6 - -External forces and joint parameters -j FX FY FZ CX CY CZ FS FV QP QDP GAM -1 0 0 0 0 0 0 FS1 FV1 QP1 QDP1 GAM1 -2 0 0 0 0 0 0 FS2 FV2 QP2 QDP2 GAM2 -3 0 0 0 0 0 0 FS3 FV3 QP3 QDP3 GAM3 -4 0 0 0 0 0 0 FS4 FV4 QP4 QDP4 GAM4 -5 0 0 0 0 0 0 FS5 FV5 QP5 QDP5 GAM5 -6 FX6 FY6 FZ6 CX6 CY6 CZ6 FS6 FV6 QP6 QDP6 GAM6 - -Base velicities parameters -axis W0 WP0 V0 VP0 G -X 0 0 0 0 0 -Y 0 0 0 0 0 -Z 0 0 0 0 G3 - -Equations: -C1 = cos(th1) -S1 = sin(th1) -C2 = cos(th2) -S2 = sin(th2) -C3 = cos(th3) -S3 = sin(th3) -C4 = cos(th4) -S4 = sin(th4) -C5 = cos(th5) -S5 = sin(th5) -C6 = cos(th6) -S6 = sin(th6) -DV61 = QP1**2 -WI12 = QP1*S2 -WI22 = C2*QP1 -WP12 = QP2*WI22 -WP22 = -QP2*WI12 -DV12 = WI12**2 -DV22 = WI12*WI22 -DV42 = WI22**2 -DV62 = QP2**2 -U112 = -DV42 - DV62 -U312 = -2*WP22 -U222 = -DV12 - DV62 -U322 = 2*WP12 -U332 = -DV12 - DV42 -WI13 = C3*WI12 + S3*WI22 -WI23 = C3*WI22 - S3*WI12 -W33 = QP2 + QP3 -WP13 = C3*WP12 + QP3*WI23 + S3*WP22 -WP23 = C3*WP22 - QP3*WI13 - S3*WP12 -DV13 = WI13**2 -DV23 = WI13*WI23 -DV33 = W33*WI13 -DV43 = WI23**2 -DV53 = W33*WI23 -DV63 = W33**2 -U113 = -DV43 - DV63 -U313 = DV33 - WP23 -U223 = -DV13 - DV63 -U323 = DV53 + WP13 -U133 = DV33 + WP23 -U233 = DV53 - WP13 -U333 = -DV13 - DV43 -VSP13 = D3*U112 -VSP23 = D3*DV22 -VSP33 = D3*U312 -VP13 = C3*VSP13 + S3*VSP23 -VP23 = C3*VSP23 - S3*VSP13 -WI14 = C4*WI13 - S4*W33 -WI24 = -C4*W33 - S4*WI13 -W34 = QP4 + WI23 -WP14 = C4*WP13 + QP4*WI24 -WP24 = -QP4*WI14 - S4*WP13 -DV14 = WI14**2 -DV24 = WI14*WI24 -DV34 = W34*WI14 -DV44 = WI24**2 -DV54 = W34*WI24 -DV64 = W34**2 -U114 = -DV44 - DV64 -U214 = DV24 + WP23 -U314 = DV34 - WP24 -U124 = DV24 - WP23 -U224 = -DV14 - DV64 -U324 = DV54 + WP14 -U134 = DV34 + WP24 -U234 = DV54 - WP14 -U334 = -DV14 - DV44 -VSP14 = DV23*RL4 + VP13 -VSP24 = RL4*U223 + VP23 -VSP34 = RL4*U323 + VSP33 -VP14 = C4*VSP14 - S4*VSP34 -VP24 = -C4*VSP34 - S4*VSP14 -WI15 = C5*WI14 + S5*W34 -WI25 = C5*W34 - S5*WI14 -W35 = QP5 - WI24 -WP15 = C5*WP14 + QP5*WI25 + S5*WP23 -WP25 = C5*WP23 - QP5*WI15 - S5*WP14 -DV15 = WI15**2 -DV25 = WI15*WI25 -DV35 = W35*WI15 -DV45 = WI25**2 -DV55 = W35*WI25 -DV65 = W35**2 -U115 = -DV45 - DV65 -U215 = DV25 - WP24 -U315 = DV35 - WP25 -U125 = DV25 + WP24 -U225 = -DV15 - DV65 -U325 = DV55 + WP15 -U135 = DV35 + WP25 -U235 = DV55 - WP15 -U335 = -DV15 - DV45 -VP15 = C5*VP14 + S5*VSP24 -VP25 = C5*VSP24 - S5*VP14 -WI16 = C6*WI15 - S6*W35 -WI26 = -C6*W35 - S6*WI15 -W36 = QP6 + WI25 -WP16 = C6*WP15 + QP6*WI26 + S6*WP24 -WP26 = C6*WP24 - QP6*WI16 - S6*WP15 -DV16 = WI16**2 -DV26 = WI16*WI26 -DV36 = W36*WI16 -DV46 = WI26**2 -DV56 = W36*WI26 -DV66 = W36**2 -U116 = -DV46 - DV66 -U216 = DV26 + WP25 -U316 = DV36 - WP26 -U126 = DV26 - WP25 -U226 = -DV16 - DV66 -U326 = DV56 + WP16 -U136 = DV36 + WP26 -U236 = DV56 - WP16 -U336 = -DV16 - DV46 -VP16 = C6*VP15 + S6*VP24 -VP26 = C6*VP24 - S6*VP15 -F11 = -DV61*MX1 -F21 = -DV61*MY1 -PSI11 = QP1*XZ1 -PSI21 = QP1*YZ1 -PSI31 = QP1*ZZ1 -No11 = -PSI21*QP1 -No21 = PSI11*QP1 -F12 = DV22*MY2 + MX2*U112 -F22 = DV22*MX2 + MY2*U222 -F32 = MX2*U312 + MY2*U322 + MZ2*U332 -PSI12 = QP2*XZ2 + WI12*XX2 + WI22*XY2 -PSI22 = QP2*YZ2 + WI12*XY2 + WI22*YY2 -PSI32 = QP2*ZZ2 + WI12*XZ2 + WI22*YZ2 -No12 = -PSI22*QP2 + PSI32*WI22 + WP12*XX2 + WP22*XY2 -No22 = PSI12*QP2 - PSI32*WI12 + WP12*XY2 + WP22*YY2 -No32 = -PSI12*WI22 + PSI22*WI12 + WP12*XZ2 + WP22*YZ2 -F13 = DV23*MY3 + M3*VP13 + MX3*U113 + MZ3*U133 -F23 = DV23*MX3 + M3*VP23 + MY3*U223 + MZ3*U233 -F33 = M3*VSP33 + MX3*U313 + MY3*U323 + MZ3*U333 -PSI13 = W33*XZ3 + WI13*XX3 + WI23*XY3 -PSI23 = W33*YZ3 + WI13*XY3 + WI23*YY3 -PSI33 = W33*ZZ3 + WI13*XZ3 + WI23*YZ3 -No13 = -PSI23*W33 + PSI33*WI23 + WP13*XX3 + WP23*XY3 -No23 = PSI13*W33 - PSI33*WI13 + WP13*XY3 + WP23*YY3 -No33 = -PSI13*WI23 + PSI23*WI13 + WP13*XZ3 + WP23*YZ3 -F14 = M4*VP14 + MX4*U114 + MY4*U124 + MZ4*U134 -F24 = M4*VP24 + MX4*U214 + MY4*U224 + MZ4*U234 -F34 = M4*VSP24 + MX4*U314 + MY4*U324 + MZ4*U334 -PSI14 = W34*XZ4 + WI14*XX4 + WI24*XY4 -PSI24 = W34*YZ4 + WI14*XY4 + WI24*YY4 -PSI34 = W34*ZZ4 + WI14*XZ4 + WI24*YZ4 -No14 = -PSI24*W34 + PSI34*WI24 + WP14*XX4 + WP23*XZ4 + WP24*XY4 -No24 = PSI14*W34 - PSI34*WI14 + WP14*XY4 + WP23*YZ4 + WP24*YY4 -No34 = -PSI14*WI24 + PSI24*WI14 + WP14*XZ4 + WP23*ZZ4 + WP24*YZ4 -F15 = M5*VP15 + MX5*U115 + MY5*U125 + MZ5*U135 -F25 = M5*VP25 + MX5*U215 + MY5*U225 + MZ5*U235 -F35 = -M5*VP24 + MX5*U315 + MY5*U325 + MZ5*U335 -PSI15 = W35*XZ5 + WI15*XX5 + WI25*XY5 -PSI25 = W35*YZ5 + WI15*XY5 + WI25*YY5 -PSI35 = W35*ZZ5 + WI15*XZ5 + WI25*YZ5 -No15 = -PSI25*W35 + PSI35*WI25 + WP15*XX5 - WP24*XZ5 + WP25*XY5 -No25 = PSI15*W35 - PSI35*WI15 + WP15*XY5 - WP24*YZ5 + WP25*YY5 -No35 = -PSI15*WI25 + PSI25*WI15 + WP15*XZ5 - WP24*ZZ5 + WP25*YZ5 -F16 = M6*VP16 + MX6*U116 + MY6*U126 + MZ6*U136 -F26 = M6*VP26 + MX6*U216 + MY6*U226 + MZ6*U236 -F36 = M6*VP25 + MX6*U316 + MY6*U326 + MZ6*U336 -PSI16 = W36*XZ6 + WI16*XX6 + WI26*XY6 -PSI26 = W36*YZ6 + WI16*XY6 + WI26*YY6 -PSI36 = W36*ZZ6 + WI16*XZ6 + WI26*YZ6 -No16 = -PSI26*W36 + PSI36*WI26 + WP16*XX6 + WP25*XZ6 + WP26*XY6 -No26 = PSI16*W36 - PSI36*WI16 + WP16*XY6 + WP25*YZ6 + WP26*YY6 -No36 = -PSI16*WI26 + PSI26*WI16 + WP16*XZ6 + WP25*ZZ6 + WP26*YZ6 -E16 = F16 + FX6 -E26 = F26 + FY6 -E36 = F36 + FZ6 -N16 = CX6 + MY6*VP25 - MZ6*VP26 + No16 -N26 = CY6 - MX6*VP25 + MZ6*VP16 + No26 -N36 = CZ6 + MX6*VP26 - MY6*VP16 + No36 -FDI16 = C6*E16 - E26*S6 -FDI36 = -C6*E26 - E16*S6 -E15 = F15 + FDI16 -E25 = E36 + F25 -E35 = F35 + FDI36 -N15 = C6*N16 - MY5*VP24 - MZ5*VP25 - N26*S6 + No15 -N25 = MX5*VP24 + MZ5*VP15 + N36 + No25 -N35 = -C6*N26 + MX5*VP25 - MY5*VP15 - N16*S6 + No35 -FDI15 = C5*E15 - E25*S5 -FDI35 = C5*E25 + E15*S5 -E14 = F14 + FDI15 -E24 = -E35 + F24 -E34 = F34 + FDI35 -N14 = C5*N15 + MY4*VSP24 - MZ4*VP24 - N25*S5 + No14 -N24 = -MX4*VSP24 + MZ4*VP14 - N35 + No24 -N34 = C5*N25 + MX4*VP24 - MY4*VP14 + N15*S5 + No34 -FDI14 = C4*E14 - E24*S4 -FDI34 = -C4*E24 - E14*S4 -E13 = F13 + FDI14 -E23 = E34 + F23 -E33 = F33 + FDI34 -N13 = C4*N14 + FDI34*RL4 + MY3*VSP33 - MZ3*VP23 - N24*S4 + No13 -N23 = -MX3*VSP33 + MZ3*VP13 + N34 + No23 -N33 = -C4*N24 - FDI14*RL4 + MX3*VP23 - MY3*VP13 - N14*S4 + No33 -FDI13 = C3*E13 - E23*S3 -FDI23 = C3*E23 + E13*S3 -E12 = F12 + FDI13 -E22 = F22 + FDI23 -E32 = E33 + F32 -N12 = C3*N13 - N23*S3 + No12 -N22 = C3*N23 - D3*E33 + N13*S3 + No22 -N32 = D3*FDI23 + N33 + No32 -FDI12 = C2*E12 - E22*S2 -FDI32 = C2*E22 + E12*S2 -E11 = F11 + FDI12 -E21 = -E32 + F21 -N11 = C2*N12 - N22*S2 + No11 -N21 = -N32 + No21 -N31 = C2*N22 + N12*S2 -FDI11 = C1*E11 - E21*S1 -FDI21 = C1*E21 + E11*S1 -GAM1 = FS1*sign(QP1) + FV1*QP1 + N31 -GAM2 = FS2*sign(QP2) + FV2*QP2 + N32 -GAM3 = FS3*sign(QP3) + FV3*QP3 + N33 -GAM4 = FS4*sign(QP4) + FV4*QP4 + N34 -GAM5 = FS5*sign(QP5) + FV5*QP5 + N35 -GAM6 = FS6*sign(QP6) + FV6*QP6 + N36 -*=* diff --git a/robots/RX90/RX90_ddm.txt b/robots/RX90/RX90_ddm.txt deleted file mode 100644 index 43208e1..0000000 --- a/robots/RX90/RX90_ddm.txt +++ /dev/null @@ -1,937 +0,0 @@ -Direct dynamic model using Newton - Euler Algorith - -Geometric parameters -j ant sigma mu gamma b alpha d theta r -1 0 0 1 0 0 0 0 th1 0 -2 1 0 1 0 0 pi/2 0 th2 0 -3 2 0 1 0 0 0 D3 th3 0 -4 3 0 1 0 0 -pi/2 0 th4 RL4 -5 4 0 1 0 0 pi/2 0 th5 0 -6 5 0 1 0 0 -pi/2 0 th6 0 - -Dynamic inertia parameters -j XX XY XZ YY YZ ZZ MX MY MZ M IA -1 XX1 XY1 XZ1 YY1 YZ1 ZZ1 MX1 MY1 MZ1 M1 IA1 -2 XX2 XY2 XZ2 YY2 YZ2 ZZ2 MX2 MY2 MZ2 M2 IA2 -3 XX3 XY3 XZ3 YY3 YZ3 ZZ3 MX3 MY3 MZ3 M3 IA3 -4 XX4 XY4 XZ4 YY4 YZ4 ZZ4 MX4 MY4 MZ4 M4 IA4 -5 XX5 XY5 XZ5 YY5 YZ5 ZZ5 MX5 MY5 MZ5 M5 IA5 -6 XX6 XY6 XZ6 YY6 YZ6 ZZ6 MX6 MY6 MZ6 M6 IA6 - -External forces and joint parameters -j FX FY FZ CX CY CZ FS FV QP QDP GAM -1 0 0 0 0 0 0 FS1 FV1 QP1 QDP1 GAM1 -2 0 0 0 0 0 0 FS2 FV2 QP2 QDP2 GAM2 -3 0 0 0 0 0 0 FS3 FV3 QP3 QDP3 GAM3 -4 0 0 0 0 0 0 FS4 FV4 QP4 QDP4 GAM4 -5 0 0 0 0 0 0 FS5 FV5 QP5 QDP5 GAM5 -6 FX6 FY6 FZ6 CX6 CY6 CZ6 FS6 FV6 QP6 QDP6 GAM6 - -Base velicities parameters -axis W0 WP0 V0 VP0 G -X 0 0 0 0 0 -Y 0 0 0 0 0 -Z 0 0 0 0 G3 - -Equations: -C1 = cos(th1) -S1 = sin(th1) -C2 = cos(th2) -S2 = sin(th2) -C3 = cos(th3) -S3 = sin(th3) -C4 = cos(th4) -S4 = sin(th4) -C5 = cos(th5) -S5 = sin(th5) -C6 = cos(th6) -S6 = sin(th6) -WI12 = QP1*S2 -WI22 = C2*QP1 -WI13 = C3*WI12 + S3*WI22 -WI23 = C3*WI22 - S3*WI12 -W33 = QP2 + QP3 -JPR133 = D3*S3 -JPR233 = C3*D3 -WI14 = C4*WI13 - S4*W33 -WI24 = -C4*W33 - S4*WI13 -W34 = QP4 + WI23 -JPR114 = -RL4*S4 -JPR214 = -C4*RL4 -WI15 = C5*WI14 + S5*W34 -WI25 = C5*W34 - S5*WI14 -W35 = QP5 - WI24 -WI16 = C6*WI15 - S6*W35 -WI26 = -C6*W35 - S6*WI15 -W36 = QP6 + WI25 -JW11 = QP1*XZ1 -JW21 = QP1*YZ1 -JW31 = QP1*ZZ1 -KW11 = -JW21*QP1 -KW21 = JW11*QP1 -SW11 = -MX1*QP1**2 -SW21 = -MY1*QP1**2 -JW12 = QP2*XZ2 + WI12*XX2 + WI22*XY2 -JW22 = QP2*YZ2 + WI12*XY2 + WI22*YY2 -JW32 = QP2*ZZ2 + WI12*XZ2 + WI22*YZ2 -KW12 = -JW22*QP2 + JW32*WI22 -KW22 = JW12*QP2 - JW32*WI12 -KW32 = -JW12*WI22 + JW22*WI12 -SW12 = -QP2*(MX2*QP2 - MZ2*WI12) + WI22*(-MX2*WI22 + MY2*WI12) -SW22 = QP2*(-MY2*QP2 + MZ2*WI22) - WI12*(-MX2*WI22 + MY2*WI12) -SW32 = WI12*(MX2*QP2 - MZ2*WI12) - WI22*(-MY2*QP2 + MZ2*WI22) -WQ12 = QP2*WI22 -WQ22 = -QP2*WI12 -JW13 = W33*XZ3 + WI13*XX3 + WI23*XY3 -JW23 = W33*YZ3 + WI13*XY3 + WI23*YY3 -JW33 = W33*ZZ3 + WI13*XZ3 + WI23*YZ3 -KW13 = -JW23*W33 + JW33*WI23 -KW23 = JW13*W33 - JW33*WI13 -KW33 = -JW13*WI23 + JW23*WI13 -SW13 = -W33*(MX3*W33 - MZ3*WI13) + WI23*(-MX3*WI23 + MY3*WI13) -SW23 = W33*(-MY3*W33 + MZ3*WI23) - WI13*(-MX3*WI23 + MY3*WI13) -SW33 = WI13*(MX3*W33 - MZ3*WI13) - WI23*(-MY3*W33 + MZ3*WI23) -WQ13 = QP3*WI23 -WQ23 = -QP3*WI13 -LW13 = C3*(-D3*QP2**2 - D3*WI22**2) + D3*S3*WI12*WI22 -LW23 = C3*D3*WI12*WI22 - S3*(-D3*QP2**2 - D3*WI22**2) -LW33 = D3*QP2*WI12 -JW14 = W34*XZ4 + WI14*XX4 + WI24*XY4 -JW24 = W34*YZ4 + WI14*XY4 + WI24*YY4 -JW34 = W34*ZZ4 + WI14*XZ4 + WI24*YZ4 -KW14 = -JW24*W34 + JW34*WI24 -KW24 = JW14*W34 - JW34*WI14 -KW34 = -JW14*WI24 + JW24*WI14 -SW14 = -W34*(MX4*W34 - MZ4*WI14) + WI24*(-MX4*WI24 + MY4*WI14) -SW24 = W34*(-MY4*W34 + MZ4*WI24) - WI14*(-MX4*WI24 + MY4*WI14) -SW34 = WI14*(MX4*W34 - MZ4*WI14) - WI24*(-MY4*W34 + MZ4*WI24) -WQ14 = QP4*WI24 -WQ24 = -QP4*WI14 -LW14 = C4*RL4*WI13*WI23 - RL4*S4*W33*WI23 -LW24 = -C4*RL4*W33*WI23 - RL4*S4*WI13*WI23 -LW34 = -RL4*W33**2 - RL4*WI13**2 -JW15 = W35*XZ5 + WI15*XX5 + WI25*XY5 -JW25 = W35*YZ5 + WI15*XY5 + WI25*YY5 -JW35 = W35*ZZ5 + WI15*XZ5 + WI25*YZ5 -KW15 = -JW25*W35 + JW35*WI25 -KW25 = JW15*W35 - JW35*WI15 -KW35 = -JW15*WI25 + JW25*WI15 -SW15 = -W35*(MX5*W35 - MZ5*WI15) + WI25*(-MX5*WI25 + MY5*WI15) -SW25 = W35*(-MY5*W35 + MZ5*WI25) - WI15*(-MX5*WI25 + MY5*WI15) -SW35 = WI15*(MX5*W35 - MZ5*WI15) - WI25*(-MY5*W35 + MZ5*WI25) -WQ15 = QP5*WI25 -WQ25 = -QP5*WI15 -JW16 = W36*XZ6 + WI16*XX6 + WI26*XY6 -JW26 = W36*YZ6 + WI16*XY6 + WI26*YY6 -JW36 = W36*ZZ6 + WI16*XZ6 + WI26*YZ6 -KW16 = -JW26*W36 + JW36*WI26 -KW26 = JW16*W36 - JW36*WI16 -KW36 = -JW16*WI26 + JW26*WI16 -SW16 = -W36*(MX6*W36 - MZ6*WI16) + WI26*(-MX6*WI26 + MY6*WI16) -SW26 = W36*(-MY6*W36 + MZ6*WI26) - WI16*(-MX6*WI26 + MY6*WI16) -SW36 = WI16*(MX6*W36 - MZ6*WI16) - WI26*(-MY6*W36 + MZ6*WI26) -WQ16 = QP6*WI26 -WQ26 = -QP6*WI16 -VBE16 = -FX6 - SW16 -VBE26 = -FY6 - SW26 -VBE36 = -FZ6 - SW36 -VBE46 = -CX6 - KW16 -VBE56 = -CY6 - KW26 -VBE66 = -CZ6 - KW36 -JD6 = 1/(IA6 + ZZ6) -JU16 = -JD6*MY6 -JU26 = JD6*MX6 -JU46 = JD6*XZ6 -JU56 = JD6*YZ6 -JU66 = JD6*ZZ6 -GW6 = -FS6*sign(QP6) - FV6*QP6 + GAM6 + VBE66 -GK116 = JU16*MY6 + M6 -GK216 = JU26*MY6 -GK416 = JU46*MY6 -GK516 = JU56*MY6 + MZ6 -GK616 = JU66*MY6 - MY6 -GK126 = -JU16*MX6 -GK226 = -JU26*MX6 + M6 -GK426 = -JU46*MX6 - MZ6 -GK526 = -JU56*MX6 -GK626 = -JU66*MX6 + MX6 -GK146 = -JU16*XZ6 -GK246 = -JU26*XZ6 - MZ6 -GK446 = -JU46*XZ6 + XX6 -GK546 = -JU56*XZ6 + XY6 -GK646 = -JU66*XZ6 + XZ6 -GK156 = -JU16*YZ6 + MZ6 -GK256 = -JU26*YZ6 -GK456 = -JU46*YZ6 + XY6 -GK556 = -JU56*YZ6 + YY6 -GK656 = -JU66*YZ6 + YZ6 -GK166 = -JU16*ZZ6 - MY6 -GK266 = -JU26*ZZ6 + MX6 -GK466 = -JU46*ZZ6 + XZ6 -GK566 = -JU56*ZZ6 + YZ6 -GK666 = -JU66*ZZ6 + ZZ6 -NG16 = GK146*WQ16 + GK156*WQ26 -NG26 = GK246*WQ16 + GK256*WQ26 -NG36 = -MX6*WQ26 + MY6*WQ16 -NG46 = GK446*WQ16 + GK456*WQ26 -NG56 = GK546*WQ16 + GK556*WQ26 -NG66 = GK646*WQ16 + GK656*WQ26 -VS16 = GW6*JU16 + NG16 -VS26 = GW6*JU26 + NG26 -VS46 = GW6*JU46 + NG46 -VS56 = GW6*JU56 + NG56 -VS66 = GW6*JU66 + NG66 -AP16 = -VBE16 + VS16 -AP26 = -VBE26 + VS26 -AP36 = NG36 - VBE36 -AP46 = -VBE46 + VS46 -AP56 = -VBE56 + VS56 -AP66 = -VBE66 + VS66 -GX116 = C6*GK116 - GK216*S6 -GX316 = -C6*GK216 - GK116*S6 -GX416 = C6*GK416 - GK516*S6 -GX616 = -C6*GK516 - GK416*S6 -GX126 = C6*GK126 - GK226*S6 -GX326 = -C6*GK226 - GK126*S6 -GX426 = C6*GK426 - GK526*S6 -GX626 = -C6*GK526 - GK426*S6 -GX436 = C6*MY6 + MX6*S6 -GX636 = C6*MX6 - MY6*S6 -GX146 = C6*GK146 - GK246*S6 -GX346 = -C6*GK246 - GK146*S6 -GX446 = C6*GK446 - GK546*S6 -GX646 = -C6*GK546 - GK446*S6 -GX156 = C6*GK156 - GK256*S6 -GX356 = -C6*GK256 - GK156*S6 -GX456 = C6*GK456 - GK556*S6 -GX656 = -C6*GK556 - GK456*S6 -GX166 = C6*GK166 - GK266*S6 -GX366 = -C6*GK266 - GK166*S6 -GX466 = C6*GK466 - GK566*S6 -GX666 = -C6*GK566 - GK466*S6 -TKT116 = C6*GX116 - GX126*S6 -TKT136 = -C6*GX126 - GX116*S6 -TKT336 = -C6*GX326 - GX316*S6 -TKT146 = C6*GX146 - GX156*S6 -TKT346 = C6*GX346 - GX356*S6 -TKT446 = C6*GX446 - GX456*S6 -TKT166 = -C6*GX156 - GX146*S6 -TKT366 = -C6*GX356 - GX346*S6 -TKT466 = -C6*GX456 - GX446*S6 -TKT566 = -C6*GK656 - GK646*S6 -TKT666 = -C6*GX656 - GX646*S6 -MJE115 = M5 + TKT116 -MJE225 = M5 + M6 -MJE335 = M5 + TKT336 -MJE245 = GX436 - MZ5 -MJE345 = MY5 + TKT346 -MJE445 = TKT446 + XX5 -MJE155 = GX166 + MZ5 -MJE355 = GX366 - MX5 -MJE455 = GX466 + XY5 -MJE555 = GK666 + YY5 -MJE165 = -MY5 + TKT166 -MJE265 = GX636 + MX5 -MJE465 = TKT466 + XZ5 -MJE565 = TKT566 + YZ5 -MJE665 = TKT666 + ZZ5 -VBE15 = -AP16*C6 + AP26*S6 - SW15 -VBE25 = -AP36 - SW25 -VBE35 = AP16*S6 + AP26*C6 - SW35 -VBE45 = -AP46*C6 + AP56*S6 - KW15 -VBE55 = -AP66 - KW25 -VBE65 = AP46*S6 + AP56*C6 - KW35 -JD5 = 1/(IA5 + MJE665) -JU15 = JD5*MJE165 -JU25 = JD5*MJE265 -JU35 = JD5*TKT366 -JU45 = JD5*MJE465 -JU55 = JD5*MJE565 -JU65 = JD5*MJE665 -GW5 = -FS5*sign(QP5) - FV5*QP5 + GAM5 + VBE65 -GK115 = -JU15*MJE165 + MJE115 -GK215 = -JU25*MJE165 -GK315 = -JU35*MJE165 + TKT136 -GK415 = -JU45*MJE165 + TKT146 -GK515 = GX166 - JU55*MJE165 + MZ5 -GK615 = -JU65*MJE165 - MY5 + TKT166 -GK125 = -JU15*MJE265 -GK225 = -JU25*MJE265 + MJE225 -GK325 = -JU35*MJE265 -GK425 = GX436 - JU45*MJE265 - MZ5 -GK525 = -JU55*MJE265 -GK625 = GX636 - JU65*MJE265 + MX5 -GK135 = -JU15*TKT366 + TKT136 -GK235 = -JU25*TKT366 -GK335 = -JU35*TKT366 + MJE335 -GK435 = -JU45*TKT366 + MY5 + TKT346 -GK535 = GX366 - JU55*TKT366 - MX5 -GK635 = -JU65*TKT366 + TKT366 -GK145 = -JU15*MJE465 + TKT146 -GK245 = -JU25*MJE465 + MJE245 -GK345 = -JU35*MJE465 + MJE345 -GK445 = -JU45*MJE465 + MJE445 -GK545 = GX466 - JU55*MJE465 + XY5 -GK645 = -JU65*MJE465 + TKT466 + XZ5 -GK155 = -JU15*MJE565 + MJE155 -GK255 = -JU25*MJE565 -GK355 = -JU35*MJE565 + MJE355 -GK455 = -JU45*MJE565 + MJE455 -GK555 = -JU55*MJE565 + MJE555 -GK655 = -JU65*MJE565 + TKT566 + YZ5 -GK165 = -JU15*MJE665 + MJE165 -GK265 = -JU25*MJE665 + MJE265 -GK365 = -JU35*MJE665 + TKT366 -GK465 = -JU45*MJE665 + MJE465 -GK565 = -JU55*MJE665 + MJE565 -GK665 = -JU65*MJE665 + MJE665 -NG15 = GK145*WQ15 + GK155*WQ25 -NG25 = GK245*WQ15 + GK255*WQ25 -NG35 = GK345*WQ15 + GK355*WQ25 -NG45 = GK445*WQ15 + GK455*WQ25 -NG55 = GK545*WQ15 + GK555*WQ25 -NG65 = GK645*WQ15 + GK655*WQ25 -VS15 = GW5*JU15 + NG15 -VS25 = GW5*JU25 + NG25 -VS35 = GW5*JU35 + NG35 -VS45 = GW5*JU45 + NG45 -VS55 = GW5*JU55 + NG55 -VS65 = GW5*JU65 + NG65 -AP15 = -VBE15 + VS15 -AP25 = -VBE25 + VS25 -AP35 = -VBE35 + VS35 -AP45 = -VBE45 + VS45 -AP55 = -VBE55 + VS55 -AP65 = -VBE65 + VS65 -GX115 = C5*GK115 - GK215*S5 -GX315 = C5*GK215 + GK115*S5 -GX415 = C5*GK415 - GK515*S5 -GX615 = C5*GK515 + GK415*S5 -GX125 = C5*GK125 - GK225*S5 -GX325 = C5*GK225 + GK125*S5 -GX425 = C5*GK425 - GK525*S5 -GX625 = C5*GK525 + GK425*S5 -GX135 = C5*GK135 - GK235*S5 -GX335 = C5*GK235 + GK135*S5 -GX435 = C5*GK435 - GK535*S5 -GX635 = C5*GK535 + GK435*S5 -GX145 = C5*GK145 - GK245*S5 -GX345 = C5*GK245 + GK145*S5 -GX445 = C5*GK445 - GK545*S5 -GX645 = C5*GK545 + GK445*S5 -GX155 = C5*GK155 - GK255*S5 -GX355 = C5*GK255 + GK155*S5 -GX455 = C5*GK455 - GK555*S5 -GX655 = C5*GK555 + GK455*S5 -GX165 = C5*GK165 - GK265*S5 -GX365 = C5*GK265 + GK165*S5 -GX465 = C5*GK465 - GK565*S5 -GX665 = C5*GK565 + GK465*S5 -TKT115 = C5*GX115 - GX125*S5 -TKT135 = C5*GX125 + GX115*S5 -TKT235 = -C5*GK325 - GK315*S5 -TKT335 = C5*GX325 + GX315*S5 -TKT145 = C5*GX145 - GX155*S5 -TKT245 = -C5*GK345 + GK355*S5 -TKT345 = C5*GX345 - GX355*S5 -TKT445 = C5*GX445 - GX455*S5 -TKT165 = C5*GX155 + GX145*S5 -TKT265 = -C5*GK355 - GK345*S5 -TKT365 = C5*GX355 + GX345*S5 -TKT465 = C5*GX455 + GX445*S5 -TKT565 = -C5*GK655 - GK645*S5 -TKT665 = C5*GX655 + GX645*S5 -MJE114 = M4 + TKT115 -MJE224 = GK335 + M4 -MJE334 = M4 + TKT335 -MJE244 = -MZ4 + TKT245 -MJE344 = MY4 + TKT345 -MJE444 = TKT445 + XX4 -MJE154 = -GX165 + MZ4 -MJE354 = -GX365 - MX4 -MJE454 = -GX465 + XY4 -MJE554 = GK665 + YY4 -MJE164 = -MY4 + TKT165 -MJE264 = MX4 + TKT265 -MJE464 = TKT465 + XZ4 -MJE564 = TKT565 + YZ4 -MJE664 = TKT665 + ZZ4 -VBE14 = -AP15*C5 + AP25*S5 - SW14 -VBE24 = AP35 - SW24 -VBE34 = -AP15*S5 - AP25*C5 - SW34 -VBE44 = -AP45*C5 + AP55*S5 - KW14 -VBE54 = AP65 - KW24 -VBE64 = -AP45*S5 - AP55*C5 - KW34 -JD4 = 1/(IA4 + MJE664) -JU14 = JD4*MJE164 -JU24 = JD4*MJE264 -JU34 = JD4*TKT365 -JU44 = JD4*MJE464 -JU54 = JD4*MJE564 -JU64 = JD4*MJE664 -GW4 = -FS4*sign(QP4) - FV4*QP4 + GAM4 + VBE64 -GK114 = -JU14*MJE164 + MJE114 -GK214 = -GX135 - JU24*MJE164 -GK314 = -JU34*MJE164 + TKT135 -GK414 = -JU44*MJE164 + TKT145 -GK514 = -GX165 - JU54*MJE164 + MZ4 -GK614 = -JU64*MJE164 - MY4 + TKT165 -GK124 = -GX135 - JU14*MJE264 -GK224 = -JU24*MJE264 + MJE224 -GK324 = -JU34*MJE264 + TKT235 -GK424 = -JU44*MJE264 - MZ4 + TKT245 -GK524 = GK365 - JU54*MJE264 -GK624 = -JU64*MJE264 + MX4 + TKT265 -GK134 = -JU14*TKT365 + TKT135 -GK234 = -JU24*TKT365 + TKT235 -GK334 = -JU34*TKT365 + MJE334 -GK434 = -JU44*TKT365 + MY4 + TKT345 -GK534 = -GX365 - JU54*TKT365 - MX4 -GK634 = -JU64*TKT365 + TKT365 -GK144 = -JU14*MJE464 + TKT145 -GK244 = -JU24*MJE464 + MJE244 -GK344 = -JU34*MJE464 + MJE344 -GK444 = -JU44*MJE464 + MJE444 -GK544 = -GX465 - JU54*MJE464 + XY4 -GK644 = -JU64*MJE464 + TKT465 + XZ4 -GK154 = -JU14*MJE564 + MJE154 -GK254 = GK365 - JU24*MJE564 -GK354 = -JU34*MJE564 + MJE354 -GK454 = -JU44*MJE564 + MJE454 -GK554 = -JU54*MJE564 + MJE554 -GK654 = -JU64*MJE564 + TKT565 + YZ4 -GK164 = -JU14*MJE664 + MJE164 -GK264 = -JU24*MJE664 + MJE264 -GK364 = -JU34*MJE664 + TKT365 -GK464 = -JU44*MJE664 + MJE464 -GK564 = -JU54*MJE664 + MJE564 -GK664 = -JU64*MJE664 + MJE664 -NG14 = GK114*LW14 + GK124*LW24 + GK134*LW34 + GK144*WQ14 + GK154*WQ24 -NG24 = GK214*LW14 + GK224*LW24 + GK234*LW34 + GK244*WQ14 + GK254*WQ24 -NG34 = GK314*LW14 + GK324*LW24 + GK334*LW34 + GK344*WQ14 + GK354*WQ24 -NG44 = GK414*LW14 + GK424*LW24 + GK434*LW34 + GK444*WQ14 + GK454*WQ24 -NG54 = GK514*LW14 + GK524*LW24 + GK534*LW34 + GK544*WQ14 + GK554*WQ24 -NG64 = GK614*LW14 + GK624*LW24 + GK634*LW34 + GK644*WQ14 + GK654*WQ24 -VS14 = GW4*JU14 + NG14 -VS24 = GW4*JU24 + NG24 -VS34 = GW4*JU34 + NG34 -VS44 = GW4*JU44 + NG44 -VS54 = GW4*JU54 + NG54 -VS64 = GW4*JU64 + NG64 -AP14 = -VBE14 + VS14 -AP24 = -VBE24 + VS24 -AP34 = -VBE34 + VS34 -AP44 = -VBE44 + VS44 -AP54 = -VBE54 + VS54 -AP64 = -VBE64 + VS64 -GX114 = C4*GK114 - GK214*S4 -GX314 = -C4*GK214 - GK114*S4 -GX414 = C4*GK414 + GK114*JPR114 + GK214*JPR214 - GK514*S4 -GX614 = -C4*GK514 + GK114*JPR214 - GK214*JPR114 - GK414*S4 -GX124 = C4*GK124 - GK224*S4 -GX324 = -C4*GK224 - GK124*S4 -GX424 = C4*GK424 + GK124*JPR114 + GK224*JPR214 - GK524*S4 -GX624 = -C4*GK524 + GK124*JPR214 - GK224*JPR114 - GK424*S4 -GX134 = C4*GK134 - GK234*S4 -GX334 = -C4*GK234 - GK134*S4 -GX434 = C4*GK434 + GK134*JPR114 + GK234*JPR214 - GK534*S4 -GX634 = -C4*GK534 + GK134*JPR214 - GK234*JPR114 - GK434*S4 -GX144 = C4*GK144 - GK244*S4 -GX344 = -C4*GK244 - GK144*S4 -GX444 = C4*GK444 + GK144*JPR114 + GK244*JPR214 - GK544*S4 -GX644 = -C4*GK544 + GK144*JPR214 - GK244*JPR114 - GK444*S4 -GX154 = C4*GK154 - GK254*S4 -GX354 = -C4*GK254 - GK154*S4 -GX454 = C4*GK454 + GK154*JPR114 + GK254*JPR214 - GK554*S4 -GX654 = -C4*GK554 + GK154*JPR214 - GK254*JPR114 - GK454*S4 -GX164 = C4*GK164 - GK264*S4 -GX364 = -C4*GK264 - GK164*S4 -GX464 = C4*GK464 + GK164*JPR114 + GK264*JPR214 - GK564*S4 -GX664 = -C4*GK564 + GK164*JPR214 - GK264*JPR114 - GK464*S4 -TKT114 = C4*GX114 - GX124*S4 -TKT134 = -C4*GX124 - GX114*S4 -TKT234 = -C4*GK324 - GK314*S4 -TKT334 = -C4*GX324 - GX314*S4 -TKT144 = C4*GX144 + GX114*JPR114 + GX124*JPR214 - GX154*S4 -TKT244 = C4*GK344 + GK314*JPR114 + GK324*JPR214 - GK354*S4 -TKT344 = C4*GX344 + GX314*JPR114 + GX324*JPR214 - GX354*S4 -TKT444 = C4*GX444 + GX414*JPR114 + GX424*JPR214 - GX454*S4 -TKT164 = -C4*GX154 + GX114*JPR214 - GX124*JPR114 - GX144*S4 -TKT264 = -C4*GK354 + GK314*JPR214 - GK324*JPR114 - GK344*S4 -TKT364 = -C4*GX354 + GX314*JPR214 - GX324*JPR114 - GX344*S4 -TKT464 = -C4*GX454 + GX414*JPR214 - GX424*JPR114 - GX444*S4 -TKT564 = -C4*GK654 + GK614*JPR214 - GK624*JPR114 - GK644*S4 -TKT664 = -C4*GX654 + GX614*JPR214 - GX624*JPR114 - GX644*S4 -MJE113 = M3 + TKT114 -MJE223 = GK334 + M3 -MJE333 = M3 + TKT334 -MJE243 = -MZ3 + TKT244 -MJE343 = MY3 + TKT344 -MJE443 = TKT444 + XX3 -MJE153 = GX164 + MZ3 -MJE353 = GX364 - MX3 -MJE453 = GX464 + XY3 -MJE553 = GK664 + YY3 -MJE163 = -MY3 + TKT164 -MJE263 = MX3 + TKT264 -MJE463 = TKT464 + XZ3 -MJE563 = TKT564 + YZ3 -MJE663 = TKT664 + ZZ3 -VBE13 = -AP14*C4 + AP24*S4 - SW13 -VBE23 = -AP34 - SW23 -VBE33 = AP14*S4 + AP24*C4 - SW33 -VBE43 = -AP14*JPR114 - AP24*JPR214 - AP44*C4 + AP54*S4 - KW13 -VBE53 = -AP64 - KW23 -VBE63 = -AP14*JPR214 + AP24*JPR114 + AP44*S4 + AP54*C4 - KW33 -JD3 = 1/(IA3 + MJE663) -JU13 = JD3*MJE163 -JU23 = JD3*MJE263 -JU33 = JD3*TKT364 -JU43 = JD3*MJE463 -JU53 = JD3*MJE563 -JU63 = JD3*MJE663 -GW3 = -FS3*sign(QP3) - FV3*QP3 + GAM3 + VBE63 -GK113 = -JU13*MJE163 + MJE113 -GK213 = GX134 - JU23*MJE163 -GK313 = -JU33*MJE163 + TKT134 -GK413 = -JU43*MJE163 + TKT144 -GK513 = GX164 - JU53*MJE163 + MZ3 -GK613 = -JU63*MJE163 - MY3 + TKT164 -GK123 = GX134 - JU13*MJE263 -GK223 = -JU23*MJE263 + MJE223 -GK323 = -JU33*MJE263 + TKT234 -GK423 = -JU43*MJE263 - MZ3 + TKT244 -GK523 = GK364 - JU53*MJE263 -GK623 = -JU63*MJE263 + MX3 + TKT264 -GK133 = -JU13*TKT364 + TKT134 -GK233 = -JU23*TKT364 + TKT234 -GK333 = -JU33*TKT364 + MJE333 -GK433 = -JU43*TKT364 + MY3 + TKT344 -GK533 = GX364 - JU53*TKT364 - MX3 -GK633 = -JU63*TKT364 + TKT364 -GK143 = -JU13*MJE463 + TKT144 -GK243 = -JU23*MJE463 + MJE243 -GK343 = -JU33*MJE463 + MJE343 -GK443 = -JU43*MJE463 + MJE443 -GK543 = GX464 - JU53*MJE463 + XY3 -GK643 = -JU63*MJE463 + TKT464 + XZ3 -GK153 = -JU13*MJE563 + MJE153 -GK253 = GK364 - JU23*MJE563 -GK353 = -JU33*MJE563 + MJE353 -GK453 = -JU43*MJE563 + MJE453 -GK553 = -JU53*MJE563 + MJE553 -GK653 = -JU63*MJE563 + TKT564 + YZ3 -GK163 = -JU13*MJE663 + MJE163 -GK263 = -JU23*MJE663 + MJE263 -GK363 = -JU33*MJE663 + TKT364 -GK463 = -JU43*MJE663 + MJE463 -GK563 = -JU53*MJE663 + MJE563 -GK663 = -JU63*MJE663 + MJE663 -NG13 = GK113*LW13 + GK123*LW23 + GK133*LW33 + GK143*WQ13 + GK153*WQ23 -NG23 = GK213*LW13 + GK223*LW23 + GK233*LW33 + GK243*WQ13 + GK253*WQ23 -NG33 = GK313*LW13 + GK323*LW23 + GK333*LW33 + GK343*WQ13 + GK353*WQ23 -NG43 = GK413*LW13 + GK423*LW23 + GK433*LW33 + GK443*WQ13 + GK453*WQ23 -NG53 = GK513*LW13 + GK523*LW23 + GK533*LW33 + GK543*WQ13 + GK553*WQ23 -NG63 = GK613*LW13 + GK623*LW23 + GK633*LW33 + GK643*WQ13 + GK653*WQ23 -VS13 = GW3*JU13 + NG13 -VS23 = GW3*JU23 + NG23 -VS33 = GW3*JU33 + NG33 -VS43 = GW3*JU43 + NG43 -VS53 = GW3*JU53 + NG53 -VS63 = GW3*JU63 + NG63 -AP13 = -VBE13 + VS13 -AP23 = -VBE23 + VS23 -AP33 = -VBE33 + VS33 -AP43 = -VBE43 + VS43 -AP53 = -VBE53 + VS53 -AP63 = -VBE63 + VS63 -GX113 = C3*GK113 - GK213*S3 -GX213 = C3*GK213 + GK113*S3 -GX413 = C3*GK413 - GK513*S3 -GX513 = C3*GK513 - D3*GK313 + GK413*S3 -GX613 = GK113*JPR133 + GK213*JPR233 + GK613 -GX123 = C3*GK123 - GK223*S3 -GX223 = C3*GK223 + GK123*S3 -GX423 = C3*GK423 - GK523*S3 -GX523 = C3*GK523 - D3*GK323 + GK423*S3 -GX623 = GK123*JPR133 + GK223*JPR233 + GK623 -GX133 = C3*GK133 - GK233*S3 -GX233 = C3*GK233 + GK133*S3 -GX433 = C3*GK433 - GK533*S3 -GX533 = C3*GK533 - D3*GK333 + GK433*S3 -GX633 = GK133*JPR133 + GK233*JPR233 + GK633 -GX143 = C3*GK143 - GK243*S3 -GX243 = C3*GK243 + GK143*S3 -GX443 = C3*GK443 - GK543*S3 -GX543 = C3*GK543 - D3*GK343 + GK443*S3 -GX643 = GK143*JPR133 + GK243*JPR233 + GK643 -GX153 = C3*GK153 - GK253*S3 -GX253 = C3*GK253 + GK153*S3 -GX453 = C3*GK453 - GK553*S3 -GX553 = C3*GK553 - D3*GK353 + GK453*S3 -GX653 = GK153*JPR133 + GK253*JPR233 + GK653 -GX163 = C3*GK163 - GK263*S3 -GX263 = C3*GK263 + GK163*S3 -GX463 = C3*GK463 - GK563*S3 -GX563 = C3*GK563 - D3*GK363 + GK463*S3 -GX663 = GK163*JPR133 + GK263*JPR233 + GK663 -TKT113 = C3*GX113 - GX123*S3 -TKT123 = C3*GX123 + GX113*S3 -TKT223 = C3*GX223 + GX213*S3 -TKT143 = C3*GX143 - GX153*S3 -TKT243 = C3*GX243 - GX253*S3 -TKT343 = C3*GK343 - GK353*S3 -TKT443 = C3*GX443 - GX453*S3 -TKT153 = C3*GX153 - D3*GX133 + GX143*S3 -TKT253 = C3*GX253 - D3*GX233 + GX243*S3 -TKT353 = C3*GK353 - D3*GK333 + GK343*S3 -TKT453 = C3*GX453 - D3*GX433 + GX443*S3 -TKT553 = C3*GX553 - D3*GX533 + GX543*S3 -TKT163 = GX113*JPR133 + GX123*JPR233 + GX163 -TKT263 = GX213*JPR133 + GX223*JPR233 + GX263 -TKT363 = GK313*JPR133 + GK323*JPR233 + GK363 -TKT463 = GX413*JPR133 + GX423*JPR233 + GX463 -TKT563 = GX513*JPR133 + GX523*JPR233 + GX563 -TKT663 = GX613*JPR133 + GX623*JPR233 + GX663 -MJE112 = M2 + TKT113 -MJE222 = M2 + TKT223 -MJE332 = GK333 + M2 -MJE242 = -MZ2 + TKT243 -MJE342 = MY2 + TKT343 -MJE442 = TKT443 + XX2 -MJE152 = MZ2 + TKT153 -MJE352 = -MX2 + TKT353 -MJE452 = TKT453 + XY2 -MJE552 = TKT553 + YY2 -MJE162 = -MY2 + TKT163 -MJE262 = MX2 + TKT263 -MJE462 = TKT463 + XZ2 -MJE562 = TKT563 + YZ2 -MJE662 = TKT663 + ZZ2 -VBE12 = -AP13*C3 + AP23*S3 - SW12 -VBE22 = -AP13*S3 - AP23*C3 - SW22 -VBE32 = -AP33 - SW32 -VBE42 = -AP43*C3 + AP53*S3 - KW12 -VBE52 = AP33*D3 - AP43*S3 - AP53*C3 - KW22 -VBE62 = -AP13*JPR133 - AP23*JPR233 - AP63 - KW32 -JD2 = 1/(IA2 + MJE662) -JU12 = JD2*MJE162 -JU22 = JD2*MJE262 -JU32 = JD2*TKT363 -JU42 = JD2*MJE462 -JU52 = JD2*MJE562 -JU62 = JD2*MJE662 -GW2 = -FS2*sign(QP2) - FV2*QP2 + GAM2 + VBE62 -GK112 = -JU12*MJE162 + MJE112 -GK212 = -JU22*MJE162 + TKT123 -GK312 = GX133 - JU32*MJE162 -GK412 = -JU42*MJE162 + TKT143 -GK512 = -JU52*MJE162 + MZ2 + TKT153 -GK612 = -JU62*MJE162 - MY2 + TKT163 -GK122 = -JU12*MJE262 + TKT123 -GK222 = -JU22*MJE262 + MJE222 -GK322 = GX233 - JU32*MJE262 -GK422 = -JU42*MJE262 - MZ2 + TKT243 -GK522 = -JU52*MJE262 + TKT253 -GK622 = -JU62*MJE262 + MX2 + TKT263 -GK132 = GX133 - JU12*TKT363 -GK232 = GX233 - JU22*TKT363 -GK332 = -JU32*TKT363 + MJE332 -GK432 = -JU42*TKT363 + MY2 + TKT343 -GK532 = -JU52*TKT363 - MX2 + TKT353 -GK632 = -JU62*TKT363 + TKT363 -GK142 = -JU12*MJE462 + TKT143 -GK242 = -JU22*MJE462 + MJE242 -GK342 = -JU32*MJE462 + MJE342 -GK442 = -JU42*MJE462 + MJE442 -GK542 = -JU52*MJE462 + TKT453 + XY2 -GK642 = -JU62*MJE462 + TKT463 + XZ2 -GK152 = -JU12*MJE562 + MJE152 -GK252 = -JU22*MJE562 + TKT253 -GK352 = -JU32*MJE562 + MJE352 -GK452 = -JU42*MJE562 + MJE452 -GK552 = -JU52*MJE562 + MJE552 -GK652 = -JU62*MJE562 + TKT563 + YZ2 -GK162 = -JU12*MJE662 + MJE162 -GK262 = -JU22*MJE662 + MJE262 -GK362 = -JU32*MJE662 + TKT363 -GK462 = -JU42*MJE662 + MJE462 -GK562 = -JU52*MJE662 + MJE562 -GK662 = -JU62*MJE662 + MJE662 -NG12 = GK142*WQ12 + GK152*WQ22 -NG22 = GK242*WQ12 + GK252*WQ22 -NG32 = GK342*WQ12 + GK352*WQ22 -NG42 = GK442*WQ12 + GK452*WQ22 -NG52 = GK542*WQ12 + GK552*WQ22 -NG62 = GK642*WQ12 + GK652*WQ22 -VS12 = GW2*JU12 + NG12 -VS22 = GW2*JU22 + NG22 -VS32 = GW2*JU32 + NG32 -VS42 = GW2*JU42 + NG42 -VS52 = GW2*JU52 + NG52 -VS62 = GW2*JU62 + NG62 -AP12 = -VBE12 + VS12 -AP22 = -VBE22 + VS22 -AP32 = -VBE32 + VS32 -AP42 = -VBE42 + VS42 -AP52 = -VBE52 + VS52 -AP62 = -VBE62 + VS62 -GX112 = C2*GK112 - GK212*S2 -GX312 = C2*GK212 + GK112*S2 -GX412 = C2*GK412 - GK512*S2 -GX612 = C2*GK512 + GK412*S2 -GX122 = C2*GK122 - GK222*S2 -GX322 = C2*GK222 + GK122*S2 -GX422 = C2*GK422 - GK522*S2 -GX622 = C2*GK522 + GK422*S2 -GX132 = C2*GK132 - GK232*S2 -GX332 = C2*GK232 + GK132*S2 -GX432 = C2*GK432 - GK532*S2 -GX632 = C2*GK532 + GK432*S2 -GX142 = C2*GK142 - GK242*S2 -GX342 = C2*GK242 + GK142*S2 -GX442 = C2*GK442 - GK542*S2 -GX642 = C2*GK542 + GK442*S2 -GX152 = C2*GK152 - GK252*S2 -GX352 = C2*GK252 + GK152*S2 -GX452 = C2*GK452 - GK552*S2 -GX652 = C2*GK552 + GK452*S2 -GX162 = C2*GK162 - GK262*S2 -GX362 = C2*GK262 + GK162*S2 -GX462 = C2*GK462 - GK562*S2 -GX662 = C2*GK562 + GK462*S2 -TKT112 = C2*GX112 - GX122*S2 -TKT132 = C2*GX122 + GX112*S2 -TKT232 = -C2*GK322 - GK312*S2 -TKT332 = C2*GX322 + GX312*S2 -TKT142 = C2*GX142 - GX152*S2 -TKT242 = -C2*GK342 + GK352*S2 -TKT342 = C2*GX342 - GX352*S2 -TKT442 = C2*GX442 - GX452*S2 -TKT162 = C2*GX152 + GX142*S2 -TKT262 = -C2*GK352 - GK342*S2 -TKT362 = C2*GX352 + GX342*S2 -TKT462 = C2*GX452 + GX442*S2 -TKT562 = -C2*GK652 - GK642*S2 -TKT662 = C2*GX652 + GX642*S2 -MJE111 = M1 + TKT112 -MJE221 = GK332 + M1 -MJE331 = M1 + TKT332 -MJE241 = -MZ1 + TKT242 -MJE341 = MY1 + TKT342 -MJE441 = TKT442 + XX1 -MJE151 = -GX162 + MZ1 -MJE351 = -GX362 - MX1 -MJE451 = -GX462 + XY1 -MJE551 = GK662 + YY1 -MJE161 = -MY1 + TKT162 -MJE261 = MX1 + TKT262 -MJE461 = TKT462 + XZ1 -MJE561 = TKT562 + YZ1 -MJE661 = TKT662 + ZZ1 -VBE11 = -AP12*C2 + AP22*S2 - SW11 -VBE21 = AP32 - SW21 -VBE31 = -AP12*S2 - AP22*C2 -VBE41 = -AP42*C2 + AP52*S2 - KW11 -VBE51 = AP62 - KW21 -VBE61 = -AP42*S2 - AP52*C2 -JD1 = 1/(IA1 + MJE661) -JU11 = JD1*MJE161 -JU21 = JD1*MJE261 -JU31 = JD1*TKT362 -JU41 = JD1*MJE461 -JU51 = JD1*MJE561 -JU61 = JD1*MJE661 -GW1 = -FS1*sign(QP1) - FV1*QP1 + GAM1 + VBE61 -GK111 = -JU11*MJE161 + MJE111 -GK211 = -GX132 - JU21*MJE161 -GK311 = -JU31*MJE161 + TKT132 -GK411 = -JU41*MJE161 + TKT142 -GK511 = -GX162 - JU51*MJE161 + MZ1 -GK611 = -JU61*MJE161 - MY1 + TKT162 -GK121 = -GX132 - JU11*MJE261 -GK221 = -JU21*MJE261 + MJE221 -GK321 = -JU31*MJE261 + TKT232 -GK421 = -JU41*MJE261 - MZ1 + TKT242 -GK521 = GK362 - JU51*MJE261 -GK621 = -JU61*MJE261 + MX1 + TKT262 -GK131 = -JU11*TKT362 + TKT132 -GK231 = -JU21*TKT362 + TKT232 -GK331 = -JU31*TKT362 + MJE331 -GK431 = -JU41*TKT362 + MY1 + TKT342 -GK531 = -GX362 - JU51*TKT362 - MX1 -GK631 = -JU61*TKT362 + TKT362 -GK141 = -JU11*MJE461 + TKT142 -GK241 = -JU21*MJE461 + MJE241 -GK341 = -JU31*MJE461 + MJE341 -GK441 = -JU41*MJE461 + MJE441 -GK541 = -GX462 - JU51*MJE461 + XY1 -GK641 = -JU61*MJE461 + TKT462 + XZ1 -GK151 = -JU11*MJE561 + MJE151 -GK251 = GK362 - JU21*MJE561 -GK351 = -JU31*MJE561 + MJE351 -GK451 = -JU41*MJE561 + MJE451 -GK551 = -JU51*MJE561 + MJE551 -GK651 = -JU61*MJE561 + TKT562 + YZ1 -GK161 = -JU11*MJE661 + MJE161 -GK261 = -JU21*MJE661 + MJE261 -GK361 = -JU31*MJE661 + TKT362 -GK461 = -JU41*MJE661 + MJE461 -GK561 = -JU51*MJE661 + MJE561 -GK661 = -JU61*MJE661 + MJE661 -VS11 = GW1*JU11 -VS21 = GW1*JU21 -VS31 = GW1*JU31 -VS41 = GW1*JU41 -VS51 = GW1*JU51 -VS61 = GW1*JU61 -AP11 = -VBE11 + VS11 -AP21 = -VBE21 + VS21 -AP31 = -VBE31 + VS31 -AP41 = -VBE41 + VS41 -AP51 = -VBE51 + VS51 -AP61 = -VBE61 + VS61 -GX111 = C1*GK111 - GK211*S1 -GX211 = C1*GK211 + GK111*S1 -GX411 = C1*GK411 - GK511*S1 -GX511 = C1*GK511 + GK411*S1 -GX121 = C1*GK121 - GK221*S1 -GX221 = C1*GK221 + GK121*S1 -GX421 = C1*GK421 - GK521*S1 -GX521 = C1*GK521 + GK421*S1 -GX131 = C1*GK131 - GK231*S1 -GX231 = C1*GK231 + GK131*S1 -GX431 = C1*GK431 - GK531*S1 -GX531 = C1*GK531 + GK431*S1 -GX141 = C1*GK141 - GK241*S1 -GX241 = C1*GK241 + GK141*S1 -GX441 = C1*GK441 - GK541*S1 -GX541 = C1*GK541 + GK441*S1 -GX151 = C1*GK151 - GK251*S1 -GX251 = C1*GK251 + GK151*S1 -GX451 = C1*GK451 - GK551*S1 -GX551 = C1*GK551 + GK451*S1 -GX161 = C1*GK161 - GK261*S1 -GX261 = C1*GK261 + GK161*S1 -GX461 = C1*GK461 - GK561*S1 -GX561 = C1*GK561 + GK461*S1 -TKT111 = C1*GX111 - GX121*S1 -TKT121 = C1*GX121 + GX111*S1 -TKT221 = C1*GX221 + GX211*S1 -TKT141 = C1*GX141 - GX151*S1 -TKT241 = C1*GX241 - GX251*S1 -TKT341 = C1*GK341 - GK351*S1 -TKT441 = C1*GX441 - GX451*S1 -TKT151 = C1*GX151 + GX141*S1 -TKT251 = C1*GX251 + GX241*S1 -TKT351 = C1*GK351 + GK341*S1 -TKT451 = C1*GX451 + GX441*S1 -TKT551 = C1*GX551 + GX541*S1 -QDP1 = GW1*JD1 -VR42 = QDP1*S2 + WQ12 -VR52 = C2*QDP1 + WQ22 -GU2 = JU42*VR42 + JU52*VR52 -QDP2 = -GU2 + GW2*JD2 -VR13 = JPR133*QDP2 + LW13 -VR23 = JPR233*QDP2 + LW23 -VR33 = -D3*VR52 + LW33 -VR43 = C3*VR42 + S3*VR52 + WQ13 -VR53 = C3*VR52 - S3*VR42 + WQ23 -GU3 = JU13*VR13 + JU23*VR23 + JU33*VR33 + JU43*VR43 + JU53*VR53 + JU63*QDP2 -QDP3 = -GU3 + GW3*JD3 -WP33 = QDP2 + QDP3 -VR14 = C4*VR13 + JPR114*VR43 + JPR214*WP33 + LW14 - S4*VR33 -VR24 = -C4*VR33 - JPR114*WP33 + JPR214*VR43 + LW24 - S4*VR13 -VR34 = LW34 + VR23 -VR44 = C4*VR43 - S4*WP33 + WQ14 -VR54 = -C4*WP33 - S4*VR43 + WQ24 -GU4 = JU14*VR14 + JU24*VR24 + JU34*VR34 + JU44*VR44 + JU54*VR54 + JU64*VR53 -QDP4 = -GU4 + GW4*JD4 -WP34 = QDP4 + VR53 -VR15 = C5*VR14 + S5*VR34 -VR25 = C5*VR34 - S5*VR14 -VR45 = C5*VR44 + S5*WP34 + WQ15 -VR55 = C5*WP34 - S5*VR44 + WQ25 -GU5 = JU15*VR15 + JU25*VR25 - JU35*VR24 + JU45*VR45 + JU55*VR55 - JU65*VR54 -QDP5 = -GU5 + GW5*JD5 -WP35 = QDP5 - VR54 -VR16 = C6*VR15 + S6*VR24 -VR26 = C6*VR24 - S6*VR15 -VR46 = C6*VR45 - S6*WP35 + WQ16 -VR56 = -C6*WP35 - S6*VR45 + WQ26 -GU6 = JU16*VR16 + JU26*VR26 + JU46*VR46 + JU56*VR56 + JU66*VR55 -QDP6 = -GU6 + GW6*JD6 -WP36 = QDP6 + VR55 -DY11 = MJE161*QDP1 -DY21 = MJE261*QDP1 -DY31 = QDP1*TKT362 -DY41 = MJE461*QDP1 -DY51 = MJE561*QDP1 -DY61 = MJE661*QDP1 -N11 = DY41 - VBE41 -N21 = DY51 - VBE51 -N31 = DY61 - VBE61 -E11 = DY11 - VBE11 -E21 = DY21 - VBE21 -E31 = DY31 - VBE31 -DY12 = MJE152*VR52 + MJE162*QDP2 + TKT143*VR42 -DY22 = MJE242*VR42 + MJE262*QDP2 + TKT253*VR52 -DY32 = MJE342*VR42 + MJE352*VR52 + QDP2*TKT363 -DY42 = MJE442*VR42 + MJE452*VR52 + MJE462*QDP2 -DY52 = MJE552*VR52 + MJE562*QDP2 + VR42*(TKT453 + XY2) -DY62 = MJE662*QDP2 + VR42*(TKT463 + XZ2) + VR52*(TKT563 + YZ2) -N12 = DY42 - VBE42 -N22 = DY52 - VBE52 -N32 = DY62 - VBE62 -E12 = DY12 - VBE12 -E22 = DY22 - VBE22 -E32 = DY32 - VBE32 -DY13 = GX134*VR23 + MJE113*VR13 + MJE153*VR53 + MJE163*WP33 + TKT134*VR33 + TKT144*VR43 -DY23 = GK364*VR53 + GX134*VR13 + MJE223*VR23 + MJE243*VR43 + MJE263*WP33 + TKT234*VR33 -DY33 = MJE333*VR33 + MJE343*VR43 + MJE353*VR53 + TKT134*VR13 + TKT234*VR23 + TKT364*WP33 -DY43 = MJE443*VR43 + MJE453*VR53 + MJE463*WP33 + TKT144*VR13 + VR23*(-MZ3 + TKT244) + VR33*(MY3 + TKT344) -DY53 = GK364*VR23 + MJE553*VR53 + MJE563*WP33 + VR13*(GX164 + MZ3) + VR33*(GX364 - MX3) + VR43*(GX464 + XY3) -DY63 = MJE663*WP33 + TKT364*VR33 + VR13*(-MY3 + TKT164) + VR23*(MX3 + TKT264) + VR43*(TKT464 + XZ3) + VR53*(TKT564 + YZ3) -N13 = DY43 - VBE43 -N23 = DY53 - VBE53 -N33 = DY63 - VBE63 -E13 = DY13 - VBE13 -E23 = DY23 - VBE23 -E33 = DY33 - VBE33 -DY14 = -GX135*VR24 + MJE114*VR14 + MJE154*VR54 + MJE164*WP34 + TKT135*VR34 + TKT145*VR44 -DY24 = GK365*VR54 - GX135*VR14 + MJE224*VR24 + MJE244*VR44 + MJE264*WP34 + TKT235*VR34 -DY34 = MJE334*VR34 + MJE344*VR44 + MJE354*VR54 + TKT135*VR14 + TKT235*VR24 + TKT365*WP34 -DY44 = MJE444*VR44 + MJE454*VR54 + MJE464*WP34 + TKT145*VR14 + VR24*(-MZ4 + TKT245) + VR34*(MY4 + TKT345) -DY54 = GK365*VR24 + MJE554*VR54 + MJE564*WP34 + VR14*(-GX165 + MZ4) + VR34*(-GX365 - MX4) + VR44*(-GX465 + XY4) -DY64 = MJE664*WP34 + TKT365*VR34 + VR14*(-MY4 + TKT165) + VR24*(MX4 + TKT265) + VR44*(TKT465 + XZ4) + VR54*(TKT565 + YZ4) -N14 = DY44 - VBE44 -N24 = DY54 - VBE54 -N34 = DY64 - VBE64 -E14 = DY14 - VBE14 -E24 = DY24 - VBE24 -E34 = DY34 - VBE34 -DY15 = MJE115*VR15 + MJE155*VR55 + MJE165*WP35 - TKT136*VR24 + TKT146*VR45 -DY25 = MJE225*VR25 + MJE245*VR45 + MJE265*WP35 -DY35 = -MJE335*VR24 + MJE345*VR45 + MJE355*VR55 + TKT136*VR15 + TKT366*WP35 -DY45 = MJE445*VR45 + MJE455*VR55 + MJE465*WP35 + TKT146*VR15 - VR24*(MY5 + TKT346) + VR25*(GX436 - MZ5) -DY55 = MJE555*VR55 + MJE565*WP35 + VR15*(GX166 + MZ5) - VR24*(GX366 - MX5) + VR45*(GX466 + XY5) -DY65 = MJE665*WP35 - TKT366*VR24 + VR15*(-MY5 + TKT166) + VR25*(GX636 + MX5) + VR45*(TKT466 + XZ5) + VR55*(TKT566 + YZ5) -N15 = DY45 - VBE45 -N25 = DY55 - VBE55 -N35 = DY65 - VBE65 -E15 = DY15 - VBE15 -E25 = DY25 - VBE25 -E35 = DY35 - VBE35 -DY16 = M6*VR16 - MY6*WP36 + MZ6*VR56 -DY26 = M6*VR26 + MX6*WP36 - MZ6*VR46 -DY36 = M6*VR25 - MX6*VR56 + MY6*VR46 -DY46 = MY6*VR25 - MZ6*VR26 + VR46*XX6 + VR56*XY6 + WP36*XZ6 -DY56 = -MX6*VR25 + MZ6*VR16 + VR46*XY6 + VR56*YY6 + WP36*YZ6 -DY66 = MX6*VR26 - MY6*VR16 + VR46*XZ6 + VR56*YZ6 + WP36*ZZ6 -N16 = DY46 - VBE46 -N26 = DY56 - VBE56 -N36 = DY66 - VBE66 -E16 = DY16 - VBE16 -E26 = DY26 - VBE26 -E36 = DY36 - VBE36 -*=* diff --git a/robots/RX90/RX90_dim.txt b/robots/RX90/RX90_dim.txt deleted file mode 100644 index e1690ed..0000000 --- a/robots/RX90/RX90_dim.txt +++ /dev/null @@ -1,237 +0,0 @@ -Dynamic identification model using Newton - Euler Algorith - -Geometric parameters -j ant sigma mu gamma b alpha d theta r -1 0 0 1 0 0 0 0 th1 0 -2 1 0 1 0 0 pi/2 0 th2 0 -3 2 0 1 0 0 0 D3 th3 0 -4 3 0 1 0 0 -pi/2 0 th4 RL4 -5 4 0 1 0 0 pi/2 0 th5 0 -6 5 0 1 0 0 -pi/2 0 th6 0 - -Dynamic inertia parameters -j XX XY XZ YY YZ ZZ MX MY MZ M IA -1 XX1 XY1 XZ1 YY1 YZ1 ZZ1 MX1 MY1 MZ1 M1 IA1 -2 XX2 XY2 XZ2 YY2 YZ2 ZZ2 MX2 MY2 MZ2 M2 IA2 -3 XX3 XY3 XZ3 YY3 YZ3 ZZ3 MX3 MY3 MZ3 M3 IA3 -4 XX4 XY4 XZ4 YY4 YZ4 ZZ4 MX4 MY4 MZ4 M4 IA4 -5 XX5 XY5 XZ5 YY5 YZ5 ZZ5 MX5 MY5 MZ5 M5 IA5 -6 XX6 XY6 XZ6 YY6 YZ6 ZZ6 MX6 MY6 MZ6 M6 IA6 - -External forces and joint parameters -j FX FY FZ CX CY CZ FS FV QP QDP GAM -1 0 0 0 0 0 0 FS1 FV1 QP1 QDP1 GAM1 -2 0 0 0 0 0 0 FS2 FV2 QP2 QDP2 GAM2 -3 0 0 0 0 0 0 FS3 FV3 QP3 QDP3 GAM3 -4 0 0 0 0 0 0 FS4 FV4 QP4 QDP4 GAM4 -5 0 0 0 0 0 0 FS5 FV5 QP5 QDP5 GAM5 -6 FX6 FY6 FZ6 CX6 CY6 CZ6 FS6 FV6 QP6 QDP6 GAM6 - -Base velicities parameters -axis W0 WP0 V0 VP0 G -X 0 0 0 0 0 -Y 0 0 0 0 0 -Z 0 0 0 0 G3 - -Equations: -C1 = cos(th1) -S1 = sin(th1) -C2 = cos(th2) -S2 = sin(th2) -C3 = cos(th3) -S3 = sin(th3) -C4 = cos(th4) -S4 = sin(th4) -C5 = cos(th5) -S5 = sin(th5) -C6 = cos(th6) -S6 = sin(th6) -DV61 = QP1**2 -WI12 = QP1*S2 -WI22 = C2*QP1 -WP12 = QDP1*S2 + QP2*WI22 -WP22 = C2*QDP1 - QP2*WI12 -DV12 = WI12**2 -DV22 = WI12*WI22 -DV32 = QP2*WI12 -DV42 = WI22**2 -DV52 = QP2*WI22 -DV62 = QP2**2 -U112 = -DV42 - DV62 -U212 = DV22 + QDP2 -U312 = DV32 - WP22 -U122 = DV22 - QDP2 -U222 = -DV12 - DV62 -U322 = DV52 + WP12 -U132 = DV32 + WP22 -U232 = DV52 - WP12 -U332 = -DV12 - DV42 -WI13 = C3*WI12 + S3*WI22 -WI23 = C3*WI22 - S3*WI12 -W33 = QP2 + QP3 -WP13 = C3*WP12 + QP3*WI23 + S3*WP22 -WP23 = C3*WP22 - QP3*WI13 - S3*WP12 -WP33 = QDP2 + QDP3 -DV13 = WI13**2 -DV23 = WI13*WI23 -DV33 = W33*WI13 -DV43 = WI23**2 -DV53 = W33*WI23 -DV63 = W33**2 -U113 = -DV43 - DV63 -U213 = DV23 + WP33 -U313 = DV33 - WP23 -U123 = DV23 - WP33 -U223 = -DV13 - DV63 -U323 = DV53 + WP13 -U133 = DV33 + WP23 -U233 = DV53 - WP13 -U333 = -DV13 - DV43 -VSP13 = D3*U112 -VSP23 = D3*U212 -VSP33 = D3*U312 -VP13 = C3*VSP13 + S3*VSP23 -VP23 = C3*VSP23 - S3*VSP13 -WI14 = C4*WI13 - S4*W33 -WI24 = -C4*W33 - S4*WI13 -W34 = QP4 + WI23 -WP14 = C4*WP13 + QP4*WI24 - S4*WP33 -WP24 = -C4*WP33 - QP4*WI14 - S4*WP13 -WP34 = QDP4 + WP23 -DV14 = WI14**2 -DV24 = WI14*WI24 -DV34 = W34*WI14 -DV44 = WI24**2 -DV54 = W34*WI24 -DV64 = W34**2 -U114 = -DV44 - DV64 -U214 = DV24 + WP34 -U314 = DV34 - WP24 -U124 = DV24 - WP34 -U224 = -DV14 - DV64 -U324 = DV54 + WP14 -U134 = DV34 + WP24 -U234 = DV54 - WP14 -U334 = -DV14 - DV44 -VSP14 = RL4*U123 + VP13 -VSP24 = RL4*U223 + VP23 -VSP34 = RL4*U323 + VSP33 -VP14 = C4*VSP14 - S4*VSP34 -VP24 = -C4*VSP34 - S4*VSP14 -WI15 = C5*WI14 + S5*W34 -WI25 = C5*W34 - S5*WI14 -W35 = QP5 - WI24 -WP15 = C5*WP14 + QP5*WI25 + S5*WP34 -WP25 = C5*WP34 - QP5*WI15 - S5*WP14 -WP35 = QDP5 - WP24 -DV15 = WI15**2 -DV25 = WI15*WI25 -DV35 = W35*WI15 -DV45 = WI25**2 -DV55 = W35*WI25 -DV65 = W35**2 -U115 = -DV45 - DV65 -U215 = DV25 + WP35 -U315 = DV35 - WP25 -U125 = DV25 - WP35 -U225 = -DV15 - DV65 -U325 = DV55 + WP15 -U135 = DV35 + WP25 -U235 = DV55 - WP15 -U335 = -DV15 - DV45 -VP15 = C5*VP14 + S5*VSP24 -VP25 = C5*VSP24 - S5*VP14 -WI16 = C6*WI15 - S6*W35 -WI26 = -C6*W35 - S6*WI15 -W36 = QP6 + WI25 -WP16 = C6*WP15 + QP6*WI26 - S6*WP35 -WP26 = -C6*WP35 - QP6*WI16 - S6*WP15 -WP36 = QDP6 + WP25 -DV16 = WI16**2 -DV26 = WI16*WI26 -DV36 = W36*WI16 -DV46 = WI26**2 -DV56 = W36*WI26 -DV66 = W36**2 -U116 = -DV46 - DV66 -U216 = DV26 + WP36 -U316 = DV36 - WP26 -U126 = DV26 - WP36 -U226 = -DV16 - DV66 -U326 = DV56 + WP16 -U136 = DV36 + WP26 -U236 = DV56 - WP16 -U336 = -DV16 - DV46 -VP16 = C6*VP15 + S6*VP24 -VP26 = C6*VP24 - S6*VP15 -DG1 = 0 -N10 = C1*QDP1 - DV61*S1 -N20 = C1*DV61 + QDP1*S1 -DG1IA1 = QDP1 -DG1FS1 = sign(QP1) -DG1FV1 = QP1 -N11 = C2*WP12 - DV32*S2 -N31 = C2*DV32 + S2*WP12 -DG2 = -DV22 -No12 = -QP2*WI12 + WP22 -No22 = QP2*WI22 + WP12 -No32 = WI12**2 - WI22**2 -FDI12 = C2*U112 - S2*U212 -FDI32 = C2*U212 + S2*U112 -FDI11 = C1*FDI12 + S1*U312 -FDI21 = -C1*U312 + FDI12*S1 -DG2IA2 = QDP2 -DG2FS2 = sign(QP2) -DG2FV2 = QP2 -N12 = C3*WP13 - DV33*S3 -N22 = C3*DV33 + S3*WP13 -DG3 = -DV23 -No13 = -W33*WI13 + WP23 -No23 = W33*WI23 + WP13 -No33 = WI13**2 - WI23**2 -FDI13 = C3*U113 - S3*U213 -FDI23 = C3*U213 + S3*U113 -N32 = D3*FDI23 + VP23 -DG3IA3 = QDP3 -DG3FS3 = sign(QP3) -DG3FV3 = QP3 -N13 = C4*WP14 - DV34*S4 -N33 = -C4*DV34 - S4*WP14 -DG4 = -DV24 -No14 = -W34*WI14 + WP24 -No24 = W34*WI24 + WP14 -No34 = WI14**2 - WI24**2 -FDI14 = C4*U114 - S4*U214 -FDI34 = -C4*U214 - S4*U114 -DG4IA4 = QDP4 -DG4FS4 = sign(QP4) -DG4FV4 = QP4 -N14 = C5*WP15 - DV35*S5 -N34 = C5*DV35 + S5*WP15 -DG5 = -DV25 -No15 = -W35*WI15 + WP25 -No25 = W35*WI25 + WP15 -No35 = WI15**2 - WI25**2 -FDI15 = C5*U115 - S5*U215 -FDI35 = C5*U215 + S5*U115 -DG5IA5 = QDP5 -DG5FS5 = sign(QP5) -DG5FV5 = QP5 -N16 = CX6 + WP16 -N26 = CY6 + DV36 -N36 = CZ6 - DV26 -FDI16 = C6*FX6 - FY6*S6 -FDI36 = -C6*FY6 - FX6*S6 -N15 = C6*N16 - N26*S6 -N35 = -C6*N26 - N16*S6 -DG6 = N36 -No16 = -W36*WI16 + WP26 -No26 = W36*WI26 + WP16 -No36 = WI16**2 - WI26**2 -E16 = FX6 + U116 -E26 = FY6 + U216 -E36 = FZ6 + U316 -DG6IA6 = QDP6 -DG6FS6 = sign(QP6) -DG6FV6 = QP6 -*=* diff --git a/robots/RX90/RX90_idm.txt b/robots/RX90/RX90_idm.txt deleted file mode 100644 index c943f1b..0000000 --- a/robots/RX90/RX90_idm.txt +++ /dev/null @@ -1,273 +0,0 @@ -Inverse dynamic model using Newton - Euler Algorith - -Geometric parameters -j ant sigma mu gamma b alpha d theta r -1 0 0 1 0 0 0 0 th1 0 -2 1 0 1 0 0 pi/2 0 th2 0 -3 2 0 1 0 0 0 D3 th3 0 -4 3 0 1 0 0 -pi/2 0 th4 RL4 -5 4 0 1 0 0 pi/2 0 th5 0 -6 5 0 1 0 0 -pi/2 0 th6 0 - -Dynamic inertia parameters -j XX XY XZ YY YZ ZZ MX MY MZ M IA -1 XX1 XY1 XZ1 YY1 YZ1 ZZ1 MX1 MY1 MZ1 M1 IA1 -2 XX2 XY2 XZ2 YY2 YZ2 ZZ2 MX2 MY2 MZ2 M2 IA2 -3 XX3 XY3 XZ3 YY3 YZ3 ZZ3 MX3 MY3 MZ3 M3 IA3 -4 XX4 XY4 XZ4 YY4 YZ4 ZZ4 MX4 MY4 MZ4 M4 IA4 -5 XX5 XY5 XZ5 YY5 YZ5 ZZ5 MX5 MY5 MZ5 M5 IA5 -6 XX6 XY6 XZ6 YY6 YZ6 ZZ6 MX6 MY6 MZ6 M6 IA6 - -External forces and joint parameters -j FX FY FZ CX CY CZ FS FV QP QDP GAM -1 0 0 0 0 0 0 FS1 FV1 QP1 QDP1 GAM1 -2 0 0 0 0 0 0 FS2 FV2 QP2 QDP2 GAM2 -3 0 0 0 0 0 0 FS3 FV3 QP3 QDP3 GAM3 -4 0 0 0 0 0 0 FS4 FV4 QP4 QDP4 GAM4 -5 0 0 0 0 0 0 FS5 FV5 QP5 QDP5 GAM5 -6 FX6 FY6 FZ6 CX6 CY6 CZ6 FS6 FV6 QP6 QDP6 GAM6 - -Base velicities parameters -axis W0 WP0 V0 VP0 G -X 0 0 0 0 0 -Y 0 0 0 0 0 -Z 0 0 0 0 G3 - -Equations: -C1 = cos(th1) -S1 = sin(th1) -C2 = cos(th2) -S2 = sin(th2) -C3 = cos(th3) -S3 = sin(th3) -C4 = cos(th4) -S4 = sin(th4) -C5 = cos(th5) -S5 = sin(th5) -C6 = cos(th6) -S6 = sin(th6) -DV61 = QP1**2 -WI12 = QP1*S2 -WI22 = C2*QP1 -WP12 = QDP1*S2 + QP2*WI22 -WP22 = C2*QDP1 - QP2*WI12 -DV12 = WI12**2 -DV22 = WI12*WI22 -DV32 = QP2*WI12 -DV42 = WI22**2 -DV52 = QP2*WI22 -DV62 = QP2**2 -U112 = -DV42 - DV62 -U212 = DV22 + QDP2 -U312 = DV32 - WP22 -U122 = DV22 - QDP2 -U222 = -DV12 - DV62 -U322 = DV52 + WP12 -U132 = DV32 + WP22 -U232 = DV52 - WP12 -U332 = -DV12 - DV42 -WI13 = C3*WI12 + S3*WI22 -WI23 = C3*WI22 - S3*WI12 -W33 = QP2 + QP3 -WP13 = C3*WP12 + QP3*WI23 + S3*WP22 -WP23 = C3*WP22 - QP3*WI13 - S3*WP12 -WP33 = QDP2 + QDP3 -DV13 = WI13**2 -DV23 = WI13*WI23 -DV33 = W33*WI13 -DV43 = WI23**2 -DV53 = W33*WI23 -DV63 = W33**2 -U113 = -DV43 - DV63 -U213 = DV23 + WP33 -U313 = DV33 - WP23 -U123 = DV23 - WP33 -U223 = -DV13 - DV63 -U323 = DV53 + WP13 -U133 = DV33 + WP23 -U233 = DV53 - WP13 -U333 = -DV13 - DV43 -VSP13 = D3*U112 -VSP23 = D3*U212 -VSP33 = D3*U312 -VP13 = C3*VSP13 + S3*VSP23 -VP23 = C3*VSP23 - S3*VSP13 -WI14 = C4*WI13 - S4*W33 -WI24 = -C4*W33 - S4*WI13 -W34 = QP4 + WI23 -WP14 = C4*WP13 + QP4*WI24 - S4*WP33 -WP24 = -C4*WP33 - QP4*WI14 - S4*WP13 -WP34 = QDP4 + WP23 -DV14 = WI14**2 -DV24 = WI14*WI24 -DV34 = W34*WI14 -DV44 = WI24**2 -DV54 = W34*WI24 -DV64 = W34**2 -U114 = -DV44 - DV64 -U214 = DV24 + WP34 -U314 = DV34 - WP24 -U124 = DV24 - WP34 -U224 = -DV14 - DV64 -U324 = DV54 + WP14 -U134 = DV34 + WP24 -U234 = DV54 - WP14 -U334 = -DV14 - DV44 -VSP14 = RL4*U123 + VP13 -VSP24 = RL4*U223 + VP23 -VSP34 = RL4*U323 + VSP33 -VP14 = C4*VSP14 - S4*VSP34 -VP24 = -C4*VSP34 - S4*VSP14 -WI15 = C5*WI14 + S5*W34 -WI25 = C5*W34 - S5*WI14 -W35 = QP5 - WI24 -WP15 = C5*WP14 + QP5*WI25 + S5*WP34 -WP25 = C5*WP34 - QP5*WI15 - S5*WP14 -WP35 = QDP5 - WP24 -DV15 = WI15**2 -DV25 = WI15*WI25 -DV35 = W35*WI15 -DV45 = WI25**2 -DV55 = W35*WI25 -DV65 = W35**2 -U115 = -DV45 - DV65 -U215 = DV25 + WP35 -U315 = DV35 - WP25 -U125 = DV25 - WP35 -U225 = -DV15 - DV65 -U325 = DV55 + WP15 -U135 = DV35 + WP25 -U235 = DV55 - WP15 -U335 = -DV15 - DV45 -VP15 = C5*VP14 + S5*VSP24 -VP25 = C5*VSP24 - S5*VP14 -WI16 = C6*WI15 - S6*W35 -WI26 = -C6*W35 - S6*WI15 -W36 = QP6 + WI25 -WP16 = C6*WP15 + QP6*WI26 - S6*WP35 -WP26 = -C6*WP35 - QP6*WI16 - S6*WP15 -WP36 = QDP6 + WP25 -DV16 = WI16**2 -DV26 = WI16*WI26 -DV36 = W36*WI16 -DV46 = WI26**2 -DV56 = W36*WI26 -DV66 = W36**2 -U116 = -DV46 - DV66 -U216 = DV26 + WP36 -U316 = DV36 - WP26 -U126 = DV26 - WP36 -U226 = -DV16 - DV66 -U326 = DV56 + WP16 -U136 = DV36 + WP26 -U236 = DV56 - WP16 -U336 = -DV16 - DV46 -VP16 = C6*VP15 + S6*VP24 -VP26 = C6*VP24 - S6*VP15 -F11 = -DV61*MX1 - MY1*QDP1 -F21 = -DV61*MY1 + MX1*QDP1 -PSI11 = QP1*XZ1 -PSI21 = QP1*YZ1 -PSI31 = QP1*ZZ1 -No11 = -PSI21*QP1 + QDP1*XZ1 -No21 = PSI11*QP1 + QDP1*YZ1 -No31 = QDP1*ZZ1 -F12 = MX2*U112 + MY2*U122 + MZ2*U132 -F22 = MX2*U212 + MY2*U222 + MZ2*U232 -F32 = MX2*U312 + MY2*U322 + MZ2*U332 -PSI12 = QP2*XZ2 + WI12*XX2 + WI22*XY2 -PSI22 = QP2*YZ2 + WI12*XY2 + WI22*YY2 -PSI32 = QP2*ZZ2 + WI12*XZ2 + WI22*YZ2 -No12 = -PSI22*QP2 + PSI32*WI22 + QDP2*XZ2 + WP12*XX2 + WP22*XY2 -No22 = PSI12*QP2 - PSI32*WI12 + QDP2*YZ2 + WP12*XY2 + WP22*YY2 -No32 = -PSI12*WI22 + PSI22*WI12 + QDP2*ZZ2 + WP12*XZ2 + WP22*YZ2 -F13 = M3*VP13 + MX3*U113 + MY3*U123 + MZ3*U133 -F23 = M3*VP23 + MX3*U213 + MY3*U223 + MZ3*U233 -F33 = M3*VSP33 + MX3*U313 + MY3*U323 + MZ3*U333 -PSI13 = W33*XZ3 + WI13*XX3 + WI23*XY3 -PSI23 = W33*YZ3 + WI13*XY3 + WI23*YY3 -PSI33 = W33*ZZ3 + WI13*XZ3 + WI23*YZ3 -No13 = -PSI23*W33 + PSI33*WI23 + WP13*XX3 + WP23*XY3 + WP33*XZ3 -No23 = PSI13*W33 - PSI33*WI13 + WP13*XY3 + WP23*YY3 + WP33*YZ3 -No33 = -PSI13*WI23 + PSI23*WI13 + WP13*XZ3 + WP23*YZ3 + WP33*ZZ3 -F14 = M4*VP14 + MX4*U114 + MY4*U124 + MZ4*U134 -F24 = M4*VP24 + MX4*U214 + MY4*U224 + MZ4*U234 -F34 = M4*VSP24 + MX4*U314 + MY4*U324 + MZ4*U334 -PSI14 = W34*XZ4 + WI14*XX4 + WI24*XY4 -PSI24 = W34*YZ4 + WI14*XY4 + WI24*YY4 -PSI34 = W34*ZZ4 + WI14*XZ4 + WI24*YZ4 -No14 = -PSI24*W34 + PSI34*WI24 + WP14*XX4 + WP24*XY4 + WP34*XZ4 -No24 = PSI14*W34 - PSI34*WI14 + WP14*XY4 + WP24*YY4 + WP34*YZ4 -No34 = -PSI14*WI24 + PSI24*WI14 + WP14*XZ4 + WP24*YZ4 + WP34*ZZ4 -F15 = M5*VP15 + MX5*U115 + MY5*U125 + MZ5*U135 -F25 = M5*VP25 + MX5*U215 + MY5*U225 + MZ5*U235 -F35 = -M5*VP24 + MX5*U315 + MY5*U325 + MZ5*U335 -PSI15 = W35*XZ5 + WI15*XX5 + WI25*XY5 -PSI25 = W35*YZ5 + WI15*XY5 + WI25*YY5 -PSI35 = W35*ZZ5 + WI15*XZ5 + WI25*YZ5 -No15 = -PSI25*W35 + PSI35*WI25 + WP15*XX5 + WP25*XY5 + WP35*XZ5 -No25 = PSI15*W35 - PSI35*WI15 + WP15*XY5 + WP25*YY5 + WP35*YZ5 -No35 = -PSI15*WI25 + PSI25*WI15 + WP15*XZ5 + WP25*YZ5 + WP35*ZZ5 -F16 = M6*VP16 + MX6*U116 + MY6*U126 + MZ6*U136 -F26 = M6*VP26 + MX6*U216 + MY6*U226 + MZ6*U236 -F36 = M6*VP25 + MX6*U316 + MY6*U326 + MZ6*U336 -PSI16 = W36*XZ6 + WI16*XX6 + WI26*XY6 -PSI26 = W36*YZ6 + WI16*XY6 + WI26*YY6 -PSI36 = W36*ZZ6 + WI16*XZ6 + WI26*YZ6 -No16 = -PSI26*W36 + PSI36*WI26 + WP16*XX6 + WP26*XY6 + WP36*XZ6 -No26 = PSI16*W36 - PSI36*WI16 + WP16*XY6 + WP26*YY6 + WP36*YZ6 -No36 = -PSI16*WI26 + PSI26*WI16 + WP16*XZ6 + WP26*YZ6 + WP36*ZZ6 -E16 = F16 + FX6 -E26 = F26 + FY6 -E36 = F36 + FZ6 -N16 = CX6 + MY6*VP25 - MZ6*VP26 + No16 -N26 = CY6 - MX6*VP25 + MZ6*VP16 + No26 -N36 = CZ6 + MX6*VP26 - MY6*VP16 + No36 -FDI16 = C6*E16 - E26*S6 -FDI36 = -C6*E26 - E16*S6 -E15 = F15 + FDI16 -E25 = E36 + F25 -E35 = F35 + FDI36 -N15 = C6*N16 - MY5*VP24 - MZ5*VP25 - N26*S6 + No15 -N25 = MX5*VP24 + MZ5*VP15 + N36 + No25 -N35 = -C6*N26 + MX5*VP25 - MY5*VP15 - N16*S6 + No35 -FDI15 = C5*E15 - E25*S5 -FDI35 = C5*E25 + E15*S5 -E14 = F14 + FDI15 -E24 = -E35 + F24 -E34 = F34 + FDI35 -N14 = C5*N15 + MY4*VSP24 - MZ4*VP24 - N25*S5 + No14 -N24 = -MX4*VSP24 + MZ4*VP14 - N35 + No24 -N34 = C5*N25 + MX4*VP24 - MY4*VP14 + N15*S5 + No34 -FDI14 = C4*E14 - E24*S4 -FDI34 = -C4*E24 - E14*S4 -E13 = F13 + FDI14 -E23 = E34 + F23 -E33 = F33 + FDI34 -N13 = C4*N14 + FDI34*RL4 + MY3*VSP33 - MZ3*VP23 - N24*S4 + No13 -N23 = -MX3*VSP33 + MZ3*VP13 + N34 + No23 -N33 = -C4*N24 - FDI14*RL4 + MX3*VP23 - MY3*VP13 - N14*S4 + No33 -FDI13 = C3*E13 - E23*S3 -FDI23 = C3*E23 + E13*S3 -E12 = F12 + FDI13 -E22 = F22 + FDI23 -E32 = E33 + F32 -N12 = C3*N13 - N23*S3 + No12 -N22 = C3*N23 - D3*E33 + N13*S3 + No22 -N32 = D3*FDI23 + N33 + No32 -FDI12 = C2*E12 - E22*S2 -FDI32 = C2*E22 + E12*S2 -E11 = F11 + FDI12 -E21 = -E32 + F21 -N11 = C2*N12 - N22*S2 + No11 -N21 = -N32 + No21 -N31 = C2*N22 + N12*S2 + No31 -FDI11 = C1*E11 - E21*S1 -FDI21 = C1*E21 + E11*S1 -GAM1 = FS1*sign(QP1) + FV1*QP1 + IA1*QDP1 + N31 -GAM2 = FS2*sign(QP2) + FV2*QP2 + IA2*QDP2 + N32 -GAM3 = FS3*sign(QP3) + FV3*QP3 + IA3*QDP3 + N33 -GAM4 = FS4*sign(QP4) + FV4*QP4 + IA4*QDP4 + N34 -GAM5 = FS5*sign(QP5) + FV5*QP5 + IA5*QDP5 + N35 -GAM6 = FS6*sign(QP6) + FV6*QP6 + IA6*QDP6 + N36 -*=* diff --git a/robots/RX90/RX90_inm.txt b/robots/RX90/RX90_inm.txt deleted file mode 100644 index dc3fa31..0000000 --- a/robots/RX90/RX90_inm.txt +++ /dev/null @@ -1,320 +0,0 @@ -Inertia Matrix using composite links - -Geometric parameters -j ant sigma mu gamma b alpha d theta r -1 0 0 1 0 0 0 0 th1 0 -2 1 0 1 0 0 pi/2 0 th2 0 -3 2 0 1 0 0 0 D3 th3 0 -4 3 0 1 0 0 -pi/2 0 th4 RL4 -5 4 0 1 0 0 pi/2 0 th5 0 -6 5 0 1 0 0 -pi/2 0 th6 0 - -Dynamic inertia parameters -j XX XY XZ YY YZ ZZ MX MY MZ M IA -1 XX1 XY1 XZ1 YY1 YZ1 ZZ1 MX1 MY1 MZ1 M1 IA1 -2 XX2 XY2 XZ2 YY2 YZ2 ZZ2 MX2 MY2 MZ2 M2 IA2 -3 XX3 XY3 XZ3 YY3 YZ3 ZZ3 MX3 MY3 MZ3 M3 IA3 -4 XX4 XY4 XZ4 YY4 YZ4 ZZ4 MX4 MY4 MZ4 M4 IA4 -5 XX5 XY5 XZ5 YY5 YZ5 ZZ5 MX5 MY5 MZ5 M5 IA5 -6 XX6 XY6 XZ6 YY6 YZ6 ZZ6 MX6 MY6 MZ6 M6 IA6 - -External forces and joint parameters -j FX FY FZ CX CY CZ FS FV QP QDP GAM -1 0 0 0 0 0 0 FS1 FV1 QP1 QDP1 GAM1 -2 0 0 0 0 0 0 FS2 FV2 QP2 QDP2 GAM2 -3 0 0 0 0 0 0 FS3 FV3 QP3 QDP3 GAM3 -4 0 0 0 0 0 0 FS4 FV4 QP4 QDP4 GAM4 -5 0 0 0 0 0 0 FS5 FV5 QP5 QDP5 GAM5 -6 FX6 FY6 FZ6 CX6 CY6 CZ6 FS6 FV6 QP6 QDP6 GAM6 - -Base velicities parameters -axis W0 WP0 V0 VP0 G -X 0 0 0 0 0 -Y 0 0 0 0 0 -Z 0 0 0 0 G3 - -Equations: -C1 = cos(th1) -S1 = sin(th1) -C2 = cos(th2) -S2 = sin(th2) -C3 = cos(th3) -S3 = sin(th3) -C4 = cos(th4) -S4 = sin(th4) -C5 = cos(th5) -S5 = sin(th5) -C6 = cos(th6) -S6 = sin(th6) -AS16 = C6*MX6 - MY6*S6 -AS36 = -C6*MY6 - MX6*S6 -AJ116 = C6*XX6 - S6*XY6 -AJ316 = -C6*XY6 - S6*XX6 -AJ126 = C6*XY6 - S6*YY6 -AJ326 = -C6*YY6 - S6*XY6 -AJ136 = C6*XZ6 - S6*YZ6 -AJ336 = -C6*YZ6 - S6*XZ6 -AJA116 = AJ116*C6 - AJ126*S6 -AJA316 = AJ316*C6 - AJ326*S6 -AJA136 = -AJ116*S6 - AJ126*C6 -AJA336 = -AJ316*S6 - AJ326*C6 -JP115 = AJA116 + XX5 -JP215 = AJ136 + XY5 -JP315 = AJA316 + XZ5 -JP225 = YY5 + ZZ6 -JP325 = AJ336 + YZ5 -JP135 = AJA136 + XZ5 -JP335 = AJA336 + ZZ5 -MSP15 = AS16 + MX5 -MSP25 = MY5 + MZ6 -MSP35 = AS36 + MZ5 -MP5 = M5 + M6 -AS15 = C5*MSP15 - MSP25*S5 -AS35 = C5*MSP25 + MSP15*S5 -AJ115 = C5*JP115 - JP215*S5 -AJ315 = C5*JP215 + JP115*S5 -AJ125 = C5*JP215 - JP225*S5 -AJ325 = C5*JP225 + JP215*S5 -AJ135 = C5*JP135 - JP325*S5 -AJ335 = C5*JP325 + JP135*S5 -AJA115 = AJ115*C5 - AJ125*S5 -AJA215 = -C5*JP315 + JP325*S5 -AJA315 = AJ315*C5 - AJ325*S5 -AJA135 = AJ115*S5 + AJ125*C5 -AJA235 = -C5*JP325 - JP315*S5 -AJA335 = AJ315*S5 + AJ325*C5 -JP114 = AJA115 + XX4 -JP214 = AJA215 + XY4 -JP314 = AJA315 + XZ4 -JP124 = -AJ135 + XY4 -JP224 = JP335 + YY4 -JP324 = -AJ335 + YZ4 -JP134 = AJA135 + XZ4 -JP234 = AJA235 + YZ4 -JP334 = AJA335 + ZZ4 -MSP14 = AS15 + MX4 -MSP24 = -MSP35 + MY4 -MSP34 = AS35 + MZ4 -MP4 = M4 + MP5 -AS14 = C4*MSP14 - MSP24*S4 -AS34 = -C4*MSP24 - MSP14*S4 -AJ114 = C4*JP114 - JP214*S4 -AJ314 = -C4*JP214 - JP114*S4 -AJ124 = C4*JP124 - JP224*S4 -AJ324 = -C4*JP224 - JP124*S4 -AJ134 = C4*JP134 - JP234*S4 -AJ334 = -C4*JP234 - JP134*S4 -AJA114 = AJ114*C4 - AJ124*S4 -AJA214 = C4*JP314 - JP324*S4 -AJA314 = AJ314*C4 - AJ324*S4 -AJA134 = -AJ114*S4 - AJ124*C4 -AJA234 = -C4*JP324 - JP314*S4 -AJA334 = -AJ314*S4 - AJ324*C4 -PAS114 = -MSP34*RL4 -PAS124 = AS14*RL4 -PAS324 = AS34*RL4 -JP113 = AJA114 + MP4*RL4**2 - 2*PAS114 + XX3 -JP213 = AJA214 - PAS124 + XY3 -JP313 = AJA314 + XZ3 -JP123 = AJ134 - PAS124 + XY3 -JP223 = JP334 + YY3 -JP323 = AJ334 - PAS324 + YZ3 -JP133 = AJA134 + XZ3 -JP233 = AJA234 - PAS324 + YZ3 -JP333 = AJA334 + MP4*RL4**2 - 2*PAS114 + ZZ3 -MSP13 = AS14 + MX3 -MSP23 = MP4*RL4 + MSP34 + MY3 -MSP33 = AS34 + MZ3 -MP3 = M3 + MP4 -AS13 = C3*MSP13 - MSP23*S3 -AS23 = C3*MSP23 + MSP13*S3 -AJ113 = C3*JP113 - JP213*S3 -AJ213 = C3*JP213 + JP113*S3 -AJ123 = C3*JP123 - JP223*S3 -AJ223 = C3*JP223 + JP123*S3 -AJ133 = C3*JP133 - JP233*S3 -AJ233 = C3*JP233 + JP133*S3 -AJA113 = AJ113*C3 - AJ123*S3 -AJA213 = AJ213*C3 - AJ223*S3 -AJA313 = C3*JP313 - JP323*S3 -AJA123 = AJ113*S3 + AJ123*C3 -AJA223 = AJ213*S3 + AJ223*C3 -AJA323 = C3*JP323 + JP313*S3 -PAS213 = AS23*D3 -PAS313 = D3*MSP33 -PAS223 = -AS13*D3 -JP112 = AJA113 + XX2 -JP212 = AJA213 - PAS213 + XY2 -JP312 = AJA313 - PAS313 + XZ2 -JP122 = AJA123 - PAS213 + XY2 -JP222 = AJA223 + D3**2*MP3 - 2*PAS223 + YY2 -JP322 = AJA323 + YZ2 -JP132 = AJ133 - PAS313 + XZ2 -JP232 = AJ233 + YZ2 -JP332 = D3**2*MP3 + JP333 - 2*PAS223 + ZZ2 -MSP12 = AS13 + D3*MP3 + MX2 -MSP22 = AS23 + MY2 -MSP32 = MSP33 + MZ2 -MP2 = M2 + MP3 -AS12 = C2*MSP12 - MSP22*S2 -AS32 = C2*MSP22 + MSP12*S2 -AJ112 = C2*JP112 - JP212*S2 -AJ312 = C2*JP212 + JP112*S2 -AJ122 = C2*JP122 - JP222*S2 -AJ322 = C2*JP222 + JP122*S2 -AJ132 = C2*JP132 - JP232*S2 -AJ332 = C2*JP232 + JP132*S2 -AJA112 = AJ112*C2 - AJ122*S2 -AJA212 = -C2*JP312 + JP322*S2 -AJA312 = AJ312*C2 - AJ322*S2 -AJA132 = AJ112*S2 + AJ122*C2 -AJA232 = -C2*JP322 - JP312*S2 -AJA332 = AJ312*S2 + AJ322*C2 -JP111 = AJA112 + XX1 -JP211 = AJA212 + XY1 -JP311 = AJA312 + XZ1 -JP121 = -AJ132 + XY1 -JP221 = JP332 + YY1 -JP321 = -AJ332 + YZ1 -JP131 = AJA132 + XZ1 -JP231 = AJA232 + YZ1 -JP331 = AJA332 + ZZ1 -MSP11 = AS12 + MX1 -MSP21 = -MSP32 + MY1 -MSP31 = AS32 + MZ1 -MP1 = M1 + MP2 -AS11 = C1*MSP11 - MSP21*S1 -AS21 = C1*MSP21 + MSP11*S1 -AJ111 = C1*JP111 - JP211*S1 -AJ211 = C1*JP211 + JP111*S1 -AJ121 = C1*JP121 - JP221*S1 -AJ221 = C1*JP221 + JP121*S1 -AJ131 = C1*JP131 - JP231*S1 -AJ231 = C1*JP231 + JP131*S1 -AJA111 = AJ111*C1 - AJ121*S1 -AJA211 = AJ211*C1 - AJ221*S1 -AJA311 = C1*JP311 - JP321*S1 -AJA121 = AJ111*S1 + AJ121*C1 -AJA221 = AJ211*S1 + AJ221*C1 -AJA321 = C1*JP321 + JP311*S1 -JP110 = AJA111 + XX0 -JP210 = AJA211 + XY0 -JP310 = AJA311 + XZ0 -JP120 = AJA121 + XY0 -JP220 = AJA221 + YY0 -JP320 = AJA321 + YZ0 -JP130 = AJ131 + XZ0 -JP230 = AJ231 + YZ0 -JP330 = JP331 + ZZ0 -MSP10 = AS11 + MX0 -MSP20 = AS21 + MY0 -MSP30 = MSP31 + MZ0 -MP0 = M0 + MP1 -EC10 = -AS32*C1 -EC20 = -AS32*S1 -NC10 = AJ132*C1 + JP332*S1 -NC20 = AJ132*S1 - C1*JP332 -ND32 = D3*MSP13 + JP333 -ED11 = -AS13*S2 - AS23*C2 -ED31 = AS13*C2 - AS23*S2 -ND11 = AJ133*C2 - AJ233*S2 -ND31 = AJ133*S2 + AJ233*C2 -ED10 = C1*ED11 -ED20 = ED11*S1 -ND10 = C1*ND11 + ND32*S1 -ND20 = -C1*ND32 + ND11*S1 -NE33 = AJ334 + MSP24*RL4 -EE12 = AS34*C3 -EE22 = AS34*S3 -NE12 = AJ134*C3 - JP334*S3 -NE22 = AJ134*S3 + AS14*D3 + C3*JP334 -EE11 = C2*EE12 - EE22*S2 -EE31 = C2*EE22 + EE12*S2 -NE11 = C2*NE12 - NE22*S2 -NE31 = C2*NE22 + NE12*S2 -EE10 = -AS14*S1 + C1*EE11 -EE20 = AS14*C1 + EE11*S1 -NE10 = C1*NE11 + NE33*S1 -NE20 = -C1*NE33 + NE11*S1 -EF13 = -AS35*C4 -EF33 = AS35*S4 -NF13 = AJ135*C4 + AS15*RL4 + JP335*S4 -NF33 = -AJ135*S4 + AS35*RL4 + C4*JP335 -EF12 = -AS15*S3 + C3*EF13 -EF22 = AS15*C3 + EF13*S3 -NF12 = -AJ335*S3 + C3*NF13 -NF22 = AJ335*C3 - D3*EF33 + NF13*S3 -NF32 = AS15*D3 + NF33 -EF11 = C2*EF12 - EF22*S2 -EF31 = C2*EF22 + EF12*S2 -NF11 = C2*NF12 - NF22*S2 -NF31 = C2*NF22 + NF12*S2 -EF10 = C1*EF11 + EF33*S1 -EF20 = -C1*EF33 + EF11*S1 -NF10 = C1*NF11 + NF32*S1 -NF20 = -C1*NF32 + NF11*S1 -EG14 = AS36*C5 -EG34 = AS36*S5 -NG14 = AJ136*C5 - S5*ZZ6 -NG34 = AJ136*S5 + C5*ZZ6 -EG13 = -AS16*S4 + C4*EG14 -EG33 = -AS16*C4 - EG14*S4 -NG13 = AJ336*S4 + C4*NG14 + EG34*RL4 -NG33 = AJ336*C4 - EG14*RL4 - NG14*S4 -EG12 = C3*EG13 - EG34*S3 -EG22 = C3*EG34 + EG13*S3 -NG12 = C3*NG13 - NG34*S3 -NG22 = C3*NG34 - D3*EG33 + NG13*S3 -NG32 = D3*EG34 + NG33 -EG11 = C2*EG12 - EG22*S2 -EG31 = C2*EG22 + EG12*S2 -NG11 = C2*NG12 - NG22*S2 -NG31 = C2*NG22 + NG12*S2 -EG10 = C1*EG11 + EG33*S1 -EG20 = -C1*EG33 + EG11*S1 -NG10 = C1*NG11 + NG32*S1 -NG20 = -C1*NG32 + NG11*S1 -A11 = 0 -A12 = 0 -A22 = IA1 + JP331 -A13 = 0 -A23 = AJ332 -A33 = IA2 + JP332 -A14 = 0 -A24 = ND31 -A34 = ND32 -A44 = IA3 + JP333 -A15 = 0 -A25 = NE31 -A35 = NE33 -A45 = NE33 -A55 = IA4 + JP334 -A16 = 0 -A26 = NF31 -A36 = NF32 -A46 = NF33 -A56 = AJ335 -A66 = IA5 + JP335 -A17 = 0 -A27 = NG31 -A37 = NG32 -A47 = NG33 -A57 = NG34 -A67 = AJ336 -A77 = IA6 + ZZ6 -JP140 = 0 -JP240 = -MSP30 -JP340 = MSP20 -JP440 = JP110 -JP150 = MSP30 -JP250 = 0 -JP350 = -MSP10 -JP450 = JP120 -JP550 = JP220 -JP160 = -MSP20 -JP260 = MSP10 -JP360 = 0 -JP460 = JP130 -JP560 = JP230 -JP660 = JP330 -*=* diff --git a/robots/RX90/RX90_jac.txt b/robots/RX90/RX90_jac.txt deleted file mode 100644 index 0a3ffcf..0000000 --- a/robots/RX90/RX90_jac.txt +++ /dev/null @@ -1,63 +0,0 @@ -Jacobian matrix for frame 6 -Projection frame 0, intermediat frame 0 - -Geometric parameters -j ant sigma mu gamma b alpha d theta r -1 0 0 1 0 0 0 0 th1 0 -2 1 0 1 0 0 pi/2 0 th2 0 -3 2 0 1 0 0 0 D3 th3 0 -4 3 0 1 0 0 -pi/2 0 th4 RL4 -5 4 0 1 0 0 pi/2 0 th5 0 -6 5 0 1 0 0 -pi/2 0 th6 0 - -Equations: -cos(th2 + th3) = -sin(th2)*sin(th3) + cos(th2)*cos(th3) -sin(th2 + th3) = sin(th2)*cos(th3) + sin(th3)*cos(th2) -sin(th2) = -sin(th3)*cos(th2 + th3) + sin(th2 + th3)*cos(th3) -cos(th2) = sin(th3)*sin(th2 + th3) + cos(th3)*cos(th2 + th3) -J11 = 0 -J21 = 0 -J31 = 0 -J41 = 0 -J51 = 0 -J61 = 1 -J12 = 0 -J22 = 0 -J32 = 0 -J42 = sin(th1) -J52 = -cos(th1) -J62 = 0 -J13 = D3*sin(th2)*cos(th1) -J23 = D3*sin(th1)*sin(th2) -J33 = -D3*cos(th2) -J43 = sin(th1) -J53 = -cos(th1) -J63 = 0 -J14 = D3*sin(th1)*cos(th3) -J24 = -D3*cos(th1)*cos(th3) -J34 = 0 -J44 = -sin(th2 + th3)*cos(th1) -J54 = -sin(th1)*sin(th2 + th3) -J64 = cos(th2 + th3) -J15 = D3*sin(th1)*sin(th3)*sin(th4) + D3*sin(th2)*cos(th1)*cos(th4) - RL4*sin(th1)*sin(th4) + RL4*cos(th1)*cos(th4)*cos(th2 + th3) -J25 = D3*sin(th1)*sin(th2)*cos(th4) - D3*sin(th3)*sin(th4)*cos(th1) + RL4*sin(th1)*cos(th4)*cos(th2 + th3) + RL4*sin(th4)*cos(th1) -J35 = (-D3*cos(th2) + RL4*sin(th2 + th3))*cos(th4) -J45 = sin(th1)*cos(th4) + sin(th4)*cos(th1)*cos(th2 + th3) -J55 = sin(th1)*sin(th4)*cos(th2 + th3) - cos(th1)*cos(th4) -J65 = sin(th4)*sin(th2 + th3) -J16 = -D3*sin(th1)*sin(th3)*sin(th5)*cos(th4) + D3*sin(th1)*cos(th3)*cos(th5) + D3*sin(th2)*sin(th4)*sin(th5)*cos(th1) + RL4*sin(th1)*sin(th5)*cos(th4) + RL4*sin(th4)*sin(th5)*cos(th1)*cos(th2 + th3) -J26 = D3*sin(th1)*sin(th2)*sin(th4)*sin(th5) + D3*sin(th3)*sin(th5)*cos(th1)*cos(th4) - D3*cos(th1)*cos(th3)*cos(th5) + RL4*sin(th1)*sin(th4)*sin(th5)*cos(th2 + th3) - RL4*sin(th5)*cos(th1)*cos(th4) -J36 = (-D3*cos(th2) + RL4*sin(th2 + th3))*sin(th4)*sin(th5) -J46 = sin(th1)*sin(th4)*sin(th5) - sin(th5)*cos(th1)*cos(th4)*cos(th2 + th3) - sin(th2 + th3)*cos(th1)*cos(th5) -J56 = -sin(th1)*sin(th5)*cos(th4)*cos(th2 + th3) - sin(th1)*sin(th2 + th3)*cos(th5) - sin(th4)*sin(th5)*cos(th1) -J66 = -sin(th5)*sin(th2 + th3)*cos(th4) + cos(th5)*cos(th2 + th3) -L11 = 0 -L21 = -D3*sin(th2) - RL4*cos(th2 + th3) -L31 = (D3*cos(th2) - RL4*sin(th2 + th3))*sin(th1) -L12 = D3*sin(th2) + RL4*cos(th2 + th3) -L22 = 0 -L32 = -(D3*cos(th2) - RL4*sin(th2 + th3))*cos(th1) -L13 = -(D3*cos(th2) - RL4*sin(th2 + th3))*sin(th1) -L23 = (D3*cos(th2) - RL4*sin(th2 + th3))*cos(th1) -L33 = 0 -*=* diff --git a/robots/RX90/RX90_regp.txt b/robots/RX90/RX90_regp.txt deleted file mode 100644 index 4abadfa..0000000 --- a/robots/RX90/RX90_regp.txt +++ /dev/null @@ -1,102 +0,0 @@ -Base parameters computation - -Geometric parameters -j ant sigma mu gamma b alpha d theta r -1 0 0 1 0 0 0 0 th1 0 -2 1 0 1 0 0 pi/2 0 th2 0 -3 2 0 1 0 0 0 D3 th3 0 -4 3 0 1 0 0 -pi/2 0 th4 RL4 -5 4 0 1 0 0 pi/2 0 th5 0 -6 5 0 1 0 0 -pi/2 0 th6 0 - -Dynamic inertia parameters -j XX XY XZ YY YZ ZZ MX MY MZ M IA -1 XX1 XY1 XZ1 YY1 YZ1 ZZ1 MX1 MY1 MZ1 M1 IA1 -2 XX2 XY2 XZ2 YY2 YZ2 ZZ2 MX2 MY2 MZ2 M2 IA2 -3 XX3 XY3 XZ3 YY3 YZ3 ZZ3 MX3 MY3 MZ3 M3 IA3 -4 XX4 XY4 XZ4 YY4 YZ4 ZZ4 MX4 MY4 MZ4 M4 IA4 -5 XX5 XY5 XZ5 YY5 YZ5 ZZ5 MX5 MY5 MZ5 M5 IA5 -6 XX6 XY6 XZ6 YY6 YZ6 ZZ6 MX6 MY6 MZ6 M6 IA6 - -External forces and joint parameters -j FX FY FZ CX CY CZ FS FV QP QDP GAM -1 0 0 0 0 0 0 FS1 FV1 QP1 QDP1 GAM1 -2 0 0 0 0 0 0 FS2 FV2 QP2 QDP2 GAM2 -3 0 0 0 0 0 0 FS3 FV3 QP3 QDP3 GAM3 -4 0 0 0 0 0 0 FS4 FV4 QP4 QDP4 GAM4 -5 0 0 0 0 0 0 FS5 FV5 QP5 QDP5 GAM5 -6 FX6 FY6 FZ6 CX6 CY6 CZ6 FS6 FV6 QP6 QDP6 GAM6 - -Base velicities parameters -axis W0 WP0 V0 VP0 G -X 0 0 0 0 0 -Y 0 0 0 0 0 -Z 0 0 0 0 G3 - -Equations: -C1 = cos(th1) -S1 = sin(th1) -C2 = cos(th2) -S2 = sin(th2) -C3 = cos(th3) -S3 = sin(th3) -C4 = cos(th4) -S4 = sin(th4) -C5 = cos(th5) -S5 = sin(th5) -C6 = cos(th6) -S6 = sin(th6) -XXR6 = XX6 - YY6 -MR5 = M5 + M6 -XXR5 = XX5 - YY5 + YY6 -ZZR5 = YY6 + ZZ5 -MYR5 = MY5 + MZ6 -LamMS214 = -C4*RL4 -LamMS514 = RL4*S4 -LamMS134 = 2*RL4 -LamM14 = RL4**2 -MR4 = M4 + MR5 -XXR4 = XX4 - YY4 + YY5 -ZZR4 = YY5 + ZZ4 -MYR4 = MY4 - MZ5 -LamMS213 = -D3*S3 -LamMS413 = 2*C3*D3 -LamMS223 = -C3*D3 -LamMS423 = -2*D3*S3 -LamM43 = D3**2 -MR3 = M3 + MR4 -XXR3 = LamM14*MR4 + LamMS134*MZ4 + XX3 - YY3 + YY4 -ZZR3 = LamM14*MR4 + LamMS134*MZ4 + YY4 + ZZ3 -MYR3 = MR4*RL4 + MY3 + MZ4 -YYR2 = LamM43*MR3 + YY2 + YY3 -MZR2 = MZ2 + MZ3 -MR2 = M2 + MR3 -XXR2 = XX2 + YY3 - YYR2 -XZR2 = -D3*MZ3 + XZ2 -ZZR2 = IA2 + LamM43*MR3 + ZZ2 -MXR2 = D3*MR3 + MX2 -MR1 = M1 + MR2 -ZZR1 = IA1 + YYR2 + ZZ1 -*=* - -RX90 grouped inertia parameters - -Geometric parameters -j ant sigma mu gamma b alpha d theta r -1 0 0 1 0 0 0 0 th1 0 -2 1 0 1 0 0 pi/2 0 th2 0 -3 2 0 1 0 0 0 D3 th3 0 -4 3 0 1 0 0 -pi/2 0 th4 RL4 -5 4 0 1 0 0 pi/2 0 th5 0 -6 5 0 1 0 0 -pi/2 0 th6 0 - -Dynamic inertia parameters -j XX XY XZ YY YZ ZZ MX MY MZ M IA -1 0 0 0 0 0 ZZR1 0 0 0 0 0 -2 XXR2 XY2 XZR2 0 YZ2 ZZR2 MXR2 MY2 0 0 0 -3 XXR3 XY3 XZ3 0 YZ3 ZZR3 MX3 MYR3 0 0 IA3 -4 XXR4 XY4 XZ4 0 YZ4 ZZR4 MX4 MYR4 0 0 IA4 -5 XXR5 XY5 XZ5 0 YZ5 ZZR5 MX5 MYR5 0 0 IA5 -6 XXR6 XY6 XZ6 0 YZ6 ZZ6 MX6 MY6 0 0 IA6 - -*=* diff --git a/robots/SR400/SR400.par b/robots/SR400/SR400.par deleted file mode 100644 index a75fd45..0000000 --- a/robots/SR400/SR400.par +++ /dev/null @@ -1,57 +0,0 @@ -(* Robotname = 'SR400' *) -NL = 8 -NJ = 9 -NF = 10 -Type = 2 -is_mobile = 0 - -(* Geometric parameters *) -ant = {0,1,2,3,4,5,1,7,8,3} -sigma = {0,0,0,0,0,0,0,0,0,2} -b = {1,0,0,0,0,0,0,0,0,0} -d = {0,d2,d3,d4,0,0,d2,-d8,d3,-d8} -r = {0,0,0,r4,0,0,0,0,0,0} -gamma = {0,0,0,0,0,0,0,0,0,pi/2} -alpha = {0,-pi/2,0,-pi/2,pi/2,-pi/2,-pi/2,0,0,0} -mu = {1,1,0,1,1,1,1,0,0,0} -theta = {th1,th2,th3,th4,th5,th6,th7,th8,th9,0} - -(* Dynamic parameters and external forces *) -XX = {XX1,XX2,XX3,XX4,XX5,XX6,XX7,XX8} -XY = {XY1,XY2,XY3,XY4,XY5,XY6,XY7,XY8} -XZ = {XZ1,XZ2,XZ3,XZ4,XZ5,XZ6,XZ7,XZ8} -YY = {YY1,YY2,YY3,YY4,YY5,YY6,YY7,YY8} -YZ = {YZ1,YZ2,YZ3,YZ4,YZ5,YZ6,YZ7,YZ8} -ZZ = {ZZ1,ZZ2,ZZ3,ZZ4,ZZ5,ZZ6,ZZ7,ZZ8} -MX = {MX1,MX2,MX3,MX4,MX5,MX6,MX7,MX8} -MY = {MY1,MY2,MY3,MY4,MY5,MY6,MY7,MY8} -MZ = {MZ1,MZ2,MZ3,MZ4,MZ5,MZ6,MZ7,MZ8} -M = {M1,M2,M3,M4,M5,M6,M7,M8} -IA = {IA1,IA2,IA3,IA4,IA5,IA6,IA7,IA8} -FV = {FV1,FV2,FV3,FV4,FV5,FV6,FV7,FV8} -FS = {FS1,FS2,FS3,FS4,FS5,FS6,FS7,FS8} -FX = {0,0,0,0,0,0,0,FX8} -FY = {0,0,0,0,0,0,0,FY8} -FZ = {0,0,0,0,0,0,0,FZ8} -CX = {0,0,0,0,0,0,0,CX8} -CY = {0,0,0,0,0,0,0,CY8} -CZ = {0,0,0,0,0,0,0,CZ8} - -(* Joint parameters *) -QP = {QP1,QP2,QP3,QP4,QP5,QP6,QP7,QP8,QP9} -QDP = {QDP1,QDP2,QDP3,QDP4,QDP5,QDP6,QDP7,QDP8,QDP9} -GAM = {GAM1,GAM2,GAM3,GAM4,GAM5,GAM6,GAM7,GAM8,GAM9} - -(* Speed and acceleration of the base *) -W0 = {0,0,0} -WP0 = {0,0,0} -V0 = {0,0,0} -VP0 = {0,0,0} - -(* Acceleration of gravity *) -G = {0,0,G3} - -(* Transformation of 0 frame position fT0 *) -Z = {1,0,0,0,0,1,0,0,0,0,1,0,0,0,0,1} - -(* End of definition *) diff --git a/robots/Stanford/Stanford.par b/robots/Stanford/Stanford.par deleted file mode 100644 index aa7b73e..0000000 --- a/robots/Stanford/Stanford.par +++ /dev/null @@ -1,57 +0,0 @@ -(* Robotname = 'Stanford' *) -NL = 6 -NJ = 6 -NF = 6 -Type = 0 -is_mobile = 0 - -(* Geometric parameters *) -ant = {0,1,2,3,4,5} -sigma = {0,0,1,0,0,0} -b = {0,0,0,0,0,0} -d = {0,0,0,0,0,0} -r = {0,RL2,r3,0,0,0} -gamma = {0,0,0,0,0,0} -alpha = {0,-pi/2,pi/2,0,-pi/2,pi/2} -mu = {0,0,0,0,0,0} -theta = {th1,th2,0,th4,th5,th6} - -(* Dynamic parameters and external forces *) -XX = {XX1,XX2,XX3,XX4,XX5,XX6} -XY = {XY1,XY2,XY3,XY4,XY5,XY6} -XZ = {XZ1,XZ2,XZ3,XZ4,XZ5,XZ6} -YY = {YY1,YY2,YY3,YY4,YY5,YY6} -YZ = {YZ1,YZ2,YZ3,YZ4,YZ5,YZ6} -ZZ = {ZZ1,ZZ2,ZZ3,ZZ4,ZZ5,ZZ6} -MX = {MX1,MX2,MX3,MX4,MX5,MX6} -MY = {MY1,MY2,MY3,MY4,MY5,MY6} -MZ = {MZ1,MZ2,MZ3,MZ4,MZ5,MZ6} -M = {M1,M2,M3,M4,M5,M6} -IA = {IA1,IA2,IA3,IA4,IA5,IA6} -FV = {FV1,FV2,FV3,FV4,FV5,FV6} -FS = {FS1,FS2,FS3,FS4,FS5,FS6} -FX = {0,0,0,0,0,FX6} -FY = {0,0,0,0,0,FY6} -FZ = {0,0,0,0,0,FZ6} -CX = {0,0,0,0,0,CX6} -CY = {0,0,0,0,0,CY6} -CZ = {0,0,0,0,0,CZ6} - -(* Joint parameters *) -QP = {QP1,QP2,QP3,QP4,QP5,QP6} -QDP = {QDP1,QDP2,QDP3,QDP4,QDP5,QDP6} -GAM = {GAM1,GAM2,GAM3,GAM4,GAM5,GAM6} - -(* Speed and acceleration of the base *) -W0 = {0,0,0} -WP0 = {0,0,0} -V0 = {0,0,0} -VP0 = {0,0,0} - -(* Acceleration of gravity *) -G = {0,0,G3} - -(* Transformation of 0 frame position fT0 *) -Z = {1,0,0,0,0,1,0,0,0,0,1,0,0,0,0,1} - -(* End of definition *) diff --git a/robots/Stanford4/Stanford4.par b/robots/Stanford4/Stanford4.par deleted file mode 100644 index 44d2762..0000000 --- a/robots/Stanford4/Stanford4.par +++ /dev/null @@ -1,57 +0,0 @@ -(* Robotname = 'Stanford4' *) -NL = 4 -NJ = 4 -NF = 4 -Type = 0 -is_mobile = 0 - -(* Geometric parameters *) -ant = {0,1,2,3} -sigma = {0,0,1,0} -b = {0,0,0,0} -d = {0,0,0,0} -r = {0,RL2,r3,0} -gamma = {0,0,0,0} -alpha = {0,-pi/2,pi/2,0} -mu = {0,0,0,0} -theta = {th1,th2,0,th4} - -(* Dynamic parameters and external forces *) -XX = {XX1,XX2,XX3,XX4} -XY = {XY1,XY2,XY3,XY4} -XZ = {XZ1,XZ2,XZ3,XZ4} -YY = {YY1,YY2,YY3,YY4} -YZ = {YZ1,YZ2,YZ3,YZ4} -ZZ = {ZZ1,ZZ2,ZZ3,ZZ4} -MX = {MX1,MX2,MX3,MX4} -MY = {MY1,MY2,MY3,MY4} -MZ = {MZ1,MZ2,MZ3,MZ4} -M = {M1,M2,M3,M4} -IA = {IA1,IA2,IA3,IA4} -FV = {FV1,FV2,FV3,FV4} -FS = {FS1,FS2,FS3,FS4} -FX = {0,0,0,FX4} -FY = {0,0,0,FY4} -FZ = {0,0,0,FZ4} -CX = {0,0,0,CX4} -CY = {0,0,0,CY4} -CZ = {0,0,0,CZ4} - -(* Joint parameters *) -QP = {QP1,QP2,QP3,QP4} -QDP = {QDP1,QDP2,QDP3,QDP4} -GAM = {GAM1,GAM2,GAM3,GAM4} - -(* Speed and acceleration of the base *) -W0 = {0,0,0} -WP0 = {0,0,0} -V0 = {0,0,0} -VP0 = {0,0,0} - -(* Acceleration of gravity *) -G = {0,0,G3} - -(* Transformation of 0 frame position fT0 *) -Z = {1,0,0,0,0,1,0,0,0,0,1,0,0,0,0,1} - -(* End of definition *) diff --git a/robots/TwoLoops/TwoLoops.par b/robots/TwoLoops/TwoLoops.par deleted file mode 100644 index 3172bd5..0000000 --- a/robots/TwoLoops/TwoLoops.par +++ /dev/null @@ -1,57 +0,0 @@ -(* Robotname = 'TwoLoops' *) -NL = 8 -NJ = 10 -NF = 12 -Type = 2 -is_mobile = 0 - -(* Geometric parameters *) -ant = {0,1,2,3,4,1,4,3,7,6,8,11} -sigma = {0,0,0,0,0,0,0,0,0,0,2,2} -b = {0,0,0,0,0,d12*cos(g12),0,0,0,0,0,0} -d = {0,0,d3,d4,0,-d12*sin(g12),d7,0,d4,d3,d7,0} -r = {0,0,0,0,0,0,0,0,0,0,0,0} -gamma = {0,0,0,0,0,0,g12 + pi/2,0,0,0,g12 + pi/2,0} -alpha = {0,pi/2,0,0,pi/2,pi/2,0,0,0,0,0,0} -mu = {1,1,1,0,1,0,0,0,0,0,0,0} -theta = {th1,th2,th3,th4,th5,th6,th7,th8,th9,th10,0,0} - -(* Dynamic parameters and external forces *) -XX = {XX1,XX2,XX3,XX4,XX5,XX6,XX7,XX8} -XY = {XY1,XY2,XY3,XY4,XY5,XY6,XY7,XY8} -XZ = {XZ1,XZ2,XZ3,XZ4,XZ5,XZ6,XZ7,XZ8} -YY = {YY1,YY2,YY3,YY4,YY5,YY6,YY7,YY8} -YZ = {YZ1,YZ2,YZ3,YZ4,YZ5,YZ6,YZ7,YZ8} -ZZ = {ZZ1,ZZ2,ZZ3,ZZ4,ZZ5,ZZ6,ZZ7,ZZ8} -MX = {MX1,MX2,MX3,MX4,MX5,MX6,MX7,MX8} -MY = {MY1,MY2,MY3,MY4,MY5,MY6,MY7,MY8} -MZ = {MZ1,MZ2,MZ3,MZ4,MZ5,MZ6,MZ7,MZ8} -M = {M1,M2,M3,M4,M5,M6,M7,M8} -IA = {IA1,IA2,IA3,IA4,IA5,IA6,IA7,IA8} -FV = {FV1,FV2,FV3,FV4,FV5,FV6,FV7,FV8} -FS = {FS1,FS2,FS3,FS4,FS5,FS6,FS7,FS8} -FX = {0,0,0,0,0,0,FX7,FX8} -FY = {0,0,0,0,0,0,FY7,FY8} -FZ = {0,0,0,0,0,0,FZ7,FZ8} -CX = {0,0,0,0,0,0,CX7,CX8} -CY = {0,0,0,0,0,0,CY7,CY8} -CZ = {0,0,0,0,0,0,CZ7,CZ8} - -(* Joint parameters *) -QP = {QP1,QP2,QP3,QP4,QP5,QP6,QP7,QP8,QP9,QP10} -QDP = {QDP1,QDP2,QDP3,QDP4,QDP5,QDP6,QDP7,QDP8,QDP9,QDP10} -GAM = {GAM1,GAM2,GAM3,GAM4,GAM5,GAM6,GAM7,GAM8,GAM9,GAM10} - -(* Speed and acceleration of the base *) -W0 = {0,0,0} -WP0 = {0,0,0} -V0 = {0,0,0} -VP0 = {0,0,0} - -(* Acceleration of gravity *) -G = {0,0,G3} - -(* Transformation of 0 frame position fT0 *) -Z = {1,0,0,0,0,1,0,0,0,0,1,0,0,0,0,1} - -(* End of definition *) diff --git a/robots/testLoop/testLoop.par b/robots/testLoop/testLoop.par deleted file mode 100644 index bf7a36e..0000000 --- a/robots/testLoop/testLoop.par +++ /dev/null @@ -1,57 +0,0 @@ -(* Robotname = 'testLoop' *) -NL = 4 -NJ = 5 -NF = 6 -Type = 2 -is_mobile = 0 - -(* Geometric parameters *) -ant = {0,1,0,3,4,2} -sigma = {0,0,0,0,0,2} -b = {0,0,0,0,0,0} -d = {0,d,0,d,d,d} -r = {0,0,0,0,0,0} -gamma = {0,0,0,0,0,0} -alpha = {0,0,0,0,0,0} -mu = {1,0,1,0,0,0} -theta = {th1,th2,th3,th4,th5,0} - -(* Dynamic parameters and external forces *) -XX = {XX1,XX2,XX3,XX4} -XY = {XY1,XY2,XY3,XY4} -XZ = {XZ1,XZ2,XZ3,XZ4} -YY = {YY1,YY2,YY3,YY4} -YZ = {YZ1,YZ2,YZ3,YZ4} -ZZ = {ZZ1,ZZ2,ZZ3,ZZ4} -MX = {MX1,MX2,MX3,MX4} -MY = {MY1,MY2,MY3,MY4} -MZ = {MZ1,MZ2,MZ3,MZ4} -M = {M1,M2,M3,M4} -IA = {IA1,IA2,IA3,IA4} -FV = {FV1,FV2,FV3,FV4} -FS = {FS1,FS2,FS3,FS4} -FX = {0,0,0,FX4} -FY = {0,0,0,FY4} -FZ = {0,0,0,FZ4} -CX = {0,0,0,CX4} -CY = {0,0,0,CY4} -CZ = {0,0,0,CZ4} - -(* Joint parameters *) -QP = {QP1,QP2,QP3,QP4,QP5} -QDP = {QDP1,QDP2,QDP3,QDP4,QDP5} -GAM = {GAM1,GAM2,GAM3,GAM4,GAM5} - -(* Speed and acceleration of the base *) -W0 = {0,0,0} -WP0 = {0,0,0} -V0 = {0,0,0} -VP0 = {0,0,0} - -(* Acceleration of gravity *) -G = {0,0,G3} - -(* Transformation of 0 frame position fT0 *) -Z = {1,0,0,0,0,1,0,0,0,0,1,0,0,0,0,1} - -(* End of definition *) diff --git a/setup.py b/setup.py new file mode 100644 index 0000000..64f6a96 --- /dev/null +++ b/setup.py @@ -0,0 +1,38 @@ +#!/usr/bin/env python +# -*- coding: utf-8 -*- + + +import os +from setuptools import setup, find_packages + + +BIN_FOLDER = 'bin' + + +def readme(): + with open('README.md') as f: + return f.read() + + +def apply_folder_join(item): + return os.path.join(BIN_FOLDER, item) + + +if os.name is 'nt': + bin_scripts = ['symoro-bin.py'] +else: + bin_scripts = ['symoro-bin'] +bin_scripts = map(apply_folder_join, bin_scripts) + + +setup( + name='symoro', + version='0.2', + description='SYmoblic MOdelling of RObots software package', + url='http://github.com/symoro/symoro', + scripts=bin_scripts, + packages=find_packages(), + zip_safe=False +) + + diff --git a/pysymoro/core/__init__.py b/symoroui/__init__.py similarity index 100% rename from pysymoro/core/__init__.py rename to symoroui/__init__.py diff --git a/symoroui/definition.py b/symoroui/definition.py new file mode 100644 index 0000000..ff9f3d9 --- /dev/null +++ b/symoroui/definition.py @@ -0,0 +1,260 @@ +# -*- coding: utf-8 -*- + + +# This file is part of the OpenSYMORO project. Please see +# https://github.com/symoro/symoro/blob/master/LICENCE for the licence. + + +""" +This module creates the dialog box to define a new robot and +visualisation parameters. +""" + + +import wx +import wx.lib.scrolledpanel as scrolled + +from sympy import Expr, Symbol + +from symoroutils import tools + + +class DialogDefinition(wx.Dialog): + """Creates the dialog box to define a new robot.""" + def __init__( + self, prefix, name, nl, nj, structure, is_floating, + is_mobile, parent=None + ): + super(DialogDefinition, self).__init__( + parent, + style=wx.SYSTEM_MENU | wx.CAPTION | wx.CLOSE_BOX | wx.CLIP_CHILDREN + ) + self.init_ui(name, nl, nj, is_floating, is_mobile, structure) + self.SetTitle(prefix + ": New robot definition") + + def init_ui(self, name, nl, nj, is_floating, is_mobile, structure): + szr_topmost = wx.BoxSizer(wx.VERTICAL) + # title + szr_topmost.Add( + wx.StaticText(self, label="Robot definition"), 0, + wx.ALL | wx.ALIGN_CENTER_HORIZONTAL, 25 + ) + # grid + szr_grd = wx.GridBagSizer(15, 15) + szr_grd.Add( + wx.StaticText(self, label='Name of the robot:'), + pos=(0, 0), flag=wx.BOTTOM | wx.ALIGN_LEFT, border=2 + ) + szr_grd.Add( + wx.TextCtrl(self, size=(92, -1), + name='name', value=name), pos=(0, 1) + ) + szr_grd.Add( + wx.StaticText(self, label='Number of moving links:'), + pos=(1, 0), flag=wx.BOTTOM | wx.TOP | wx.ALIGN_LEFT, + border=2 + ) + self.spin_links = wx.SpinCtrl( + self, size=(92, -1), value=str(nl), min=1 + ) + self.spin_links.Bind(wx.EVT_SPINCTRL, self.OnSpinNL) + szr_grd.Add(self.spin_links, pos=(1, 1)) + szr_grd.Add( + wx.StaticText(self, label='Number of joints:'), pos=(2, 0), + flag=wx.BOTTOM | wx.TOP | wx.ALIGN_LEFT, border=2 + ) + self.spin_joints = wx.SpinCtrl( + self, size=(92, -1), value=str(nj), min=0 + ) + szr_grd.Add(self.spin_joints, pos=(2, 1)) + szr_grd.Add( + wx.StaticText(self, label='Type of structure'), pos=(3, 0), + flag=wx.BOTTOM | wx.TOP | wx.ALIGN_LEFT, border=2 + ) + self.cmb_structure = wx.ComboBox( + self, size=(92, -1), name='structure', style=wx.CB_READONLY, + choices=[tools.SIMPLE, tools.TREE, tools.CLOSED_LOOP], + value=structure + ) + szr_grd.Add(self.cmb_structure, pos=(3, 1)) + self.cmb_structure.Bind(wx.EVT_COMBOBOX, self.OnTypeChanged) + self.OnTypeChanged(None) + szr_topmost.Add( + szr_grd, 0, wx.ALL | wx.ALIGN_CENTER_HORIZONTAL, 15 + ) + # radio box for base type + self.rbx_base_type = wx.RadioBox( + self, label='Base Type', style=wx.RA_SPECIFY_COLS, + majorDimension=3, name='rbx_base', + choices=['Fixed', 'Floating', 'Mobile'] + ) + self.rbx_base_type.Bind(wx.EVT_RADIOBOX, self.OnBaseType) + self.chk_keep_geo = wx.CheckBox( + self, label=' Keep geometric parameters' + ) + self.chk_keep_geo.Value = True + self.chk_keep_dyn = wx.CheckBox( + self, label=' Keep dynamic parameters' + ) + self.chk_keep_dyn.Value = True + self.chk_keep_joint = wx.CheckBox( + self, label=' Keep joint parameters' + ) + self.chk_keep_joint.Value = True + self.chk_keep_base = wx.CheckBox( + self, label=' Keep base parameters' + ) + self.chk_keep_base.Value = True + szr_topmost.Add( + self.rbx_base_type, 0, + wx.LEFT | wx.RIGHT | wx.ALIGN_CENTER, 15 + ) + szr_topmost.Add( + self.chk_keep_geo, 0, + wx.TOP | wx.LEFT | wx.ALIGN_LEFT, 15 + ) + szr_topmost.Add( + self.chk_keep_dyn, 0, + wx.TOP | wx.LEFT | wx.ALIGN_LEFT, 15 + ) + szr_topmost.Add( + self.chk_keep_joint, 0, + wx.TOP | wx.LEFT | wx.ALIGN_LEFT, 15 + ) + szr_topmost.Add( + self.chk_keep_base, 0, + wx.TOP | wx.LEFT | wx.ALIGN_LEFT, 15 + ) + szr_horizontal = wx.BoxSizer(wx.HORIZONTAL) + btn_ok = wx.Button(self, wx.ID_OK, "OK") + btn_ok.Bind(wx.EVT_BUTTON, self.OnOK) + btn_cancel = wx.Button(self, wx.ID_CANCEL, "Cancel") + btn_cancel.Bind(wx.EVT_BUTTON, self.OnCancel) + szr_horizontal.Add(btn_ok, 0, wx.ALL, 25) + szr_horizontal.Add(btn_cancel, 0, wx.ALL, 25) + szr_topmost.Add(szr_horizontal) + self.SetSizerAndFit(szr_topmost) + + def OnOK(self, _): + self.EndModal(wx.ID_OK) + + def OnCancel(self, _): + self.EndModal(wx.ID_CANCEL) + + def OnTypeChanged(self, _): + if self.cmb_structure.GetSelection() == 2: + self.spin_joints.Enable(True) + else: + self.spin_joints.Enable(False) + self.OnSpinNL(None) + + def OnBaseType(self, _): + idx = self.rbx_base_type.GetSelection() + value = True if idx == 0 else False + self.chk_keep_base.Value = value + + def OnSpinNL(self, _): + self.spin_joints.SetRange(int(self.spin_links.Value), 100) + if self.cmb_structure.GetSelection() != 2: + self.spin_joints.Value = self.spin_links.Value + + def get_values(self): + name = self.FindWindowByName('name').Value + nl = int(self.spin_links.Value) + nj = int(self.spin_joints.Value) + base_type_idx = self.rbx_base_type.GetSelection() + is_floating = True if base_type_idx == 1 else False + is_mobile = True if base_type_idx == 2 else False + params = { + 'name': name, + 'num_links': nl, + 'num_joints': nj, + 'num_frames': (2 * nj) -nl, + 'structure': self.cmb_structure.Value, + 'is_floating': is_floating, + 'is_mobile': is_mobile, + 'keep_geo': self.chk_keep_geo.Value, + 'keep_dyn': self.chk_keep_dyn.Value, + 'keep_joint': self.chk_keep_joint.Value, + 'keep_base': self.chk_keep_base.Value + } + return params + + +class DialogVisualisation(wx.Dialog): + """Creates the dialog box to define the visualisation parameters.""" + def __init__(self, prefix, robo, par_dict, parent=None): + super(DialogVisualisation, self).__init__(parent) + self.robo = robo + self.par_dict = par_dict + self.SetTitle(prefix + ": Enter numerical values") + self.construct_sym() + self.init_ui() + + def has_syms(self): + return len(self.syms) > 0 + + def construct_sym(self): + params = ['gamma', 'b', 'alpha', 'd', 'theta', 'r'] + q_vec = self.robo.q_vec + self.syms = set() + for par in params: + for i in range(1, self.robo.NF): + val = self.robo.get_val(i, par) + if val in q_vec: + continue + if isinstance(val, Expr): + for at in val.atoms(Symbol): + self.syms.add(at) + + def init_ui(self): + panel = scrolled.ScrolledPanel(self, -1) + szr_vertical = wx.BoxSizer(wx.VERTICAL) + self.widgets = {} + for par in self.syms: + if par in self.par_dict: + val = str(self.par_dict[par]) + else: + val = 1. + szr_horizontal = wx.BoxSizer(wx.HORIZONTAL) + label = wx.StaticText( + panel, label=str(par), + size=(60, -1), style=wx.ALIGN_RIGHT + ) + szr_horizontal.Add(label, 0, wx.ALIGN_CENTER_VERTICAL) + szr_horizontal.AddSpacer(5) + txt_box = wx.TextCtrl(panel, size=(120, -1), value=str(val)) + szr_horizontal.Add(txt_box) + szr_vertical.Add( + szr_horizontal, 0, + wx.ALL | wx.ALIGN_CENTER_HORIZONTAL, 5 + ) + self.widgets[str(par)] = txt_box + panel.SetSizer(szr_vertical) + panel.SetAutoLayout(1) + panel.SetupScrolling() + szr_topmost = wx.BoxSizer(wx.VERTICAL) + szr_topmost.Add(panel, 1, wx.ALL | wx.EXPAND, 2) + szr_horizontal = wx.BoxSizer(wx.HORIZONTAL) + btn_ok = wx.Button(self, wx.ID_OK, "OK") + btn_ok.Bind(wx.EVT_BUTTON, self.OnOK) + btn_cancel = wx.Button(self, wx.ID_CANCEL, "Cancel") + btn_cancel.Bind(wx.EVT_BUTTON, self.OnCancel) + szr_horizontal.Add(btn_ok, 0, wx.ALL, 15) + szr_horizontal.Add(btn_cancel, 0, wx.ALL, 15) + szr_topmost.Add(szr_horizontal, 0, wx.ALIGN_CENTER_HORIZONTAL, 0) + self.SetSizer(szr_topmost) + + def OnOK(self, _): + self.EndModal(wx.ID_OK) + + def OnCancel(self, _): + self.EndModal(wx.ID_CANCEL) + + def get_values(self): + result = {} + for par in self.syms: + result[par] = self.widgets[str(par)].Value + return result + + diff --git a/pysymoro/gui/ui_geometry.py b/symoroui/geometry.py similarity index 69% rename from pysymoro/gui/ui_geometry.py rename to symoroui/geometry.py index d211843..0df2ed4 100644 --- a/pysymoro/gui/ui_geometry.py +++ b/symoroui/geometry.py @@ -1,7 +1,21 @@ -__author__ = 'Izzat' +# -*- coding: utf-8 -*- + + +# This file is part of the OpenSYMORO project. Please see +# https://github.com/symoro/symoro/blob/master/LICENCE for the licence. + + +""" +This module creates the dialog boxes to specify parameters for +geometric model calculations. +""" + + import wx + class DialogTrans(wx.Dialog): + """Creates the dialog box for transformation matrix selection.""" def __init__(self, prefix, nf, parent=None): super(DialogTrans, self).__init__(parent, style=wx.SYSTEM_MENU | wx.CAPTION | wx.CLOSE_BOX | wx.CLIP_CHILDREN) @@ -13,7 +27,6 @@ def init_ui(self): main_sizer = wx.BoxSizer(wx.VERTICAL) hor_sizer = wx.BoxSizer(wx.HORIZONTAL) main_sizer.Add(hor_sizer, 0, wx.ALL | wx.ALIGN_CENTER_HORIZONTAL, 25) - # Insert, Delete buttons and comboboxes grid = wx.GridBagSizer(hgap=40, vgap=11) lab_top = wx.StaticText(self, label='Original frame') @@ -37,7 +50,6 @@ def init_ui(self): grid.Add(insert_btn, pos=(1, 1)) grid.Add(delete_btn, pos=(3, 1)) hor_sizer.Add(grid) - # Transformations label and list ver_sizer = wx.BoxSizer(wx.VERTICAL) hor_sizer.AddSpacer(40) @@ -48,11 +60,9 @@ def init_ui(self): self.listbox = wx.ListBox(self, size=(80, 120), style=wx.LB_SINGLE) self.result = set() ver_sizer.Add(self.listbox) - self.check_short = wx.CheckBox(self, label='Trigonometric short form') main_sizer.Add(self.check_short, 0, wx.LEFT | wx.ALIGN_LEFT, 25) main_sizer.AddSpacer(15) - # OK Cancel hor_sizer2 = wx.BoxSizer(wx.HORIZONTAL) main_sizer.Add(hor_sizer2, 0, wx.ALL | wx.ALIGN_CENTER_HORIZONTAL, 12) @@ -63,7 +73,6 @@ def init_ui(self): hor_sizer2.Add(ok_btn) hor_sizer2.AddSpacer(22) hor_sizer2.Add(cancel_btn) - self.SetSizerAndFit(main_sizer) def OnOK(self, _): @@ -90,6 +99,7 @@ def GetValues(self): class DialogFast(wx.Dialog): + """Creates the dialog box Fast Geometric model parameters.""" def __init__(self, prefix, nf, parent=None): super(DialogFast, self).__init__(parent, style=wx.SYSTEM_MENU | wx.CAPTION | wx.CLOSE_BOX | wx.CLIP_CHILDREN) @@ -99,10 +109,8 @@ def __init__(self, prefix, nf, parent=None): def init_ui(self): mainSizer = wx.BoxSizer(wx.VERTICAL) - #title label_main = wx.StaticText(self, label="Calculation of iTj") - #input grid = wx.GridBagSizer(hgap=25, vgap=5) lab_left = wx.StaticText(self, label='Frame i') @@ -125,14 +133,12 @@ def init_ui(self): cancel_btn.Bind(wx.EVT_BUTTON, self.OnCancel) grid.Add(cancel_btn, pos=(2, 1)) grid.Add(ok_btn, pos=(2, 0)) - mainSizer.AddSpacer(30) mainSizer.Add(label_main, 0, wx.LEFT | wx.RIGHT | wx.ALIGN_CENTER_HORIZONTAL, 60) mainSizer.AddSpacer(30) mainSizer.Add(grid, flag=wx.ALIGN_CENTER) mainSizer.AddSpacer(20) - self.SetSizerAndFit(mainSizer) def OnOK(self, _): @@ -146,6 +152,7 @@ def GetValues(self): class DialogPaul(wx.Dialog): + """Creates the dialog box to specify Paul method parameters.""" def __init__(self, prefix, endeffs, EMPTY, parent=None): super(DialogPaul, self).__init__(parent, style=wx.SYSTEM_MENU | wx.CAPTION | wx.CLOSE_BOX | wx.CLIP_CHILDREN) @@ -155,7 +162,6 @@ def __init__(self, prefix, endeffs, EMPTY, parent=None): def init_ui(self, EMPTY): main_sizer = wx.BoxSizer(wx.VERTICAL) - #title label_cmb = wx.StaticText(self, label="For frame :") main_sizer.Add(label_cmb, 0, wx.TOP | wx.ALIGN_CENTER_HORIZONTAL, 20) @@ -166,7 +172,6 @@ def init_ui(self, EMPTY): main_sizer.Add(self.cmb, 0, wx.BOTTOM | wx.ALIGN_CENTER_HORIZONTAL, 10) lbl = wx.StaticText(self, label="Components taken into account :") main_sizer.Add(lbl, 0, wx.ALL | wx.ALIGN_CENTER_HORIZONTAL, 20) - #input grid = wx.GridBagSizer(hgap=15, vgap=15) names = ['S', 'N', 'A', 'P'] @@ -188,10 +193,8 @@ def init_ui(self, EMPTY): label = wx.StaticText(self, label=(' 1' if i == 3 else ' 0'), id=12 + i) grid.Add(label, pos=(4, i)) - main_sizer.Add(grid, 0, wx.ALIGN_CENTER | wx.LEFT | wx.RIGHT, 35) main_sizer.AddSpacer(20) - #buttons hor_sizer = wx.BoxSizer(wx.HORIZONTAL) ok_btn = wx.Button(self, wx.ID_OK, "OK") @@ -201,7 +204,91 @@ def init_ui(self, EMPTY): hor_sizer.Add(ok_btn, 0, wx.ALL, 15) hor_sizer.Add(cancel_btn, 0, wx.ALL, 15) main_sizer.Add(hor_sizer, 0, wx.ALIGN_CENTER_HORIZONTAL, 0) + self.SetSizerAndFit(main_sizer) + + def OnOK(self, _): + self.EndModal(wx.ID_OK) + def OnCancel(self, _): + self.EndModal(wx.ID_CANCEL) + + def OnVectorChecked(self, evt): + name = evt.EventObject.Name + index = 4 if evt.EventObject.Value else 0 + for i in range(1, 4): + cmb = self.FindWindowByName(name + str(i)) + cmb.SetSelection(index) + + def OnComboBox(self, evt): + name = evt.EventObject.Name + if evt.EventObject.GetSelection() != 4: + check_box = self.FindWindowByName(name[0]) + check_box.SetValue(False) + + def get_values(self): + lst = [] + for i in range(16): + widget = self.FindWindowById(i) + if isinstance(widget, wx.ComboBox): + lst.append(widget.Value) + else: + lst.append(widget.LabelText) + return lst, int(self.cmb.Value) + + +class DialogPieper(wx.Dialog): + """Creates the dialog box to specify Pieper method parameters.""" + def __init__(self, prefix, endeffs, EMPTY, parent=None): + super(DialogPieper, self).__init__(parent, style=wx.SYSTEM_MENU | + wx.CAPTION | wx.CLOSE_BOX | wx.CLIP_CHILDREN) + self.endeffs = endeffs + self.init_ui(EMPTY) + self.SetTitle(prefix + ": IGM Pieper Method (pie)") + + def init_ui(self, EMPTY): + main_sizer = wx.BoxSizer(wx.VERTICAL) + #title + label_cmb = wx.StaticText(self, label="For frame :") + main_sizer.Add(label_cmb, 0, wx.TOP | wx.ALIGN_CENTER_HORIZONTAL, 20) + self.cmb = wx.ComboBox(self, size=(80, -1), + choices=[str(i) for i in self.endeffs], style=wx.CB_READONLY) + self.cmb.SetSelection(0) + main_sizer.AddSpacer(5) + main_sizer.Add(self.cmb, 0, wx.BOTTOM | wx.ALIGN_CENTER_HORIZONTAL, 10) + lbl = wx.StaticText(self, label="Components taken into account :") + main_sizer.Add(lbl, 0, wx.ALL | wx.ALIGN_CENTER_HORIZONTAL, 20) + #input + grid = wx.GridBagSizer(hgap=15, vgap=15) + names = ['S', 'N', 'A', 'P'] + for i, name in enumerate(names): + check_box = wx.CheckBox(self, wx.ID_ANY, + label=' ' + name, name=name) + check_box.SetValue(True) + check_box.Bind(wx.EVT_CHECKBOX, self.OnVectorChecked) + grid.Add(check_box, pos=(0, i), flag=wx.ALIGN_CENTER_HORIZONTAL) + for j in range(1, 4): + w_name = name + str(j) + cmb = wx.ComboBox(self, + choices=[EMPTY, '-1', '0', '1', w_name], + name=w_name, style=wx.CB_READONLY, + size=(90, -1), id=(j-1)*4 + i) + cmb.SetSelection(4) + cmb.Bind(wx.EVT_COMBOBOX, self.OnComboBox) + grid.Add(cmb, pos=(j, i)) + label = wx.StaticText(self, + label=(' 1' if i == 3 else ' 0'), id=12 + i) + grid.Add(label, pos=(4, i)) + main_sizer.Add(grid, 0, wx.ALIGN_CENTER | wx.LEFT | wx.RIGHT, 35) + main_sizer.AddSpacer(20) + #buttons + hor_sizer = wx.BoxSizer(wx.HORIZONTAL) + ok_btn = wx.Button(self, wx.ID_OK, "OK") + ok_btn.Bind(wx.EVT_BUTTON, self.OnOK) + cancel_btn = wx.Button(self, wx.ID_CANCEL, "Cancel") + cancel_btn.Bind(wx.EVT_BUTTON, self.OnCancel) + hor_sizer.Add(ok_btn, 0, wx.ALL, 15) + hor_sizer.Add(cancel_btn, 0, wx.ALL, 15) + main_sizer.Add(hor_sizer, 0, wx.ALIGN_CENTER_HORIZONTAL, 0) self.SetSizerAndFit(main_sizer) def OnOK(self, _): @@ -231,4 +318,6 @@ def get_values(self): lst.append(widget.Value) else: lst.append(widget.LabelText) - return lst, int(self.cmb.Value) \ No newline at end of file + return lst, int(self.cmb.Value) + + diff --git a/pysymoro/gui/ui_kinematics.py b/symoroui/kinematics.py similarity index 90% rename from pysymoro/gui/ui_kinematics.py rename to symoroui/kinematics.py index 4372e89..8b5c878 100644 --- a/pysymoro/gui/ui_kinematics.py +++ b/symoroui/kinematics.py @@ -1,21 +1,36 @@ -__author__ = 'Izzat' +# -*- coding: utf-8 -*- + + +# This file is part of the OpenSYMORO project. Please see +# https://github.com/symoro/symoro/blob/master/LICENCE for the licence. + + +""" +This module creates the dialog box for differrent kinematic model +parameters. +""" + + import wx + class DialogJacobian(wx.Dialog): + """ + Creates the dialog box to specify parameters for the calculation + of the Jacobian matrix. + """ def __init__(self, prefix, robo, parent=None): super(DialogJacobian, self).__init__(parent, style=wx.SYSTEM_MENU | wx.CAPTION | wx.CLOSE_BOX | wx.CLIP_CHILDREN) self.robo = robo - self.InitUI() + self.init_ui() self.SetTitle(prefix + ": Jacobian matrix (jac)") - def InitUI(self): + def init_ui(self): sizer = wx.BoxSizer(wx.VERTICAL) - #title label_main = wx.StaticText(self, label="Calculation of i Jr j") sizer.Add(label_main, 0, wx.ALL | wx.ALIGN_CENTER_HORIZONTAL, 25) - #input choices = [str(i) for i in range(self.robo.NF)] label = wx.StaticText(self, label='Frame number ( r )') @@ -25,24 +40,20 @@ def InitUI(self): self.cmb_frame.SetSelection(len(choices)-1) self.cmb_frame.Bind(wx.EVT_COMBOBOX, self.OnFrameChanged) sizer.Add(self.cmb_frame, 0, wx.ALL | wx.ALIGN_CENTER_HORIZONTAL, 10) - chain = self.robo.chain(int(self.cmb_frame.Value)) choices = [str(i) for i in reversed(chain + [0])] label = wx.StaticText(self, label='Projection frame ( i )') sizer.Add(label, 0, wx.ALL | wx.ALIGN_CENTER_HORIZONTAL, 2) - self.cmb_inter = wx.ComboBox(self, size=(50, -1), - choices=choices, style=wx.CB_READONLY) - self.cmb_inter.SetSelection(0) - sizer.Add(self.cmb_inter, 0, wx.ALL | wx.ALIGN_CENTER_HORIZONTAL, 10) - - label = wx.StaticText(self, label='Intermediate frame ( j )') - sizer.Add(label, 0, wx.ALL | wx.ALIGN_CENTER_HORIZONTAL, 2) self.cmb_proj = wx.ComboBox(self, size=(50, -1), choices=choices, style=wx.CB_READONLY) - self.cmb_proj.SetSelection(len(choices)-1) self.cmb_proj.SetSelection(0) sizer.Add(self.cmb_proj, 0, wx.ALL | wx.ALIGN_CENTER_HORIZONTAL, 10) - + label = wx.StaticText(self, label='Intermediate frame ( j )') + sizer.Add(label, 0, wx.ALL | wx.ALIGN_CENTER_HORIZONTAL, 2) + self.cmb_inter = wx.ComboBox(self, size=(50, -1), + choices=choices, style=wx.CB_READONLY) + self.cmb_inter.SetSelection(0) + sizer.Add(self.cmb_inter, 0, wx.ALL | wx.ALIGN_CENTER_HORIZONTAL, 10) hor_sizer = wx.BoxSizer(wx.HORIZONTAL) ok_btn = wx.Button(self, wx.ID_OK, "OK") ok_btn.Bind(wx.EVT_BUTTON, self.OnOK) @@ -51,7 +62,6 @@ def InitUI(self): hor_sizer.Add(ok_btn, 0, wx.ALL, 25) hor_sizer.Add(cancel_btn, 0, wx.ALL, 25) sizer.Add(hor_sizer) - self.SetSizerAndFit(sizer) def OnFrameChanged(self, _): @@ -72,21 +82,24 @@ def get_values(self): return int(self.cmb_frame.Value), \ int(self.cmb_proj.Value), int(self.cmb_inter.Value) + class DialogDeterminant(wx.Dialog): + """ + Creates the dialog box to specify parameters for the + calculation of the determinant of a Jacobian. + """ def __init__(self, prefix, robo, parent=None): super(DialogDeterminant, self).__init__(parent, style=wx.SYSTEM_MENU | wx.CAPTION | wx.CLOSE_BOX | wx.CLIP_CHILDREN) self.robo = robo - self.InitUI() + self.init_ui() self.SetTitle(prefix + ": Determinant of a jacobian matrix (det)") - def InitUI(self): + def init_ui(self): mainSizer = wx.BoxSizer(wx.HORIZONTAL) - #title mainSizer.AddSpacer(10) grid = wx.GridBagSizer(5, 15) - choices = [str(i) for i in range(self.robo.NF)] label = wx.StaticText(self, label='Frame number ( r )') grid.Add(label, pos=(0, 0), span=(1, 2), @@ -97,28 +110,25 @@ def InitUI(self): self.cmb_frame.Bind(wx.EVT_COMBOBOX, self.OnFrameChanged) grid.Add(self.cmb_frame, pos=(1, 0), span=(1, 2), flag=wx.ALIGN_CENTER_HORIZONTAL | wx.BOTTOM, border=15) - chain = self.robo.chain(int(self.cmb_frame.Value)) choices = [str(i) for i in reversed(chain + [0])] - label = wx.StaticText(self, label='Projection frame ( i )') + label = wx.StaticText(self, label='Projection frame ( j )') grid.Add(label, pos=(2, 0), span=(1, 2), flag=wx.ALIGN_CENTER_HORIZONTAL | wx.ALL) - self.cmb_inter = wx.ComboBox(self, size=(50, -1), - choices=choices, style=wx.CB_READONLY) - self.cmb_inter.SetSelection(0) - grid.Add(self.cmb_inter, pos=(3, 0), span=(1, 2), - flag=wx.ALIGN_CENTER_HORIZONTAL | wx.BOTTOM, border=15) - - label = wx.StaticText(self, label='Intermediate frame ( j )') - grid.Add(label, pos=(4, 0), span=(1, 2), - flag=wx.ALIGN_CENTER_HORIZONTAL | wx.ALL) self.cmb_proj = wx.ComboBox(self, size=(50, -1), choices=choices, style=wx.CB_READONLY) self.cmb_proj.SetSelection(len(choices)-1) self.cmb_proj.SetSelection(0) - grid.Add(self.cmb_proj, pos=(5, 0), span=(1, 2), + grid.Add(self.cmb_proj, pos=(3, 0), span=(1, 2), + flag=wx.ALIGN_CENTER_HORIZONTAL | wx.BOTTOM, border=15) + label = wx.StaticText(self, label='Intermediate frame ( i )') + grid.Add(label, pos=(4, 0), span=(1, 2), + flag=wx.ALIGN_CENTER_HORIZONTAL | wx.ALL) + self.cmb_inter = wx.ComboBox(self, size=(50, -1), + choices=choices, style=wx.CB_READONLY) + self.cmb_inter.SetSelection(0) + grid.Add(self.cmb_inter, pos=(5, 0), span=(1, 2), flag=wx.ALIGN_CENTER_HORIZONTAL | wx.BOTTOM, border=15) - label_main = wx.StaticText(self, label="Definition of sub-matrix") grid.Add(label_main, pos=(6, 0), span=(1, 2), flag=wx.ALIGN_CENTER_HORIZONTAL | wx.ALL) @@ -126,7 +136,6 @@ def InitUI(self): label="(Select rows and columns to be deleted)") grid.Add(label_main, pos=(7, 0), span=(1, 2), flag=wx.ALIGN_CENTER_HORIZONTAL | wx.BOTTOM, border=5) - #input label_row = wx.StaticText(self, label="Rows:") label_col = wx.StaticText(self, label="Columns:") @@ -146,7 +155,6 @@ def InitUI(self): flag=wx.ALIGN_CENTER_HORIZONTAL | wx.ALL, border=5) grid.Add(cancel_btn, pos=(10, 1), flag=wx.ALIGN_CENTER_HORIZONTAL | wx.ALL, border=5) - mainSizer.Add(grid, 0, wx.ALL | wx.ALIGN_CENTER_HORIZONTAL, 15) self.SetSizerAndFit(mainSizer) self.OnFrameChanged(None) @@ -173,4 +181,6 @@ def get_values(self): col_selected = [i for i in range(len(self.box_col.Items)) if not self.box_col.IsChecked(i)] return int(self.cmb_frame.Value), int(self.cmb_proj.Value),\ - int(self.cmb_inter.Value), row_selected, col_selected \ No newline at end of file + int(self.cmb_inter.Value), row_selected, col_selected + + diff --git a/symoroui/labels.py b/symoroui/labels.py new file mode 100644 index 0000000..cb5780c --- /dev/null +++ b/symoroui/labels.py @@ -0,0 +1,184 @@ +# -*- coding: utf-8 -*- + + +# This file is part of the OpenSYMORO project. Please see +# https://github.com/symoro/symoro/blob/master/LICENCE for the licence. + + +""" +This module contains the labels used in the user interface as a dict. +The main purpose of this to represent the labels symbolically and use it +in multiple places. Also this gives the advantage of modifying and +maintaining the labels easily since all the labels are placed in the +same place. +""" + + +from collections import namedtuple +from collections import OrderedDict + + +# main window +MAIN_WIN = dict( + prog_name="SYMORO", + window_title="SYMORO - SYmbolic MOdelling of RObots" +) + +# interface contents +# TODO: add event handlers to dict entries as well +BOX_TITLES = dict( + robot_des="Robot Description", + robot_type="Robot Type", + gravity="Gravity Components", + location="Robot Location", + geom_params="Geometric Parameters", + dyn_params="Dynamic Parameters and External Forces", + base_vel_acc="Velocity and Acceleration of the base", + joint_params="Joint Stiffness, Velocity and Acceleration" +) +# named tuple to hold the content field entries +FieldEntry = namedtuple( + 'FieldEntry', ['label', 'name', 'control', 'place', 'handler', 'id'] +) +# joint velocity and acceleration params +JOINT_PARAMS = OrderedDict([ + ('joint', FieldEntry('Joint', 'joint', 'cmb', (0, 0), 'OnJointChanged', -1)), + ('eta', FieldEntry('eta', 'eta', 'cmb', (0, 1), 'OnSpeedChanged', -1)), + ('stiff', FieldEntry('k', 'k', 'txt', (1, 0), 'OnSpeedChanged', -1)), + ('gam', FieldEntry('GAM', 'GAM', 'txt', (1, 1), 'OnSpeedChanged', -1)), + ('qp', FieldEntry('QP', 'QP', 'txt', (2, 0), 'OnSpeedChanged', -1)), + ('qdp', FieldEntry('QDP', 'QDP', 'txt', (2, 1), 'OnSpeedChanged', -1)) +]) +# base velocity and acceleration params +BASE_VEL_ACC = OrderedDict([ + ('vx', FieldEntry('VXb', 'V0X', 'txt', (0, 0), 'OnBaseTwistChanged', 0)), + ('vy', FieldEntry('VYb', 'V0Y', 'txt', (1, 0), 'OnBaseTwistChanged', 1)), + ('vz', FieldEntry('VZb', 'V0Z', 'txt', (2, 0), 'OnBaseTwistChanged', 2)), + ('wx', FieldEntry('WXb', 'W0X', 'txt', (0, 1), 'OnBaseTwistChanged', 0)), + ('wy', FieldEntry('WYb', 'W0Y', 'txt', (1, 1), 'OnBaseTwistChanged', 1)), + ('wz', FieldEntry('WZb', 'W0Z', 'txt', (2, 1), 'OnBaseTwistChanged', 2)), + ('vpx', FieldEntry('VPXb', 'VP0X', 'txt', (0, 2), 'OnBaseTwistChanged', 0)), + ('vpy', FieldEntry('VPYb', 'VP0Y', 'txt', (1, 2), 'OnBaseTwistChanged', 1)), + ('vpz', FieldEntry('VPZb', 'VP0Z', 'txt', (2, 2), 'OnBaseTwistChanged', 2)), + ('wpx', FieldEntry('WPXb', 'WP0X', 'txt', (0, 3), 'OnBaseTwistChanged', 0)), + ('wpy', FieldEntry('WPYb', 'WP0Y', 'txt', (1, 3), 'OnBaseTwistChanged', 1)), + ('wpz', FieldEntry('WPZb', 'WP0Z', 'txt', (2, 3), 'OnBaseTwistChanged', 2)) +]) +# inertial params +DYN_PARAMS_I = OrderedDict([ + ('xx', FieldEntry('XX', 'XX', 'txt', (0, 0), 'OnDynParamChanged', -1)), + ('xy', FieldEntry('XY', 'XY', 'txt', (0, 1), 'OnDynParamChanged', -1)), + ('xz', FieldEntry('XZ', 'XZ', 'txt', (0, 2), 'OnDynParamChanged', -1)), + ('yy', FieldEntry('YY', 'YY', 'txt', (0, 3), 'OnDynParamChanged', -1)), + ('yz', FieldEntry('YZ', 'YZ', 'txt', (0, 4), 'OnDynParamChanged', -1)), + ('zz', FieldEntry('ZZ', 'ZZ', 'txt', (0, 5), 'OnDynParamChanged', -1)) +]) +# mass tensor params +DYN_PARAMS_M = OrderedDict([ + ('msx', FieldEntry('MX', 'MX', 'txt', (1, 0), 'OnDynParamChanged', -1)), + ('msy', FieldEntry('MY', 'MY', 'txt', (1, 1), 'OnDynParamChanged', -1)), + ('msz', FieldEntry('MZ', 'MZ', 'txt', (1, 2), 'OnDynParamChanged', -1)), + ('mass', FieldEntry('M', 'M', 'txt', (1, 3), 'OnDynParamChanged', -1)) +]) +# friction and rotor inertia params +DYN_PARAMS_X = OrderedDict([ + ('ia', FieldEntry('IA', 'IA', 'txt', (1, 4), 'OnDynParamChanged', -1)), + ('frc', FieldEntry('FS', 'FS', 'txt', (1, 5), 'OnDynParamChanged', -1)), + ('frv', FieldEntry('FV', 'FV', 'txt', (1, 6), 'OnDynParamChanged', -1)) +]) +# external force, moments params +DYN_PARAMS_F = OrderedDict([ + ('fx_ext', FieldEntry('FX', 'FX', 'txt', (2, 0), 'OnDynParamChanged', -1)), + ('fy_ext', FieldEntry('FY', 'FY', 'txt', (2, 1), 'OnDynParamChanged', -1)), + ('fz_ext', FieldEntry('FZ', 'FZ', 'txt', (2, 2), 'OnDynParamChanged', -1)), + ('mx_ext', FieldEntry('CX', 'CX', 'txt', (2, 3), 'OnDynParamChanged', -1)), + ('my_ext', FieldEntry('CY', 'CY', 'txt', (2, 4), 'OnDynParamChanged', -1)), + ('mz_ext', FieldEntry('CZ', 'CZ', 'txt', (2, 5), 'OnDynParamChanged', -1)) +]) +# dynamic params got by concatenation +DYN_PARAMS = OrderedDict( + [('link', FieldEntry('Link', 'link', 'cmb', None, 'OnLinkChanged', -1))] + \ + DYN_PARAMS_I.items() + \ + DYN_PARAMS_M.items() + \ + DYN_PARAMS_X.items() + \ + DYN_PARAMS_F.items() +) +# geometric params +GEOM_PARAMS = OrderedDict([ + ('frame', FieldEntry('Frame', 'frame', 'cmb', (0, 0), 'OnFrameChanged', -1)), + ('ant', FieldEntry('ant', 'ant', 'cmb', (1, 0), 'OnGeoParamChanged', -1)), + ('sigma', FieldEntry('sigma', 'sigma', 'cmb', (0, 1), 'OnGeoParamChanged', -1)), + ('mu', FieldEntry('mu', 'mu', 'cmb', (1, 1), 'OnGeoParamChanged', -1)), + ('gamma', FieldEntry('gamma', 'gamma', 'txt', (0, 2), 'OnGeoParamChanged', -1)), + ('b', FieldEntry('b', 'b', 'txt', (1, 2), 'OnGeoParamChanged', -1)), + ('alpha', FieldEntry('alpha', 'alpha', 'txt', (0, 3), 'OnGeoParamChanged', -1)), + ('d', FieldEntry('d', 'd', 'txt', (1, 3), 'OnGeoParamChanged', -1)), + ('theta', FieldEntry('theta', 'theta', 'txt', (0, 4), 'OnGeoParamChanged', -1)), + ('r', FieldEntry('r', 'r', 'txt', (1, 4), 'OnGeoParamChanged', -1)) +]) +# gravity component params +GRAVITY_CMPNTS = OrderedDict([ + ('gx', FieldEntry('GX', 'GX', 'txt', (0, 0), 'OnBaseTwistChanged', 0)), + ('gy', FieldEntry('GY', 'GY', 'txt', (0, 1), 'OnBaseTwistChanged', 1)), + ('gz', FieldEntry('GZ', 'GZ', 'txt', (0, 2), 'OnBaseTwistChanged', 2)) +]) +# robot type params +ROBOT_TYPE = OrderedDict([ + ('name', FieldEntry('Name of the robot:', 'name', 'lbl', (0, 0), None, -1)), + ('num_links', FieldEntry('Number of moving links:', 'NL', 'lbl', (1, 0), None, -1)), + ('num_joints', FieldEntry('Number of joints:', 'NJ', 'lbl', (2, 0), None, -1)), + ('num_frames', FieldEntry('Number of frames:', 'NF', 'lbl', (3, 0), None, -1)), + ('structure', FieldEntry('Type of structure:', 'type', 'lbl', (4, 0), None, -1)), + ('is_floating', FieldEntry('Is Floating Base:', 'floating', 'lbl', (5, 0), None, -1)), + ('is_mobile', FieldEntry('Is Mobile Robot:', 'mobile', 'lbl', (6, 0), None, -1)), + ('num_loops', FieldEntry('Number of closed loops:', 'loops', 'lbl', (7, 0), None, -1)) +]) + +# menu bar +# TODO: add event handlers to dict entries as well +MAIN_MENU = OrderedDict( + file_menu="&File", + geom_menu="&Geometric", + kin_menu="&Kinematic", + dyn_menu="&Dynamic", + iden_menu="&Identification", + optim_menu="&Optimiser", + viz_menu="&Visualisation" +) +VIZ_MENU = OrderedDict(m_viz="&Visualisation") +IDEN_MENU = OrderedDict( + m_base_inertial_params="Base Inertial parameters", + m_dyn_iden_model="Dynamic Identification Model", + m_energy_iden_model="Energy Identification Model" +) +DYN_MENU = OrderedDict( + m_idym="Inverse Dynamic Model", + m_inertia_matrix="Inertia matrix", + m_h_term="Centrifugal, Coriolis & Gravity torques", + m_ddym="Direct Dynamic Model" +) +KIN_MENU = OrderedDict( + m_jac_matrix="Jacobian matrix", + m_determinant="Determinant of a Jacobian", + m_kin_constraint="Kinematic constraint equation of loops", + m_vel="Velocities", + m_acc="Accelerations", + m_jpqp="Jpqp" +) +GEOM_MENU = OrderedDict( + m_trans_matrix="Transformation matrix", + m_fast_dgm="Fast Geometric model", + m_igm_paul="IGM - Paul method", + m_igm_pieper="IGM - Pieper method", + m_geom_constraint="Geometric constraint equation of loops" +) +FILE_MENU = OrderedDict( + m_new="&New", + m_open="&Open", + m_save="&Save", + m_save_as="Save &As", + m_pref="Preferences -- (unavailable)", + m_exit="E&xit" +) + + diff --git a/symoroui/layout.py b/symoroui/layout.py new file mode 100644 index 0000000..159f64a --- /dev/null +++ b/symoroui/layout.py @@ -0,0 +1,1022 @@ +# -*- coding: utf-8 -*- + + +# This file is part of the OpenSYMORO project. Please see +# https://github.com/symoro/symoro/blob/master/LICENCE for the licence. + + +""" +This module creates the main window user interface and draws the +interface on the screen for the SYMORO package. +""" + + +import os +import re +import shutil +from collections import OrderedDict + +import wx + +from pysymoro.robot import Robot +from pysymoro import geometry +from pysymoro import kinematics +from pysymoro import invgeom +from pysymoro import pieper +from symoroutils import configfile +from symoroutils import filemgr +from symoroutils import parfile +from symoroutils import samplerobots +from symoroutils import tools +from symoroui import definition as ui_definition +from symoroui import geometry as ui_geometry +from symoroui import kinematics as ui_kinematics +from symoroui import labels as ui_labels +from symoroviz import graphics + + +class MainFrame(wx.Frame): + """This Frame contains the main window for SYMORO""" + def __init__(self, *args, **kwargs): + """Constructor : creates the UI and draws it on the screen.""" + wx.Frame.__init__(self, *args, **kwargs) + self.Bind(wx.EVT_CLOSE, self.OnClose) + # create status bar + self.statusbar = self.CreateStatusBar() + # create menu bar + self.create_menu() + # object to store different ui elements and their keys + self.widgets = {} + self.widget_keys = {} + # object to store parameter values got from dialog box input + self.par_dict = {} + # setup panel and sizer for content + self.panel = wx.Panel(self) + self.szr_topmost = wx.BoxSizer(wx.VERTICAL) + self.create_ui() + self.panel.SetSizerAndFit(self.szr_topmost) + self.Fit() + # load robot + self.robo = self.load_robot() + # update fields with data + self.feed_data() + # configure status bar + self.statusbar.SetFieldsCount(number=2) + self.statusbar.SetStatusWidths(widths=[-1, -1]) + self.statusbar.SetStatusText(text="Ready", number=0) + self.statusbar.SetStatusText( + text="Location of robot files is {0}".format( + filemgr.get_base_path() + ), number = 1 + ) + + def load_robot(self): + """ + Load either the default robot or the last used robot at the + start of the program. + + Returns: + An instance of `Robot` class. + """ + par_file_path = configfile.get_last_robot() + if par_file_path is None: + # when last used robot was not saved + return samplerobots.rx90() + elif not os.path.exists(par_file_path): + # when the PAR file does not exist + self.message_error("The PAR file does not exist.") + return samplerobots.rx90() + else: + robo_name = os.path.split(par_file_path)[1][:-4] + robo, flag = parfile.readpar(robo_name, par_file_path) + if robo is None: + robo = samplerobots.rx90() + self.message_error("File could not be read!") + elif flag == tools.FAIL: + robo = samplerobots.rx90() + self.message_warning( + "While reading file an error occured." + ) + return robo + + def params_in_grid(self, szr_grd, elements, rows, cols, width=70): + """Method to display a set of fields in a grid.""" + for idx, key in enumerate(elements): + label = elements[key].label + name = elements[key].name + control = elements[key].control + place = elements[key].place + handler = getattr(self, elements[key].handler) + field_id = int(elements[key].id) + if control is 'cmb': + ctrl = wx.ComboBox( + parent=self.panel, style=wx.CB_READONLY, + size=(width, -1), name=name + ) + ctrl.Bind(wx.EVT_COMBOBOX, handler) + elif control is 'lbl': + ctrl = wx.StaticText( + parent=self.panel, size=(width, -1), name=name + ) + else: + ctrl = wx.TextCtrl( + parent=self.panel, size=(width, -1), + name=name, id=field_id + ) + ctrl.Bind(wx.EVT_KILL_FOCUS, handler) + self.widgets[name] = ctrl + self.widget_keys[key] = ctrl + szr_ele = wx.BoxSizer(wx.HORIZONTAL) + szr_ele.Add( + wx.StaticText( + self.panel, label=label, style=wx.ALIGN_RIGHT + ), proportion=0, flag=wx.ALL | wx.ALIGN_RIGHT, border=5 + ) + szr_ele.Add( + ctrl, proportion=0, + flag=wx.ALL | wx.ALIGN_LEFT, border=1 + ) + szr_grd.Add( + szr_ele, pos=(place[0], place[1]), + flag=wx.ALL | wx.ALIGN_RIGHT, border=2 + ) + + def create_ui(self): + """Method to create the contents of the user interface""" + # main box - robot description box + szr_robot_des = wx.StaticBoxSizer( + wx.StaticBox( + self.panel, label=ui_labels.BOX_TITLES['robot_des'] + ), wx.HORIZONTAL + ) + szr_robot_des.AddSpacer(3) + szr_left_col = wx.BoxSizer(wx.VERTICAL) + szr_right_col = wx.BoxSizer(wx.VERTICAL) + szr_robot_des.Add(szr_left_col, 0, wx.ALL | wx.EXPAND, 5) + szr_robot_des.Add(szr_right_col, 0, wx.ALL | wx.EXPAND, 5) + self.szr_topmost.Add(szr_robot_des, 0, wx.ALL, 10) + # left col - robot type box + szr_robot_type = wx.StaticBoxSizer( + wx.StaticBox( + self.panel, label=ui_labels.BOX_TITLES['robot_type'] + ), wx.HORIZONTAL + ) + szr_grd_robot_type = wx.GridBagSizer(10, 12) + for idx, key in enumerate(ui_labels.ROBOT_TYPE): + label = ui_labels.ROBOT_TYPE[key].label + name = ui_labels.ROBOT_TYPE[key].name + self.widgets[name] = wx.StaticText( + self.panel, size=(150, -1), + name=ui_labels.ROBOT_TYPE[key].name + ) + szr_grd_robot_type.Add( + wx.StaticText(self.panel, label=label), + pos=(idx, 0), flag=wx.LEFT, border=10 + ) + szr_grd_robot_type.Add( + self.widgets[name], pos=(idx, 1), + flag=wx.LEFT | wx.RIGHT, border=10 + ) + szr_robot_type.Add( + szr_grd_robot_type, 0, wx.TOP | wx.BOTTOM | wx.EXPAND, 6 + ) + szr_left_col.Add(szr_robot_type) + szr_left_col.AddSpacer(8) + # left col - gravity components box + szr_gravity = wx.StaticBoxSizer( + wx.StaticBox( + self.panel, label=ui_labels.BOX_TITLES['gravity'] + ), wx.HORIZONTAL + ) + szr_grd_gravity = wx.GridBagSizer(5, 5) + self.params_in_grid( + szr_grd_gravity, elements=ui_labels.GRAVITY_CMPNTS, + rows=1, cols=3, width=70, + ) + szr_gravity.Add(szr_grd_gravity) + szr_left_col.Add(szr_gravity, 0, wx.ALL | wx.EXPAND, 0) + szr_left_col.AddSpacer(8) + # left col - location of the robot box + # NOTE: the columns of the T matrix are filled first + szr_location = wx.StaticBoxSizer( + wx.StaticBox( + self.panel, label=ui_labels.BOX_TITLES['location'] + ), wx.HORIZONTAL + ) + szr_grd_loc = wx.GridBagSizer(1, 1) + for i in range(4): + for j in range(3): + idx = (j*4) + i + name = 'Z'+str(idx) + txt_z_element = wx.TextCtrl( + parent=self.panel, name=name, + id=idx, size=(60, -1) + ) + self.widgets[name] = txt_z_element + self.widget_keys[name.lower()] = txt_z_element + txt_z_element.Bind( + wx.EVT_KILL_FOCUS, self.OnZParamChanged + ) + szr_grd_loc.Add( + txt_z_element, pos=(j + 1, i + 1), + flag=wx.ALIGN_LEFT, border=5 + ) + lbl_row = wx.StaticText(self.panel, label='Z'+str(i + 1)) + lbl_col = wx.StaticText(self.panel, label='Z'+str(i + 1)) + lbl_last_row = wx.StaticText( + self.panel, label=' '+str(0 if i < 3 else 1) + ) + szr_grd_loc.Add( + lbl_row, pos=(i+1, 0), flag=wx.RIGHT, border=3 + ) + szr_grd_loc.Add( + lbl_col, pos=(0, i+1), + flag=wx.ALIGN_CENTER_HORIZONTAL, border=3 + ) + szr_grd_loc.Add( + lbl_last_row, pos=(4, i+1), + flag=wx.ALIGN_LEFT, border=3 + ) + szr_location.Add(szr_grd_loc, 0, wx.ALL | wx.EXPAND, 5) + szr_left_col.Add(szr_location, 0, wx.ALL | wx.EXPAND, 0) + # right col - geometric params box + szr_geom_params = wx.StaticBoxSizer( + wx.StaticBox( + self.panel, label=ui_labels.BOX_TITLES['geom_params'] + ), wx.HORIZONTAL + ) + szr_grd_geom = wx.GridBagSizer(0, 5) + self.params_in_grid( + szr_grd_geom, elements=ui_labels.GEOM_PARAMS, + rows=2, cols=5, width=70 + ) + szr_geom_params.Add(szr_grd_geom) + szr_right_col.Add(szr_geom_params, 0, wx.ALL | wx.EXPAND, 0) + szr_right_col.AddSpacer(8) + # right col - dynamic params and external forces box + szr_dyn_params = wx.StaticBoxSizer( + wx.StaticBox( + self.panel, label=ui_labels.BOX_TITLES['dyn_params'] + ), wx.VERTICAL + ) + cmb_link = wx.ComboBox( + self.panel, style=wx.CB_READONLY, size=(100, -1), + name=ui_labels.DYN_PARAMS['link'].name + ) + cmb_link.Bind( + wx.EVT_COMBOBOX, + getattr(self, ui_labels.DYN_PARAMS['link'].handler) + ) + self.widgets['link'] = cmb_link + self.widget_keys['link'] = cmb_link + szr_link = wx.BoxSizer(wx.HORIZONTAL) + szr_link.Add( + wx.StaticText( + self.panel, label=ui_labels.DYN_PARAMS['link'].label + ), proportion=0, + flag=wx.ALL | wx.ALIGN_LEFT, border=5 + ) + szr_link.AddSpacer((4,4)) + szr_link.Add(cmb_link, flag=wx.ALL | wx.ALIGN_RIGHT) + szr_dyn_params.Add(szr_link, flag=wx.ALL | wx.ALIGN_CENTER) + szr_grd_dyn = wx.GridBagSizer(0, 0) + # add dynamic params to the grid + elements = OrderedDict(ui_labels.DYN_PARAMS.items()[1:]) + self.params_in_grid( + szr_grd_dyn, elements=elements, rows=4, cols=6, width=75 + ) + szr_dyn_params.Add(szr_grd_dyn) + szr_dyn_params.AddSpacer(4) + szr_right_col.Add(szr_dyn_params, 0, wx.ALL | wx.EXPAND, 0) + szr_right_col.AddSpacer(8) + # box sizer for the last row in right col + szr_velacc = wx.BoxSizer(wx.HORIZONTAL) + # right col - velocity and acceleration of the base box + szr_base_velacc = wx.StaticBoxSizer( + wx.StaticBox( + self.panel, label=ui_labels.BOX_TITLES['base_vel_acc'] + ), wx.HORIZONTAL + ) + szr_grd_base_velacc = wx.GridBagSizer(0, 0) + self.params_in_grid( + szr_grd_base_velacc, elements=ui_labels.BASE_VEL_ACC, + rows=3, cols=4, width=60 + ) + szr_base_velacc.Add(szr_grd_base_velacc) + szr_velacc.Add(szr_base_velacc) + szr_velacc.AddSpacer(8) + # right col - joint velocity and acceleration box + szr_joint_velacc = wx.StaticBoxSizer( + wx.StaticBox( + self.panel, label=ui_labels.BOX_TITLES['joint_params'] + ), wx.HORIZONTAL + ) + szr_grd_joint_velacc = wx.GridBagSizer(5, 5) + self.params_in_grid( + szr_grd_joint_velacc, elements=ui_labels.JOINT_PARAMS, + rows=3, cols=2, width=75, + ) + szr_joint_velacc.Add(szr_grd_joint_velacc, + flag=wx.ALL | wx.ALIGN_CENTER, border=2 + ) + szr_velacc.Add(szr_joint_velacc, 1, wx.ALL | wx.EXPAND, 0) + szr_right_col.Add(szr_velacc, 1, wx.ALL | wx.EXPAND, 0) + self.szr_topmost.AddSpacer(10) + + def Change(self, index, name, event_object): + prev_value = str(self.robo.get_val(index, name)) + if event_object.Value != prev_value: + if self.robo.put_val(index, name, event_object.Value) == tools.FAIL: + message = "Unacceptable value '%s' has been input in %s%s" \ + % (event_object.Value, name, index) + self.message_error(message) + event_object.Value = prev_value + else: + self.changed = True + + def OnGeoParamChanged(self, event): + frame_index = int(self.widgets['frame'].Value) + self.Change(frame_index, event.EventObject.Name, event.EventObject) + if event.EventObject.Name == 'ant': + self.widgets['type'].SetLabel(self.robo.structure) + if event.EventObject.Name == 'sigma': + self.update_geo_params() + + def OnDynParamChanged(self, event): + link_index = int(self.widgets['link'].Value) + self.Change(link_index, event.EventObject.Name, event.EventObject) + + def OnSpeedChanged(self, event): + joint_index = int(self.widgets['joint'].Value) + self.Change(joint_index, event.EventObject.Name, event.EventObject) + if event.EventObject.Name == 'eta': + self.update_joint_params() + + def OnBaseTwistChanged(self, event): + index = int(event.EventObject.Id) + name = event.EventObject.Name[:-1] + self.Change(index, name, event.EventObject) + + def OnZParamChanged(self, event): + index = int(event.EventObject.Id) + self.Change(index, 'Z', event.EventObject) + + def OnFrameChanged(self, event): + frame_index = int(event.EventObject.Value) + cmb = self.widgets['ant'] + cmb.SetItems([str(i) for i in range(frame_index)]) + self.update_geo_params() + + def OnLinkChanged(self, event): + self.update_dyn_params() + + def OnJointChanged(self, event): + self.update_joint_params() + + def update_params(self, index, pars): + for par in pars: + widget = self.widgets[par] + widget.ChangeValue(str(self.robo.get_val(index, par))) + + def update_geo_params(self): + self.robo.set_defaults(joint=True, geom=True) + pars = self._extract_param_names(ui_labels.GEOM_PARAMS) + index = int(self.widgets['frame'].Value) + for par in pars[0:3]: + self.widgets[par].SetValue(str(self.robo.get_val(index, par))) + self.update_params(index, pars[3:]) + self.update_joint_params() + + def update_dyn_params(self): + pars = self._extract_param_names(ui_labels.DYN_PARAMS) + index = int(self.widgets['link'].Value) + self.update_params(index, pars) + + def update_joint_params(self): + self.robo.set_defaults(joint=True) + pars = self._extract_param_names(ui_labels.JOINT_PARAMS) + index = int(self.widgets['joint'].Value) + self.widgets[pars[0]].SetValue( + str(self.robo.get_val(index, pars[0])) + ) + self.update_params(index, pars[1:]) + + def update_base_twist_params(self): + pars = dict( + ui_labels.BASE_VEL_ACC.items() + \ + ui_labels.GRAVITY_CMPNTS.items() + ) + for key in pars: + par = pars[key].name + idx = pars[key].id + name = par[:-1] + widget = self.widgets[par] + widget.ChangeValue(str(self.robo.get_val(idx, name))) + + def update_z_params(self): + T = self.robo.Z + for i in range(12): + widget = self.widgets['Z' + str(i)] + widget.ChangeValue(str(T[i])) + + def _extract_param_names(self, params): + names = list() + for key in params: + if key in ['frame', 'link', 'joint']: continue + names.append(params[key].name) + return names + + def feed_data(self): + # Robot Type + names = [ + ('name', self.robo.name), ('NF', self.robo.nf), + ('NL', self.robo.nl), ('NJ', self.robo.nj), + ('type', self.robo.structure), + ('floating', self.robo.is_floating), + ('mobile', self.robo.is_mobile), + ('loops', self.robo.nj-self.robo.nl) + ] + for name, info in names: + label = self.widgets[name] + label.SetLabel(str(info)) + idx_start = 1 + if self.robo.is_floating or self.robo.is_mobile: + idx_start = 0 + lsts = [ + ('frame', [str(i) for i in range(1, self.robo.NF)]), + ('link', [ + str(i) for i in range(idx_start, self.robo.NL) + ]), + ('joint', [str(i) for i in range(1, self.robo.NJ)]), + ('ant', ['0']), ('sigma', ['0', '1', '2']), + ('mu', ['0', '1']), ('eta', ['0', '1']) + ] + for name, lst in lsts: + cmb = self.widgets[name] + cmb.SetItems(lst) + cmb.SetSelection(0) + self.update_geo_params() + self.update_dyn_params() + self.update_joint_params() + self.update_base_twist_params() + self.update_z_params() + self.update_menu() + self.changed = False + self.par_dict = {} + + def update_menu(self): + """Update menu items""" + # get menu bar + menu_bar = self.GetMenuBar() + # get list of menus and assign to individual objects + menu_lst = menu_bar.GetMenus() + geom_menu = menu_lst[1][0] + kin_menu = menu_lst[2][0] + dyn_menu = menu_lst[3][0] + # geom_menu - get menu items + idx = len(geom_menu.GetMenuItems()) - 1 + m_geom_constraint = geom_menu.FindItemByPosition(idx) + # kin_menu - get menu items + idx = len(kin_menu.GetMenuItems()) - 1 + m_kin_constraint = kin_menu.FindItemByPosition(idx) + # dyn_menu - get menu items + idx = len(dyn_menu.GetMenuItems()) - 1 + m_ddym = dyn_menu.FindItemByPosition(idx) + # set direct dynamic model status + if self.robo.is_mobile or \ + (self.robo.structure is tools.CLOSED_LOOP): + ddym_enable = False + else: + ddym_enable = True + # set constraint equations status + if self.robo.structure is not tools.CLOSED_LOOP: + constraint_enable = False + else: + constraint_enable = True + # enable/disable menu items + m_ddym.Enable(ddym_enable) + m_geom_constraint.Enable(constraint_enable) + m_kin_constraint.Enable(constraint_enable) + menu_bar.UpdateMenus() + + def create_menu(self): + """Create the menu bar""" + menu_bar = wx.MenuBar() + # menu item - file + file_menu = wx.Menu() + m_new = wx.MenuItem( + file_menu, wx.ID_NEW, ui_labels.FILE_MENU['m_new'] + ) + self.Bind(wx.EVT_MENU, self.OnNew, m_new) + file_menu.AppendItem(m_new) + m_open = wx.MenuItem( + file_menu, wx.ID_OPEN, ui_labels.FILE_MENU['m_open'] + ) + self.Bind(wx.EVT_MENU, self.OnOpen, m_open) + file_menu.AppendItem(m_open) + m_save = wx.MenuItem( + file_menu, wx.ID_SAVE, ui_labels.FILE_MENU['m_save'] + ) + self.Bind(wx.EVT_MENU, self.OnSave, m_save) + file_menu.AppendItem(m_save) + m_save_as = wx.MenuItem( + file_menu, wx.ID_SAVEAS, ui_labels.FILE_MENU['m_save_as'] + ) + self.Bind(wx.EVT_MENU, self.OnSaveAs, m_save_as) + file_menu.AppendItem(m_save_as) + m_pref = wx.MenuItem( + file_menu, wx.ID_ANY, ui_labels.FILE_MENU['m_pref'] + ) + file_menu.AppendItem(m_pref) + file_menu.AppendSeparator() + m_exit = wx.MenuItem( + file_menu, wx.ID_EXIT, ui_labels.FILE_MENU['m_exit'] + ) + self.Bind(wx.EVT_MENU, self.OnClose, m_exit) + file_menu.AppendItem(m_exit) + menu_bar.Append(file_menu, ui_labels.MAIN_MENU['file_menu']) + # menu item - geometric + geom_menu = wx.Menu() + m_trans_matrix = wx.MenuItem( + geom_menu, wx.ID_ANY, ui_labels.GEOM_MENU['m_trans_matrix'] + ) + self.Bind( + wx.EVT_MENU, self.OnTransformationMatrix, m_trans_matrix + ) + geom_menu.AppendItem(m_trans_matrix) + m_fast_dgm = wx.MenuItem( + geom_menu, wx.ID_ANY, ui_labels.GEOM_MENU['m_fast_dgm'] + ) + self.Bind(wx.EVT_MENU, self.OnFastGeometricModel, m_fast_dgm) + geom_menu.AppendItem(m_fast_dgm) + m_igm_paul = wx.MenuItem( + geom_menu, wx.ID_ANY, ui_labels.GEOM_MENU['m_igm_paul'] + ) + self.Bind(wx.EVT_MENU, self.OnIgmPaul, m_igm_paul) + geom_menu.AppendItem(m_igm_paul) + m_igm_pieper = wx.MenuItem( + geom_menu, wx.ID_ANY, ui_labels.GEOM_MENU['m_igm_pieper'] + ) + self.Bind(wx.EVT_MENU, self.OnIgmPieper, m_igm_pieper) + geom_menu.AppendItem(m_igm_pieper) + m_geom_constraint = wx.MenuItem( + geom_menu, wx.ID_ANY, ui_labels.GEOM_MENU['m_geom_constraint'] + ) + self.Bind(wx.EVT_MENU, self.OnConstraintGeoEq, m_geom_constraint) + geom_menu.AppendItem(m_geom_constraint) + menu_bar.Append(geom_menu, ui_labels.MAIN_MENU['geom_menu']) + # menu item - kinematic + kin_menu = wx.Menu() + m_jac_matrix = wx.MenuItem( + kin_menu, wx.ID_ANY, ui_labels.KIN_MENU['m_jac_matrix'] + ) + self.Bind(wx.EVT_MENU, self.OnJacobianMatrix, m_jac_matrix) + kin_menu.AppendItem(m_jac_matrix) + m_determinant = wx.MenuItem( + kin_menu, wx.ID_ANY, ui_labels.KIN_MENU['m_determinant'] + ) + self.Bind(wx.EVT_MENU, self.OnDeterminant, m_determinant) + kin_menu.AppendItem(m_determinant) + m_vel = wx.MenuItem( + kin_menu, wx.ID_ANY, ui_labels.KIN_MENU['m_vel'] + ) + self.Bind(wx.EVT_MENU, self.OnVelocities, m_vel) + kin_menu.AppendItem(m_vel) + m_acc = wx.MenuItem( + kin_menu, wx.ID_ANY, ui_labels.KIN_MENU['m_acc'] + ) + self.Bind(wx.EVT_MENU, self.OnAccelerations, m_acc) + kin_menu.AppendItem(m_acc) + m_jpqp = wx.MenuItem( + kin_menu, wx.ID_ANY, ui_labels.KIN_MENU['m_jpqp'] + ) + self.Bind(wx.EVT_MENU, self.OnJpqp, m_jpqp) + kin_menu.AppendItem(m_jpqp) + #TODO: add the dialog, ask for projection frame + m_kin_constraint = wx.MenuItem( + kin_menu, wx.ID_ANY, ui_labels.KIN_MENU['m_kin_constraint'] + ) + self.Bind(wx.EVT_MENU, self.OnCkel, m_kin_constraint) + kin_menu.AppendItem(m_kin_constraint) + menu_bar.Append(kin_menu, ui_labels.MAIN_MENU['kin_menu']) + # menu item - dynamic + dyn_menu = wx.Menu() + m_idym = wx.MenuItem( + dyn_menu, wx.ID_ANY, ui_labels.DYN_MENU['m_idym'] + ) + self.Bind(wx.EVT_MENU, self.OnInverseDynamic, m_idym) + dyn_menu.AppendItem(m_idym) + m_inertia_matrix = wx.MenuItem( + dyn_menu, wx.ID_ANY, ui_labels.DYN_MENU['m_inertia_matrix'] + ) + self.Bind(wx.EVT_MENU, self.OnInertiaMatrix, m_inertia_matrix) + dyn_menu.AppendItem(m_inertia_matrix) + m_h_term = wx.MenuItem( + dyn_menu, wx.ID_ANY, ui_labels.DYN_MENU['m_h_term'] + ) + self.Bind(wx.EVT_MENU, self.OnCentrCoriolGravTorq, m_h_term) + dyn_menu.AppendItem(m_h_term) + m_ddym = wx.MenuItem( + dyn_menu, wx.ID_ANY, ui_labels.DYN_MENU['m_ddym'] + ) + self.Bind(wx.EVT_MENU, self.OnDirectDynamicModel, m_ddym) + dyn_menu.AppendItem(m_ddym) + menu_bar.Append(dyn_menu, ui_labels.MAIN_MENU['dyn_menu']) + # menu item - identification + iden_menu = wx.Menu() + m_base_inertial_params = wx.MenuItem( + iden_menu, wx.ID_ANY, + ui_labels.IDEN_MENU['m_base_inertial_params'] + ) + self.Bind( + wx.EVT_MENU, self.OnBaseInertialParams, m_base_inertial_params + ) + iden_menu.AppendItem(m_base_inertial_params) + m_dyn_iden_model = wx.MenuItem( + iden_menu, wx.ID_ANY, ui_labels.IDEN_MENU['m_dyn_iden_model'] + ) + self.Bind(wx.EVT_MENU, self.OnDynIdentifModel, m_dyn_iden_model) + iden_menu.AppendItem(m_dyn_iden_model) + # TODO: uncomment lines below to include Energy Identification + # Model + #m_energy_iden_model = wx.MenuItem( + # iden_menu, wx.ID_ANY, + # ui_labels.IDEN_MENU['m_energy_iden_model'] + #) + #self.Bind( + # wx.EVT_MENU, self.OnEnergyIdentifModel, m_energy_iden_model + #) + #iden_menu.AppendItem(m_energy_iden_model) + menu_bar.Append(iden_menu, ui_labels.MAIN_MENU['iden_menu']) + # menu item - visualisation + viz_menu = wx.Menu() + m_viz = wx.MenuItem( + viz_menu, wx.ID_ANY, ui_labels.VIZ_MENU['m_viz'] + ) + self.Bind(wx.EVT_MENU, self.OnVisualisation, m_viz) + viz_menu.AppendItem(m_viz) + menu_bar.Append(viz_menu, ui_labels.MAIN_MENU['viz_menu']) + # set menu bar + self.SetMenuBar(menu_bar) + + def message_error(self, message): + dlg = wx.MessageDialog( + parent=None, + message=message, + caption='Error', + style=wx.OK | wx.ICON_ERROR + ) + dlg.ShowModal() + dlg.Destroy() + + def message_warning(self, message): + dlg = wx.MessageDialog( + parent=None, + message=message, + caption='Warning', + style=wx.OK | wx.ICON_WARNING + ) + dlg.ShowModal() + dlg.Destroy() + + def message_info(self, message): + dlg = wx.MessageDialog( + parent=None, + message=message, + caption='Information', + style=wx.OK | wx.ICON_INFORMATION + ) + dlg.ShowModal() + dlg.Destroy() + + def model_success(self, out_file_path): + msg = ("The output of the computed model has been saved at:\n") + msg = msg + out_file_path + self.message_info(msg) + + def prompt_file_save(self, model_symo): + """ + Prompt a file dialog box and save the file. + + Args: + model_symo: An instance of SymbolManager. + Returns: + A string indicating the location (file path) where the + output file is saved. + """ + # get the old file name + old_file_path = model_symo.file_out.name + old_fname = os.path.split(old_file_path)[1][:-4] + # get extension + pattern = re.compile('([\s\w-]*)[_]([a-z]*)') + match = re.match(pattern, old_fname) + extension = match.group(2).strip() + # create file dialog + dialog = wx.FileDialog( + self, + message="Rename output file", + style=wx.FD_SAVE | wx.FD_OVERWRITE_PROMPT, + defaultFile=old_fname, + defaultDir=self.robo.directory + ) + # get the new file name if wx.ID_OK + if not dialog.ShowModal() == wx.ID_CANCEL: + new_fname = dialog.GetFilename() + # add extension based on model type + match = re.match(pattern, new_fname) + if match: + if not match.group(2).strip() == extension: + new_fname = new_fname + "_" + extension + else: + new_fname = new_fname + "_" + extension + # add txt extension + new_fname = new_fname + ".txt" + new_file_path = os.path.join(dialog.GetDirectory(), new_fname) + # perform move operation + shutil.move(old_file_path, new_file_path) + return new_file_path + else: + return old_file_path + + def OnNew(self, event): + dialog = ui_definition.DialogDefinition( + ui_labels.MAIN_WIN['prog_name'], + self.robo.name, self.robo.nl, + self.robo.nj, self.robo.structure, + self.robo.is_floating, self.robo.is_mobile + ) + if dialog.ShowModal() == wx.ID_OK: + result = dialog.get_values() + new_robo = Robot( + name=result['name'], + NL=result['num_links'], + NJ=result['num_joints'], + NF=result['num_frames'], + structure=result['structure'], + is_floating=result['is_floating'], + is_mobile=result['is_mobile'] + ) + new_robo.set_defaults(base=True) + if result['keep_geo']: + nf = min(self.robo.NF, new_robo.NF) + new_robo.ant[:nf] = self.robo.ant[:nf] + new_robo.sigma[:nf] = self.robo.sigma[:nf] + new_robo.mu[:nf] = self.robo.mu[:nf] + new_robo.gamma[:nf] = self.robo.gamma[:nf] + new_robo.alpha[:nf] = self.robo.alpha[:nf] + new_robo.theta[:nf] = self.robo.theta[:nf] + new_robo.b[:nf] = self.robo.b[:nf] + new_robo.d[:nf] = self.robo.d[:nf] + new_robo.r[:nf] = self.robo.r[:nf] + if result['keep_dyn']: + nl = min(self.robo.NL, new_robo.NL) + new_robo.Nex[:nl] = self.robo.Nex[:nl] + new_robo.Fex[:nl] = self.robo.Fex[:nl] + new_robo.FS[:nl] = self.robo.FS[:nl] + new_robo.IA[:nl] = self.robo.IA[:nl] + new_robo.FV[:nl] = self.robo.FV[:nl] + new_robo.MS[:nl] = self.robo.MS[:nl] + new_robo.M[:nl] = self.robo.M[:nl] + new_robo.J[:nl] = self.robo.J[:nl] + if result['keep_joint']: + nj = min(self.robo.NJ, new_robo.NJ) + new_robo.eta[:nj] = self.robo.eta[:nj] + new_robo.k[:nj] = self.robo.k[:nj] + new_robo.qdot[:nj] = self.robo.qdot[:nj] + new_robo.qddot[:nj] = self.robo.qddot[:nj] + new_robo.GAM[:nj] = self.robo.GAM[:nj] + if result['keep_base']: + new_robo.Z = self.robo.Z + new_robo.w0 = self.robo.w0 + new_robo.wdot0 = self.robo.wdot0 + new_robo.v0 = self.robo.v0 + new_robo.vdot0 = self.robo.vdot0 + new_robo.G = self.robo.G + new_robo.set_defaults(joint=True) + self.robo = new_robo + self.robo.directory = filemgr.get_folder_path(self.robo.name) + self.feed_data() + dialog.Destroy() + + def OnOpen(self, event): + if self.changed: + dialog_res = wx.MessageBox( + 'Do you want to save changes?', + 'Please confirm', + wx.ICON_QUESTION | wx.YES_NO | wx.CANCEL, + self + ) + if dialog_res == wx.CANCEL: + return + elif dialog_res == wx.YES: + if self.OnSave(None) == tools.FAIL: + return + dialog = wx.FileDialog( + self, + message="Choose PAR file", + style=wx.FD_OPEN, + wildcard='*.par', + defaultDir=filemgr.get_base_path() + ) + if dialog.ShowModal() == wx.ID_OK: + new_robo, flag = parfile.readpar( + dialog.GetFilename()[:-4], dialog.GetPath() + ) + if new_robo is None: + self.message_error('File could not be read!') + else: + if flag == tools.FAIL: + self.message_warning( + "While reading file an error occured." + ) + self.robo = new_robo + self.feed_data() + + def OnSave(self, event): + parfile.writepar(self.robo) + self.changed = False + + def OnSaveAs(self, event): + dialog = wx.FileDialog( + self, + message="Save PAR file", + style=wx.FD_SAVE | wx.FD_OVERWRITE_PROMPT, + defaultFile=self.robo.name+'.par', + defaultDir=self.robo.directory, + wildcard='*.par' + ) + if dialog.ShowModal() == wx.ID_CANCEL: + return tools.FAIL + self.robo.directory = dialog.GetDirectory() + self.robo.name = dialog.GetFilename()[:-4] + parfile.writepar(self.robo) + self.widgets['name'].SetLabel(self.robo.name) + self.changed = False + + def OnTransformationMatrix(self, event): + dialog = ui_geometry.DialogTrans( + ui_labels.MAIN_WIN['prog_name'], self.robo.NF + ) + if dialog.ShowModal() == wx.ID_OK: + frames, trig_subs = dialog.GetValues() + model_symo = geometry.direct_geometric( + self.robo, frames, trig_subs + ) + out_file_path = self.prompt_file_save(model_symo) + self.model_success(out_file_path) + dialog.Destroy() + + def OnFastGeometricModel(self, event): + dialog = ui_geometry.DialogFast( + ui_labels.MAIN_WIN['prog_name'], self.robo.NF + ) + if dialog.ShowModal() == wx.ID_OK: + i, j = dialog.GetValues() + model_symo = geometry.direct_geometric_fast(self.robo, i, j) + out_file_path = self.prompt_file_save(model_symo) + self.model_success(out_file_path) + dialog.Destroy() + + def OnIgmPaul(self, event): + dialog = ui_geometry.DialogPaul( + ui_labels.MAIN_WIN['prog_name'], + self.robo.endeffectors, + str(invgeom.EMPTY) + ) + if dialog.ShowModal() == wx.ID_OK: + lst_T, n = dialog.get_values() + model_symo = invgeom.igm_paul(self.robo, lst_T, n) + out_file_path = self.prompt_file_save(model_symo) + self.model_success(out_file_path) + dialog.Destroy() + + def OnIgmPieper(self, event): + dialog = ui_geometry.DialogPieper( + ui_labels.MAIN_WIN['prog_name'], + self.robo.endeffectors, + str(invgeom.EMPTY) + ) + if dialog.ShowModal() == wx.ID_OK: + lst_T, n = dialog.get_values() + model_symo = pieper.igm_pieper(self.robo, lst_T, n) + out_file_path = self.prompt_file_save(model_symo) + self.model_success(out_file_path) + dialog.Destroy() + + def OnConstraintGeoEq(self, event): + pass + + def OnJacobianMatrix(self, event): + dialog = ui_kinematics.DialogJacobian( + ui_labels.MAIN_WIN['prog_name'], self.robo + ) + if dialog.ShowModal() == wx.ID_OK: + n, i, j = dialog.get_values() + model_symo = kinematics.jacobian(self.robo, n, i, j) + out_file_path = self.prompt_file_save(model_symo) + self.model_success(out_file_path) + dialog.Destroy() + + def OnDeterminant(self, event): + dialog = ui_kinematics.DialogDeterminant( + ui_labels.MAIN_WIN['prog_name'], self.robo + ) + if dialog.ShowModal() == wx.ID_OK: + model_symo = kinematics.jacobian_determinant( + self.robo, *dialog.get_values() + ) + out_file_path = self.prompt_file_save(model_symo) + self.model_success(out_file_path) + dialog.Destroy() + + def OnCkel(self, event): + model_symo = kinematics.kinematic_constraints(self.robo) + if model_symo == tools.FAIL: + self.message_warning("There are no loops") + else: + out_file_path = self.prompt_file_save(model_symo) + self.model_success(out_file_path) + + def OnVelocities(self, event): + model_symo = kinematics.velocities(self.robo) + out_file_path = self.prompt_file_save(model_symo) + self.model_success(out_file_path) + + def OnAccelerations(self, event): + model_symo = kinematics.accelerations(self.robo) + out_file_path = self.prompt_file_save(model_symo) + self.model_success(out_file_path) + + def OnJpqp(self, event): + model_symo = kinematics.jdot_qdot(self.robo) + out_file_path = self.prompt_file_save(model_symo) + self.model_success(out_file_path) + + def OnInverseDynamic(self, event): + model_symo = self.robo.compute_idym() + out_file_path = self.prompt_file_save(model_symo) + self.model_success(out_file_path) + + def OnInertiaMatrix(self, event): + model_symo = self.robo.compute_inertiamatrix() + out_file_path = self.prompt_file_save(model_symo) + self.model_success(out_file_path) + + def OnCentrCoriolGravTorq(self, event): + model_symo = self.robo.compute_pseudotorques() + out_file_path = self.prompt_file_save(model_symo) + self.model_success(out_file_path) + + def OnDirectDynamicModel(self, event): + model_symo = self.robo.compute_ddym() + out_file_path = self.prompt_file_save(model_symo) + self.model_success(out_file_path) + + def OnBaseInertialParams(self, event): + model_symo, base_robo = self.robo.compute_baseparams() + out_file_path = self.prompt_file_save(model_symo) + self.model_success(out_file_path) + parfile.writepar(base_robo) + msg = ("A new robot with the Base Inertial Parameters was\n") + msg = msg + ("created and the corresponding PAR file is ") + msg = msg + ("saved at:\n\n") + msg = msg + base_robo.par_file_path + self.message_info(msg) + + def OnDynIdentifModel(self, event): + model_symo = self.robo.compute_dynidenmodel() + out_file_path = self.prompt_file_save(model_symo) + self.model_success(out_file_path) + + def OnVisualisation(self, event): + dialog = ui_definition.DialogVisualisation( + ui_labels.MAIN_WIN['prog_name'], self.robo, self.par_dict + ) + if dialog.has_syms(): + if dialog.ShowModal() == wx.ID_OK: + self.par_dict = dialog.get_values() + graphics.MainWindow( + ui_labels.MAIN_WIN['prog_name'], self.robo, + self.par_dict, self + ) + else: + graphics.MainWindow( + ui_labels.MAIN_WIN['prog_name'], self.robo, + self.par_dict, self + ) + + def OnClose(self, event): + if self.changed: + result = wx.MessageBox( + 'Do you want to save changes?', 'Please confirm', + wx.ICON_QUESTION | wx.YES_NO | wx.CANCEL, self) + if result == wx.YES: + if self.OnSave(event) == tools.FAIL: + return + elif result == wx.CANCEL: + return + configfile.set_last_robot(self.robo.par_file_path) + self.Destroy() + wx.GetApp().ExitMainLoop() + + diff --git a/pysymoro/gui/__init__.py b/symoroutils/__init__.py similarity index 100% rename from pysymoro/gui/__init__.py rename to symoroutils/__init__.py diff --git a/symoroutils/configfile.py b/symoroutils/configfile.py new file mode 100644 index 0000000..98200f1 --- /dev/null +++ b/symoroutils/configfile.py @@ -0,0 +1,111 @@ +# -*- coding: utf-8 -*- + + +# This file is part of the OpenSYMORO project. Please see +# https://github.com/symoro/symoro/blob/master/LICENCE for the licence. + + +"""Setup, read, write a config file, folder for SYMORO package.""" + + +import os +import ConfigParser + + +CONFIG_FILE_NAME = 'settings.conf' + + +def get_prog_config_path(): + """ + Return the folder path for storing SYMORO program settings. + + Returns: + A string specifying the path to store SYMORO settings. + """ + prog_name = 'symoro' + if os.name is 'nt': + return os.path.join(os.environ['APPDATA'], prog_name) + else: + return os.path.join(os.environ['HOME'], '.config', prog_name) + + +def get_config_file_path(): + """ + Return the path for SYMORO settings file. + + Returns: + A string specifying the path to SYOMRO `settings.conf` file. + """ + return os.path.join(get_prog_config_path(), CONFIG_FILE_NAME) + + +def make_config_folder(): + """ + Check if the config folder exists and create the same if it does + not exist. + """ + config_folder_path = get_prog_config_path() + if not os.path.exists(config_folder_path): + os.makedirs(config_folder_path) + + +def save_config(curr_config): + """ + Save the current configuration to the disk in a file. + + Args: + curr_config: An instance of the `ConfigParser` class. + """ + make_config_folder() + if not isinstance(curr_config, ConfigParser.ConfigParser): + raise TypeError( + "`curr_config` should be an instance of `ConfigParser`" + ) + config_file_path = get_config_file_path() + with open(config_file_path, 'w') as config_file: + curr_config.write(config_file) + + +def get_config(): + """ + Return the program settings from the settings file. When a settings + file does not exist return a new settings object. + + Returns: + A `ConfigParser` object with the program settings. + """ + config = ConfigParser.ConfigParser() + config_file_path = get_config_file_path() + if os.path.exists(config_file_path): + # check if settings file exists + config.read(config_file_path) + return config + + +def get_last_robot(): + """ + Return the path to the last used robot's PAR file. + + Returns: + A string specifying the path to the last used robot's PAR file. + """ + config = get_config() + if config.has_option('startup', 'last-robot'): + return config.get('startup', 'last-robot') + else: + return None + + +def set_last_robot(robo_par_file): + """ + Set the last used robot in the settings file. + + Args: + robo_par_file: A string specifying the path to robot's PAR file. + """ + config = ConfigParser.ConfigParser() + config.add_section('startup') + config.set('startup', 'last-robot', robo_par_file) + save_config(config) + + diff --git a/symoroutils/enum.py b/symoroutils/enum.py new file mode 100644 index 0000000..17a4fb2 --- /dev/null +++ b/symoroutils/enum.py @@ -0,0 +1,812 @@ +""" +Python Enumerations + +NOTE: This module is to provide enum support in Python27. Technically, +this module could be made as a dependency for symoro package. However, +in order to reduce the number of dependencies, the source code is +included here. See below under the LICENSE section for further details +on usage of this file. + +######################### LICENSE begins ############################## + +Copyright (c) 2013, Ethan Furman. +All rights reserved. + +Redistribution and use in source and binary forms, with or without +modification, are permitted provided that the following conditions +are met: + + Redistributions of source code must retain the above + copyright notice, this list of conditions and the + following disclaimer. + + Redistributions in binary form must reproduce the above + copyright notice, this list of conditions and the following + disclaimer in the documentation and/or other materials + provided with the distribution. + + Neither the name Ethan Furman nor the names of any + contributors may be used to endorse or promote products + derived from this software without specific prior written + permission. + +THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE +LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +POSSIBILITY OF SUCH DAMAGE. + +########################### LICENSE ends ############################## +""" + +import sys as _sys + +__all__ = ['Enum', 'IntEnum', 'unique'] + +pyver = float('%s.%s' % _sys.version_info[:2]) + +try: + any +except NameError: + def any(iterable): + for element in iterable: + if element: + return True + return False + +try: + from collections import OrderedDict +except ImportError: + OrderedDict = None + +try: + basestring +except NameError: + # In Python 2 basestring is the ancestor of both str and unicode + # in Python 3 it's just str, but was missing in 3.1 + basestring = str + +class _RouteClassAttributeToGetattr(object): + """Route attribute access on a class to __getattr__. + + This is a descriptor, used to define attributes that act differently when + accessed through an instance and through a class. Instance access remains + normal, but access to an attribute through a class will be routed to the + class's __getattr__ method; this is done by raising AttributeError. + + """ + def __init__(self, fget=None): + self.fget = fget + + def __get__(self, instance, ownerclass=None): + if instance is None: + raise AttributeError() + return self.fget(instance) + + def __set__(self, instance, value): + raise AttributeError("can't set attribute") + + def __delete__(self, instance): + raise AttributeError("can't delete attribute") + + +def _is_descriptor(obj): + """Returns True if obj is a descriptor, False otherwise.""" + return ( + hasattr(obj, '__get__') or + hasattr(obj, '__set__') or + hasattr(obj, '__delete__')) + + +def _is_dunder(name): + """Returns True if a __dunder__ name, False otherwise.""" + return (name[:2] == name[-2:] == '__' and + name[2:3] != '_' and + name[-3:-2] != '_' and + len(name) > 4) + + +def _is_sunder(name): + """Returns True if a _sunder_ name, False otherwise.""" + return (name[0] == name[-1] == '_' and + name[1:2] != '_' and + name[-2:-1] != '_' and + len(name) > 2) + + +def _make_class_unpicklable(cls): + """Make the given class un-picklable.""" + def _break_on_call_reduce(self, protocol=None): + raise TypeError('%r cannot be pickled' % self) + cls.__reduce_ex__ = _break_on_call_reduce + cls.__module__ = '' + + +class _EnumDict(dict): + """Track enum member order and ensure member names are not reused. + + EnumMeta will use the names found in self._member_names as the + enumeration member names. + + """ + def __init__(self): + super(_EnumDict, self).__init__() + self._member_names = [] + + def __setitem__(self, key, value): + """Changes anything not dundered or not a descriptor. + + If a descriptor is added with the same name as an enum member, the name + is removed from _member_names (this may leave a hole in the numerical + sequence of values). + + If an enum member name is used twice, an error is raised; duplicate + values are not checked for. + + Single underscore (sunder) names are reserved. + + Note: in 3.x __order__ is simply discarded as a not necessary piece + leftover from 2.x + + """ + if pyver >= 3.0 and key == '__order__': + return + if _is_sunder(key): + raise ValueError('_names_ are reserved for future Enum use') + elif _is_dunder(key): + pass + elif key in self._member_names: + # descriptor overwriting an enum? + raise TypeError('Attempted to reuse key: %r' % key) + elif not _is_descriptor(value): + if key in self: + # enum overwriting a descriptor? + raise TypeError('Key already defined as: %r' % self[key]) + self._member_names.append(key) + super(_EnumDict, self).__setitem__(key, value) + + +# Dummy value for Enum as EnumMeta explicity checks for it, but of course until +# EnumMeta finishes running the first time the Enum class doesn't exist. This +# is also why there are checks in EnumMeta like `if Enum is not None` +Enum = None + + +class EnumMeta(type): + """Metaclass for Enum""" + @classmethod + def __prepare__(metacls, cls, bases): + return _EnumDict() + + def __new__(metacls, cls, bases, classdict): + # an Enum class is final once enumeration items have been defined; it + # cannot be mixed with other types (int, float, etc.) if it has an + # inherited __new__ unless a new __new__ is defined (or the resulting + # class will fail). + if type(classdict) is dict: + original_dict = classdict + classdict = _EnumDict() + for k, v in original_dict.items(): + classdict[k] = v + + member_type, first_enum = metacls._get_mixins_(bases) + __new__, save_new, use_args = metacls._find_new_(classdict, member_type, + first_enum) + # save enum items into separate mapping so they don't get baked into + # the new class + members = dict((k, classdict[k]) for k in classdict._member_names) + for name in classdict._member_names: + del classdict[name] + + # py2 support for definition order + __order__ = classdict.get('__order__') + if __order__ is None: + if pyver < 3.0: + __order__ = [name for (name, value) in sorted(members.items(), key=lambda item: item[1])] + else: + __order__ = classdict._member_names + else: + del classdict['__order__'] + if pyver < 3.0: + __order__ = __order__.replace(',', ' ').split() + aliases = [name for name in members if name not in __order__] + __order__ += aliases + + # check for illegal enum names (any others?) + invalid_names = set(members) & set(['mro']) + if invalid_names: + raise ValueError('Invalid enum member name(s): %s' % ( + ', '.join(invalid_names), )) + + # create our new Enum type + enum_class = super(EnumMeta, metacls).__new__(metacls, cls, bases, classdict) + enum_class._member_names_ = [] # names in random order + if OrderedDict is not None: + enum_class._member_map_ = OrderedDict() + else: + enum_class._member_map_ = {} # name->value map + enum_class._member_type_ = member_type + + # Reverse value->name map for hashable values. + enum_class._value2member_map_ = {} + + # instantiate them, checking for duplicates as we go + # we instantiate first instead of checking for duplicates first in case + # a custom __new__ is doing something funky with the values -- such as + # auto-numbering ;) + if __new__ is None: + __new__ = enum_class.__new__ + for member_name in __order__: + value = members[member_name] + if not isinstance(value, tuple): + args = (value, ) + else: + args = value + if member_type is tuple: # special case for tuple enums + args = (args, ) # wrap it one more time + if not use_args or not args: + enum_member = __new__(enum_class) + if not hasattr(enum_member, '_value_'): + enum_member._value_ = value + else: + enum_member = __new__(enum_class, *args) + if not hasattr(enum_member, '_value_'): + enum_member._value_ = member_type(*args) + value = enum_member._value_ + enum_member._name_ = member_name + enum_member.__objclass__ = enum_class + enum_member.__init__(*args) + # If another member with the same value was already defined, the + # new member becomes an alias to the existing one. + for name, canonical_member in enum_class._member_map_.items(): + if canonical_member.value == enum_member._value_: + enum_member = canonical_member + break + else: + # Aliases don't appear in member names (only in __members__). + enum_class._member_names_.append(member_name) + enum_class._member_map_[member_name] = enum_member + try: + # This may fail if value is not hashable. We can't add the value + # to the map, and by-value lookups for this value will be + # linear. + enum_class._value2member_map_[value] = enum_member + except TypeError: + pass + + + # If a custom type is mixed into the Enum, and it does not know how + # to pickle itself, pickle.dumps will succeed but pickle.loads will + # fail. Rather than have the error show up later and possibly far + # from the source, sabotage the pickle protocol for this class so + # that pickle.dumps also fails. + # + # However, if the new class implements its own __reduce_ex__, do not + # sabotage -- it's on them to make sure it works correctly. We use + # __reduce_ex__ instead of any of the others as it is preferred by + # pickle over __reduce__, and it handles all pickle protocols. + unpicklable = False + if '__reduce_ex__' not in classdict: + if member_type is not object: + methods = ('__getnewargs_ex__', '__getnewargs__', + '__reduce_ex__', '__reduce__') + if not any(m in member_type.__dict__ for m in methods): + _make_class_unpicklable(enum_class) + unpicklable = True + + + # double check that repr and friends are not the mixin's or various + # things break (such as pickle) + for name in ('__repr__', '__str__', '__format__', '__reduce_ex__'): + class_method = getattr(enum_class, name) + obj_method = getattr(member_type, name, None) + enum_method = getattr(first_enum, name, None) + if name not in classdict and class_method is not enum_method: + if name == '__reduce_ex__' and unpicklable: + continue + setattr(enum_class, name, enum_method) + + # method resolution and int's are not playing nice + # Python's less than 2.6 use __cmp__ + + if pyver < 2.6: + + if issubclass(enum_class, int): + setattr(enum_class, '__cmp__', getattr(int, '__cmp__')) + + elif pyver < 3.0: + + if issubclass(enum_class, int): + for method in ( + '__le__', + '__lt__', + '__gt__', + '__ge__', + '__eq__', + '__ne__', + '__hash__', + ): + setattr(enum_class, method, getattr(int, method)) + + # replace any other __new__ with our own (as long as Enum is not None, + # anyway) -- again, this is to support pickle + if Enum is not None: + # if the user defined their own __new__, save it before it gets + # clobbered in case they subclass later + if save_new: + setattr(enum_class, '__member_new__', enum_class.__dict__['__new__']) + setattr(enum_class, '__new__', Enum.__dict__['__new__']) + return enum_class + + def __call__(cls, value, names=None, module=None, type=None): + """Either returns an existing member, or creates a new enum class. + + This method is used both when an enum class is given a value to match + to an enumeration member (i.e. Color(3)) and for the functional API + (i.e. Color = Enum('Color', names='red green blue')). + + When used for the functional API: `module`, if set, will be stored in + the new class' __module__ attribute; `type`, if set, will be mixed in + as the first base class. + + Note: if `module` is not set this routine will attempt to discover the + calling module by walking the frame stack; if this is unsuccessful + the resulting class will not be pickleable. + + """ + if names is None: # simple value lookup + return cls.__new__(cls, value) + # otherwise, functional API: we're creating a new Enum type + return cls._create_(value, names, module=module, type=type) + + def __contains__(cls, member): + return isinstance(member, cls) and member.name in cls._member_map_ + + def __delattr__(cls, attr): + # nicer error message when someone tries to delete an attribute + # (see issue19025). + if attr in cls._member_map_: + raise AttributeError( + "%s: cannot delete Enum member." % cls.__name__) + super(EnumMeta, cls).__delattr__(attr) + + def __dir__(self): + return (['__class__', '__doc__', '__members__', '__module__'] + + self._member_names_) + + @property + def __members__(cls): + """Returns a mapping of member name->value. + + This mapping lists all enum members, including aliases. Note that this + is a copy of the internal mapping. + + """ + return cls._member_map_.copy() + + def __getattr__(cls, name): + """Return the enum member matching `name` + + We use __getattr__ instead of descriptors or inserting into the enum + class' __dict__ in order to support `name` and `value` being both + properties for enum members (which live in the class' __dict__) and + enum members themselves. + + """ + if _is_dunder(name): + raise AttributeError(name) + try: + return cls._member_map_[name] + except KeyError: + raise AttributeError(name) + + def __getitem__(cls, name): + return cls._member_map_[name] + + def __iter__(cls): + return (cls._member_map_[name] for name in cls._member_names_) + + def __reversed__(cls): + return (cls._member_map_[name] for name in reversed(cls._member_names_)) + + def __len__(cls): + return len(cls._member_names_) + + def __repr__(cls): + return "" % cls.__name__ + + def __setattr__(cls, name, value): + """Block attempts to reassign Enum members. + + A simple assignment to the class namespace only changes one of the + several possible ways to get an Enum member from the Enum class, + resulting in an inconsistent Enumeration. + + """ + member_map = cls.__dict__.get('_member_map_', {}) + if name in member_map: + raise AttributeError('Cannot reassign members.') + super(EnumMeta, cls).__setattr__(name, value) + + def _create_(cls, class_name, names=None, module=None, type=None): + """Convenience method to create a new Enum class. + + `names` can be: + + * A string containing member names, separated either with spaces or + commas. Values are auto-numbered from 1. + * An iterable of member names. Values are auto-numbered from 1. + * An iterable of (member name, value) pairs. + * A mapping of member name -> value. + + """ + metacls = cls.__class__ + if type is None: + bases = (cls, ) + else: + bases = (type, cls) + classdict = metacls.__prepare__(class_name, bases) + __order__ = [] + + # special processing needed for names? + if isinstance(names, basestring): + names = names.replace(',', ' ').split() + if isinstance(names, (tuple, list)) and isinstance(names[0], basestring): + names = [(e, i+1) for (i, e) in enumerate(names)] + + # Here, names is either an iterable of (name, value) or a mapping. + for item in names: + if isinstance(item, basestring): + member_name, member_value = item, names[item] + else: + member_name, member_value = item + classdict[member_name] = member_value + __order__.append(member_name) + # only set __order__ in classdict if name/value was not from a mapping + if not isinstance(item, basestring): + classdict['__order__'] = ' '.join(__order__) + enum_class = metacls.__new__(metacls, class_name, bases, classdict) + + # TODO: replace the frame hack if a blessed way to know the calling + # module is ever developed + if module is None: + try: + module = _sys._getframe(2).f_globals['__name__'] + except (AttributeError, ValueError): + pass + if module is None: + _make_class_unpicklable(enum_class) + else: + enum_class.__module__ = module + + return enum_class + + @staticmethod + def _get_mixins_(bases): + """Returns the type for creating enum members, and the first inherited + enum class. + + bases: the tuple of bases that was given to __new__ + + """ + if not bases or Enum is None: + return object, Enum + + + # double check that we are not subclassing a class with existing + # enumeration members; while we're at it, see if any other data + # type has been mixed in so we can use the correct __new__ + member_type = first_enum = None + for base in bases: + if (base is not Enum and + issubclass(base, Enum) and + base._member_names_): + raise TypeError("Cannot extend enumerations") + # base is now the last base in bases + if not issubclass(base, Enum): + raise TypeError("new enumerations must be created as " + "`ClassName([mixin_type,] enum_type)`") + + # get correct mix-in type (either mix-in type of Enum subclass, or + # first base if last base is Enum) + if not issubclass(bases[0], Enum): + member_type = bases[0] # first data type + first_enum = bases[-1] # enum type + else: + for base in bases[0].__mro__: + # most common: (IntEnum, int, Enum, object) + # possible: (, , + # , , + # ) + if issubclass(base, Enum): + if first_enum is None: + first_enum = base + else: + if member_type is None: + member_type = base + + return member_type, first_enum + + if pyver < 3.0: + @staticmethod + def _find_new_(classdict, member_type, first_enum): + """Returns the __new__ to be used for creating the enum members. + + classdict: the class dictionary given to __new__ + member_type: the data type whose __new__ will be used by default + first_enum: enumeration to check for an overriding __new__ + + """ + # now find the correct __new__, checking to see of one was defined + # by the user; also check earlier enum classes in case a __new__ was + # saved as __member_new__ + __new__ = classdict.get('__new__', None) + if __new__: + return None, True, True # __new__, save_new, use_args + + N__new__ = getattr(None, '__new__') + O__new__ = getattr(object, '__new__') + if Enum is None: + E__new__ = N__new__ + else: + E__new__ = Enum.__dict__['__new__'] + # check all possibles for __member_new__ before falling back to + # __new__ + for method in ('__member_new__', '__new__'): + for possible in (member_type, first_enum): + try: + target = possible.__dict__[method] + except (AttributeError, KeyError): + target = getattr(possible, method, None) + if target not in [ + None, + N__new__, + O__new__, + E__new__, + ]: + if method == '__member_new__': + classdict['__new__'] = target + return None, False, True + if isinstance(target, staticmethod): + target = target.__get__(member_type) + __new__ = target + break + if __new__ is not None: + break + else: + __new__ = object.__new__ + + # if a non-object.__new__ is used then whatever value/tuple was + # assigned to the enum member name will be passed to __new__ and to the + # new enum member's __init__ + if __new__ is object.__new__: + use_args = False + else: + use_args = True + + return __new__, False, use_args + else: + @staticmethod + def _find_new_(classdict, member_type, first_enum): + """Returns the __new__ to be used for creating the enum members. + + classdict: the class dictionary given to __new__ + member_type: the data type whose __new__ will be used by default + first_enum: enumeration to check for an overriding __new__ + + """ + # now find the correct __new__, checking to see of one was defined + # by the user; also check earlier enum classes in case a __new__ was + # saved as __member_new__ + __new__ = classdict.get('__new__', None) + + # should __new__ be saved as __member_new__ later? + save_new = __new__ is not None + + if __new__ is None: + # check all possibles for __member_new__ before falling back to + # __new__ + for method in ('__member_new__', '__new__'): + for possible in (member_type, first_enum): + target = getattr(possible, method, None) + if target not in ( + None, + None.__new__, + object.__new__, + Enum.__new__, + ): + __new__ = target + break + if __new__ is not None: + break + else: + __new__ = object.__new__ + + # if a non-object.__new__ is used then whatever value/tuple was + # assigned to the enum member name will be passed to __new__ and to the + # new enum member's __init__ + if __new__ is object.__new__: + use_args = False + else: + use_args = True + + return __new__, save_new, use_args + + +######################################################## +# In order to support Python 2 and 3 with a single +# codebase we have to create the Enum methods separately +# and then use the `type(name, bases, dict)` method to +# create the class. +######################################################## +temp_enum_dict = {} +temp_enum_dict['__doc__'] = "Generic enumeration.\n\n Derive from this class to define new enumerations.\n\n" + +def __new__(cls, value): + # all enum instances are actually created during class construction + # without calling this method; this method is called by the metaclass' + # __call__ (i.e. Color(3) ), and by pickle + if type(value) is cls: + # For lookups like Color(Color.red) + value = value.value + #return value + # by-value search for a matching enum member + # see if it's in the reverse mapping (for hashable values) + try: + if value in cls._value2member_map_: + return cls._value2member_map_[value] + except TypeError: + # not there, now do long search -- O(n) behavior + for member in cls._member_map_.values(): + if member.value == value: + return member + raise ValueError("%s is not a valid %s" % (value, cls.__name__)) +temp_enum_dict['__new__'] = __new__ +del __new__ + +def __repr__(self): + return "<%s.%s: %r>" % ( + self.__class__.__name__, self._name_, self._value_) +temp_enum_dict['__repr__'] = __repr__ +del __repr__ + +def __str__(self): + return "%s.%s" % (self.__class__.__name__, self._name_) +temp_enum_dict['__str__'] = __str__ +del __str__ + +def __dir__(self): + added_behavior = [m for m in self.__class__.__dict__ if m[0] != '_'] + return (['__class__', '__doc__', '__module__', 'name', 'value'] + added_behavior) +temp_enum_dict['__dir__'] = __dir__ +del __dir__ + +def __format__(self, format_spec): + # mixed-in Enums should use the mixed-in type's __format__, otherwise + # we can get strange results with the Enum name showing up instead of + # the value + + # pure Enum branch + if self._member_type_ is object: + cls = str + val = str(self) + # mix-in branch + else: + cls = self._member_type_ + val = self.value + return cls.__format__(val, format_spec) +temp_enum_dict['__format__'] = __format__ +del __format__ + + +#################################### +# Python's less than 2.6 use __cmp__ + +if pyver < 2.6: + + def __cmp__(self, other): + if type(other) is self.__class__: + if self is other: + return 0 + return -1 + return NotImplemented + raise TypeError("unorderable types: %s() and %s()" % (self.__class__.__name__, other.__class__.__name__)) + temp_enum_dict['__cmp__'] = __cmp__ + del __cmp__ + +else: + + def __le__(self, other): + raise TypeError("unorderable types: %s() <= %s()" % (self.__class__.__name__, other.__class__.__name__)) + temp_enum_dict['__le__'] = __le__ + del __le__ + + def __lt__(self, other): + raise TypeError("unorderable types: %s() < %s()" % (self.__class__.__name__, other.__class__.__name__)) + temp_enum_dict['__lt__'] = __lt__ + del __lt__ + + def __ge__(self, other): + raise TypeError("unorderable types: %s() >= %s()" % (self.__class__.__name__, other.__class__.__name__)) + temp_enum_dict['__ge__'] = __ge__ + del __ge__ + + def __gt__(self, other): + raise TypeError("unorderable types: %s() > %s()" % (self.__class__.__name__, other.__class__.__name__)) + temp_enum_dict['__gt__'] = __gt__ + del __gt__ + + +def __eq__(self, other): + if type(other) is self.__class__: + return self is other + return NotImplemented +temp_enum_dict['__eq__'] = __eq__ +del __eq__ + +def __ne__(self, other): + if type(other) is self.__class__: + return self is not other + return NotImplemented +temp_enum_dict['__ne__'] = __ne__ +del __ne__ + +def __hash__(self): + return hash(self._name_) +temp_enum_dict['__hash__'] = __hash__ +del __hash__ + +def __reduce_ex__(self, proto): + return self.__class__, (self._value_, ) +temp_enum_dict['__reduce_ex__'] = __reduce_ex__ +del __reduce_ex__ + +# _RouteClassAttributeToGetattr is used to provide access to the `name` +# and `value` properties of enum members while keeping some measure of +# protection from modification, while still allowing for an enumeration +# to have members named `name` and `value`. This works because enumeration +# members are not set directly on the enum class -- __getattr__ is +# used to look them up. + +@_RouteClassAttributeToGetattr +def name(self): + return self._name_ +temp_enum_dict['name'] = name +del name + +@_RouteClassAttributeToGetattr +def value(self): + return self._value_ +temp_enum_dict['value'] = value +del value + +Enum = EnumMeta('Enum', (object, ), temp_enum_dict) +del temp_enum_dict + +# Enum has now been created +########################### + +class IntEnum(int, Enum): + """Enum where members are also (and must be) ints""" + + +def unique(enumeration): + """Class decorator that ensures only unique members exist in an enumeration.""" + duplicates = [] + for name, member in enumeration.__members__.items(): + if name != member.name: + duplicates.append((name, member.name)) + if duplicates: + duplicate_names = ', '.join( + ["%s -> %s" % (alias, name) for (alias, name) in duplicates] + ) + raise ValueError('duplicate names found in %r: %s' % + (enumeration, duplicate_names) + ) + return enumeration diff --git a/symoroutils/filemgr.py b/symoroutils/filemgr.py new file mode 100644 index 0000000..9de7d15 --- /dev/null +++ b/symoroutils/filemgr.py @@ -0,0 +1,100 @@ +# -*- coding: utf-8 -*- + + +# This file is part of the OpenSYMORO project. Please see +# https://github.com/symoro/symoro/blob/master/LICENCE for the licence. + + +"""Perform file management operations for the SYMORO package.""" + + +import os + + +SYMORO_ROBOTS_FOLDER = "symoro-robots" + + +def get_base_path(base_folder=SYMORO_ROBOTS_FOLDER): + """ + Return the base path for storing all SYMORO robot files. + + Returns: + A string specifying the base folder path. + """ + home_folder = os.path.expanduser("~") + return os.path.join(home_folder, base_folder) + + +def get_clean_name(name, char='-'): + """ + Return a string that is lowercase and all whitespaces are replaced + by a specified character. + + Args: + name: The string to be cleaned up. + char: The character to replace all whitespaces. The default + character is "-" (hyphen). + + Returns: + A string that is fully lowercase and all whitespaces replaced by + the specified character. + + >>> get_clean_name('Random teXt') + 'random-text' + >>> get_clean_name('Random teXt', '#') + 'random#text' + """ + return name.lower().replace(' ', char) + + +def make_folders(folder_path): + """ + Check if a specified folder path exists and create the folder path + if it does not exist. + + Args: + folder_path: The folder path (string) to check and create. + """ + if not os.path.exists(folder_path): + os.makedirs(folder_path) + + +def get_folder_path(robot_name): + """ + Return the folder path to store the robot data. Also create the + folders if they are not already present. + + Args: + robot_name: The name of the robot (string). + + Returns: + A string specifying the folder path. + """ + robot_name = get_clean_name(robot_name) + folder_path = os.path.join(get_base_path(), robot_name) + make_folders(folder_path) + return folder_path + + +def get_file_path(robo, ext=None): + """ + Create the file path with the appropriate extension appended to + the file name using an underscore. + + Args: + robo: An instance of the `Robot` class. + ext: The extension (string) that is to be appended to the file + name with an underscore. + + Returns: + The file path (string) created. + """ + if ext is None: + fname = '{0}.par'.format(get_clean_name(robo.name)) + else: + fname = '{0}_{1}.txt'.format(get_clean_name(robo.name), ext) + file_path = os.path.join(robo.directory, fname) + make_folders(robo.directory) + return file_path + + diff --git a/symoroutils/genfunc.py b/symoroutils/genfunc.py new file mode 100644 index 0000000..13ce1d5 --- /dev/null +++ b/symoroutils/genfunc.py @@ -0,0 +1,142 @@ +# -*- coding: utf-8 -*- +""" +This module provides MATLAB function generation +""" + +from sympy import Matrix, Symbol + + +def gen_fheader_matlab(symo, name, args, + multival=False): + func_head = [] + func_head.append('function RESULT=%s (' % name) + func_head.append(convert_syms_matlab(args)) + func_head.append(')\n') + return func_head + + +def convert_mat_matlab(to_return): + sym_list = convert_to_list(to_return, keep_const=True) + res = [] + sym_iter = iter(sym_list) + for i in xrange(to_return.shape[0]): + for j in xrange(to_return.shape[1]): + res.append(str(sym_iter.next())) + res.append(',') + res[-1] = ';' + res.pop() + return "".join(res) + + +def convert_syms_matlab(syms): + """Converts 'syms' structure to a string + + Parameters + ========== + syms: list, Matrix or tuple of them + rpl_liter: bool + if true, all literals will be replaced with _ + It is done to evoid expression like [x, 0] = args[1] + Because it will cause exception of assigning to literal + """ + sym_list = convert_to_list(syms, keep_const=False) + res = [] + for item in iter(sym_list): + res.append(str(item)) + res.append(',') + res.pop() + return "".join(res) + + +def convert_to_list(syms, keep_const=True): + cond1 = isinstance(syms, tuple) + cond2 = isinstance(syms, list) + cond3 = isinstance(syms, Matrix) + if cond1 or cond2 or cond3: + res = [] + for item in syms: + res.extend(convert_to_list(item, keep_const)) + return res + elif isinstance(syms, Symbol) or keep_const: + return [syms] + else: + return [] + + +def gen_fbody_matlab(symo, name, to_return, args, ret_name=''): + """Generates list of string statements of the function that + computes symbolf from to_return. arg_syms are considered to + be known + """ + # set of defined symbols + arg_syms = symo.extract_syms(args) + # final symbols to be compute + multiline_res = False # may be useful for C/C++ code + if len(to_return) > 16 and isinstance(to_return, Matrix): + multiline_res = True + to_return_list = [symo.sydi[s] for s in to_return if s in symo.sydi] + res_syms = symo.extract_syms(to_return_list) + else: + res_syms = symo.extract_syms(to_return) + if isinstance(to_return, Matrix): + to_ret_str = convert_mat_matlab(to_return) + else: + to_ret_str = convert_syms_matlab(to_return) + # defines order of computation + order_list = symo.sift_syms(res_syms, arg_syms) + # list of instructions in final function + func_body = [] + # will be switched to true when branching detected + space = ' ' + folded = 1 # indentation = 1 + number of 'for' statements + multival = False + glob = 0 + glob_item = '' + for s in order_list: + if s not in symo.sydi: + if glob == 0: + glob += 1 + glob_item += '%sglobal %s' % (space * folded, s) + elif glob < 12: + glob_item += ' %s' % s + glob += 1 + else: + glob = 0 + glob_item += ' %s\n' % s + else: + if isinstance(symo.sydi[s], tuple): + multival = True + items = ['%sfor %s=' % (space * folded, s)] + for x in symo.sydi[s]: + items.append('%s' % x) + items.append(',') + items.append('\n') + item = "".join(items) + folded += 1 + else: + item = '%s%s=%s;\n' % (space * folded, s, symo.sydi[s]) + item = item.replace('**', '^') + func_body.append(item) + if multiline_res: + rows, cols = to_return.shape + func_body.insert(0, '%sRESULT=zeros(%s,%s);\n' % (space, rows, cols)) + form_str = space + 'RESULT(%s,%s)=%s;\n' + for i in xrange(rows): + for j in xrange(cols): + s = to_return[i, j] + if s in symo.sydi: + item = form_str % (i + 1, j + 1, symo.sydi[s]) + item = item.replace('**', '^') + func_body.append(item) + elif multival: + func_body.insert(0, '%sRESULT=[];\n' % (space)) + item = '%sRESULT=[RESULT;%s];\n' % (space * folded, to_ret_str) + func_body.append(item) + else: + item = '%sRESULT=[%s];\n' % (space * folded, to_ret_str) + func_body.append(item) + for f in xrange(folded-1, 0, -1): + func_body.append('%send\n' % (space * f)) + func_body.append('end\n') + func_body.insert(0, glob_item + '\n') + return func_body diff --git a/symoroutils/paramsinit.py b/symoroutils/paramsinit.py new file mode 100644 index 0000000..a0024a4 --- /dev/null +++ b/symoroutils/paramsinit.py @@ -0,0 +1,160 @@ +# -*- coding: utf-8 -*- + + +# This file is part of the OpenSYMORO project. Please see +# https://github.com/symoro/symoro/blob/master/LICENCE for the licence. + + +""" +This module contains the methods used to initialise the different +matrices and parameters for various models. +""" + + +from copy import copy + +from sympy import zeros +from sympy import Matrix +from symoroutils import tools + + +class ParamsInit(object): + """ + This class contains methods that are used to initialise the different + matrices and parameters for various models. All the methods in this + class are class-methods. + """ + @classmethod + def init_jplus(cls, robo): + """Copies the inertia parameters. + Used for composed link inertia computation + + Returns + ======= + Jplus: list of Matrices 3x3 + MSplus: list of Matrices 3x1 + Mplus: list of var + """ + j_plus = copy(robo.J) + j_plus.append(zeros(3, 3)) + ms_plus = copy(robo.MS) + ms_plus.append(zeros(3, 1)) + m_plus = copy(robo.M) + m_plus.append(0) + return j_plus, ms_plus, m_plus + + @classmethod + def init_mat(cls, robo, num=3): + """Generates a list of Matrices.Size of the + list is number of links. + + Parameters + ========== + robo: Robot + Instance of robot description container + num: int, optional + size of the matries, default is 3 + + Returns + ======= + list of Matrices numxnum + """ + return [zeros(num, num) for i in xrange(robo.NL)] + + @classmethod + def init_vec(cls, robo, num=3, ext=0): + """Generates a list of vectors. + Size of the list is number of links. + + Parameters + ========== + robo: Robot + Instance of robot description container + num: int, optional + size of the vectors, default is 3 + ext: int, optional + additional vector instances over number of links + + Returns + ======= + list of Matrices Nx1 + """ + return [zeros(num, 1) for i in xrange(robo.NL+ext)] + + @classmethod + def init_scalar(cls, robo): + """Generates a list of vars. + Size of the list is number of links. + """ + return [0 for i in xrange(robo.NL)] + + @classmethod + def init_w(cls, robo): + """Generates a list of vectors for angular velocities. + Size of the list is number of links + 1. + The zero vector is the base angular velocity + """ + omega = cls.init_vec(robo) + omega[0] = robo.w0 + return omega + + @classmethod + def init_v(cls, robo): + """Generates a list of vectors for linear velocities. + Size of the list is number of links + 1. + The zero vector is the base angular velocity + """ + vel = cls.init_vec(robo) + vel[0] = robo.v0 + return vel + + @classmethod + def init_wv_dot(cls, robo, gravity=True): + """Generates lists of vectors for + angular and linear accelerations. + Size of the list is number of links + 1. + The zero vector is the base angular velocity + + Returns + ======= + vdot: list of Matrices 3x1 + wdot: list of Matrices 3x1 + """ + wdot = cls.init_vec(robo) + wdot[0] = robo.wdot0 + vdot = cls.init_vec(robo) + vdot[0] = robo.vdot0 + if gravity: + vdot[0] -= robo.G + return wdot, vdot + + @classmethod + def init_u(cls, robo): + """Generates a list of auxiliary U matrices""" + u_aux_matrix = ParamsInit.init_mat(robo) + # the value for the -1th base frame + u_aux_matrix.append( + tools.skew(robo.w0)**2 + tools.skew(robo.wdot0) + ) + return u_aux_matrix + + @classmethod + def product_combinations(cls, vec): + """Generates 6-vector of different v elements' + product combinations + + Parameters + ========== + vec: Matrix 3x1 + vector + + Returns + ======= + product_combinations: Matrix 6x1 + """ + return Matrix([ + vec[0]*vec[0], vec[0]*vec[1], vec[0]*vec[2], + vec[1]*vec[1], vec[1]*vec[2], vec[2]*vec[2] + ]) + + diff --git a/symoroutils/parfile.py b/symoroutils/parfile.py new file mode 100644 index 0000000..b55872e --- /dev/null +++ b/symoroutils/parfile.py @@ -0,0 +1,193 @@ +# -*- coding: utf-8 -*- + + +# This file is part of the OpenSYMORO project. Please see +# https://github.com/symoro/symoro/blob/master/LICENCE for the licence. + + +""" +This module performs writing and reading data into PAR file. PAR is a +plain text file used to represent the different parameters of the robot. +""" + + +import os +import re + +from symoroutils import filemgr +from symoroutils import tools +from pysymoro import robot + + +_keywords = [ + 'ant', 'sigma', 'b', 'd', 'r', 'gamma', 'alpha', 'mu', 'theta', + 'XX', 'XY', 'XZ', 'YY', 'YZ', 'ZZ', 'MX', 'MY', 'MZ', 'M', + 'IA', 'FV', 'FS', 'FX', 'FY', 'FZ', 'CX', 'CY', 'CZ', + 'eta', 'k', 'QP', 'QDP', 'GAM', 'W0', 'WP0', 'V0', 'VP0', 'Z', 'G' +] +_NF = ['ant', 'sigma', 'b', 'd', 'r', 'gamma', 'alpha', 'mu', 'theta'] +_NJ = ['eta', 'k', 'QP', 'QDP', 'GAM'] +_NL = [ + 'XX', 'XY', 'XZ', 'YY', 'YZ', 'ZZ', 'MX', 'MY', 'MZ', 'M', + 'IA', 'FV', 'FS', 'FX', 'FY', 'FZ', 'CX', 'CY', 'CZ' +] +_VEC = ['W0', 'WP0', 'V0', 'VP0'] +_ZERO_BASED = {'W0', 'WP0', 'V0', 'VP0', 'Z', 'G'} +_bool_dict = { + 'True': True, + 'False': False, + 'true': True, + 'false': False, + '1': True, + '0': False +} +_keyword_repl = { + 'Ant': 'ant', + 'Mu': 'mu', + 'Sigma': 'sigma', + 'B': 'b', + 'Alpha': 'alpha', + 'Theta': 'theta', + 'R': 'r' +} + + +def _extract_vals(robo, key, line): + line = line.replace('{', '') + line = line.replace('}', '') + if key in _ZERO_BASED: + k = 0 + elif (robo.is_floating or robo.is_mobile) and key in _NL: + k = 0 + else: + k = 1 + items = line.split(',') + items_proc = [] + prev_item = False + for i, v in enumerate(items): + if v.find('atan2') == -1 and not prev_item: + items_proc.append(v) + elif prev_item: + items_proc.append('%s,%s' % (items[i-1], v)) + prev_item = False + else: + prev_item = True + for i, v in enumerate(items_proc): + if robo.put_val(i+k, key, v.strip()) == tools.FAIL: + return tools.FAIL + + +def _write_par_list(robo, f, key, N0, N): + f.write('{0} = {{{1}'.format(key, robo.get_val(N0, key))) + for i in xrange(N0 + 1, N): + f.write(',{0}'.format(robo.get_val(i, key))) + f.write('}\n') + + +def writepar(robo): + fname = robo.par_file_path + with open(fname, 'w') as f: + # robot description + f.write('(* Robotname = \'{0}\' *)\n'.format(robo.name)) + f.write('NL = {0}\n'.format(robo.nl)) + f.write('NJ = {0}\n'.format(robo.nj)) + f.write('NF = {0}\n'.format(robo.nf)) + f.write('Type = {0}\n'.format(tools.TYPES.index(robo.structure))) + f.write('is_floating = {0}\n'.format(robo.is_floating)) + f.write('is_mobile = {0}\n'.format(robo.is_mobile)) + # geometric parameters + f.write('\n(* Geometric parameters *)\n') + for key in _NF: + _write_par_list(robo, f, key, 1, robo.NF) + # dynamic parameters + f.write('\n(* Dynamic parameters and external forces *)\n') + N0 = 0 if robo.is_floating or robo.is_mobile else 1 + for key in _NL: + _write_par_list(robo, f, key, N0, robo.NL) + # joint parameters + f.write('\n(* Joint parameters *)\n') + for key in _NJ: + _write_par_list(robo, f, key, 1, robo.NJ) + # base parameters - velocity and acceleration + f.write('\n(* Velocity and acceleration of the base *)\n') + for key in _VEC: + _write_par_list(robo, f, key, 0, 3) + # gravity vector + f.write('\n(* Acceleration of gravity *)\n') + _write_par_list(robo, f, 'G', 0, 3) + # base parameters - Z matrix + f.write('\n(* Transformation of 0 frame position fT0 *)\n') + _write_par_list(robo, f, 'Z', 0, 16) + f.write('\n(* End of definition *)\n') + + +def readpar(robo_name, file_path): + """Return: + robo: an instance of Robot, read from file + flag: indicates if any errors accured. (tools.FAIL) + """ + with open(file_path, 'r') as f: + f.seek(0) + d = {} + is_floating = False + is_mobile = False + for line in f.readlines(): + # check for robot name + name_pattern = r"\(\*.*Robotname.*=.*\'([\s\w-]*)\'.*\*\)" + match = re.match(name_pattern, line) + if match: + robo_name = match.group(1).strip() + # check for joint numbers, link numbers, type + for s in ('NJ', 'NL', 'Type'): + match = re.match(r'^%s.*=([\d\s]*)(\(\*.*)?' % s, line) + if match: + d[s] = int(match.group(1)) + continue + # check for is_floating + match = re.match(r'^is_floating.*=([\w\s]*)(\(\*.*)?', line) + if match: + is_floating = _bool_dict[(match.group(1).strip())] + # check for is_mobile + match = re.match(r'^is_mobile.*=([\w\s]*)(\(\*.*)?', line) + if match: + is_mobile = _bool_dict[(match.group(1).strip())] + if len(d) < 2: + return None, tools.FAIL + NF = d['NJ']*2 - d['NL'] + #initialize the Robot instance + robo = robot.Robot( + name=robo_name, + NL=d['NL'], NJ=d['NJ'], NF=NF, + structure=tools.TYPES[d['Type']], + is_floating=is_floating, + is_mobile=is_mobile, + directory=os.path.dirname(file_path), + par_file_path=file_path + ) + # fitting the data + acc_line = '' + key = '' + f.seek(0) + flag = tools.OK + for line in f.readlines(): + if line.find('(*') != -1: + continue + line = line.replace('Pi', 'pi') + match = re.match(r'^(.*)=.*\{(.*)', line) + if match: + acc_line == '' + key = match.group(1).strip() + acc_line = match.group(2).strip() + else: + acc_line += line + if acc_line.find('}') != -1: + if key in _keyword_repl: + key = _keyword_repl[key] + if key in _keywords: + if _extract_vals(robo, key, acc_line) == tools.FAIL: + flag = tools.FAIL + acc_line = '' + key = '' + return robo, flag + + diff --git a/symoroutils/samplerobots.py b/symoroutils/samplerobots.py new file mode 100644 index 0000000..b93388c --- /dev/null +++ b/symoroutils/samplerobots.py @@ -0,0 +1,161 @@ +# -*- coding: utf-8 -*- + + +# This file is part of the OpenSYMORO project. Please see +# https://github.com/symoro/symoro/blob/master/LICENCE for the licence. + + +""" +This module contains some sample robots (parameters) that can be used +for multiple purposes. +""" + + +from sympy import pi, var, zeros +from sympy import Matrix + +from pysymoro.robot import Robot +from symoroutils import tools + + +def cart_pole(): + """Generate Robot instance of classical CartPole dynamic system.""" + #TODO: bring it to the new notation with 0-frame + robo = Robot('CartPole', 2, 2, 2, False) + robo.ant = (-1, 0, 1) + robo.sigma = (0, 1, 0) + robo.alpha = (0, pi/2, pi/2) + robo.d = (0, 0, 0) + robo.theta = (0, pi/2, var('th2')) + robo.r = (0, var('r1'), 0) + robo.b = (0, 0, 0) + robo.gamma = (0, 0, 0) + robo.structure = tools.SIMPLE + robo.num = range(0, 3) + robo.Nex = [zeros(3, 1) for i in robo.num] + robo.Fex = [zeros(3, 1) for i in robo.num] + robo.FS = [0 for i in robo.num] + robo.IA = [0 for i in robo.num] + robo.FV = [var('FV{0}'.format(i)) for i in robo.num] + robo.MS = [zeros(3, 1) for i in robo.num] + robo.MS[1][0] = var('MX2') + robo.M = [var('M{0}'.format(i)) for i in robo.num] + robo.GAM = [var('GAM{0}'.format(i)) for i in robo.num] + inertia_matrix_terms = ("XX{0}, XY{0}, XZ{0}, ") + \ + ("XY{0}, YY{0}, YZ{0}, ") + \ + ("XZ{0}, YZ{0}, ZZ{0}") + robo.J = [ + Matrix(3, 3, var(inertia_matrix_terms.format(i))) \ + for i in robo.num + ] + robo.G = Matrix([0, 0, -var('G3')]) + robo.w0 = zeros(3, 1) + robo.wdot0 = zeros(3, 1) + robo.v0 = zeros(3, 1) + robo.vdot0 = zeros(3, 1) + robo.q = [0, var('r1'), var('th2')] + robo.qdot = [0, var('r1d'), var('th2d')] + robo.qddot = [0, var('r1dd'), var('th2dd')] + return robo + + +def planar2r(): + """Generate Robot instance of 2R Planar robot""" + robo = Robot('Planar2R', 2, 2, 2, False) + robo.structure = tools.SIMPLE + robo.sigma = [2, 0, 0] + robo.mu = [0, 1, 1] + robo.gamma = [0, 0, 0] + robo.b = [0, 0, 0] + robo.alpha = [0, 0, 0] + robo.d = [0, 0, var('L1')] + robo.theta = [0, var('q1'), var('q2')] + robo.r = [0, 0, 0] + robo.num = range(0, 3) + robo.Nex = [zeros(3, 1) for i in robo.num] + robo.Fex = [zeros(3, 1) for i in robo.num] + robo.FS = [0 for i in robo.num] + robo.IA = [0 for i in robo.num] + robo.FV = [var('FV{0}'.format(i)) for i in robo.num] + robo.MS = [Matrix(var('MX{0}, MY{0}, MZ{0}'.format(i))) for i in robo.num] + robo.M = [var('M{0}'.format(i)) for i in robo.num] + robo.GAM = [var('GAM{0}'.format(i)) for i in robo.num] + inertia_matrix_terms = ("XX{0}, XY{0}, XZ{0}, ") + \ + ("XY{0}, YY{0}, YZ{0}, ") + \ + ("XZ{0}, YZ{0}, ZZ{0}") + robo.J = [ + Matrix(3, 3, var(inertia_matrix_terms.format(i))) \ + for i in robo.num + ] + robo.G = Matrix([0, 0, -var('G3')]) + robo.w0 = zeros(3, 1) + robo.wdot0 = zeros(3, 1) + robo.v0 = zeros(3, 1) + robo.vdot0 = zeros(3, 1) + robo.q = [0, var('q1'), var('q2')] + robo.qdot = [0, var('QP1'), var('QP2')] + robo.qddot = [0, var('QDP1'), var('QDP2')] + return robo + + +def sr400(): + #TODO: bring it to the new notation with 0-frame + """Generate Robot instance of SR400""" + robo = Robot('SR400', 8, 9, 10, False) + robo.ant = [-1, 0, 1, 2, 3, 4, 5, 1, 7, 8, 3] + robo.sigma = [0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 2] + robo.mu = [0, 1, 1, 0, 1, 1, 1, 1, 0, 0, 0] + robo.alpha = [0, 0, -pi/2, 0, -pi/2, pi/2, -pi/2, -pi/2, 0, 0, 0] + d_var = var('D:9') + robo.d = [0, 0, d_var[2], d_var[3], d_var[4], 0, 0, + d_var[2], d_var[8], d_var[3], -d_var[8]] + robo.theta = [0] + list(var('th1:10')) + [0] + robo.r = [0, 0, 0, 0, var('RL4'), 0, 0, 0, 0, 0, 0] + robo.b = [0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0] + robo.gamma = [0, 0, 0, 0, 0, 0, 0, 0, 0, 0, pi/2] + robo.structure = tools.CLOSED_LOOP + return robo + + +def rx90(): + """Generate Robot instance of RX90""" + robo = Robot('RX90', 6, 6, 6, False) + # table of geometric parameters RX90 + robo.sigma = [2, 0, 0, 0, 0, 0, 0] + robo.alpha = [0, 0, pi/2, 0, -pi/2, pi/2, -pi/2] + robo.d = [0, 0, 0, var('D3'), 0, 0, 0] + robo.theta = [0] + list(var('th1:7')) + robo.r = [0, 0, 0, 0, var('RL4'), 0, 0] + robo.b = [0, 0, 0, 0, 0, 0, 0] + robo.gamma = [0, 0, 0, 0, 0, 0, 0] + robo.mu = [0, 1, 1, 1, 1, 1, 1] + robo.structure = tools.SIMPLE + robo.w0 = zeros(3, 1) + robo.wdot0 = zeros(3, 1) + robo.v0 = zeros(3, 1) + robo.vdot0 = zeros(3, 1) + num = range(0, 7) + robo.qdot = [var('QP{0}'.format(i)) for i in num] + robo.qddot = [var('QDP{0}'.format(i)) for i in num] + robo.Nex= [zeros(3, 1) for i in num] + robo.Nex[-1] = Matrix(var('CX{0}, CY{0}, CZ{0}'.format(robo.NJ))) + robo.Fex = [zeros(3, 1) for i in num] + robo.Fex[-1] = Matrix(var('FX{0}, FY{0}, FZ{0}'.format(robo.NJ))) + robo.FS = [var('FS{0}'.format(i)) for i in num] + robo.IA = [var('IA{0}'.format(i)) for i in num] + robo.FV = [var('FV{0}'.format(i)) for i in num] + robo.MS = [Matrix(var('MX{0}, MY{0}, MZ{0}'.format(i))) for i in num] + robo.M = [var('M{0}'.format(i)) for i in num] + robo.GAM = [var('GAM{0}'.format(i)) for i in num] + inertia_matrix_terms = ("XX{0}, XY{0}, XZ{0}, ") + \ + ("XY{0}, YY{0}, YZ{0}, ") + \ + ("XZ{0}, YZ{0}, ZZ{0}") + robo.J = [ + Matrix(3, 3, var(inertia_matrix_terms.format(i))) \ + for i in num + ] + robo.G = Matrix([0, 0, var('G3')]) + return robo + + + diff --git a/symoroutils/symbolmgr.py b/symoroutils/symbolmgr.py new file mode 100644 index 0000000..06e5f53 --- /dev/null +++ b/symoroutils/symbolmgr.py @@ -0,0 +1,625 @@ +# -*- coding: utf-8 -*- + + +# This file is part of the OpenSYMORO project. Please see +# https://github.com/symoro/symoro/blob/master/LICENCE for the licence. + + +"""This module contains the Symbol Manager tools.""" + +import itertools +import os + +from sympy import sin, cos +from sympy import Symbol, Matrix, Expr +from sympy import Mul, Add, factor, var, sympify + +from symoroutils import filemgr +from symoroutils import tools +from genfunc import gen_fheader_matlab, gen_fbody_matlab + +class SymbolManager(object): + """Symbol manager, responsible for symbol replacing, file writing.""" + def __init__(self, file_out='disp', sydi=dict()): + """Default values correspond to empty dictionary and screen output. + """ + self.file_out = file_out + """Output descriptor. Can be None, 'disp', file + defines the output destination""" + self.sydi = dict((k, sydi[k]) for k in sydi) + """Dictionary. All the substitutions are saved in it""" + self.revdi = dict((sydi[k], k) for k in sydi) + """Dictionary. Revers to the self.sydi""" + self.order_list = sydi.keys() + """keeps the order of variables to be compute""" + + def simp(self, sym): + sym = factor(sym) + new_sym = tools.ONE + for expr in Mul.make_args(sym): + if expr.is_Pow: + expr, pow_val = expr.args + else: + pow_val = 1 + expr = self.C2S2_simp(expr) + expr = self.CS12_simp(expr, silent=True) + new_sym *= expr**pow_val + return new_sym + + def C2S2_simp(self, sym): + """ + Example + ======= + >> print C2S2_simp(sympify("-C**2*RL + S*(D - RL*S)")) + D*S - RL + """ + if not sym.is_Add: + repl_dict = {} + for term in sym.atoms(Add): + repl_dict[term] = self.C2S2_simp(term) + sym = sym.xreplace(repl_dict) + return sym + names, short_form = tools.trignometric_info(sym) + for name in names: + if short_form: + cos_term, sin_term = tools.cos_sin_syms(name) + else: + cos_term, sin_term = cos(name), sin(name) + sym = self.try_opt( + tools.ONE, None, sin_term**2, cos_term**2, sym + ) + return sym + + def CS12_simp(self, sym, silent=False): + """ + Example + ======= + >> print SymbolManager().CS12_simp(sympify("C2*C3 - S2*S3")) + C23 = C2*C3 - S2*S3 + C23 + >> print SymbolManager().CS12_simp(sympify("C2*S3*R + S2*C3*R")) + S23 = C2*S3 + S2*C3 + R*S23 + """ + if not sym.is_Add: + repl_dict = {} + for term in sym.atoms(Add): + repl_dict[term] = self.CS12_simp(term) + sym = sym.xreplace(repl_dict) + return sym + names, short_form = tools.trignometric_info(sym) + names = list(names) + if short_form: + names.sort() + sym2 = sym + for n1, n2 in itertools.combinations(names, 2): + if short_form: + C1, S1 = tools.cos_sin_syms(n1) + C2, S2 = tools.cos_sin_syms(n2) + np1, nm1 = tools.get_pos_neg(n1) + np2, nm2 = tools.get_pos_neg(n2) + n12 = tools.ang_sum(np1, np2, nm1, nm2) + nm12 = tools.ang_sum(np1, nm2, nm1, np2) + C12, S12 = tools.cos_sin_syms(n12) + C1m2, S1m2 = tools.cos_sin_syms(nm12) + else: + C1, S1 = cos(n1), sin(n1) + C2, S2 = cos(n2), sin(n2) + C12, S12 = cos(n1+n2), sin(n1+n2) + C1m2, S1m2 = cos(n1-n2), sin(n1-n2) + sym2 = self.try_opt(S12, S1m2, S1*C2, C1*S2, sym2, silent) + sym2 = self.try_opt(C12, C1m2, C1*C2, -S1*S2, sym2, silent) + if sym2 != sym: + return self.CS12_simp(sym2, silent) + else: + return sym + + def try_opt(self, A, Am, B, C, old_sym, silent=False): + """Replaces B + C by A or B - C by Am. + Chooses the best option. + """ + Bcfs = tools.get_max_coef_list(old_sym, B) + Ccfs = tools.get_max_coef_list(old_sym, C) + if Bcfs != [] and Ccfs != []: + Res = old_sym + Res_tmp = Res + for coef in Bcfs: + Res_tmp += A*coef - B*coef - C*coef + if tools.sym_less(Res_tmp, Res): + Res = Res_tmp + if tools.sym_less(Res, old_sym) and Am is None: + if not A.is_number and not silent: + self.add_to_dict(A, B + C) + return Res + elif Am is not None: + Res2 = old_sym + Res_tmp = Res2 + for coef in Bcfs: + Res_tmp += Am*coef - B*coef + C*coef + if tools.sym_less(Res_tmp, Res2): + Res2 = Res_tmp + if tools.sym_less(Res2, Res) and tools.sym_less(Res2, old_sym): + if not Am.is_number and not silent: + self.add_to_dict(Am, B - C) + return Res2 + elif tools.sym_less(Res, old_sym): + if not A.is_number and not silent: + self.add_to_dict(A, B + C) + return Res + return old_sym + + def add_to_dict(self, new_sym, old_sym): + """Internal function. + Extends symbol dictionary by (new_sym, old_sym) pair + """ + new_sym = sympify(new_sym) + if new_sym.as_coeff_Mul()[0] == -tools.ONE: + new_sym = -new_sym + old_sym = -old_sym + if new_sym not in self.sydi: + self.sydi[new_sym] = old_sym + self.revdi[old_sym] = new_sym + self.order_list.append(new_sym) + self.write_equation(new_sym, old_sym) + + def trig_replace(self, M, angle, name): + """Replaces trigonometric expressions cos(x) + and sin(x) by CX and SX + + Parameters + ========== + M: var or Matrix + Object of substitution + angle: var + symbol that stands for the angle value + name: int or string + brief name X for the angle + + Notes + ===== + The cos(x) and sin(x) will be replaced by CX and SX, + where X is the name and x is the angle + """ + if not isinstance(angle, Expr) or angle.is_number: + return M + cos_sym, sin_sym = tools.cos_sin_syms(name) + sym_list = [(cos_sym, cos(angle)), (sin_sym, sin(angle))] + subs_dict = {} + for sym, sym_old in sym_list: + if -1 in Mul.make_args(sym_old): + sym_old = -sym_old + subs_dict[sym_old] = sym + self.add_to_dict(sym, sym_old) + for i1 in xrange(M.shape[0]): + for i2 in xrange(M.shape[1]): + M[i1, i2] = M[i1, i2].subs(subs_dict) + return M + + #TODO remove index + def replace(self, old_sym, name, index='', forced=False): + """Creates a new symbol for the symbolic expression old_sym. + + Parameters + ========== + old_sym: var + Symbolic expression to be substituted + name: string or var + denotion of the expression + index: int or string, optional + will be attached to the name. Usualy used for link or joint number. + Parameter exists for usage convenience + forced: bool, optional + If True, the new symbol will be created even if old symbol + is a simple expression + + Notes + ===== + Generaly only complex expressions, which contain + - * / ** operations + will be replaced by a new symbol + """ + if not forced: + if not isinstance(old_sym, Expr): + return old_sym + inv_sym = -old_sym + if old_sym.is_Atom or inv_sym.is_Atom: + return old_sym + for i in (1, -1): + if i * old_sym in self.revdi: + return i * self.revdi[i * old_sym] + new_sym = var(str(name) + str(index)) + self.add_to_dict(new_sym, old_sym) + return new_sym + + def mat_replace(self, M, name, index='', + forced=False, skip=0, symmet=False): + """Replaces each element in M by symbol + + Parameters + ========== + M: Matrix + Object of substitution + name: string + denotion of the expression + index: int or string, optional + will be attached to the name. Usualy used for link + or joint number. Parameter exists for usage convenience + forced: bool, optional + If True, the new symbol will be created even if old symbol + is a simple expression + skip: int, optional + Number of bottom rows of the matrix, which will be skipped. + Used in case of Transformation matrix and forced = True. + symmet: bool, optional + If true, only for upper triangle part of the matrix + symbols will be created. The bottom triangle part the + same symbols will be used + + + Returns + ======= + M: Matrix + Matrix with all the elements replaced + + Notes + ===== + -Each element M_ij will be replaced by + symbol name + i + j + index + -There are two ways to use this function (examples): + 1) >>> A = B+C+... + >>> symo.mat_replace(A, 'A') + # for the case when expression B+C+... is too big + 2) >>> A = symo.mat_replace(B+C+..., 'A') + # for the case when B+C+... is small enough + """ + if M.shape[0] > 9: + form2 = '%02d%02d' + else: + form2 = '%d%d' + for i2 in xrange(M.shape[1]): + for i1 in xrange(M.shape[0] - skip): + if symmet and i1 < i2: + M[i1, i2] = M[i2, i1] + continue + if M.shape[1] > 1: + name_index = name + form2 % (i1 + 1, i2 + 1) + else: + name_index = name + str(i1 + 1) + M[i1, i2] = self.replace(M[i1, i2], name_index, index, forced) + return M + + def unfold(self, expr): + """Unfold the expression using the dictionary. + + Parameters + ========== + expr: symbolic expression + Symbolic expression to be unfolded + + Returns + ======= + expr: symbolic expression + Unfolded expression + """ + while set(self.sydi.keys()) & expr.atoms(): + expr = expr.subs(self.sydi) + return expr + + def mat_unfold(self, mat): + for i in xrange(mat.shape[0]): + for j in xrange(mat.shape[1]): + if isinstance(mat[i, j], Expr): + mat[i, j] = self.unfold(mat[i, j]) + return mat + + def write_param(self, name, header, robo, N): + """Low-level function for writing the parameters table + + Parameters + ========== + name: string + the name of the table + header: list + the table header + robo: Robot + Instance of parameter container + N: list of int + Indices for which parameter rows will be written + """ + self.write_line(name) + self.write_line(tools.l2str(header)) + for j in N: + params = robo.get_param_vec(header, j) + self.write_line(tools.l2str(params)) + self.write_line() + + def write_params_table(self, robo, title='', geom=True, inert=False, + dynam=False, equations=True, + inert_name='Dynamic inertia parameters'): + """Writes the geometric parameters table + + Parameters + ========== + robo: Robot + Instance of the parameter container. + title: string + The document title. + + Notes + ===== + The synamic model generation program can be started with this function + """ + if title != '': + self.write_line(title) + self.write_line() + if geom: + self.write_param('Geometric parameters', robo.get_geom_head(), + robo, range(1, robo.NF)) + if inert: + if robo.is_floating or robo.is_mobile: + start_frame = 0 + else: + start_frame = 1 + + self.write_param(inert_name, robo.get_dynam_head(), + robo, range(start_frame, robo.NL)) + if dynam: + self.write_param('External forces and joint parameters', + robo.get_ext_dynam_head(), + robo, range(1, robo.NL)) + self.write_param('Base velicities parameters', + robo.get_base_vel_head(), + robo, [0, 1, 2]) + if equations: + self.write_line('Equations:') + + def unknown_sep(self, eq, known): + """If there is a sum inside trigonometric function and + the atoms are not the subset of 'known', + this function will replace the trigonometric symbol bu sum, + trying to separate known and unknown terms + """ + if not isinstance(eq, Expr) or eq.is_number: + return eq + while True: + res = False + trigs = eq.atoms(sin, cos) + for trig in trigs: + args = trig.args[0].atoms() + if args & known and not args <= known and trig in self.sydi: + eq = eq.subs(trig, self.sydi[trig]).expand() + res = True + if not res: + break + return eq + + def write_equation(self, A, B): + """Writes the equation A = B into the output + + Parameters + ========== + A: expression or var + left-hand side of the equation. + B: expression or var + right-hand side of the equation + """ + self.write_line(str(A) + ' = ' + str(B) + ';') + + def write_line(self, line=''): + """Writes string data into tha output with new line symbol + + Parameters + ========== + line: string, optional + Data to be written. If empty, it adds an empty line + """ + if self.file_out == 'disp': + print(line) + elif self.file_out is not None: + self.file_out.write(str(line) + '\n') + + def flushout(self): + """ + Flush the buffer and make sure the data is written to the disk + """ + self.file_out.flush() + if self.file_out != 'disp': + os.fsync(self.file_out.fileno()) + + def file_open(self, robo, ext): + """ + Initialize file stream + + Parameters + ========== + robo: Robot instance + provides the robot's name + ext: string + provides the file name extention + """ + fname = filemgr.get_file_path(robo, ext) + self.file_out = open(fname, 'w') + + def file_close(self): + """ + Initialize file stream + + Parameters + ========== + robo: Robot instance + provides the robot's name + ext: string + provides the file name extention + """ + if self.file_out is not None: + self.write_line('*=*') + self.file_out.close() + + def gen_fheader(self, name, *args): + fun_head = [] + fun_head.append('def %s(*args):\n' % name) + imp_s_1 = 'from numpy import pi, sin, cos, sign\n' + imp_s_2 = 'from numpy import array, arctan2 as atan2, sqrt\n' + fun_head.append(' %s' % imp_s_1) + fun_head.append(' %s' % imp_s_2) + for i, var_list in enumerate(args): + v_str_list = self.convert_syms(args[i], True) + fun_head.append(' %s=args[%s]\n' % (v_str_list, i)) + return fun_head + + def convert_syms(self, syms, rpl_liter=False): + """Converts 'syms' structure to sintactically correct string + + Parameters + ========== + syms: list, Matrix or tuple of them + rpl_liter: bool + if true, all literals will be replaced with _ + It is done to evoid expression like [x, 0] = args[1] + Because it will cause exception of assigning to literal + """ + if isinstance(syms, tuple) or isinstance(syms, list): + syms = [self.convert_syms(item, rpl_liter) for item in syms] + res = '[' + for i, s in enumerate(syms): + res += s + if i < len(syms) - 1: + res += ',' + res += ']' + return res + elif isinstance(syms, Matrix): + res = '[' + for i in xrange(syms.shape[0]): + res += self.convert_syms(list(syms[i, :]), rpl_liter) + if i < syms.shape[0] - 1: + res += ',' + res += ']' + return res + elif rpl_liter and sympify(syms).is_number: + return '_' + else: + return str(syms) + + def extract_syms(self, syms): + """ returns set of all symbols from list or matrix + or tuple of them + """ + if isinstance(syms, tuple) or isinstance(syms, list): + atoms = (self.extract_syms(item) for item in syms) + return reduce(set.__or__, atoms, set()) + elif isinstance(syms, Matrix): + return self.extract_syms(list(syms)) + elif isinstance(syms, Expr): + return syms.atoms(Symbol) + else: + return set() + + def sift_syms(self, rq_syms, wr_syms): + """Returns ordered list of variables to be compute + """ + order_list = [] # vars that are defined in sydi + for s in reversed(self.order_list): + if s in rq_syms and not s in wr_syms: + order_list.insert(0, s) + s_val = self.sydi[s] + if isinstance(s_val, Expr): + atoms = s_val.atoms(Symbol) + rq_syms |= {s for s in atoms if not s.is_number} + rq_vals = [s for s in rq_syms if not (s in self.sydi or s in wr_syms)] + # required vars that are not defined in sydi + # will be set to '1.' + return rq_vals + order_list + + def gen_fbody(self, name, to_return, args): + """Generates list of string statements of the function that + computes symbolf from to_return. wr_syms are considered to + be known + """ + # set of defined symbols + wr_syms = self.extract_syms(args) + # final symbols to be compute + syms = self.extract_syms(to_return) + # defines order of computation + order_list = self.sift_syms(syms, wr_syms) + # list of instructions in final function + fun_body = [] + # will be switched to true when branching detected + space = ' ' + folded = 1 # indentation = 1 + number of 'for' statements + multival = False + for s in order_list: + if s not in self.sydi: + item = '%s%s=1.\n' % (space * folded, s) + elif isinstance(self.sydi[s], tuple): + multival = True + item = '%sfor %s in %s:\n' % (space * folded, s, self.sydi[s]) + folded += 1 + else: + item = '%s%s=%s\n' % (space * folded, s, self.sydi[s]) + fun_body.append(item) + ret_expr = self.convert_syms(to_return) + if multival: + fun_body.insert(0, ' %s_result=[]\n' % (name)) + item = '%s%s_result.append(%s)\n' % (space*folded, name, ret_expr) + else: + item = ' %s_result=%s\n' % (name, ret_expr) + fun_body.append(item) + fun_body.append(' return %s_result\n' % (name)) + return fun_body + + def gen_func_string(self, name, to_return, args, syntax='python'): + #TODO self, name, toret, *args, **kwargs + """ Returns function string. The rest is the same as for + gen_func + + Parameters + ========== + name: string + Future function's name, must be different for + different fucntions + to_return: list, Matrix or tuple of them + Determins the shape of the output and symbols inside it + *args: any number of lists, Matrices or tuples of them + Determins the shape of the input and symbols + names to assigned + + Notes + ===== + -All unassigned used symbols will be set to '1.0'. + -This function must be called only after the model that + computes symbols in to_return have been generated. + """ + #if kwargs.get + if syntax == 'python': + fun_head = self.gen_fheader(name, args) + fun_body = self.gen_fbody(name, to_return, args) + elif syntax == 'matlab': + fun_head = gen_fheader_matlab(self, name, args, to_return) + fun_body = gen_fbody_matlab(self, name, to_return, args) + fun_string = "".join(fun_head + fun_body) + return fun_string + + def gen_func(self, name, to_return, args): + """ Returns function that computes what is in to_return + using args as arguments + + Parameters + ========== + name: string + Future function's name, must be different for + different fucntions + to_return: list, Matrix or tuple of them + Determins the shape of the output and symbols inside it + *args: any number of lists, Matrices or tuples of them + Determins the shape of the input and symbols + names to assigned + + Notes + ===== + -All unassigned used symbols will be set to '1.0'. + -This function must be called only after the model that + computes symbols in to_return have been generated. + """ + exec self.gen_func_string(name, to_return, args) + return eval('%s' % name) + + diff --git a/symoroutils/tests/test_filemgr.py b/symoroutils/tests/test_filemgr.py new file mode 100644 index 0000000..7a971c3 --- /dev/null +++ b/symoroutils/tests/test_filemgr.py @@ -0,0 +1,93 @@ +#!/usr/bin/env python +# -*- coding: utf-8 -*- + + +# This file is part of the OpenSYMORO project. Please see +# https://github.com/symoro/symoro/blob/master/LICENCE for the licence. + + +"""Unit test module for functions in filemgr.py file.""" + + +import doctest +import os +import shutil +import unittest +from collections import namedtuple + +from symoroutils import filemgr + + +class TestFileMgr(unittest.TestCase): + """Unit test for functions in filemgr.py file""" + def setUp(self): + # strings that will be used by different test cases + self.home_folder = os.path.expanduser("~") + self.robots_folder = "symoro-robots" + self.base_path = os.path.join( + self.home_folder, self.robots_folder + ) + self.rob1 = "File Test Robot" + self.rob2 = "FileTestRobot" + # tmp robot created using namedtuple just for the purpose of + # test case + RobotTmp = namedtuple('RobotTmp', ['name', 'directory']) + self.tmp_robot = RobotTmp('Tmp Robot', self.base_path) + + def test_get_base_path(self): + self.assertEqual(filemgr.get_base_path(), self.base_path) + + def test_get_folder_path(self): + rob1_clean = filemgr.get_clean_name(self.rob1) + rob2_clean = filemgr.get_clean_name(self.rob2) + # scenario 1 + self.assertEqual( + filemgr.get_folder_path(self.rob1), + os.path.join(self.base_path, rob1_clean) + ) + # scenario 2 + self.assertEqual( + filemgr.get_folder_path(self.rob2), + os.path.join(self.base_path, rob2_clean) + ) + + def test_get_file_path(self): + robot_name_clean = filemgr.get_clean_name(self.tmp_robot.name) + # scenario 1 + par_checker = os.path.join( + self.tmp_robot.directory, + '%s.par' % robot_name_clean + ) + par_format = filemgr.get_file_path(self.tmp_robot) + self.assertEqual(par_format, par_checker) + # scenario 2 + trm_checker = os.path.join( + self.tmp_robot.directory, + '%s_%s.txt' % (robot_name_clean, 'trm') + ) + trm_format = filemgr.get_file_path(self.tmp_robot, 'trm') + self.assertEqual(trm_format, trm_checker) + + def tearDown(self): + # delete the temp folders and files created + rob1_path = filemgr.get_folder_path(self.rob1) + rob2_path = filemgr.get_folder_path(self.rob2) + if os.path.exists(rob1_path): + shutil.rmtree(rob1_path) + if os.path.exists(rob2_path): + shutil.rmtree(rob2_path) + + +def main(): + # load and run the doctests + doc_suite = doctest.DocTestSuite(filemgr) + unittest.TextTestRunner(verbosity=2).run(doc_suite) + # load and run the unittests + unit_suite = unittest.TestLoader().loadTestsFromTestCase(TestFileMgr) + unittest.TextTestRunner(verbosity=2).run(unit_suite) + + +if __name__ == '__main__': + main() + + diff --git a/symoroutils/tests/test_parfile.py b/symoroutils/tests/test_parfile.py new file mode 100644 index 0000000..d0dc25e --- /dev/null +++ b/symoroutils/tests/test_parfile.py @@ -0,0 +1,53 @@ +#!/usr/bin/env python +# -*- coding: utf-8 -*- + + +# This file is part of the OpenSYMORO project. Please see +# https://github.com/symoro/symoro/blob/master/LICENCE for the licence. + + +"""Unit test module for functions in parfile.py file.""" + + +import os +import unittest + +from symoroutils import filemgr +from symoroutils import parfile +from symoroutils import samplerobots +from symoroutils import tools +from pysymoro import robot + + +class TestParfile(unittest.TestCase): + def setUp(self): + self.orig_robo = samplerobots.rx90() + + def test_readwrite(self): + parfile.writepar(self.orig_robo) + fname = filemgr.get_clean_name(self.orig_robo.name) + ".par" + file_path = os.path.join(self.orig_robo.directory, fname) + new_robo, flag = parfile.readpar(self.orig_robo.name, file_path) + self.assertEqual(flag, tools.OK) + l1 = self.orig_robo.get_geom_head() + l2 = self.orig_robo.get_dynam_head() + l3 = self.orig_robo.get_ext_dynam_head() + for name in l3[1:]+l2[1:]+l1[1:]: + for i in xrange(1, self.orig_robo.NL): + self.assertEqual(self.orig_robo.get_val(i, name), + new_robo.get_val(i, name)) + + +def main(): + # load and run the doctests +# doc_suite = doctest.DocTestSuite(parfile) +# unittest.TextTestRunner(verbosity=2).run(doc_suite) + # load and run the unittests + unit_suite = unittest.TestLoader().loadTestsFromTestCase(TestParfile) + unittest.TextTestRunner(verbosity=2).run(unit_suite) + + +if __name__ == '__main__': + main() + + diff --git a/symoroutils/tests/test_symbolmgr.py b/symoroutils/tests/test_symbolmgr.py new file mode 100644 index 0000000..d869d04 --- /dev/null +++ b/symoroutils/tests/test_symbolmgr.py @@ -0,0 +1,145 @@ +#!/usr/bin/env python +# -*- coding: utf-8 -*- + + +# This file is part of the OpenSYMORO project. Please see +# https://github.com/symoro/symoro/blob/master/LICENCE for the licence. + + +"""Unit test for SymbolManager class.""" + + +import unittest + +from sympy import sympify, var, Matrix +from sympy.abc import A, B, C, X, Y, Z + +from symoroutils import symbolmgr +from symoroutils import tools + + +class TestSymbolManager(unittest.TestCase): + def setUp(self): + self.symo = symbolmgr.SymbolManager() + + def test_get_max_coef(self): + print("\n") + expr1 = A*B*X + C**2 - X + expr2 = Y*Z - B + self.assertEqual(tools.get_max_coef(expr1*X + expr2, X), expr1) + expr3 = -A**3*B**2*X**5*(X-Y)**7 + expr3x = -A**3*B**2*X**5*(-X-Y)**7 + expr3y = -A**3*B**2*X**5*(-X+Y)**7 + expr4 = B*X**2*(X-Y)**3 + self.assertEqual(tools.get_max_coef(expr3*expr4, expr4), expr3) + self.assertEqual(tools.get_max_coef(expr3x, expr4), tools.ZERO) + res = tools.get_max_coef(expr3y, expr4)*expr4-expr3y + self.assertEqual(res.expand(), tools.ZERO) + + def test_name_extraction(self): + print("\n") + expr1 = sympify("C2*S3*R + S2*C3*R") + self.assertEqual(tools.get_trig_couple_names(expr1), {'2', '3'}) + expr2 = sympify("CG2*S3*R + SG2*C1*R") + self.assertEqual(tools.get_trig_couple_names(expr2), {'G2'}) + expr2 = sympify("CA2*SA3*R + SG2*C3*R") + self.assertEqual(tools.get_trig_couple_names(expr2), set()) + expr3 = sympify("C2*S3*R + S1*C4*R") + self.assertEqual(tools.get_trig_couple_names(expr3), set()) + + def test_name_operations(self): + print("\n") + self.assertEqual(tools.reduce_str('12', '13'), ('2', '3')) + self.assertEqual(tools.reduce_str('124', '123'), ('4', '3')) + self.assertEqual(tools.reduce_str('124', '134'), ('2', '3')) + self.assertEqual(tools.reduce_str('12', '124'), ('', '4')) + self.assertEqual(tools.reduce_str('1G2', 'G24'), ('1', '4')) + self.assertEqual(tools.reduce_str('1G2G4', '13G4'), ('G2', '3')) + + def test_try_opt(self): + print("\n") + e1 = A*(B-C)*X**2 + B*X**3 + A*(B-C)*Y**2 + B*X*Y**2 + e2 = X**2 + e3 = Y**2 + e4 = tools.ONE + e5 = tools.ZERO + self.assertEqual(self.symo.try_opt(e4, e5, e2, e3, e1), A*(B-C) + B*X) + e6 = A*(B-C)*X**2 + B*X**3 - A*(B - C)*Y**2 - B*X*Y**2 + self.assertEqual(self.symo.try_opt(e4, e5, e2, e3, e6), e5) + e7 = A*B + self.assertEqual(self.symo.try_opt(e4, e7, e2, e3, e6), + e7*A*(B-C) + e7*B*X) + self.assertEqual(self.symo.try_opt(e7, e4, e2, e3, e1), + e7*A*(B-C) + e7*B*X) + + def test_trig_simp(self): + print("\n") + e1 = sympify("S2**2 + C2**2") + e1ans = sympify("1") + self.assertEqual(self.symo.C2S2_simp(e1), e1ans) + e1 = sympify("S1**2 + C2**2") + self.assertEqual(self.symo.C2S2_simp(e1), e1) + e1 = sympify("S2**3 + C2**2") + self.assertEqual(self.symo.C2S2_simp(e1), e1) + e1 = sympify("S2**2 + 2*C2**2") + e1ans = sympify("C2**2 + 1") + self.assertEqual(self.symo.C2S2_simp(e1), e1ans) + e1 = sympify("S1**2 + S1**2*C1 + C1**2 + C1**3 + C1**4") + e1ans = sympify("C1**4 + C1 + 1") + self.assertEqual(self.symo.C2S2_simp(e1), e1ans) + e2 = sympify("C1*S2 - C2*S1") + e2ans = sympify("-S1m2") + self.assertEqual(self.symo.CS12_simp(e2), e2ans) + e2 = sympify("(C1*S2 - C2*S1)*(C1*S2 + C2*S1)") + e2ans = sympify("-S1m2*S12") + self.assertEqual(self.symo.CS12_simp(e2), e2ans) + e2 = sympify("""C2*D3*S3m78 - C2m7*D8*S3 - + C3*D8*S2m7 - C3m78*D3*S2 + D2*S3""") + e2ans = sympify("D2*S3 - D3*S278m3 - D8*S23m7") + self.assertEqual(self.symo.CS12_simp(e2), e2ans) + e2 = sympify("sin(g+th2)*sin(th3+th8)-cos(g+th2)*cos(th3+th8)") + e2ans = sympify("-cos(g+th2+th3+th8)") + self.assertEqual(self.symo.CS12_simp(e2), e2ans) + e3 = sympify("""-a1*sin(th2+th1)*sin(th3)*cos(th1)- + a1*cos(th1)*cos(th2+th1)*cos(th3)""") + e3ans = sympify("-a1*cos(th1)*cos(th1 + th2 - th3)") + self.assertEqual(self.symo.CS12_simp(e3), e3ans) + e4 = sympify("""C2*C3*C4**2*C5**2*C6**4*D3**2*RL4*S5 + + 2*C2*C3*C4**2*C5**2*C6**2*D3**2*RL4*S5*S6**2 + + C2*C3*C4**2*C5**2*D3**2*RL4*S5*S6**4 + + C2*C3*C4**2*C6**4*D3**2*RL4*S5**3 + + 2*C2*C3*C4**2*C6**2*D3**2*RL4*S5**3*S6**2 + + C2*C3*C4**2*D3**2*RL4*S5**3*S6**4 + + C2*C3*C5**2*C6**4*D3**2*RL4*S4**2*S5 + + 2*C2*C3*C5**2*C6**2*D3**2*RL4*S4**2*S5*S6**2 + + C2*C3*C5**2*D3**2*RL4*S4**2*S5*S6**4 + + C2*C3*C6**4*D3**2*RL4*S4**2*S5**3 + + 2*C2*C3*C6**2*D3**2*RL4*S4**2*S5**3*S6**2 + + C2*C3*D3**2*RL4*S4**2*S5**3*S6**4 - + C3*C4**2*C5**2*C6**4*D3*RL4**2*S23*S5 - + 2*C3*C4**2*C5**2*C6**2*D3*RL4**2*S23*S5*S6**2 - + C3*C4**2*C5**2*D3*RL4**2*S23*S5*S6**4 - + C3*C4**2*C6**4*D3*RL4**2*S23*S5**3 - + 2*C3*C4**2*C6**2*D3*RL4**2*S23*S5**3*S6**2 - + C3*C4**2*D3*RL4**2*S23*S5**3*S6**4 - + C3*C5**2*C6**4*D3*RL4**2*S23*S4**2*S5 - + 2*C3*C5**2*C6**2*D3*RL4**2*S23*S4**2*S5*S6**2 - + C3*C5**2*D3*RL4**2*S23*S4**2*S5*S6**4 - + C3*C6**4*D3*RL4**2*S23*S4**2*S5**3 - + 2*C3*C6**2*D3*RL4**2*S23*S4**2*S5**3*S6**2 - + C3*D3*RL4**2*S23*S4**2*S5**3*S6**4""") + e4ans = sympify("C3*D3*RL4*S5*(C2*D3 - RL4*S23)") + self.assertEqual((self.symo.simp(e4)-e4ans).expand(), tools.ZERO) + + +def main(): + suite = unittest.TestLoader().loadTestsFromTestCase( + TestSymbolManager + ) + unittest.TextTestRunner(verbosity=2).run(suite) + + +if __name__ == '__main__': + main() + + diff --git a/symoroutils/tools.py b/symoroutils/tools.py new file mode 100644 index 0000000..93f762b --- /dev/null +++ b/symoroutils/tools.py @@ -0,0 +1,206 @@ +# -*- coding: utf-8 -*- + + +# This file is part of the OpenSYMORO project. Please see +# https://github.com/symoro/symoro/blob/master/LICENCE for the licence. + + +""" +This module contains the miscellaneous helper functions needed by the +SYMORO software package. +""" + + +import re + +from sympy import Matrix +from sympy import Integer +from sympy import sin, cos +from sympy import Mul, Add, var + + +ZERO = Integer(0) +ONE = Integer(1) +FAIL = 1 +OK = 0 +CLOSED_LOOP = 'Closed loop' +SIMPLE = 'Simple' +TREE = 'Tree' +TYPES = [SIMPLE, TREE, CLOSED_LOOP] +INT_KEYS = ['ant', 'sigma', 'mu'] + + +def skew(vec): + """skew-symmetry : Generates vectorial preproduct matrix + + Parameters + ========== + v: Matrix 3x1 + vector + + Returns + ======= + hat: Matrix 3x3 + """ + return Matrix([ + [0, -vec[2], vec[1]], + [vec[2], 0, -vec[0]], + [-vec[1], vec[0], 0] + ]) + + +def l2str(list_var, spacing=8): + """Converts a list into string, that will be + written into the text table. + + Parameters + ========== + list_var: list + List to be converted + spacing: int, optional + Defines the size of one cell of the table + + Returns + ======= + ret_str: string + String representation + + Notes + ===== + l2str([1, 2, 3]) will be converted into '1 2 3 ' + """ + ret_str = '' + for i in list_var: + ret_str += str(i) + ' '*(spacing-len(str(i))) + return ret_str + + +def find_trig_names(sym, pref=r'', pref_len=0, post=r'', post_len=0): + search_res = re.findall(pref + r'[AGm0-9]*' + post, str(sym)) + if post_len == 0: + return set([s[pref_len:] for s in search_res]) + else: + return set([s[pref_len:-post_len] for s in search_res]) + + +def get_trig_couple_names(sym): + names_s = find_trig_names(sym, r'S', 1) + names_c = find_trig_names(sym, r'C', 1) + return names_c & names_s + + +def get_max_coef_mul(sym, x_term): + k, ex = x_term.as_coeff_Mul() + coef = sym / k + pow_x = ex.as_powers_dict() + pow_c = coef.as_powers_dict() + pow_c[-1] = 0 + for j, pow_j in pow_x.iteritems(): + num_j = -j + if j in pow_c and pow_c[j] >= pow_j: + pow_c[j] -= pow_j + elif num_j in pow_c and pow_c[num_j] >= pow_j: + pow_c[num_j] -= pow_j + if pow_j % 2: + pow_c[-1] += 1 + else: + return ZERO + return Mul.fromiter(c**p for c, p in pow_c.iteritems()) + + +def get_max_coef_list(sym, x_term): + return [get_max_coef_mul(s, x_term) for s in Add.make_args(sym)] + + +def get_max_coef(sym, x_term): + return Add.fromiter( + get_max_coef_mul(s, x_term) for s in Add.make_args(sym) + ) + + +def get_pos_neg(str_term): + if str_term.find('m') != -1: + s_split = str_term.split('m') + return s_split[0], s_split[1] + else: + return str_term, '' + + +def reduce_str(str1, str2): + while True: + for j, char in enumerate(str1): + if char in 'AG': + i = str2.find(str1[j:j+2]) + k = 2 + else: + i = str2.find(char) + k = 1 + if i != -1: + if i+k < len(str2): + str2_tail = str2[i+k:] + else: + str2_tail = '' + if j+k < len(str1): + str1_tail = str1[j+k:] + else: + str1_tail = '' + str2 = str2[:i] + str2_tail + str1 = str1[:j] + str1_tail + break + else: + break + return str1, str2 + + +def ang_sum(np1, np2, nm1, nm2): + np2, nm1 = reduce_str(np2, nm1) + np1, nm2 = reduce_str(np1, nm2) + if len(nm1) + len(nm2) == 0: + return np1 + np2 + else: + return np1 + np2 + 'm' + nm1 + nm2 + + +def cos_sin_syms(name): + if isinstance(name, str) and name[0] == 'm': + cos_term, sin_term = var('C{0}, S{0}'.format(name[1:])) + return cos_term, -sin_term + else: + return var('C{0}, S{0}'.format(name)) + + +def sym_less(val_a, val_b): + val_a_measure = val_a.count_ops() + val_b_measure = val_b.count_ops() + return val_a_measure < val_b_measure + + +def get_angles(expr): + angles_s = set() + for sin_term in expr.atoms(sin): + angles_s |= set(sin_term.args) + angles_c = set() + for cos_term in expr.atoms(cos): + angles_c |= set(cos_term.args) + return angles_s & angles_c + + +def cancel_terms(sym, x_term, coef): + if coef.is_Add: + for arg_c in coef.args: + sym = cancel_terms(sym, x_term, arg_c) + else: + terms = Add.make_args(sym) + return Add.fromiter(t for t in terms if t != x_term*coef) + + +def trignometric_info(sym): + if not sym.has(sin) and not sym.has(cos): + short_form = True + names = get_trig_couple_names(sym) + else: + short_form = False + names = get_angles(sym) + return names, short_form + + diff --git a/pysymoro/visualize/__init__.py b/symoroviz/__init__.py similarity index 100% rename from pysymoro/visualize/__init__.py rename to symoroviz/__init__.py diff --git a/pysymoro/visualize/graphics.py b/symoroviz/graphics.py similarity index 63% rename from pysymoro/visualize/graphics.py rename to symoroviz/graphics.py index 63d6157..294a678 100644 --- a/pysymoro/visualize/graphics.py +++ b/symoroviz/graphics.py @@ -1,26 +1,41 @@ -__author__ = 'Izzat' +# -*- coding: utf-8 -*- + + +# This file is part of the OpenSYMORO project. Please see +# https://github.com/symoro/symoro/blob/master/LICENCE for the licence. + + +from math import atan2 + import wx import wx.lib.agw.floatspin as FS +from wx.glcanvas import GLCanvas + import OpenGL.GL as gl import OpenGL.GLU as glu -from objects import Frame, RevoluteJoint, FixedJoint, PrismaticJoint -from wx.glcanvas import GLCanvas + from numpy import sin, cos, radians, pi, inf, nan -from math import atan2 + from sympy import Expr -from core.symoro import Symoro, CLOSED_LOOP -from core.invgeom import loop_solve -from core.geometry import dgm +from pysymoro.invgeom import loop_solve +from pysymoro.geometry import dgm +from symoroutils import samplerobots +from symoroutils import symbolmgr +from symoroutils import tools + +from symoroviz.objects import Frame +from symoroviz.objects import RevoluteJoint +from symoroviz.objects import FixedJoint +from symoroviz.objects import PrismaticJoint #TODO: Fullscreen camera rotation bug #TODO: X-, Z-axis #TODO: Random button -class myGLCanvas(GLCanvas): - +class VizGlCanvas(GLCanvas): def __init__(self, parent, robo, params, size=(600, 600)): - super(myGLCanvas, self).__init__(parent, size=size) + super(VizGlCanvas, self).__init__(parent, size=size) self.Bind(wx.EVT_PAINT, self.OnPaintAll) self.Bind(wx.EVT_SIZE, self.OnSize) self.Bind(wx.EVT_LEFT_DOWN, self.OnMouseDown) @@ -37,9 +52,9 @@ def __init__(self, parent, robo, params, size=(600, 600)): self.q_pas_sym = self.robo.q_passive self.q_act_sym = self.robo.q_active self.pars_num = params - self.init = 0 - self.distance = 5. - self.fov = 40. + self.init = 0.0 + self.distance = 5.0 + self.fov = 40.0 self.jnt_objs = [] self.construct_hierarchy() self.dgms = {} @@ -51,7 +66,9 @@ def OnEraseBackground(self, event): pass def assign_mono_scale(self): - """ This function calculates coefficients which are used + """Sets the coefficients used to draw objects + + This function calculates coefficients which are used to draw the objects (Joints, links, end-effectors) It computes the minimum and maximum r or d different from 0. Then uses those sizes to determine the reference @@ -64,11 +81,10 @@ def assign_mono_scale(self): minv = dist if minv == inf: minv = 1. - self.length = 0.4*minv - print self.length + self.length = 0.4 * minv for jnt in self.jnt_objs: if isinstance(jnt, PrismaticJoint): - jnt.r = 3.5*self.length + jnt.r = 3.5 * self.length jnt.set_length(self.length) def add_items_to_frame(self, frame, index, jnt_hier): @@ -85,13 +101,16 @@ def add_items_to_frame(self, frame, index, jnt_hier): except: if val in self.q_sym: params.append(0.) - if self.robo.sigma[child_i] == 0: child_frame = RevoluteJoint(*params) elif self.robo.sigma[child_i] == 1: child_frame = PrismaticJoint(*params) else: child_frame = FixedJoint(*params) + if self.has_end(child_i): + child_frame.has_end = True + if self.has_base(child_i): + child_frame.has_base = True self.jnt_objs.append(child_frame) frame.add_child(child_frame) self.add_items_to_frame(child_frame, child_i, jnt_hier) @@ -99,8 +118,10 @@ def add_items_to_frame(self, frame, index, jnt_hier): def get_joints_dictionary(self): result = {} for jnt_i in range(self.robo.NF): - result[jnt_i] = [i for i, child in enumerate(self.robo.ant) - if child == jnt_i] + result[jnt_i] = [ + i for i, child in enumerate(self.robo.ant) + if child == jnt_i + ] return result def construct_hierarchy(self): @@ -139,17 +160,25 @@ def OnMouseUp(self, _): def OnMouseMotion(self, evt): if evt.Dragging(): x, y = evt.GetPosition() - if evt.LeftIsDown(): - dx, dy = x - self.lastx, y - self.lasty - self.lastx, self.lasty = x, y - coef = self.distance/self.length*sin(radians(self.fov/2.)) - self.hor_angle += dx*coef/self.size.width - self.ver_angle += dy*coef/self.size.height - self.ver_angle = max(min(pi/2, self.ver_angle), -pi/2) - elif evt.RightIsDown(): - dy = y - self.lasty - self.lasty = y + dx, dy = x - self.lastx, y - self.lasty + self.lastx, self.lasty = x, y + if evt.LeftIsDown() and evt.RightIsDown(): + # zoom self.distance *= 1 + 2 * float(dy) / self.size.height + else: + coef = self.distance / self.length * \ + sin(radians(self.fov/2.0)) + if evt.RightIsDown(): + # rotate + self.hor_angle += dx * (coef / self.size.width) + self.ver_angle += dy * (coef / self.size.height) + self.ver_angle = max(min(pi/2, self.ver_angle), -pi/2) + elif evt.LeftIsDown(): + # translate + self.cen_x -= dx * (coef / self.size.width) + self.cen_x = max(min(pi/2, self.cen_x), -pi/2) + self.cen_z += dy * (coef / self.size.height) + self.cen_z = max(min(pi/2, self.cen_z), -pi/2) self.CameraTransformation() self.Refresh(False) @@ -159,7 +188,7 @@ def dgm_for_frame(self, i): if i > 0 and jnt.r == 0 and jnt.d == 0 and jnt.b == 0: self.dgms[i] = self.dgm_for_frame(self.robo.ant[i]) else: - symo = Symoro(sydi=self.pars_num) + symo = symbolmgr.SymbolManager(sydi=self.pars_num) T = dgm(self.robo, symo, 0, i, fast_form=True, trig_subs=True) self.dgms[i] = symo.gen_func('dgm_generated', T, self.q_sym) return self.dgms[i] @@ -185,7 +214,7 @@ def find_solution(self, qs_act, qs_pas): self.jnt_dict[sym].q = min_sln[i] def solve(self): - if self.robo.structure != CLOSED_LOOP: + if self.robo.structure != tools.CLOSED_LOOP: return if self.l_solver is None: self.generate_loop_fcn() @@ -198,14 +227,14 @@ def solve(self): i = self.jnt_dict[sym].index if i < self.robo.NL: qs_pas.append((self.jnt_dict[sym].q, self.robo.sigma[i])) - self.find_solution(qs_act, qs_pas) def generate_loop_fcn(self): - symo = Symoro(sydi=self.pars_num) + symo = symbolmgr.SymbolManager(sydi=self.pars_num) loop_solve(self.robo, symo) - self.l_solver = symo.gen_func('IGM_gen', self.q_pas_sym, - self.q_act_sym, multival=True) + self.l_solver = symo.gen_func( + 'IGM_gen', self.q_pas_sym, self.q_act_sym + ) def centralize_to_frame(self, index): q_vec = [self.jnt_dict[sym].q for sym in self.q_sym] @@ -229,7 +258,8 @@ def CameraTransformation(self): self.cen_y-self.distance*cos(self.ver_angle)*cos(self.hor_angle), self.cen_z+self.distance*sin(self.ver_angle), self.cen_x, self.cen_y, self.cen_z, - 0.0, 0.0, 1.0) + 0.0, 0.0, 1.0 + ) # def SetCameraForLabels(self): # gl.glLoadIdentity() @@ -305,6 +335,31 @@ def show_frames(self, lst): self.jnt_objs[index].set_show_frame(True) self.OnDraw() + def has_end(self, index): + """ + Check if current index value corresponds to a terminal link. + """ + if index in range(self.robo.NL) and \ + not index in self.robo.ant: + # when index value is present in the list of links (for + # closed-loop case) and not present in the list of + # antecedent values + return True + else: + return False + + def has_base(self, index): + """ + Check if current index value is linked to the base. + """ + try: + if self.robo.ant[index] == 0: + return True + else: + return False + except IndexError: + return False + def change_lengths(self, new_length): for jnt in self.jnt_objs: jnt.set_length(new_length) @@ -315,7 +370,6 @@ def OnDraw(self): if not self.init: return gl.glClear(gl.GL_COLOR_BUFFER_BIT | gl.GL_DEPTH_BUFFER_BIT) - gl.glEnableClientState(gl.GL_VERTEX_ARRAY) gl.glEnableClientState(gl.GL_NORMAL_ARRAY) self.base.draw() @@ -323,6 +377,7 @@ def OnDraw(self): self.base.draw_frames() gl.glDisableClientState(gl.GL_VERTEX_ARRAY) gl.glDisableClientState(gl.GL_NORMAL_ARRAY) + gl.glFlush() self.SwapBuffers() def InitGL(self): @@ -332,7 +387,6 @@ def InitGL(self): light_position1 = (-0.3, 0.3, 0.5, 0.0) diffuseMaterial = (1., 1., 1., 1.0) ambientMaterial = (0.5, .5, .5, 1.0) - gl.glClearColor(1.0, 1.0, 1.0, 1.0) gl.glShadeModel(gl.GL_SMOOTH) gl.glEnable(gl.GL_DEPTH_TEST) @@ -345,132 +399,164 @@ def InitGL(self): gl.glEnable(gl.GL_LIGHTING) gl.glEnable(gl.GL_LIGHT0) gl.glEnable(gl.GL_LIGHT1) - gl.glColorMaterial(gl.GL_FRONT, gl.GL_DIFFUSE) gl.glEnable(gl.GL_COLOR_MATERIAL) - gl.glMatrixMode(gl.GL_PROJECTION) gl.glLoadIdentity() glu.gluPerspective(40.0, 1., 0.2, 200.0) - gl.glMatrixMode(gl.GL_MODELVIEW) self.CameraTransformation() class MainWindow(wx.Frame): - def __init__(self, prefix, robo, params=None, parent=None, identifier=-1): - wx.Frame.__init__( - self, parent, identifier, prefix + ': Robot representation', + def __init__( + self, prefix, robo, params=None, parent=None, identifier=-1 + ): + super(MainWindow, self).__init__( + parent, identifier, prefix + ': Robot representation', style=wx.DEFAULT_FRAME_STYLE | wx.FULL_REPAINT_ON_RESIZE ) self.robo = robo self.params_dict = params if params is not None else {} - self.solve_loops = False - self.canvas = myGLCanvas(self, robo, self.params_dict, size=(600, 600)) - self.p = wx.Panel(self) + self.canvas = VizGlCanvas( + self, robo, self.params_dict, size=(600, 600) + ) + self.p = wx.lib.scrolledpanel.ScrolledPanel(self, -1) + self.p.SetMinSize((350,650)) self.init_ui() + self.p.SetupScrolling() self.update_spin_controls() - self.sizer = wx.BoxSizer(wx.HORIZONTAL) self.sizer.Add(self.canvas, 1, wx.EXPAND) self.sizer.Add(self.p, 0, wx.EXPAND) self.SetSizerAndFit(self.sizer) - self.Show() def init_ui(self): - top_sizer = wx.BoxSizer(wx.HORIZONTAL) - gridControl = wx.GridBagSizer(hgap=10, vgap=10) - cb = wx.CheckBox(self.p, label="Exploded View") - cb.SetValue(True) - gridControl.Add(cb, pos=(0, 0), flag=wx.ALIGN_CENTER_VERTICAL) - cb.Bind(wx.EVT_CHECKBOX, self.OnChangeRepresentation) - - self.tButton = wx.ToggleButton(self.p, label="All Frames") - self.tButton.SetValue(True) - self.tButton.Bind(wx.EVT_TOGGLEBUTTON, self.OnShowAllFrames) - gridControl.Add(self.tButton, pos=(1, 0), flag=wx.ALIGN_CENTER) - - btnReset = wx.Button(self.p, label="Reset All") - btnReset.Bind(wx.EVT_BUTTON, self.OnResetJoints) - gridControl.Add(btnReset, pos=(3, 0), flag=wx.ALIGN_CENTER) - - btnRandom = wx.Button(self.p, label="Random") - btnRandom.Bind(wx.EVT_BUTTON, self.OnFindRandom) - gridControl.Add(btnRandom, pos=(4, 0), flag=wx.ALIGN_CENTER) + szr_container = wx.BoxSizer(wx.HORIZONTAL) + grd_szr_control = wx.GridBagSizer(hgap=10, vgap=10) + # exploded view checkbox + chk_exploded = wx.CheckBox(self.p, label="Exploded View") + chk_exploded.SetValue(True) + chk_exploded.Bind(wx.EVT_CHECKBOX, self.OnChangeRepresentation) + grd_szr_control.Add( + chk_exploded, pos=(0, 0), flag=wx.ALIGN_CENTER_VERTICAL + ) + # joint size slider + self.slr_joint_size = wx.Slider(self.p, minValue=1, maxValue=100) + self.slr_joint_size.SetValue(100*self.canvas.length) + self.slr_joint_size.Bind(wx.EVT_SCROLL, self.OnSliderChanged) + grd_szr_control.Add( + wx.StaticText(self.p, label="Joint Size"), + pos=(1, 0), flag=wx.ALIGN_CENTER + ) + grd_szr_control.Add( + self.slr_joint_size, pos=(2, 0), flag=wx.ALIGN_CENTER + ) + # all frame toggle button + self.tgl_btn_frames = wx.ToggleButton(self.p, label="All Frames") + self.tgl_btn_frames.SetValue(True) + self.tgl_btn_frames.Bind( + wx.EVT_TOGGLEBUTTON, self.OnShowAllFrames + ) + grd_szr_control.Add( + self.tgl_btn_frames, pos=(3, 0), flag=wx.ALIGN_CENTER + ) + # frames list box + choices = [] + for jnt in self.canvas.jnt_objs: + choices.append("Frame " + str(jnt.index)) + self.drag_pos = None + self.clb_frames = wx.CheckListBox(self.p, choices=choices) + self.clb_frames.SetChecked(range(len(choices))) + self.clb_frames.Bind(wx.EVT_CHECKLISTBOX, self.CheckFrames) + self.clb_frames.Bind(wx.EVT_LISTBOX, self.SelectFrames) + grd_szr_control.Add( + self.clb_frames, pos=(4, 0), flag=wx.ALIGN_CENTER + ) + # reset all button + btn_reset = wx.Button(self.p, label="Reset All") + btn_reset.Bind(wx.EVT_BUTTON, self.OnResetJoints) + grd_szr_control.Add(btn_reset, pos=(5, 0), flag=wx.ALIGN_CENTER) + # random buttom + btn_random = wx.Button(self.p, label="Random") + btn_random.Bind(wx.EVT_BUTTON, self.OnFindRandom) + grd_szr_control.Add(btn_random, pos=(6, 0), flag=wx.ALIGN_CENTER) + # home position button + btn_home = wx.Button(self.p, label="Default Position") + btn_home.Bind(wx.EVT_BUTTON, self.OnHomePosition) + grd_szr_control.Add(btn_home, pos=(7,0), flag=wx.ALIGN_CENTER) + # break/make loop radio buttons + if self.robo.structure == tools.CLOSED_LOOP: + choice_list = ['Break Loops', 'Make Loops'] + self.rbx_loops = wx.RadioBox( + self.p, choices=choice_list, style=wx.RA_SPECIFY_ROWS + ) + self.rbx_loops.Bind(wx.EVT_RADIOBOX, self.OnSelectLoops) + grd_szr_control.Add( + self.rbx_loops, pos=(8, 0), flag=wx.ALIGN_CENTER + ) + # help text + move_help = """ + To Translate: + Left button + + move mouse + + To Rotate: + Right button + + move mouse + + To Zoom: + Left and Right + button + move + mouse + """ + grd_szr_control.Add( + wx.StaticText(self.p, label=move_help), + pos=(9,0), flag=wx.ALIGN_LEFT + ) + # joint variables box self.spin_ctrls = {} - gridJnts = wx.GridBagSizer(hgap=10, vgap=10) + grd_szr_joints = wx.GridBagSizer(hgap=10, vgap=10) p_index = 0 for sym in self.canvas.q_sym: - if sym == 0: - continue + if sym == 0: continue jnt = self.canvas.jnt_dict[sym] if isinstance(jnt, RevoluteJoint): label = 'th' else: label = 'r' - gridJnts.Add(wx.StaticText(self.p, label=label+str(jnt.index)), - pos=(p_index, 0), flag=wx.ALIGN_CENTER_VERTICAL) - s = FS.FloatSpin(self.p, size=(70, -1), id=jnt.index, - increment=0.05, min_val=-10., max_val=10.) - s.Bind(FS.EVT_FLOATSPIN, self.OnSetJointVar) - s.SetDigits(2) -# if sym in self.canvas.q_pas_sym: -# s.Enable(False) - self.spin_ctrls[sym] = s - gridJnts.Add(s, pos=(p_index, 1), flag=wx.ALIGN_CENTER_VERTICAL) - p_index += 1 - - if self.robo.structure == CLOSED_LOOP: - choise_list = ['Break Loops', 'Make Loops'] - self.radioBox = wx.RadioBox(self.p, choices=choise_list, - style=wx.RA_SPECIFY_ROWS) - self.radioBox.Bind(wx.EVT_RADIOBOX, self.OnSelectLoops) - gridControl.Add(self.radioBox, pos=(5, 0), flag=wx.ALIGN_CENTER) - - choices = [] - for jnt in self.canvas.jnt_objs: - choices.append("Frame " + str(jnt.index)) - - self.drag_pos = None - self.check_list = wx.CheckListBox(self.p, choices=choices) - self.check_list.SetChecked(range(len(choices))) - self.check_list.Bind(wx.EVT_CHECKLISTBOX, self.CheckFrames) - self.check_list.Bind(wx.EVT_LISTBOX, self.SelectFrames) - gridControl.Add(self.check_list, pos=(2, 0), flag=wx.ALIGN_CENTER) - - q_box = wx.StaticBoxSizer(wx.StaticBox(self.p, label='Joint variables')) - q_box.Add(gridJnts, 0, wx.ALL, 10) - - ver_sizer = wx.BoxSizer(wx.VERTICAL) - ver_sizer.Add(q_box) - lbl_length = wx.StaticText(self.p, label='Joint size') - self.jnt_slider = wx.Slider(self.p, minValue=1, maxValue=100) - self.jnt_slider.SetValue(100*self.canvas.length) - self.jnt_slider.Bind(wx.EVT_SCROLL, self.OnSliderChanged) - ver_sizer.Add(lbl_length, 0, wx.TOP | wx.ALIGN_CENTER_HORIZONTAL, 15) - ver_sizer.Add(self.jnt_slider, 0, wx.ALL | wx.ALIGN_CENTER_HORIZONTAL, 5) - - top_sizer.Add(ver_sizer, 0, wx.ALL, 10) - top_sizer.AddSpacer(10) - top_sizer.Add(gridControl, 0, wx.ALL, 10) - - # button1 = wx.Button(self.panel, label="TEXT 1") - # button2 = wx.Button(self.panel, label="TEXT 2") - # check1 = wx.CheckBox(self.panel, label="Show Axes") - # self.panel.Bind(wx.EVT_CHECKBOX, None) - # - # sizer = wx.BoxSizer(wx.VERTICAL) - # sizer.Add(button1, flag=wx.BOTTOM, border=5) - # sizer.Add(button2, flag=wx.BOTTOM, border=5) - # sizer.Add(check1) - # - # border = wx.BoxSizer() - # border.Add(sizer, flag=wx.ALL | wx.EXPAND, border=5) - - self.p.SetSizerAndFit(top_sizer) + grd_szr_joints.Add( + wx.StaticText(self.p, label=label+str(jnt.index)), + pos=(p_index, 0), flag=wx.ALIGN_CENTER_VERTICAL + ) + fsn_qvar = FS.FloatSpin( + self.p, size=(70, -1), id=jnt.index, increment=0.05, + min_val=-10., max_val=10.0 + ) + fsn_qvar.Bind(FS.EVT_FLOATSPIN, self.OnSetJointVar) + fsn_qvar.SetDigits(2) + # if sym in self.canvas.q_pas_sym: + # fsn_qvar.Enable(False) + self.spin_ctrls[sym] = fsn_qvar + grd_szr_joints.Add( + fsn_qvar, pos=(p_index, 1), + flag=wx.ALIGN_CENTER_VERTICAL + ) + p_index = p_index + 1 + # add all components to sizer + szr_qbox = wx.StaticBoxSizer( + wx.StaticBox(self.p, label='Joint variables') + ) + szr_qbox.Add(grd_szr_joints, 0, wx.ALL, 10) + szr_vertical = wx.BoxSizer(wx.VERTICAL) + szr_vertical.Add(szr_qbox) + szr_container.Add(grd_szr_control, 0, wx.ALL, 10) + szr_container.AddSpacer(10) + szr_container.Add(szr_vertical, 0, wx.ALL, 10) + self.p.SetSizer(szr_container) def OnChangeRepresentation(self, evt): self.canvas.representation(evt.EventObject.GetValue()) @@ -482,12 +568,12 @@ def OnShowWorldFrame(self, evt): def OnShowAllFrames(self, _): """Shows or hides all the frames (Toggle button event handler) """ - if self.tButton.Value: + if self.tgl_btn_frames.Value: indices = range(len(self.canvas.jnt_objs)) else: indices = [] self.canvas.show_frames(indices) - self.check_list.SetChecked(indices) + self.clb_frames.SetChecked(indices) def update_spin_controls(self): for ctrl in self.spin_ctrls.values(): @@ -495,8 +581,8 @@ def update_spin_controls(self): def OnSelectLoops(self, _): for q in self.canvas.q_pas_sym: - self.spin_ctrls[q].Enable(not self.radioBox.Selection) - if self.radioBox.Selection == 1: + self.spin_ctrls[q].Enable(not self.rbx_loops.Selection) + if self.rbx_loops.Selection == 1: self.solve_loops = True self.canvas.solve() self.update_spin_controls() @@ -516,6 +602,9 @@ def OnResetJoints(self, _): def OnFindRandom(self, evt): pass + def OnHomePosition(self, evt): + self.canvas.centralize_to_frame(0) + def OnSetJointVar(self, evt): """Sets joint values from the spin-controls """ @@ -533,7 +622,7 @@ def OnSetJointVar(self, evt): def CheckFrames(self, evt): self.canvas.show_frames(evt.EventObject.GetChecked()) - self.tButton.Value = False + self.tgl_btn_frames.Value = False evt.EventObject.DeselectAll() def SelectFrames(self, evt): @@ -542,17 +631,6 @@ def SelectFrames(self, evt): self.canvas.centralize_to_frame(selections[0]) def OnSliderChanged(self, _): - self.canvas.change_lengths(self.jnt_slider.Value/100.) - - -if __name__ == '__main__': - app = wx.PySimpleApp() - import symoro - robo = symoro.Robot.RX90() - robo.d[3] = 1. - robo.r[4] = 1. - frame = MainWindow(prefix='', robo=robo) - import profile - profile.run('frame.canvas.OnPaintAll(wx.CommandEvent())', sort='cumtime') - app.MainLoop() - app.Destroy() + self.canvas.change_lengths(self.slr_joint_size.Value/100.) + + diff --git a/symoroviz/objects.py b/symoroviz/objects.py new file mode 100644 index 0000000..646aec5 --- /dev/null +++ b/symoroviz/objects.py @@ -0,0 +1,313 @@ +# -*- coding: utf-8 -*- + + +# This file is part of the OpenSYMORO project. Please see +# https://github.com/symoro/symoro/blob/master/LICENCE for the licence. + + +import OpenGL.GL as gl + +from numpy import degrees, identity, array + +from primitives import Primitives + + +class Frame(object): + def __init__(self, index=0, T=identity(4), show_frame=True): + self.children = [] + self.T = T + self.show_frame = show_frame + self.index = index + + def __str__(self): + return '(Frame {0} Children: {1})'.format(self.index, self.children) + + def __repr__(self): + return self.__str__() + + def draw_frame(self): + if self.show_frame: + gl.glPushMatrix() + # z-axis (joint axis) - red + gl.glColor3f(1, 0, 0) + self.draw_arrow() + # x-axis - green + gl.glRotatef(90, 0, 1, 0) + gl.glColor3f(0, 1, 0) + self.draw_arrow() + gl.glPopMatrix() + + def draw_arrow(self): + gl.glVertexPointer(3, gl.GL_FLOAT, 0, self.arr_vertices) + gl.glNormalPointer(gl.GL_FLOAT, 0, self.arr_normals) + gl.glDrawElements( + gl.GL_TRIANGLES, len(self.arr_indices), + gl.GL_UNSIGNED_INT, self.arr_indices + ) + + def set_show_frame(self, show=True): + self.show_frame = show + + def add_child(self, child): + self.children.append(child) + + def draw_frames(self): + gl.glPushMatrix() + gl.glMultMatrixf(self.T) + self.draw_frame() + for child in self.children: + child.draw_frames() + gl.glPopMatrix() + + def draw(self): + gl.glPushMatrix() + gl.glMultMatrixf(self.T) + for child in self.children: + child.draw() + gl.glPopMatrix() + + def set_length(self, new_length): + self.arr_vertices, self.arr_indices, self.arr_normals = \ + Primitives.arr_array(new_length) + + +class JointObject(Frame): + def __init__(self, index, theta=0., r=0., alpha=0., d=0., gamma=0., b=0.): + super(JointObject, self).__init__(index) + self.theta = theta + self.r = r + self.alpha = alpha + self.d = d + self.gamma = gamma + self.b = b + self.shift = 0. + self.init_length = 0. + self.has_base = False + self.has_end = False + + def draw_rod(self, length): + gl.glPushMatrix() + gl.glMultMatrixf(array([ + [1., 0., 0., 0.], + [0., 1., 0., 0.], + [0., 0., length, 0.], + [0., 0., 0., 1.] + ])) + gl.glColor3f(0.8, 0.51, 0.25) + gl.glVertexPointer(3, gl.GL_FLOAT, 0, self.rod_vertices) + gl.glNormalPointer(gl.GL_FLOAT, 0, self.rod_normals) + gl.glDrawElements( + gl.GL_TRIANGLES, len(self.rod_indices), + gl.GL_UNSIGNED_INT, self.rod_indices + ) + gl.glPopMatrix() + + def draw_frames(self): + gl.glPushMatrix() + gl.glRotatef(degrees(self.gamma), 0, 0, 1) + gl.glTranslatef(0, 0, self.b) + gl.glRotatef(degrees(self.alpha), 1, 0, 0) + gl.glTranslatef(self.d, 0, 0) + gl.glRotatef(degrees(self.theta), 0, 0, 1) + gl.glTranslatef(0, 0, self.r) + self.draw_frame() + for child in self.children: + child.draw_frames() + gl.glPopMatrix() + + def draw(self): + gl.glPushMatrix() + if self.b: + self.draw_rod(self.b) + gl.glTranslatef(0, 0, self.b) + gl.glRotatef(degrees(self.gamma), 0, 0, 1) + if self.d: + gl.glPushMatrix() + gl.glRotatef(90, 0, 1, 0) + self.draw_rod(self.d) + gl.glPopMatrix() + gl.glTranslatef(self.d, 0, 0) + gl.glRotatef(degrees(self.alpha), 1, 0, 0) + if self.r: + self.draw_rod(self.r) + gl.glTranslatef(0, 0, self.r) + gl.glRotatef(degrees(self.theta), 0, 0, 1) + if self.shift: + gl.glPushMatrix() + shift = self.shift*self.length + self.draw_rod(shift) + gl.glTranslatef(0, 0, shift) + self.draw_joint() + self.draw_base() + self.draw_end() + gl.glPopMatrix() + else: + self.draw_joint() + self.draw_base() + self.draw_end() + for child in self.children: + child.draw() + gl.glPopMatrix() + + def draw_base(self): + if self.has_base: + gl.glColor3f(0.0, 0.0, 0.0) + gl.glVertexPointer(3, gl.GL_FLOAT, 0, self.sph_vertices) + gl.glNormalPointer(gl.GL_FLOAT, 0, self.sph_normals) + gl.glDrawElements( + gl.GL_TRIANGLES, len(self.sph_indices), + gl.GL_UNSIGNED_INT, self.sph_indices, + ) + + def draw_end(self): + if self.has_end: + gl.glColor3f(0.0, 0.0, 1.0) + gl.glVertexPointer(3, gl.GL_FLOAT, 0, self.sph_vertices) + gl.glNormalPointer(gl.GL_FLOAT, 0, self.sph_normals) + gl.glDrawElements( + gl.GL_TRIANGLES, len(self.sph_indices), + gl.GL_UNSIGNED_INT, self.sph_indices, + ) + + def set_length(self, new_length): + if not self.init_length: + self.rod_vertices, self.rod_indices, self.rod_normals = \ + Primitives.rod_array(new_length) + self.init_length = new_length + self.length = new_length + if self.has_base or self.has_end: + self.sph_vertices, self.sph_indices, self.sph_normals = \ + Primitives.sph_array(1.5 * new_length) + super(JointObject, self).set_length(new_length) + + +class RevoluteJoint(JointObject): + def __init__(self, *args): + super(RevoluteJoint, self).__init__(*args) + self.q_init = self.theta + + def __str__(self): + return '(RevoluteJoint {0} children: {1})'.format( + self.index, self.children + ) + + def __repr__(self): + return self.__str__() + + def draw_joint(self): + gl.glColor3f(1., 1., 0.) + gl.glVertexPointer(3, gl.GL_FLOAT, 0, self.cyl_vertices) + gl.glNormalPointer(gl.GL_FLOAT, 0, self.cyl_normals) + gl.glDrawElements( + gl.GL_TRIANGLES, len(self.cyl_indices), + gl.GL_UNSIGNED_INT, self.cyl_indices + ) + + @property + def q(self): + return self.theta + + @q.setter + def q(self, theta): + self.theta = theta + + def set_length(self, new_length): + self.cyl_vertices, self.cyl_indices, self.cyl_normals = \ + Primitives.cyl_array(new_length) + super(RevoluteJoint, self).set_length(new_length) + + +class PrismaticJoint(JointObject): + def __init__(self, *args): + super(PrismaticJoint, self).__init__(*args) + self.q_init = self.r + + def __str__(self): + return '(PrismaticJoint {0} children: {1})'.format( + self.index, self.children + ) + + def __repr__(self): + return self.__str__() + + def draw_joint(self): + gl.glColor3f(1., 0.6, 0.) + gl.glVertexPointer(3, gl.GL_FLOAT, 0, self.box_vertices) + gl.glNormalPointer(gl.GL_FLOAT, 0, self.box_normals) + gl.glDrawArrays(gl.GL_QUADS, 0, 24) + + @property + def q(self): + return self.r + + @q.setter + def q(self, r): + self.r = r + + def set_length(self, new_length): + self.box_vertices, self.box_normals = \ + Primitives.box_array(new_length) + super(PrismaticJoint, self).set_length(new_length) + + def draw(self): + gl.glPushMatrix() + if self.b: + self.draw_rod(self.b) + gl.glTranslatef(0, 0, self.b) + gl.glRotatef(degrees(self.gamma), 0, 0, 1) + if self.d: + gl.glPushMatrix() + gl.glRotatef(90, 0, 1, 0) + self.draw_rod(self.d) + gl.glPopMatrix() + gl.glTranslatef(self.d, 0, 0) + gl.glRotatef(degrees(self.alpha), 1, 0, 0) + if self.shift: + gl.glPushMatrix() + shift = self.shift*self.length + self.draw_rod(shift) + gl.glTranslatef(0, 0, shift) + self.draw_joint() + self.draw_base() + self.draw_end() + gl.glPopMatrix() + else: + self.draw_joint() + self.draw_base() + self.draw_end() + if self.r: + self.draw_rod(self.r) + gl.glTranslatef(0, 0, self.r) + gl.glRotatef(degrees(self.theta), 0, 0, 1) + for child in self.children: + child.draw() + gl.glPopMatrix() + + +class FixedJoint(JointObject): + def __init__(self, *args): + super(FixedJoint, self).__init__(*args) + + def __str__(self): + return '(FixedJoint {0} children: {1})'.format( + self.index, self.children + ) + + def __repr__(self): + return self.__str__() + + def draw_joint(self): + gl.glColor3f(1., 0., 1.) + gl.glVertexPointer(3, gl.GL_FLOAT, 0, self.sph_vertices) + gl.glNormalPointer(gl.GL_FLOAT, 0, self.sph_normals) + gl.glDrawElements( + gl.GL_TRIANGLES, len(self.sph_indices), + gl.GL_UNSIGNED_INT, self.sph_indices + ) + + def set_length(self, new_length): + self.sph_vertices, self.sph_indices, self.sph_normals = \ + Primitives.sph_array(new_length) + super(FixedJoint, self).set_length(new_length) + + diff --git a/pysymoro/visualize/primitives.py b/symoroviz/primitives.py similarity index 95% rename from pysymoro/visualize/primitives.py rename to symoroviz/primitives.py index 8c23c72..c7bda5f 100644 --- a/pysymoro/visualize/primitives.py +++ b/symoroviz/primitives.py @@ -1,6 +1,12 @@ -__author__ = 'Izzat' +# -*- coding: utf-8 -*- + + +# This file is part of the OpenSYMORO project. Please see +# https://github.com/symoro/symoro/blob/master/LICENCE for the licence. + from itertools import product + from numpy import sin, cos, pi @@ -95,7 +101,6 @@ def create_box_array(length=3., width=1.): class Primitives: - @classmethod def box_array(cls, length): """ Returns: @@ -129,4 +134,6 @@ def arr_array(cls, length): """ Returns: vertices, indices, normals """ - return create_arrow_array(length) \ No newline at end of file + return create_arrow_array(length) + +