From c2465b7313147dd1dc8d7645cd81c3ab12e4e9c2 Mon Sep 17 00:00:00 2001 From: aravind Date: Sun, 3 Nov 2013 16:18:11 +0100 Subject: [PATCH 001/273] Fix `ImportError` Fix `ImportError` on modules when `main.py` is run from `pysymoro/pysymoro` folder. This is done by catching exception when importing modules. --- pysymoro/core/dynamics.py | 21 ++++++++++++++++----- pysymoro/core/geometry.py | 7 ++++++- pysymoro/core/invgeom.py | 10 ++++++++-- pysymoro/core/kinematics.py | 15 ++++++++++++--- pysymoro/core/parfile.py | 7 ++++++- pysymoro/core/symoro.py | 5 ++++- pysymoro/gui/ui_definition.py | 9 ++++++++- pysymoro/gui/ui_geometry.py | 3 ++- pysymoro/gui/ui_kinematics.py | 3 ++- pysymoro/main.py | 25 +++++++++++++++++++------ pysymoro/visualize/graphics.py | 22 +++++++++++++++------- pysymoro/visualize/objects.py | 4 +++- 12 files changed, 101 insertions(+), 30 deletions(-) diff --git a/pysymoro/core/dynamics.py b/pysymoro/core/dynamics.py index 1d4432b..8ce499d 100644 --- a/pysymoro/core/dynamics.py +++ b/pysymoro/core/dynamics.py @@ -7,14 +7,25 @@ ECN - ARIA1 2013 """ + + import sympy from sympy import Matrix from copy import copy, deepcopy -from pysymoro.core.symoro import Symoro, Init, hat, ZERO -from pysymoro.core.geometry import compute_screw_transform -from pysymoro.core.geometry import compute_rot_trans, Transform -from pysymoro.core.kinematics import compute_speeds_accelerations -from pysymoro.core.kinematics import compute_omega + +try: + from pysymoro.core.symoro import Symoro, Init, hat, ZERO + from pysymoro.core.geometry import compute_screw_transform + from pysymoro.core.geometry import compute_rot_trans, Transform + from pysymoro.core.kinematics import compute_speeds_accelerations + from pysymoro.core.kinematics import compute_omega +except ImportError: + from core.symoro import Symoro, Init, hat, ZERO + from core.geometry import compute_screw_transform + from core.geometry import compute_rot_trans, Transform + from core.kinematics import compute_speeds_accelerations + from core.kinematics import compute_omega + chars = 'ABCDEFGHJKLMNPQRSTUVWXYZ' inert_names = ('XXR', 'XYR', 'XZR', 'YYR', 'YZR', diff --git a/pysymoro/core/geometry.py b/pysymoro/core/geometry.py index 8b24705..c845841 100644 --- a/pysymoro/core/geometry.py +++ b/pysymoro/core/geometry.py @@ -9,7 +9,12 @@ """ from sympy import Matrix, zeros, eye, sin, cos -from pysymoro.core.symoro import Symoro, Init, hat + +try: + from pysymoro.core.symoro import Symoro, Init, hat +except ImportError: + from core.symoro import Symoro, Init, hat + Z_AXIS = Matrix([0, 0, 1]) diff --git a/pysymoro/core/invgeom.py b/pysymoro/core/invgeom.py index afa82aa..42a698d 100644 --- a/pysymoro/core/invgeom.py +++ b/pysymoro/core/invgeom.py @@ -11,8 +11,14 @@ from heapq import heapify, heappop from sympy import var, sin, cos, eye, atan2, sqrt, pi from sympy import Matrix, Symbol, Expr -from pysymoro.core.symoro import Symoro, ZERO, ONE, get_max_coef -from pysymoro.core.geometry import dgm + +try: + from pysymoro.core.symoro import Symoro, ZERO, ONE, get_max_coef + from pysymoro.core.geometry import dgm +except ImportError: + from core.symoro import Symoro, ZERO, ONE, get_max_coef + from core.geometry import dgm + EMPTY = var("EMPTY") diff --git a/pysymoro/core/kinematics.py b/pysymoro/core/kinematics.py index 24f9488..8b7622c 100644 --- a/pysymoro/core/kinematics.py +++ b/pysymoro/core/kinematics.py @@ -7,10 +7,19 @@ ECN - ARIA1 2013 """ + + from sympy import Matrix, zeros -from pysymoro.core.symoro import Symoro, Init, hat -from pysymoro.core.symoro import FAIL, ZERO -from pysymoro.core.geometry import dgm, Transform, compute_rot_trans + +try: + from pysymoro.core.symoro import Symoro, Init, hat + from pysymoro.core.symoro import FAIL, ZERO + from pysymoro.core.geometry import dgm, Transform, compute_rot_trans +except ImportError: + from core.symoro import Symoro, Init, hat + from core.symoro import FAIL, ZERO + from core.geometry import dgm, Transform, compute_rot_trans + TERMINAL = 0 ROOT = 1 diff --git a/pysymoro/core/parfile.py b/pysymoro/core/parfile.py index fd7ab8e..df3365c 100644 --- a/pysymoro/core/parfile.py +++ b/pysymoro/core/parfile.py @@ -1,5 +1,10 @@ import re -from pysymoro.core import symoro + +try: + from pysymoro.core import symoro +except ImportError: + from core import symoro + _keywords = ['ant', 'sigma', 'b', 'd', 'r', 'gamma', 'alpha', 'mu', 'theta', diff --git a/pysymoro/core/symoro.py b/pysymoro/core/symoro.py index 238b24b..d8f0d2c 100644 --- a/pysymoro/core/symoro.py +++ b/pysymoro/core/symoro.py @@ -6,10 +6,13 @@ ECN - ARIA1 2013 """ -import os + + import re +import os 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 diff --git a/pysymoro/gui/ui_definition.py b/pysymoro/gui/ui_definition.py index 2f109d8..aa551a0 100644 --- a/pysymoro/gui/ui_definition.py +++ b/pysymoro/gui/ui_definition.py @@ -1,8 +1,15 @@ __author__ = 'Izzat' + import wx import wx.lib.scrolledpanel as scrolled -from pysymoro.core.symoro import SIMPLE, TREE, CLOSED_LOOP + from sympy import Expr, Symbol + +try: + from pysymoro.core.symoro import SIMPLE, TREE, CLOSED_LOOP +except ImportError: + from core.symoro import SIMPLE, TREE, CLOSED_LOOP + #TODO: PROG_NAME diff --git a/pysymoro/gui/ui_geometry.py b/pysymoro/gui/ui_geometry.py index 0a0523c..e7d90d0 100644 --- a/pysymoro/gui/ui_geometry.py +++ b/pysymoro/gui/ui_geometry.py @@ -1,4 +1,5 @@ __author__ = 'Izzat' + import wx class DialogTrans(wx.Dialog): @@ -214,4 +215,4 @@ def GetValues(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/pysymoro/gui/ui_kinematics.py index 98af4c3..6e39ee2 100644 --- a/pysymoro/gui/ui_kinematics.py +++ b/pysymoro/gui/ui_kinematics.py @@ -1,4 +1,5 @@ __author__ = 'Izzat' + import wx class DialogJacobian(wx.Dialog): @@ -148,4 +149,4 @@ def OnCancel(self, e): def GetValues(self): row_selected = [i for i in range(len(self.box_row.Items)) if not self.box_row.IsChecked(i)] 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 + return int(self.cmb_frame.Value), int(self.cmb_proj.Value), int(self.cmb_inter.Value), row_selected, col_selected diff --git a/pysymoro/main.py b/pysymoro/main.py index 2d899e5..5845929 100644 --- a/pysymoro/main.py +++ b/pysymoro/main.py @@ -1,12 +1,25 @@ + __author__ = 'Izzat' -import wx -from pysymoro.core.symoro import Robot, FAIL -from pysymoro.core import geometry, kinematics, dynamics, invgeom -from pysymoro.visualize import graphics -from pysymoro.gui import ui_definition, ui_geometry, ui_kinematics -from pysymoro.core.parfile import readpar, writepar + + import os +import wx + +try: + from pysymoro.core.symoro import Robot, FAIL + from pysymoro.core import geometry, kinematics, dynamics, invgeom + from pysymoro.visualize import graphics + from pysymoro.gui import ui_definition, ui_geometry, ui_kinematics + from pysymoro.core.parfile import readpar, writepar +except ImportError: + 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' diff --git a/pysymoro/visualize/graphics.py b/pysymoro/visualize/graphics.py index 56218e9..498f3ce 100644 --- a/pysymoro/visualize/graphics.py +++ b/pysymoro/visualize/graphics.py @@ -1,17 +1,25 @@ __author__ = 'Izzat' -import wx -import wx.lib.agw.floatspin as FS + +from math import atan2 + import OpenGL.GL as gl import OpenGL.GLU as glu -from objects import Frame, RevoluteJoint, FixedJoint, PrismaticJoint +import wx +import wx.lib.agw.floatspin as FS from wx.glcanvas import GLCanvas from numpy import sin, cos, radians, pi, inf, nan -from math import atan2 -from pysymoro.core.symoro import Symoro, CLOSED_LOOP -from pysymoro.core.invgeom import loop_solve -from pysymoro.core.geometry import dgm from sympy import Expr +from objects import Frame, RevoluteJoint, FixedJoint, PrismaticJoint +try: + from pysymoro.core.symoro import Symoro, CLOSED_LOOP + from pysymoro.core.invgeom import loop_solve + from pysymoro.core.geometry import dgm +except ImportError: + from core.symoro import Symoro, CLOSED_LOOP + from core.invgeom import loop_solve + from core.geometry import dgm + #TODO: Fullscreen camera rotation bug #TODO: X-, Z-axis #TODO: Random button diff --git a/pysymoro/visualize/objects.py b/pysymoro/visualize/objects.py index 395d246..dfe6d23 100644 --- a/pysymoro/visualize/objects.py +++ b/pysymoro/visualize/objects.py @@ -1,6 +1,8 @@ __author__ = 'Izzat' + import OpenGL.GL as gl from numpy import degrees, identity, array + from primitives import Primitives @@ -187,4 +189,4 @@ def draw_joint(self): 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) \ No newline at end of file + gl.GL_UNSIGNED_INT, self.sph_indices) From 5b284fe3d1021c1e0caa5cc789b6fbfec52c63a0 Mon Sep 17 00:00:00 2001 From: aravind Date: Sun, 3 Nov 2013 16:26:05 +0100 Subject: [PATCH 002/273] Update `tests/test.py` --- pysymoro/tests/test.py | 25 ++++++++++++++++++------- 1 file changed, 18 insertions(+), 7 deletions(-) diff --git a/pysymoro/tests/test.py b/pysymoro/tests/test.py index 0ad237d..53832d2 100644 --- a/pysymoro/tests/test.py +++ b/pysymoro/tests/test.py @@ -2,16 +2,27 @@ 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 -from pysymoro.core import symoro -from pysymoro.core import geometry -from pysymoro.core import kinematics -from pysymoro.core import invgeom -from pysymoro.core.geometry import Transform as trns -from pysymoro.core import dynamics -from pysymoro.core import parfile + +try: + from pysymoro.core import symoro + from pysymoro.core import geometry + from pysymoro.core import kinematics + from pysymoro.core import invgeom + from pysymoro.core.geometry import Transform as trns + from pysymoro.core import dynamics + from pysymoro.core import parfile +except ImportError: + from core import symoro + from core import geometry + from core import kinematics + from core import invgeom + from core.geometry import Transform as trns + from core import dynamics + from core import parfile class testMisc(unittest.TestCase): From 4bb8964b6da5ba58a9fec5f38a55f3a43e9f2127 Mon Sep 17 00:00:00 2001 From: aravind Date: Sun, 3 Nov 2013 16:36:00 +0100 Subject: [PATCH 003/273] Add hashbang and encoding detail to all files --- pysymoro/core/dynamics.py | 3 +++ pysymoro/core/geometry.py | 3 +++ pysymoro/core/invgeom.py | 3 +++ pysymoro/core/kinematics.py | 3 +++ pysymoro/core/parfile.py | 3 +++ pysymoro/core/symoro.py | 3 +++ pysymoro/gui/ui_definition.py | 3 +++ pysymoro/gui/ui_geometry.py | 3 +++ pysymoro/gui/ui_kinematics.py | 3 +++ pysymoro/main.py | 2 ++ pysymoro/tests/test.py | 4 ++++ pysymoro/visualize/graphics.py | 3 +++ pysymoro/visualize/objects.py | 3 +++ pysymoro/visualize/primitives.py | 5 ++++- 14 files changed, 43 insertions(+), 1 deletion(-) diff --git a/pysymoro/core/dynamics.py b/pysymoro/core/dynamics.py index 8ce499d..5bc96b9 100644 --- a/pysymoro/core/dynamics.py +++ b/pysymoro/core/dynamics.py @@ -1,3 +1,6 @@ +#!/usr/bin/env python +# -*- coding: utf-8 -*- + """ This module of SYMORO package provides symbolic modeling of robots' dynamics. diff --git a/pysymoro/core/geometry.py b/pysymoro/core/geometry.py index c845841..bc7613d 100644 --- a/pysymoro/core/geometry.py +++ b/pysymoro/core/geometry.py @@ -1,3 +1,6 @@ +#!/usr/bin/env python +# -*- coding: utf-8 -*- + """ This module of SYMORO package provides geometric models' computation. diff --git a/pysymoro/core/invgeom.py b/pysymoro/core/invgeom.py index 42a698d..0ea0c12 100644 --- a/pysymoro/core/invgeom.py +++ b/pysymoro/core/invgeom.py @@ -1,3 +1,6 @@ +#!/usr/bin/env python +# -*- coding: utf-8 -*- + """ This module of SYMORO package provides symbolic solutions for inverse geompetric problem. diff --git a/pysymoro/core/kinematics.py b/pysymoro/core/kinematics.py index 8b7622c..6dbfabe 100644 --- a/pysymoro/core/kinematics.py +++ b/pysymoro/core/kinematics.py @@ -1,3 +1,6 @@ +#!/usr/bin/env python +# -*- coding: utf-8 -*- + """ This module of SYMORO package provides kinematic models' computation. diff --git a/pysymoro/core/parfile.py b/pysymoro/core/parfile.py index df3365c..97f71e9 100644 --- a/pysymoro/core/parfile.py +++ b/pysymoro/core/parfile.py @@ -1,3 +1,6 @@ +#!/usr/bin/env python +# -*- coding: utf-8 -*- + import re try: diff --git a/pysymoro/core/symoro.py b/pysymoro/core/symoro.py index d8f0d2c..4e08f53 100644 --- a/pysymoro/core/symoro.py +++ b/pysymoro/core/symoro.py @@ -1,3 +1,6 @@ +#!/usr/bin/env python +# -*- coding: utf-8 -*- + """ This module of SYMORO package provides description of the robot parametrizaion container and symbol replacer class. diff --git a/pysymoro/gui/ui_definition.py b/pysymoro/gui/ui_definition.py index aa551a0..d4eaffe 100644 --- a/pysymoro/gui/ui_definition.py +++ b/pysymoro/gui/ui_definition.py @@ -1,3 +1,6 @@ +#!/usr/bin/env python +# -*- coding: utf-8 -*- + __author__ = 'Izzat' import wx diff --git a/pysymoro/gui/ui_geometry.py b/pysymoro/gui/ui_geometry.py index e7d90d0..b4abaa8 100644 --- a/pysymoro/gui/ui_geometry.py +++ b/pysymoro/gui/ui_geometry.py @@ -1,3 +1,6 @@ +#!/usr/bin/env python +# -*- coding: utf-8 -*- + __author__ = 'Izzat' import wx diff --git a/pysymoro/gui/ui_kinematics.py b/pysymoro/gui/ui_kinematics.py index 6e39ee2..28cbf8b 100644 --- a/pysymoro/gui/ui_kinematics.py +++ b/pysymoro/gui/ui_kinematics.py @@ -1,3 +1,6 @@ +#!/usr/bin/env python +# -*- coding: utf-8 -*- + __author__ = 'Izzat' import wx diff --git a/pysymoro/main.py b/pysymoro/main.py index 5845929..3aefb3a 100644 --- a/pysymoro/main.py +++ b/pysymoro/main.py @@ -1,3 +1,5 @@ +#!/usr/bin/env python +# -*- coding: utf-8 -*- __author__ = 'Izzat' diff --git a/pysymoro/tests/test.py b/pysymoro/tests/test.py index 53832d2..74a9aa4 100644 --- a/pysymoro/tests/test.py +++ b/pysymoro/tests/test.py @@ -1,6 +1,10 @@ +#!/usr/bin/env python +# -*- coding: utf-8 -*- + """ Unit tests for SYMORO modules """ + import unittest from sympy import sympify, var, Matrix diff --git a/pysymoro/visualize/graphics.py b/pysymoro/visualize/graphics.py index 498f3ce..349f6a1 100644 --- a/pysymoro/visualize/graphics.py +++ b/pysymoro/visualize/graphics.py @@ -1,3 +1,6 @@ +#!/usr/bin/env python +# -*- coding: utf-8 -*- + __author__ = 'Izzat' from math import atan2 diff --git a/pysymoro/visualize/objects.py b/pysymoro/visualize/objects.py index dfe6d23..d7b0f3b 100644 --- a/pysymoro/visualize/objects.py +++ b/pysymoro/visualize/objects.py @@ -1,3 +1,6 @@ +#!/usr/bin/env python +# -*- coding: utf-8 -*- + __author__ = 'Izzat' import OpenGL.GL as gl diff --git a/pysymoro/visualize/primitives.py b/pysymoro/visualize/primitives.py index 8c23c72..35624c4 100644 --- a/pysymoro/visualize/primitives.py +++ b/pysymoro/visualize/primitives.py @@ -1,3 +1,6 @@ +#!/usr/bin/env python +# -*- coding: utf-8 -*- + __author__ = 'Izzat' from itertools import product @@ -129,4 +132,4 @@ 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) From fc79464fb78ec711c3a2c40b328db568771459bf Mon Sep 17 00:00:00 2001 From: aravind Date: Sun, 3 Nov 2013 20:21:11 +0100 Subject: [PATCH 004/273] Fix merge mistakes --- pysymoro/core/dynamics.py | 4 ++-- pysymoro/core/kinematics.py | 6 ++++-- 2 files changed, 6 insertions(+), 4 deletions(-) diff --git a/pysymoro/core/dynamics.py b/pysymoro/core/dynamics.py index ff7339b..08b8670 100644 --- a/pysymoro/core/dynamics.py +++ b/pysymoro/core/dynamics.py @@ -20,13 +20,13 @@ from pysymoro.core.symoro import Symoro, Init, hat, ZERO from pysymoro.core.geometry import compute_screw_transform from pysymoro.core.geometry import compute_rot_trans, Transform - from pysymoro.core.kinematics import compute_speeds_accelerations + from pysymoro.core.kinematics import compute_vel_acc from pysymoro.core.kinematics import compute_omega except ImportError: from core.symoro import Symoro, Init, hat, ZERO from core.geometry import compute_screw_transform from core.geometry import compute_rot_trans, Transform - from core.kinematics import compute_speeds_accelerations + from core.kinematics import compute_vel_acc from core.kinematics import compute_omega diff --git a/pysymoro/core/kinematics.py b/pysymoro/core/kinematics.py index eac5968..a7fb7ed 100644 --- a/pysymoro/core/kinematics.py +++ b/pysymoro/core/kinematics.py @@ -17,11 +17,13 @@ try: from pysymoro.core.symoro import Symoro, Init, hat from pysymoro.core.symoro import FAIL, ZERO - from pysymoro.core.geometry import dgm, Transform, compute_rot_trans + from pysymoro.core.geometry import dgm, Transform + from pysymoro.core.geometry import compute_rot_trans, Z_AXIS except ImportError: from core.symoro import Symoro, Init, hat from core.symoro import FAIL, ZERO - from core.geometry import dgm, Transform, compute_rot_trans + from core.geometry import dgm, Transform + from core.geometry import compute_rot_trans, Z_AXIS TERMINAL = 0 From 6424e8ec897c6120e49e39b3ca27a1f485896db7 Mon Sep 17 00:00:00 2001 From: aravind Date: Sat, 28 Dec 2013 10:53:09 +0100 Subject: [PATCH 005/273] Update README file with system requirements --- README.md | 10 ++++++++-- 1 file changed, 8 insertions(+), 2 deletions(-) diff --git a/README.md b/README.md index bb3f23a..e8b7ba5 100644 --- a/README.md +++ b/README.md @@ -1,5 +1,4 @@ -SYMORO -====== +##SYMORO SYmbolic MOdeling of RObots software. The language is python and the key library is sympy. @@ -7,3 +6,10 @@ This is an open-source version of SYMORO software written in Python. Hence you d For more details on SYMORO, please see http://www.irccyn.ec-nantes.fr/spip.php?article601&lang=en +###Requirements ++ python (>= 2.7, 3.* is not supported) ++ sympy (>= 0.7.3) ++ numpy (>= 1.6.1) ++ wxPython (>= 2.8.12.1) ++ OpenGL (>= 3.0.1b2) + From 03550dc3ace9e37ea272865d258997a0612c248e Mon Sep 17 00:00:00 2001 From: aravind Date: Sat, 28 Dec 2013 10:56:11 +0100 Subject: [PATCH 006/273] Update README file with system requirements --- README.md | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/README.md b/README.md index e8b7ba5..3d3a35a 100644 --- a/README.md +++ b/README.md @@ -7,7 +7,7 @@ This is an open-source version of SYMORO software written in Python. Hence you d For more details on SYMORO, please see http://www.irccyn.ec-nantes.fr/spip.php?article601&lang=en ###Requirements -+ python (>= 2.7, 3.* is not supported) ++ python (>= 2.7, 3.* is not supported) + sympy (>= 0.7.3) + numpy (>= 1.6.1) + wxPython (>= 2.8.12.1) From 3b8dec7e0a99fbea62b9e9114e489cca6fe1fcc8 Mon Sep 17 00:00:00 2001 From: aravind Date: Sun, 29 Dec 2013 06:16:35 +0100 Subject: [PATCH 007/273] Update `.gitignore` Update `.gitignore` from `https://github.com/github/gitignore/blob/master/Python.gitignore`. Modify `.gitignore` to include `bin/` and exclude some tmp files.#bin/ --- .gitignore | 234 ++++++++--------------------------------------------- 1 file changed, 34 insertions(+), 200 deletions(-) diff --git a/.gitignore b/.gitignore index 3d691a8..8c9bb68 100644 --- a/.gitignore +++ b/.gitignore @@ -1,221 +1,55 @@ -################# -## Eclipse -################# - -*.pydevproject -.idea -.project -.metadata -bin/ -tmp/ +# tmp files +*.swp *.tmp *.bak -*.swp -*~.nib -local.properties -.classpath -.settings/ -.loadpath - -# External tool builders -.externalToolBuilders/ - -# Locally stored "Eclipse launch configurations" -*.launch - -# CDT-specific -.cproject -# PDT-specific -.buildpath +# Byte-compiled / optimized / DLL files +__pycache__/ +*.py[cod] +# C extensions +*.so -################# -## 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/ +# Distribution / packaging 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.* -~$* -*~ -*.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 - - -############# -## Python -############# - -*.py[co] -*.txt -# Packages -*.egg -*.egg-info +develop-eggs/ dist/ -build/ 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/ + From 8403f4e1cba52824e756b1df2979e98294024f5d Mon Sep 17 00:00:00 2001 From: aravind Date: Sun, 29 Dec 2013 06:17:54 +0100 Subject: [PATCH 008/273] Add `bin/symoro-bin` python script Add `bin/symoro-bin` python script. This script is to replace `pysymoro/main.py` over time. --- bin/symoro-bin | 682 +++++++++++++++++++++++++++++++++++++++++++++++++ 1 file changed, 682 insertions(+) create mode 100644 bin/symoro-bin diff --git a/bin/symoro-bin b/bin/symoro-bin new file mode 100644 index 0000000..acf0ce3 --- /dev/null +++ b/bin/symoro-bin @@ -0,0 +1,682 @@ +#!/usr/bin/env python +# -*- coding: utf-8 -*- + + +__author__ = 'Izzat' + + +import os + +import wx + +from pysymoro.core.symoro import Robot, FAIL +from pysymoro.core import geometry, kinematics, dynamics, invgeom +from pysymoro.visualize import graphics +from pysymoro.gui import ui_definition, ui_geometry, ui_kinematics +from pysymoro.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() + + +def main(): + app = wx.App(redirect=False) + frame = MainFrame() + frame.Show() + app.MainLoop() + + +if __name__ == "__main__": + main() + + From 8afdf2b56037079aea8aa269782217b596a93070 Mon Sep 17 00:00:00 2001 From: aravind Date: Sun, 29 Dec 2013 06:20:36 +0100 Subject: [PATCH 009/273] Add `setup.py` file Add `setup.py` file. The current version does not include all the information but provides sufficient to get things rolling. Update `pysymoro/main.py` to include `__name__` check and call `main()` accordingly. --- pysymoro/main.py | 15 +++++++++++---- setup.py | 23 +++++++++++++++++++++++ 2 files changed, 34 insertions(+), 4 deletions(-) create mode 100644 setup.py diff --git a/pysymoro/main.py b/pysymoro/main.py index 831a192..f74dec1 100644 --- a/pysymoro/main.py +++ b/pysymoro/main.py @@ -674,8 +674,15 @@ def OnClose(self, _): return self.Destroy() -app = wx.App(redirect=False) -main = MainFrame() -main.Show() -app.MainLoop() + +def main(): + app = wx.App(redirect=False) + frame = MainFrame() + frame.Show() + app.MainLoop() + + +if __name__ == "__main__": + main() + diff --git a/setup.py b/setup.py new file mode 100644 index 0000000..f5c5088 --- /dev/null +++ b/setup.py @@ -0,0 +1,23 @@ +#!/usr/bin/env python +# -*- coding: utf-8 -*- + + +from setuptools import setup, find_packages + + +def readme(): + with open('README.md') as f: + return f.read() + + +setup( + name='symoro', + version='0.1alpha', + description='SYmoblic MOdelling of RObots software', + url='http://github.com/vijaravind/symoro', + scripts=['bin/symoro-bin'], + packages=find_packages(), + zip_safe=False +) + + From a8120a27b356463b8eb9d3bc8a227c4f270067dc Mon Sep 17 00:00:00 2001 From: aravind Date: Sun, 29 Dec 2013 07:13:23 +0100 Subject: [PATCH 010/273] Move files for better package management Move file for better packagement such that the source for the computation of the core components can be used separately. Now the UI and Visualisation source can be packaged separately. Minor change in `setup.py` to use `os.path.join()` where needed. Update all files to reflect new folder structure. --- bin/symoro-bin | 10 +++++----- pysymoro/{core => }/dynamics.py | 17 +++++------------ pysymoro/{core => }/geometry.py | 5 +---- pysymoro/{core => }/invgeom.py | 8 ++------ pysymoro/{core => }/kinematics.py | 14 ++++---------- pysymoro/main.py | 17 +++++------------ pysymoro/{core => }/parfile.py | 5 +---- pysymoro/{core => }/symoro.py | 0 pysymoro/visualize/__init__.py | 0 setup.py | 6 +++++- {pysymoro/core => symoroui}/__init__.py | 0 {pysymoro/gui => symoroui}/ui_definition.py | 5 +---- {pysymoro/gui => symoroui}/ui_geometry.py | 0 {pysymoro/gui => symoroui}/ui_kinematics.py | 0 {pysymoro/gui => symoroviz}/__init__.py | 0 {pysymoro/visualize => symoroviz}/graphics.py | 12 ++++-------- {pysymoro/visualize => symoroviz}/objects.py | 2 +- {pysymoro/visualize => symoroviz}/primitives.py | 0 {pysymoro/tests => tests}/test.py | 0 19 files changed, 34 insertions(+), 67 deletions(-) rename pysymoro/{core => }/dynamics.py (97%) rename pysymoro/{core => }/geometry.py (99%) rename pysymoro/{core => }/invgeom.py (98%) rename pysymoro/{core => }/kinematics.py (96%) rename pysymoro/{core => }/parfile.py (98%) rename pysymoro/{core => }/symoro.py (100%) delete mode 100644 pysymoro/visualize/__init__.py rename {pysymoro/core => symoroui}/__init__.py (100%) rename {pysymoro/gui => symoroui}/ui_definition.py (98%) rename {pysymoro/gui => symoroui}/ui_geometry.py (100%) rename {pysymoro/gui => symoroui}/ui_kinematics.py (100%) rename {pysymoro/gui => symoroviz}/__init__.py (100%) rename {pysymoro/visualize => symoroviz}/graphics.py (98%) rename {pysymoro/visualize => symoroviz}/objects.py (99%) rename {pysymoro/visualize => symoroviz}/primitives.py (100%) rename {pysymoro/tests => tests}/test.py (100%) diff --git a/bin/symoro-bin b/bin/symoro-bin index acf0ce3..66d874c 100644 --- a/bin/symoro-bin +++ b/bin/symoro-bin @@ -9,11 +9,11 @@ import os import wx -from pysymoro.core.symoro import Robot, FAIL -from pysymoro.core import geometry, kinematics, dynamics, invgeom -from pysymoro.visualize import graphics -from pysymoro.gui import ui_definition, ui_geometry, ui_kinematics -from pysymoro.core.parfile import readpar, writepar +from pysymoro.symoro import Robot, FAIL +from pysymoro import geometry, kinematics, dynamics, invgeom +from pysymoro.parfile import readpar, writepar +from symoroviz import graphics +from symoroui import ui_definition, ui_geometry, ui_kinematics PROG_NAME = 'SYMORO-Python' diff --git a/pysymoro/core/dynamics.py b/pysymoro/dynamics.py similarity index 97% rename from pysymoro/core/dynamics.py rename to pysymoro/dynamics.py index 08b8670..f49b46e 100644 --- a/pysymoro/core/dynamics.py +++ b/pysymoro/dynamics.py @@ -16,18 +16,11 @@ from sympy import Matrix from copy import copy, deepcopy -try: - from pysymoro.core.symoro import Symoro, Init, hat, ZERO - from pysymoro.core.geometry import compute_screw_transform - from pysymoro.core.geometry import compute_rot_trans, Transform - from pysymoro.core.kinematics import compute_vel_acc - from pysymoro.core.kinematics import compute_omega -except ImportError: - from core.symoro import Symoro, Init, hat, ZERO - from core.geometry import compute_screw_transform - from core.geometry import compute_rot_trans, Transform - from core.kinematics import compute_vel_acc - from core.kinematics import compute_omega +from pysymoro.symoro import Symoro, Init, hat, ZERO +from pysymoro.geometry import compute_screw_transform +from pysymoro.geometry import compute_rot_trans, Transform +from pysymoro.kinematics import compute_vel_acc +from pysymoro.kinematics import compute_omega chars = 'ABCDEFGHJKLMNPQRSTUVWXYZ' diff --git a/pysymoro/core/geometry.py b/pysymoro/geometry.py similarity index 99% rename from pysymoro/core/geometry.py rename to pysymoro/geometry.py index 652e037..4acc530 100644 --- a/pysymoro/core/geometry.py +++ b/pysymoro/geometry.py @@ -13,10 +13,7 @@ from sympy import Matrix, zeros, eye, sin, cos -try: - from pysymoro.core.symoro import Symoro, Init, hat -except ImportError: - from core.symoro import Symoro, Init, hat +from pysymoro.symoro import Symoro, Init, hat Z_AXIS = Matrix([0, 0, 1]) diff --git a/pysymoro/core/invgeom.py b/pysymoro/invgeom.py similarity index 98% rename from pysymoro/core/invgeom.py rename to pysymoro/invgeom.py index 0ea0c12..47ee9b1 100644 --- a/pysymoro/core/invgeom.py +++ b/pysymoro/invgeom.py @@ -15,12 +15,8 @@ from sympy import var, sin, cos, eye, atan2, sqrt, pi from sympy import Matrix, Symbol, Expr -try: - from pysymoro.core.symoro import Symoro, ZERO, ONE, get_max_coef - from pysymoro.core.geometry import dgm -except ImportError: - from core.symoro import Symoro, ZERO, ONE, get_max_coef - from core.geometry import dgm +from pysymoro.symoro import Symoro, ZERO, ONE, get_max_coef +from pysymoro.geometry import dgm EMPTY = var("EMPTY") diff --git a/pysymoro/core/kinematics.py b/pysymoro/kinematics.py similarity index 96% rename from pysymoro/core/kinematics.py rename to pysymoro/kinematics.py index a7fb7ed..d70bcea 100644 --- a/pysymoro/core/kinematics.py +++ b/pysymoro/kinematics.py @@ -14,16 +14,10 @@ from sympy import Matrix, zeros -try: - from pysymoro.core.symoro import Symoro, Init, hat - from pysymoro.core.symoro import FAIL, ZERO - from pysymoro.core.geometry import dgm, Transform - from pysymoro.core.geometry import compute_rot_trans, Z_AXIS -except ImportError: - from core.symoro import Symoro, Init, hat - from core.symoro import FAIL, ZERO - from core.geometry import dgm, Transform - from core.geometry import compute_rot_trans, Z_AXIS +from pysymoro.symoro import Symoro, Init, hat +from pysymoro.symoro import FAIL, ZERO +from pysymoro.geometry import dgm, Transform +from pysymoro.geometry import compute_rot_trans, Z_AXIS TERMINAL = 0 diff --git a/pysymoro/main.py b/pysymoro/main.py index f74dec1..9088c4e 100644 --- a/pysymoro/main.py +++ b/pysymoro/main.py @@ -8,18 +8,11 @@ import wx -try: - from pysymoro.core.symoro import Robot, FAIL - from pysymoro.core import geometry, kinematics, dynamics, invgeom - from pysymoro.visualize import graphics - from pysymoro.gui import ui_definition, ui_geometry, ui_kinematics - from pysymoro.core.parfile import readpar, writepar -except ImportError: - 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 +from pysymoro.symoro import Robot, FAIL +from pysymoro import geometry, kinematics, dynamics, invgeom +from pysymoro.parfile import readpar, writepar +from symoroviz import graphics +from symoroui import ui_definition, ui_geometry, ui_kinematics PROG_NAME = 'SYMORO-Python' diff --git a/pysymoro/core/parfile.py b/pysymoro/parfile.py similarity index 98% rename from pysymoro/core/parfile.py rename to pysymoro/parfile.py index 97f71e9..fd669a5 100644 --- a/pysymoro/core/parfile.py +++ b/pysymoro/parfile.py @@ -3,10 +3,7 @@ import re -try: - from pysymoro.core import symoro -except ImportError: - from core import symoro +from pysymoro import symoro _keywords = ['ant', 'sigma', 'b', 'd', 'r', diff --git a/pysymoro/core/symoro.py b/pysymoro/symoro.py similarity index 100% rename from pysymoro/core/symoro.py rename to pysymoro/symoro.py diff --git a/pysymoro/visualize/__init__.py b/pysymoro/visualize/__init__.py deleted file mode 100644 index e69de29..0000000 diff --git a/setup.py b/setup.py index f5c5088..8c409f0 100644 --- a/setup.py +++ b/setup.py @@ -2,9 +2,13 @@ # -*- 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() @@ -15,7 +19,7 @@ def readme(): version='0.1alpha', description='SYmoblic MOdelling of RObots software', url='http://github.com/vijaravind/symoro', - scripts=['bin/symoro-bin'], + scripts=[os.path.join(BIN_FOLDER, 'symoro-bin')], 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/pysymoro/gui/ui_definition.py b/symoroui/ui_definition.py similarity index 98% rename from pysymoro/gui/ui_definition.py rename to symoroui/ui_definition.py index 7837eed..bff37d6 100644 --- a/pysymoro/gui/ui_definition.py +++ b/symoroui/ui_definition.py @@ -8,10 +8,7 @@ from sympy import Expr, Symbol -try: - from pysymoro.core.symoro import SIMPLE, TREE, CLOSED_LOOP -except ImportError: - from core.symoro import SIMPLE, TREE, CLOSED_LOOP +from pysymoro.symoro import SIMPLE, TREE, CLOSED_LOOP #TODO: PROG_NAME diff --git a/pysymoro/gui/ui_geometry.py b/symoroui/ui_geometry.py similarity index 100% rename from pysymoro/gui/ui_geometry.py rename to symoroui/ui_geometry.py diff --git a/pysymoro/gui/ui_kinematics.py b/symoroui/ui_kinematics.py similarity index 100% rename from pysymoro/gui/ui_kinematics.py rename to symoroui/ui_kinematics.py diff --git a/pysymoro/gui/__init__.py b/symoroviz/__init__.py similarity index 100% rename from pysymoro/gui/__init__.py rename to symoroviz/__init__.py diff --git a/pysymoro/visualize/graphics.py b/symoroviz/graphics.py similarity index 98% rename from pysymoro/visualize/graphics.py rename to symoroviz/graphics.py index fa8459a..36e8d12 100644 --- a/pysymoro/visualize/graphics.py +++ b/symoroviz/graphics.py @@ -16,15 +16,11 @@ from numpy import sin, cos, radians, pi, inf, nan from sympy import Expr +from pysymoro.symoro import Symoro, CLOSED_LOOP +from pysymoro.invgeom import loop_solve +from pysymoro.geometry import dgm + from objects import Frame, RevoluteJoint, FixedJoint, PrismaticJoint -try: - from pysymoro.core.symoro import Symoro, CLOSED_LOOP - from pysymoro.core.invgeom import loop_solve - from pysymoro.core.geometry import dgm -except ImportError: - from core.symoro import Symoro, CLOSED_LOOP - from core.invgeom import loop_solve - from core.geometry import dgm #TODO: Fullscreen camera rotation bug #TODO: X-, Z-axis diff --git a/pysymoro/visualize/objects.py b/symoroviz/objects.py similarity index 99% rename from pysymoro/visualize/objects.py rename to symoroviz/objects.py index c459cad..fb03453 100644 --- a/pysymoro/visualize/objects.py +++ b/symoroviz/objects.py @@ -233,4 +233,4 @@ def draw_joint(self): 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 + super(FixedJoint, self).set_length(new_length) diff --git a/pysymoro/visualize/primitives.py b/symoroviz/primitives.py similarity index 100% rename from pysymoro/visualize/primitives.py rename to symoroviz/primitives.py diff --git a/pysymoro/tests/test.py b/tests/test.py similarity index 100% rename from pysymoro/tests/test.py rename to tests/test.py From bbe6a99ab8d488898a86ef9ec0bbd3d5a6edbc1c Mon Sep 17 00:00:00 2001 From: aravind Date: Sun, 29 Dec 2013 07:20:21 +0100 Subject: [PATCH 011/273] Update README file Update README file to include installation and usage details. --- README.md | 18 +++++++++++++++--- 1 file changed, 15 insertions(+), 3 deletions(-) diff --git a/README.md b/README.md index 3d3a35a..e3c38c1 100644 --- a/README.md +++ b/README.md @@ -1,4 +1,5 @@ -##SYMORO +SYMORO +====== SYmbolic MOdeling of RObots software. The language is python and the key library is sympy. @@ -6,10 +7,21 @@ This is an open-source version of SYMORO software written in Python. Hence you d For more details on SYMORO, please see http://www.irccyn.ec-nantes.fr/spip.php?article601&lang=en -###Requirements -+ python (>= 2.7, 3.* is not supported) + +Requirements +------------ ++ python (>= 2.7,    3.* is not supported) + sympy (>= 0.7.3) + numpy (>= 1.6.1) + wxPython (>= 2.8.12.1) + OpenGL (>= 3.0.1b2) + +Installation and Usage +---------------------- ++ Run `python setup.py develop` the very first time so that + python-egg-info is set up correctly and the relevant folders are + included in the PATH. ++ To use the software, run `symoro-bin`. This script is present in + the `bin/` folder. + From ef0696afea239662cb7175be05d51bab7cd9d54d Mon Sep 17 00:00:00 2001 From: aravind Date: Sun, 29 Dec 2013 09:10:14 +0100 Subject: [PATCH 012/273] Update `tests/test.py` to import modules correctly --- tests/test.py | 25 +++++++++---------------- 1 file changed, 9 insertions(+), 16 deletions(-) diff --git a/tests/test.py b/tests/test.py index 51c1d70..3275597 100644 --- a/tests/test.py +++ b/tests/test.py @@ -11,22 +11,13 @@ from sympy.abc import A, B, C, X, Y, Z from numpy import random, amax, matrix, eye, zeros -try: - from pysymoro.core import symoro - from pysymoro.core import geometry - from pysymoro.core import kinematics - from pysymoro.core import invgeom - from pysymoro.core.geometry import Transform as trns - from pysymoro.core import dynamics - from pysymoro.core import parfile -except ImportError: - from core import symoro - from core import geometry - from core import kinematics - from core import invgeom - from core.geometry import Transform as trns - from core import dynamics - from core import parfile +from pysymoro import symoro +from pysymoro import geometry +from pysymoro.geometry import Transform as trns +from pysymoro import kinematics +from pysymoro import invgeom +from pysymoro import dynamics +from pysymoro import parfile class testMisc(unittest.TestCase): @@ -360,3 +351,5 @@ def test_dynamics(self): ## suite.addTest(testSymoroTrig('test_trig_simp')) suite.addTest(testKinematics('test_jac')) # unittest.TextTestRunner().run(suite) + + From a182f7efc48b6fbac0e67a2372590dcef15a583a Mon Sep 17 00:00:00 2001 From: aravind Date: Sun, 29 Dec 2013 09:26:57 +0100 Subject: [PATCH 013/273] Move `tests/` folder into `pysymoro/` folder --- {tests => pysymoro/tests}/test.py | 0 1 file changed, 0 insertions(+), 0 deletions(-) rename {tests => pysymoro/tests}/test.py (100%) diff --git a/tests/test.py b/pysymoro/tests/test.py similarity index 100% rename from tests/test.py rename to pysymoro/tests/test.py From 0bc892b728d04b3c285b33756d96f2b271334776 Mon Sep 17 00:00:00 2001 From: aravind Date: Mon, 30 Dec 2013 03:28:55 +0100 Subject: [PATCH 014/273] Add `symoroutils` package --- pysymoro/parfile.py | 2 ++ symoroutils/__init__.py | 0 2 files changed, 2 insertions(+) create mode 100644 symoroutils/__init__.py diff --git a/pysymoro/parfile.py b/pysymoro/parfile.py index fd669a5..dc5b353 100644 --- a/pysymoro/parfile.py +++ b/pysymoro/parfile.py @@ -145,3 +145,5 @@ def readpar(directory, robo_name): acc_line = '' key = '' return robo, flag + + diff --git a/symoroutils/__init__.py b/symoroutils/__init__.py new file mode 100644 index 0000000..e69de29 From aca3319577939fda320690158aea515fb51a412d Mon Sep 17 00:00:00 2001 From: aravind Date: Wed, 1 Jan 2014 09:35:30 +0100 Subject: [PATCH 015/273] Rename `symoro-bin` to `symoro-bin.py` Rename `symoro-bin` python script to `symoro-bin.py`. This is so that the script is detected as a python script in Windows (Windows requires the file extension for detection). Additionally, a symoblic link -- `symoro-bin` pointing to `symoro-bin.py` is introduced for non Windows systems. --- bin/symoro-bin | 683 +--------------------------------------------- bin/symoro-bin.py | 682 +++++++++++++++++++++++++++++++++++++++++++++ 2 files changed, 683 insertions(+), 682 deletions(-) mode change 100644 => 120000 bin/symoro-bin create mode 100644 bin/symoro-bin.py diff --git a/bin/symoro-bin b/bin/symoro-bin deleted file mode 100644 index 66d874c..0000000 --- a/bin/symoro-bin +++ /dev/null @@ -1,682 +0,0 @@ -#!/usr/bin/env python -# -*- coding: utf-8 -*- - - -__author__ = 'Izzat' - - -import os - -import wx - -from pysymoro.symoro import Robot, FAIL -from pysymoro import geometry, kinematics, dynamics, invgeom -from pysymoro.parfile import readpar, writepar -from symoroviz import graphics -from symoroui import ui_definition, ui_geometry, ui_kinematics - - -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() - - -def main(): - app = wx.App(redirect=False) - frame = MainFrame() - frame.Show() - app.MainLoop() - - -if __name__ == "__main__": - main() - - 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..66d874c --- /dev/null +++ b/bin/symoro-bin.py @@ -0,0 +1,682 @@ +#!/usr/bin/env python +# -*- coding: utf-8 -*- + + +__author__ = 'Izzat' + + +import os + +import wx + +from pysymoro.symoro import Robot, FAIL +from pysymoro import geometry, kinematics, dynamics, invgeom +from pysymoro.parfile import readpar, writepar +from symoroviz import graphics +from symoroui import ui_definition, ui_geometry, ui_kinematics + + +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() + + +def main(): + app = wx.App(redirect=False) + frame = MainFrame() + frame.Show() + app.MainLoop() + + +if __name__ == "__main__": + main() + + From 0ddeb41cb00e53c8bbbe5d09e6d140f50e302653 Mon Sep 17 00:00:00 2001 From: aravind Date: Wed, 1 Jan 2014 10:15:22 +0100 Subject: [PATCH 016/273] Update `setup.py` Update `setup.py` to reflect the new "binary" script for windows. --- setup.py | 13 ++++++++++++- 1 file changed, 12 insertions(+), 1 deletion(-) diff --git a/setup.py b/setup.py index 8c409f0..972e83e 100644 --- a/setup.py +++ b/setup.py @@ -14,12 +14,23 @@ def readme(): 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.1alpha', description='SYmoblic MOdelling of RObots software', url='http://github.com/vijaravind/symoro', - scripts=[os.path.join(BIN_FOLDER, 'symoro-bin')], + scripts=bin_scripts, packages=find_packages(), zip_safe=False ) From 3852a0670926f2ffc974496ec2ddbd53b00ceff1 Mon Sep 17 00:00:00 2001 From: aravind Date: Thu, 20 Mar 2014 06:22:54 +0100 Subject: [PATCH 017/273] Minor changes to the user interface --- bin/symoro-bin.py | 14 +++++--------- 1 file changed, 5 insertions(+), 9 deletions(-) diff --git a/bin/symoro-bin.py b/bin/symoro-bin.py index 66d874c..167fe7c 100644 --- a/bin/symoro-bin.py +++ b/bin/symoro-bin.py @@ -16,14 +16,15 @@ from symoroui import ui_definition, ui_geometry, ui_kinematics -PROG_NAME = 'SYMORO-Python' +PROG_NAME = 'OpenSYMORO' 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) + title = PROG_NAME + " - SYmbolic MOdeling of RObots" + size = wx.Size(-1, -1) + style = wx.DEFAULT_FRAME_STYLE ^ wx.MAXIMIZE_BOX ^ wx.RESIZE_BORDER + wx.Frame.__init__(self, None, title=title, size=size, style=style) self.Bind(wx.EVT_CLOSE, self.OnClose) self.create_mnu() @@ -35,11 +36,6 @@ def __init__(self): 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() From 83a9da8bf267a675ce4c99746fe77e0345279e03 Mon Sep 17 00:00:00 2001 From: aravind Date: Sat, 22 Mar 2014 15:48:07 +0100 Subject: [PATCH 018/273] Add `symoroui/labels.py` --- symoroui/labels.py | 0 1 file changed, 0 insertions(+), 0 deletions(-) create mode 100644 symoroui/labels.py diff --git a/symoroui/labels.py b/symoroui/labels.py new file mode 100644 index 0000000..e69de29 From e2f672eba36cd1a3622c75057cc1b85c422c7aff Mon Sep 17 00:00:00 2001 From: aravind Date: Sat, 22 Mar 2014 15:54:06 +0100 Subject: [PATCH 019/273] Rename files and Fix module import Rename following files : symoroui/ui_definition.py -> symoroui/definiton.py symoroui/ui_geometry.py -> symoroui/geometry.py symoroui/ui_kinematics.py -> symoroui/kinematics.py Fix module import statements in `bin/symoro-bin.py` to reflect new file names. Add docstrings to `bin/symoro-bin.py` --- bin/symoro-bin.py | 29 ++++++++++++++------ symoroui/{ui_definition.py => definition.py} | 0 symoroui/{ui_geometry.py => geometry.py} | 0 symoroui/{ui_kinematics.py => kinematics.py} | 0 4 files changed, 20 insertions(+), 9 deletions(-) rename symoroui/{ui_definition.py => definition.py} (100%) rename symoroui/{ui_geometry.py => geometry.py} (100%) rename symoroui/{ui_kinematics.py => kinematics.py} (100%) diff --git a/bin/symoro-bin.py b/bin/symoro-bin.py index 167fe7c..b334f40 100644 --- a/bin/symoro-bin.py +++ b/bin/symoro-bin.py @@ -2,7 +2,10 @@ # -*- coding: utf-8 -*- -__author__ = 'Izzat' +""" +This module create the main window user interface and draws the +interface on the screen for the SYMORO package. +""" import os @@ -13,18 +16,19 @@ from pysymoro import geometry, kinematics, dynamics, invgeom from pysymoro.parfile import readpar, writepar from symoroviz import graphics -from symoroui import ui_definition, ui_geometry, ui_kinematics +from symoroui import definition as ui_definition +from symoroui import geometry as ui_geometry +from symoroui import kinematics as ui_kinematics -PROG_NAME = 'OpenSYMORO' +PROG_NAME = "OpenSYMORO" class MainFrame(wx.Frame): - def __init__(self): - title = PROG_NAME + " - SYmbolic MOdeling of RObots" - size = wx.Size(-1, -1) - style = wx.DEFAULT_FRAME_STYLE ^ wx.MAXIMIZE_BOX ^ wx.RESIZE_BORDER - wx.Frame.__init__(self, None, title=title, size=size, style=style) + """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) self.create_mnu() @@ -667,7 +671,14 @@ def OnClose(self, _): def main(): app = wx.App(redirect=False) - frame = MainFrame() + title = PROG_NAME + " - SYmbolic MOdeling of RObots" + style = wx.DEFAULT_FRAME_STYLE ^ wx.MAXIMIZE_BOX ^ wx.RESIZE_BORDER + frame = MainFrame( + parent=None, + title=title, + size=(-1, -1), + style=style + ) frame.Show() app.MainLoop() diff --git a/symoroui/ui_definition.py b/symoroui/definition.py similarity index 100% rename from symoroui/ui_definition.py rename to symoroui/definition.py diff --git a/symoroui/ui_geometry.py b/symoroui/geometry.py similarity index 100% rename from symoroui/ui_geometry.py rename to symoroui/geometry.py diff --git a/symoroui/ui_kinematics.py b/symoroui/kinematics.py similarity index 100% rename from symoroui/ui_kinematics.py rename to symoroui/kinematics.py From f3d765f198daca6298a46dd3303f162fbb7b1ab3 Mon Sep 17 00:00:00 2001 From: aravind Date: Sat, 22 Mar 2014 16:41:35 +0100 Subject: [PATCH 020/273] Start replacing label strings with dict keys Add label strings as dict values in `symoroui/labels.py` In `bin/symoro-bin.py` start replacing label strings with dict keys. --- bin/symoro-bin.py | 122 ++++++++++++++++++++++++++++++--------------- symoroui/labels.py | 18 +++++++ 2 files changed, 101 insertions(+), 39 deletions(-) diff --git a/bin/symoro-bin.py b/bin/symoro-bin.py index b334f40..4f3cc93 100644 --- a/bin/symoro-bin.py +++ b/bin/symoro-bin.py @@ -3,7 +3,7 @@ """ -This module create the main window user interface and draws the +This module creates the main window user interface and draws the interface on the screen for the SYMORO package. """ @@ -19,9 +19,7 @@ from symoroui import definition as ui_definition from symoroui import geometry as ui_geometry from symoroui import kinematics as ui_kinematics - - -PROG_NAME = "OpenSYMORO" +from symoroui import labels as ui_labels class MainFrame(wx.Frame): @@ -430,7 +428,9 @@ def create_mnu(self): ##### IDENTIFICATION iden = wx.Menu() base_inert = wx.MenuItem( - iden, wx.ID_ANY, 'Base inertial parameters (symbolic or numeric)') + 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, @@ -460,8 +460,10 @@ def create_mnu(self): 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) + ui_labels.MAINWIN['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']) @@ -502,16 +504,28 @@ def OnNew(self, _): dialog.Destroy() def message_error(self, message): - wx.MessageDialog(None, message, - 'Error', wx.OK | wx.ICON_ERROR).ShowModal() + 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() + 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() + 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' % \ @@ -520,26 +534,36 @@ def model_success(self, model_name): 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) + 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') + 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]) + 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.message_warning( + "While reading file an error occured." + ) self.robo = new_robo self.feed_data() @@ -548,10 +572,13 @@ def OnSave(self, _): 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') + 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 @@ -562,7 +589,9 @@ def OnSaveAs(self, _): self.changed = False def OnTransformationMatrix(self, _): - dialog = ui_geometry.DialogTrans(PROG_NAME, self.robo.NF) + dialog = ui_geometry.DialogTrans( + ui_labels.MAINWIN['prog_name'], self.robo.NF + ) if dialog.ShowModal() == wx.ID_OK: frames, trig_subs = dialog.GetValues() geometry.direct_geometric(self.robo, frames, trig_subs) @@ -570,7 +599,9 @@ def OnTransformationMatrix(self, _): dialog.Destroy() def OnFastGeometricModel(self, _): - dialog = ui_geometry.DialogFast(PROG_NAME, self.robo.NF) + dialog = ui_geometry.DialogFast( + ui_labels.MAINWIN['prog_name'], self.robo.NF + ) if dialog.ShowModal() == wx.ID_OK: i, j = dialog.GetValues() geometry.direct_geometric_fast(self.robo, i, j) @@ -578,8 +609,11 @@ def OnFastGeometricModel(self, _): dialog.Destroy() def OnIGMPaul(self, _): - dialog = ui_geometry.DialogPaul(PROG_NAME, self.robo.endeffectors, - str(invgeom.EMPTY)) + dialog = ui_geometry.DialogPaul( + ui_labels.MAINWIN['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) @@ -590,7 +624,9 @@ def OnConstraintGeoEq(self, _): pass def OnJacobianMatrix(self, _): - dialog = ui_kinematics.DialogJacobian(PROG_NAME, self.robo) + dialog = ui_kinematics.DialogJacobian( + ui_labels.MAINWIN['prog_name'], self.robo + ) if dialog.ShowModal() == wx.ID_OK: n, i, j = dialog.get_values() kinematics.jacobian(self.robo, n, i, j) @@ -598,7 +634,9 @@ def OnJacobianMatrix(self, _): dialog.Destroy() def OnDeterminant(self, _): - dialog = ui_kinematics.DialogDeterminant(PROG_NAME, self.robo) + dialog = ui_kinematics.DialogDeterminant( + ui_labels.MAINWIN['prog_name'], self.robo + ) if dialog.ShowModal() == wx.ID_OK: kinematics.jacobian_determinant(self.robo, *dialog.get_values()) self.model_success('det') @@ -647,14 +685,21 @@ def OnDynIdentifModel(self, _): self.model_success('dim') def OnVisualisation(self, _): - dialog = ui_definition.DialogConversion(PROG_NAME, - self.robo, self.par_dict) + dialog = ui_definition.DialogConversion( + ui_labels.MAINWIN['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) + graphics.MainWindow( + ui_labels.MAINWIN['prog_name'], self.robo, + self.par_dict, self + ) else: - graphics.MainWindow(PROG_NAME, self.robo, self.par_dict, self) + graphics.MainWindow( + ui_labels.MAINWIN['prog_name'], self.robo, + self.par_dict, self + ) def OnClose(self, _): if self.changed: @@ -671,11 +716,10 @@ def OnClose(self, _): def main(): app = wx.App(redirect=False) - title = PROG_NAME + " - SYmbolic MOdeling of RObots" style = wx.DEFAULT_FRAME_STYLE ^ wx.MAXIMIZE_BOX ^ wx.RESIZE_BORDER frame = MainFrame( parent=None, - title=title, + title=ui_labels.MAINWIN['window_title'], size=(-1, -1), style=style ) diff --git a/symoroui/labels.py b/symoroui/labels.py index e69de29..8e8143e 100644 --- a/symoroui/labels.py +++ b/symoroui/labels.py @@ -0,0 +1,18 @@ +#!/usr/bin/env python +# -*- coding: utf-8 -*- + + +""" +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. +""" + + +MAINWIN = dict( + prog_name = "OpenSYMORO", + window_title = "OpenSYMORO - SYmbolic MOdelling of RObots" +) + From c59c779c5a417dc3b50cd48b28ee2a257414a596 Mon Sep 17 00:00:00 2001 From: aravind Date: Sat, 22 Mar 2014 21:43:05 +0100 Subject: [PATCH 021/273] Remove `pysymoro/main.py` --- pysymoro/main.py | 681 ----------------------------------------------- 1 file changed, 681 deletions(-) delete mode 100644 pysymoro/main.py diff --git a/pysymoro/main.py b/pysymoro/main.py deleted file mode 100644 index 9088c4e..0000000 --- a/pysymoro/main.py +++ /dev/null @@ -1,681 +0,0 @@ -#!/usr/bin/env python -# -*- coding: utf-8 -*- - -__author__ = 'Izzat' - - -import os - -import wx - -from pysymoro.symoro import Robot, FAIL -from pysymoro import geometry, kinematics, dynamics, invgeom -from pysymoro.parfile import readpar, writepar -from symoroviz import graphics -from symoroui import ui_definition, ui_geometry, ui_kinematics - - -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() - - -def main(): - app = wx.App(redirect=False) - frame = MainFrame() - frame.Show() - app.MainLoop() - - -if __name__ == "__main__": - main() - - From a354b742c132d005405f5befdd7790bc416b5298 Mon Sep 17 00:00:00 2001 From: aravind Date: Sat, 22 Mar 2014 23:56:46 +0100 Subject: [PATCH 022/273] Modify menu bar with dict keys Replace the label strings in menu bar items with dict keys. Add required key, labels string pairs in `symoroui/labels.py`. --- bin/symoro-bin.py | 411 +++++++++++++++++++++++++-------------------- symoroui/labels.py | 52 +++++- 2 files changed, 278 insertions(+), 185 deletions(-) diff --git a/bin/symoro-bin.py b/bin/symoro-bin.py index 4f3cc93..57511d5 100644 --- a/bin/symoro-bin.py +++ b/bin/symoro-bin.py @@ -28,28 +28,32 @@ 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) - - self.create_mnu() + # create status bar + self.statusbar = self.CreateStatusBar() + # create menu bar + self.create_menu() + # set default robot self.robo = Robot.RX90() + # object to store different ui elements self.widgets = {} + # object to store parameter values got from dialog box input self.par_dict = {} - - self.statusbar = self.CreateStatusBar() - self.p = wx.Panel(self) - self.mainSizer = wx.BoxSizer(wx.VERTICAL) - + # setup panel and sizer for content + self.panel = wx.Panel(self) + self.main_sizer = wx.BoxSizer(wx.VERTICAL) self.create_ui() - self.p.SetSizerAndFit(self.mainSizer) + self.panel.SetSizerAndFit(self.main_sizer) self.Fit() + # update fields with data 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, + horBox.Add(wx.StaticText(self.panel, 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) + textBox = wx.TextCtrl(self.panel, 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) @@ -58,17 +62,17 @@ def params_grid(self, grid, rows, elems, handler=None, start_i=0, size=60): def create_ui(self): descr_sizer = wx.StaticBoxSizer( - wx.StaticBox(self.p, label='Robot Description'), wx.HORIZONTAL) + wx.StaticBox(self.panel, 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) + self.main_sizer.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) + wx.StaticBox(self.panel, label='Robot Type'), wx.HORIZONTAL) grid = wx.GridBagSizer(12, 10) gen_info = [('Name of the robot:', 'name'), @@ -78,9 +82,9 @@ def create_ui(self): ('Number of closed loops:', 'loops')] for i, (lab, name) in enumerate(gen_info): - label = wx.StaticText(self.p, label=lab) + label = wx.StaticText(self.panel, label=lab) grid.Add(label, pos=(i, 0), flag=wx.LEFT, border=10) - label = wx.StaticText(self.p, size=(125, -1), name=name) + label = wx.StaticText(self.panel, size=(125, -1), name=name) self.widgets[name] = label grid.Add(label, pos=(i, 1), flag=wx.LEFT | wx.RIGHT, border=10) @@ -90,11 +94,11 @@ def create_ui(self): ##### Gravity components sizer1.AddSpacer(8) sbs_gravity = wx.StaticBoxSizer( - wx.StaticBox(self.p, label='Gravity components'), wx.HORIZONTAL) + wx.StaticBox(self.panel, 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) + sbs_gravity.Add(wx.StaticText(self.panel, label=name), 0, wx.ALL, 4) + text_box = wx.TextCtrl(self.panel, 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) @@ -103,13 +107,13 @@ def create_ui(self): ##### Location of the robot sizer1.AddSpacer(8) sbs_location = wx.StaticBoxSizer( - wx.StaticBox(self.p, label='Location of the robot'), wx.HORIZONTAL) + wx.StaticBox(self.panel, 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)) + ver_lbl = wx.StaticText(self.panel, label='Z' + str(i + 1) + ': ') + hor_lbl = wx.StaticText(self.panel, label='Z - ' + str(i + 1)) + botLab = wx.StaticText(self.panel, 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) @@ -117,7 +121,7 @@ def create_ui(self): for j in range(3): index = j*4+i name = 'Z'+str(index) - text_box = wx.TextCtrl(self.p, name=name, + text_box = wx.TextCtrl(self.panel, name=name, size=(60, -1), id=index) self.widgets[name] = text_box text_box.Bind(wx.EVT_KILL_FOCUS, self.OnZParamChanged) @@ -127,23 +131,23 @@ def create_ui(self): ##### Geometric Params sbs = wx.StaticBoxSizer( - wx.StaticBox(self.p, label='Geometric Params'), wx.HORIZONTAL) + wx.StaticBox(self.panel, 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'), + ver_sizer.Add(wx.StaticText(self.panel, label='Frame'), 0, wx.ALL | wx.ALIGN_CENTER_HORIZONTAL, 5) - combo_box = wx.ComboBox(self.p, style=wx.CB_READONLY, + combo_box = wx.ComboBox(self.panel, 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), + label = wx.StaticText(self.panel, 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, + combo_box = wx.ComboBox(self.panel, style=wx.CB_READONLY, size=(60, -1), name=name) self.widgets[name] = combo_box combo_box.Bind(wx.EVT_COMBOBOX, self.OnGeoParamChanged) @@ -161,12 +165,12 @@ def create_ui(self): ##### Dynamic Params and external forces lbl = 'Dynamic Params and external forces' - sbs = wx.StaticBoxSizer(wx.StaticBox(self.p, label=lbl), wx.HORIZONTAL) + sbs = wx.StaticBoxSizer(wx.StaticBox(self.panel, label=lbl), wx.HORIZONTAL) grid = wx.GridBagSizer(0, 0) ver_sizer = wx.BoxSizer(wx.VERTICAL) - ver_sizer.Add(wx.StaticText(self.p, label='Link'), + ver_sizer.Add(wx.StaticText(self.panel, label='Link'), 0, wx.ALL | wx.ALIGN_CENTER_HORIZONTAL, 5) - combo_box = wx.ComboBox(self.p, style=wx.CB_READONLY, + combo_box = wx.ComboBox(self.panel, style=wx.CB_READONLY, size=(60, -1), name='link') combo_box.Bind(wx.EVT_COMBOBOX, self.OnLinkChanged) self.widgets['link'] = combo_box @@ -185,7 +189,7 @@ def create_ui(self): ##### Speed and acceleration of the base lbl = 'Speed and acceleration of the base' - sbs = wx.StaticBoxSizer(wx.StaticBox(self.p, label=lbl), wx.HORIZONTAL) + sbs = wx.StaticBoxSizer(wx.StaticBox(self.panel, label=lbl), wx.HORIZONTAL) grid = wx.GridBagSizer(0, 0) sbs.Add(grid) hor_sizer = wx.BoxSizer(wx.HORIZONTAL) @@ -201,11 +205,11 @@ def create_ui(self): ##### Joints velocity and acceleration lbl = 'Joint velocity and acceleration' - sbs2 = wx.StaticBoxSizer(wx.StaticBox(self.p, label=lbl), wx.VERTICAL) + sbs2 = wx.StaticBoxSizer(wx.StaticBox(self.panel, label=lbl), wx.VERTICAL) sbs2.AddSpacer(5) grid = wx.GridBagSizer(5, 5) sbs2.Add(grid) - combo_box = wx.ComboBox(self.p, size=(70, -1), + combo_box = wx.ComboBox(self.panel, size=(70, -1), style=wx.CB_READONLY, name='joint') self.widgets['joint'] = combo_box combo_box.Bind(wx.EVT_COMBOBOX, self.OnJointChanged) @@ -213,11 +217,11 @@ def create_ui(self): 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, + label = wx.StaticText(self.panel, 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)) + text_box = wx.TextCtrl(self.panel, 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)) @@ -225,56 +229,56 @@ def create_ui(self): 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) + self.main_sizer.AddSpacer(10) #sizer2.Add(sbs, 0, wx.ALL | wx.EXPAND, 0) - def Change(self, index, name, evtObject): + def Change(self, index, name, event_object): prev_value = str(self.robo.get_val(index, name)) - if evtObject.Value != prev_value: - if self.robo.put_val(index, name, evtObject.Value) == FAIL: + if event_object.Value != prev_value: + if self.robo.put_val(index, name, event_object.Value) == FAIL: message = "Unacceptable value '%s' has been input in %s%s" \ - % (evtObject.Value, name, index) + % (event_object.Value, name, index) self.message_error(message) - evtObject.Value = prev_value + event_object.Value = prev_value else: self.changed = True - def OnGeoParamChanged(self, evt): + def OnGeoParamChanged(self, event): frame_index = int(self.widgets['frame'].Value) - self.Change(frame_index, evt.EventObject.Name, evt.EventObject) - if evt.EventObject.Name == 'ant': + self.Change(frame_index, event.EventObject.Name, event.EventObject) + if event.EventObject.Name == 'ant': self.widgets['type'].SetLabel(self.robo.structure) - if evt.EventObject.Name == 'sigma': + if event.EventObject.Name == 'sigma': self.update_geo_params() - def OnDynParamChanged(self, evt): + def OnDynParamChanged(self, event): 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)) + self.Change(link_index, event.EventObject.Name, event.EventObject) + # print type(self.robo.get_val(link_index, event.EventObject.Name)) - def OnSpeedChanged(self, evt): + def OnSpeedChanged(self, event): joint_index = int(self.widgets['joint'].Value) - self.Change(joint_index, evt.EventObject.Name, evt.EventObject) + self.Change(joint_index, event.EventObject.Name, event.EventObject) - def OnBaseTwistChanged(self, evt): - index = int(evt.EventObject.Id) - name = evt.EventObject.Name[:-1] - self.Change(index, name, evt.EventObject) + def OnBaseTwistChanged(self, event): + index = int(event.EventObject.Id) + name = event.EventObject.Name[:-1] + self.Change(index, name, event.EventObject) - def OnZParamChanged(self, evt): - index = int(evt.EventObject.Id) - self.Change(index, 'Z', evt.EventObject) + def OnZParamChanged(self, event): + index = int(event.EventObject.Id) + self.Change(index, 'Z', event.EventObject) - def OnFrameChanged(self, evt): - frame_index = int(evt.EventObject.Value) + 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, _): + def OnLinkChanged(self, event): self.update_dyn_params() - def OnJointChanged(self, _): + def OnJointChanged(self, event): self.update_vel_params() def update_params(self, index, pars): @@ -343,124 +347,162 @@ def feed_data(self): 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...') + def create_menu(self): + """Method to 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) - m_open = file_mnu.Append(wx.ID_OPEN, '&Open...') + 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) - m_save = file_mnu.Append(wx.ID_SAVE, '&Save...') + 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) - m_save_as = file_mnu.Append(wx.ID_SAVEAS, '&Save As...') + 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_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.") + 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) - 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) + 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_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) #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, _): + 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) + 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) + 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) + 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 OnNew(self, event): dialog = ui_definition.DialogDefinition( - ui_labels.MAINWIN['prog_name'], + ui_labels.MAIN_WIN['prog_name'], self.robo.name, self.robo.nl, self.robo.nj, self.robo.structure, self.robo.is_mobile ) @@ -590,7 +632,7 @@ def OnSaveAs(self, _): def OnTransformationMatrix(self, _): dialog = ui_geometry.DialogTrans( - ui_labels.MAINWIN['prog_name'], self.robo.NF + ui_labels.MAIN_WIN['prog_name'], self.robo.NF ) if dialog.ShowModal() == wx.ID_OK: frames, trig_subs = dialog.GetValues() @@ -600,7 +642,7 @@ def OnTransformationMatrix(self, _): def OnFastGeometricModel(self, _): dialog = ui_geometry.DialogFast( - ui_labels.MAINWIN['prog_name'], self.robo.NF + ui_labels.MAIN_WIN['prog_name'], self.robo.NF ) if dialog.ShowModal() == wx.ID_OK: i, j = dialog.GetValues() @@ -608,9 +650,9 @@ def OnFastGeometricModel(self, _): self.model_success('fgm') dialog.Destroy() - def OnIGMPaul(self, _): + def OnIgmPaul(self, _): dialog = ui_geometry.DialogPaul( - ui_labels.MAINWIN['prog_name'], + ui_labels.MAIN_WIN['prog_name'], self.robo.endeffectors, str(invgeom.EMPTY) ) @@ -625,7 +667,7 @@ def OnConstraintGeoEq(self, _): def OnJacobianMatrix(self, _): dialog = ui_kinematics.DialogJacobian( - ui_labels.MAINWIN['prog_name'], self.robo + ui_labels.MAIN_WIN['prog_name'], self.robo ) if dialog.ShowModal() == wx.ID_OK: n, i, j = dialog.get_values() @@ -635,7 +677,7 @@ def OnJacobianMatrix(self, _): def OnDeterminant(self, _): dialog = ui_kinematics.DialogDeterminant( - ui_labels.MAINWIN['prog_name'], self.robo + ui_labels.MAIN_WIN['prog_name'], self.robo ) if dialog.ShowModal() == wx.ID_OK: kinematics.jacobian_determinant(self.robo, *dialog.get_values()) @@ -686,22 +728,22 @@ def OnDynIdentifModel(self, _): def OnVisualisation(self, _): dialog = ui_definition.DialogConversion( - ui_labels.MAINWIN['prog_name'], self.robo, self.par_dict + 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.MAINWIN['prog_name'], self.robo, + ui_labels.MAIN_WIN['prog_name'], self.robo, self.par_dict, self ) else: graphics.MainWindow( - ui_labels.MAINWIN['prog_name'], self.robo, + ui_labels.MAIN_WIN['prog_name'], self.robo, self.par_dict, self ) - def OnClose(self, _): + def OnClose(self, event): if self.changed: result = wx.MessageBox( 'Do you want to save changes?', 'Please confirm', @@ -719,7 +761,8 @@ def main(): style = wx.DEFAULT_FRAME_STYLE ^ wx.MAXIMIZE_BOX ^ wx.RESIZE_BORDER frame = MainFrame( parent=None, - title=ui_labels.MAINWIN['window_title'], + id=wx.ID_ANY, + title=ui_labels.MAIN_WIN['window_title'], size=(-1, -1), style=style ) diff --git a/symoroui/labels.py b/symoroui/labels.py index 8e8143e..4a88710 100644 --- a/symoroui/labels.py +++ b/symoroui/labels.py @@ -11,8 +11,58 @@ """ -MAINWIN = dict( +MAIN_WIN = dict( prog_name = "OpenSYMORO", window_title = "OpenSYMORO - SYmbolic MOdelling of RObots" ) +MAIN_MENU = dict( + file_menu = "&File", + geom_menu = "&Geometric", + kin_menu = "&Kinematic", + dyn_menu = "&Dynamic", + iden_menu = "&Identification", + optim_menu = "&Optimiser", + viz_menu = "&Visualisation" +) + +VIZ_MENU = dict(m_viz = "Visualisation") + +IDEN_MENU = dict( + m_base_inertial_params = "Base Inertial parameters", + m_dyn_iden_model = "Dynamic Identification Model", + m_energy_iden_model = "Energy Identification Model" +) + +DYN_MENU = dict( + m_idym = "Inverse Dynamic Model", + m_inertia_matrix = "Inertia matrix", + m_h_term = "Centrifugal, Coriolis & Gravity torques", + m_ddym = "Direct Dynamic Model" +) + +KIN_MENU = dict( + 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 = dict( + m_trans_matrix = "Transformation matrix", + m_fast_dgm = "Fast Geometric model", + m_igm_paul = "IGM - Paul method", + m_geom_constraint = "Geometric constraint equation of loops" +) + +FILE_MENU = dict( + m_new = "&New", + m_open = "&Open", + m_save = "&Save", + m_save_as = "Save &As", + m_pref = "Preferences\t(unavailable)", + m_exit = "E&xit" +) + From 03ad6f48694563d9caa04c832c155ed7e87ab086 Mon Sep 17 00:00:00 2001 From: aravind Date: Mon, 24 Mar 2014 17:10:17 +0100 Subject: [PATCH 023/273] Update labels with dict keys Add more labels to dict keys mapping in `symoroui/labels.py`. Update `bin/symoro-bin.py` with the correct dict keys for the entire left column of the user interface. --- bin/symoro-bin.py | 231 ++++++++++++++++++++++++++------------------- symoroui/labels.py | 51 +++++++++- 2 files changed, 182 insertions(+), 100 deletions(-) diff --git a/bin/symoro-bin.py b/bin/symoro-bin.py index 57511d5..98cf36a 100644 --- a/bin/symoro-bin.py +++ b/bin/symoro-bin.py @@ -40,12 +40,14 @@ def __init__(self, *args, **kwargs): self.par_dict = {} # setup panel and sizer for content self.panel = wx.Panel(self) - self.main_sizer = wx.BoxSizer(wx.VERTICAL) + self.szr_topmost = wx.BoxSizer(wx.VERTICAL) self.create_ui() - self.panel.SetSizerAndFit(self.main_sizer) + self.panel.SetSizerAndFit(self.szr_topmost) self.Fit() # update fields with data self.feed_data() + # set status bar to Ready + self.statusbar.SetStatusText("Ready") def params_grid(self, grid, rows, elems, handler=None, start_i=0, size=60): for i, name in enumerate(elems): @@ -61,77 +63,114 @@ def params_grid(self, grid, rows, elems, handler=None, start_i=0, size=60): flag=wx.ALL, border=2) def create_ui(self): - descr_sizer = wx.StaticBoxSizer( - wx.StaticBox(self.panel, 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.main_sizer.Add(descr_sizer, 0, wx.ALL, 10) - - # Left Side of main window - robot_type_sizer = wx.StaticBoxSizer( - wx.StaticBox(self.panel, 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.panel, label=lab) - grid.Add(label, pos=(i, 0), flag=wx.LEFT, border=10) - label = wx.StaticText(self.panel, 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.panel, label='Gravity components'), wx.HORIZONTAL) - for iden, name in enumerate(['GX', 'GY', 'GZ']): - sbs_gravity.AddSpacer(5) - sbs_gravity.Add(wx.StaticText(self.panel, label=name), 0, wx.ALL, 4) - text_box = wx.TextCtrl(self.panel, 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.panel, label='Location of the robot'), wx.HORIZONTAL) - sizer1.Add(sbs_location, 0, wx.ALL | wx.EXPAND, 0) - loc_tbl = wx.GridBagSizer(1, 1) + """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(12, 10) + 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=(125, -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 + ) + for idx, key in enumerate(ui_labels.GRAVITY_CMPNTS): + label = ui_labels.GRAVITY_CMPNTS[key].label + name = ui_labels.GRAVITY_CMPNTS[key].name + txt_gravity = wx.TextCtrl( + self.panel, name=name, size=(60, -1) + ) + self.widgets[name] = txt_gravity + szr_gravity.AddSpacer(5) + txt_gravity.Bind(wx.EVT_KILL_FOCUS, self.OnBaseTwistChanged) + szr_gravity.Add( + wx.StaticText(self.panel, label=label), 0, wx.ALL, 4 + ) + szr_gravity.Add(txt_gravity, 0, wx.ALL | wx.ALIGN_LEFT, 2) + 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): - ver_lbl = wx.StaticText(self.panel, label='Z' + str(i + 1) + ': ') - hor_lbl = wx.StaticText(self.panel, label='Z - ' + str(i + 1)) - botLab = wx.StaticText(self.panel, 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.panel, 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 + idx = j*4+i + name = 'Z'+str(idx) + txt_z_element = wx.TextCtrl( + self.panel, name=name, size=(60, -1) + ) + self.widgets[name] = 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 sbs = wx.StaticBoxSizer( - wx.StaticBox(self.panel, label='Geometric Params'), wx.HORIZONTAL) + wx.StaticBox( + self.panel, label=ui_labels.BOX_TITLES['geom_params'] + ), wx.HORIZONTAL + ) grid = wx.GridBagSizer(0, 5) ver_sizer = wx.BoxSizer(wx.VERTICAL) ver_sizer.Add(wx.StaticText(self.panel, label='Frame'), @@ -153,19 +192,18 @@ def create_ui(self): 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.panel, label=lbl), wx.HORIZONTAL) + szr_right_col.Add(sbs, 0, wx.ALL | wx.EXPAND, 0) + # right col - dynamic params and external forces box + sbs = wx.StaticBoxSizer( + wx.StaticBox( + self.panel, label=ui_labels.BOX_TITLES['dyn_params'] + ), wx.HORIZONTAL + ) grid = wx.GridBagSizer(0, 0) ver_sizer = wx.BoxSizer(wx.VERTICAL) ver_sizer.Add(wx.StaticText(self.panel, label='Link'), @@ -177,35 +215,35 @@ def create_ui(self): 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.panel, label=lbl), wx.HORIZONTAL) + szr_right_col.AddSpacer(8) + szr_right_col.Add(sbs, 0, wx.ALL | wx.EXPAND, 0) + # right col - velocity and acceleration of the base box + sbs = wx.StaticBoxSizer( + wx.StaticBox( + self.panel, label=ui_labels.BOX_TITLES['vel_acc_base'] + ), 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.panel, label=lbl), wx.VERTICAL) + # right col - joint velocity and acceleration box + sbs2 = wx.StaticBoxSizer( + wx.StaticBox( + self.panel, label=ui_labels.BOX_TITLES['joint_vel_acc'] + ), wx.HORIZONTAL + ) sbs2.AddSpacer(5) grid = wx.GridBagSizer(5, 5) sbs2.Add(grid) @@ -225,12 +263,11 @@ def create_ui(self): 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.main_sizer.AddSpacer(10) - #sizer2.Add(sbs, 0, wx.ALL | wx.EXPAND, 0) + szr_right_col.AddSpacer(8) + szr_right_col.Add(hor_sizer, 1, wx.ALL | wx.EXPAND, 0) + self.szr_topmost.AddSpacer(10) + #szr_right_col.Add(sbs, 0, wx.ALL | wx.EXPAND, 0) def Change(self, index, name, event_object): prev_value = str(self.robo.get_val(index, name)) diff --git a/symoroui/labels.py b/symoroui/labels.py index 4a88710..b40047c 100644 --- a/symoroui/labels.py +++ b/symoroui/labels.py @@ -11,11 +11,55 @@ """ +from collections import namedtuple +from collections import OrderedDict + + +# main window + MAIN_WIN = dict( - prog_name = "OpenSYMORO", - window_title = "OpenSYMORO - SYmbolic MOdelling of RObots" + prog_name = "SYMORO", + window_title = "SYMORO - SYmbolic MOdelling of RObots" +) + + +# interface contents + +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", + vel_acc_base = "Velocity and Acceleration of the base", + joint_vel_acc = "Joint Velocity and Acceleration" ) +FieldEntry = namedtuple('FieldEntry', ['label', 'name']) + +ROBOT_TYPE = dict( + name = FieldEntry('Name of the robot:', 'name'), + num_links = FieldEntry('Number of moving links:', 'NL'), + num_joints = FieldEntry('Number of joints:', 'NJ'), + num_frames = FieldEntry('Number of frames:', 'NF'), + structure = FieldEntry('Type of structure:', 'type'), + is_mobile = FieldEntry('Is Mobile:', 'mobile'), + num_loops = FieldEntry('Number of closed loops:', 'loops') +) + +GRAVITY_CMPNTS = OrderedDict( + sorted( + dict( + gx = FieldEntry('GX', 'GX'), + gy = FieldEntry('GY', 'GY'), + gz = FieldEntry('GZ', 'GZ') + ).items(), key=lambda t: t[0] + ) +) + +# menu bar + MAIN_MENU = dict( file_menu = "&File", geom_menu = "&Geometric", @@ -62,7 +106,8 @@ m_open = "&Open", m_save = "&Save", m_save_as = "Save &As", - m_pref = "Preferences\t(unavailable)", + m_pref = "Preferences -- (unavailable)", m_exit = "E&xit" ) + From 9aa92b89dfea21a6a6378a8f9856b42e018d0945 Mon Sep 17 00:00:00 2001 From: aravind Date: Tue, 25 Mar 2014 01:23:27 +0100 Subject: [PATCH 024/273] Update labels with dict keys Update `bin/symoro-bin.py` with the correct dict keys for geometric parameters of the user interface. Add corresponding label, dict key pairs to `symoroui/labels.py`. --- bin/symoro-bin.py | 88 ++++++++++++++++++++++++++++------------------ symoroui/labels.py | 51 ++++++++++++++++----------- 2 files changed, 84 insertions(+), 55 deletions(-) diff --git a/bin/symoro-bin.py b/bin/symoro-bin.py index 98cf36a..82db3fe 100644 --- a/bin/symoro-bin.py +++ b/bin/symoro-bin.py @@ -49,17 +49,53 @@ def __init__(self, *args, **kwargs): # set status bar to Ready self.statusbar.SetStatusText("Ready") - def params_grid(self, grid, rows, elems, handler=None, start_i=0, size=60): - for i, name in enumerate(elems): + def params_in_grid( + self, szr_grd, elements, rows, cols, elplace, + handler=None, start=0, width=60): + """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 + if control is 'cmb': + ctrl = wx.ComboBox( + self.panel, style=wx.CB_READONLY, + size=(width, -1), name=name + ) + ctrl.Bind(wx.EVT_COMBOBOX, handler) + else: + ctrl = wx.TextCtrl( + self.panel, size=(width, -1), name=name + ) + ctrl.Bind(wx.EVT_KILL_FOCUS, handler) + self.widgets[name] = 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=(elplace[idx][0], elplace[idx][1]), + flag=wx.ALL | wx.ALIGN_RIGHT, border=2 + ) + + def params_grid(self, grid, elements, cols, handler=None, start_i=0, size=60): + for i, name in enumerate(elements): horBox = wx.BoxSizer(wx.HORIZONTAL) horBox.Add(wx.StaticText(self.panel, label=name, size=(40, -1), style=wx.ALIGN_RIGHT), 0, wx.ALL | wx.ALIGN_RIGHT, 5) - textBox = wx.TextCtrl(self.panel, size=(size, -1), name=name, id=i%rows) + textBox = wx.TextCtrl(self.panel, size=(size, -1), + name=name, id=i%cols) 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), + grid.Add(horBox, pos=(i/cols, i % cols + start_i), flag=wx.ALL, border=2) def create_ui(self): @@ -166,38 +202,22 @@ def create_ui(self): 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 - sbs = wx.StaticBoxSizer( + szr_geom_params = wx.StaticBoxSizer( wx.StaticBox( self.panel, label=ui_labels.BOX_TITLES['geom_params'] ), wx.HORIZONTAL ) - grid = wx.GridBagSizer(0, 5) - ver_sizer = wx.BoxSizer(wx.VERTICAL) - ver_sizer.Add(wx.StaticText(self.panel, label='Frame'), - 0, wx.ALL | wx.ALIGN_CENTER_HORIZONTAL, 5) - combo_box = wx.ComboBox(self.panel, 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.panel, 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.panel, 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) - szr_right_col.Add(sbs, 0, wx.ALL | wx.EXPAND, 0) + szr_grd_geom = wx.GridBagSizer(0, 5) + self.params_in_grid( + szr_grd_geom, elements=ui_labels.GEOM_PARAMS, + rows=2, cols=5, handler=self.OnGeoParamChanged, + elplace=[ + (0,0), (1,0), (0,1), (1,1), (0,2), (1,2), + (0,3), (1,3), (0,4), (1,4) + ], start=0, width=70 + ) + szr_geom_params.Add(szr_grd_geom) + szr_right_col.Add(szr_geom_params, 0, wx.ALL | wx.EXPAND, 0) # right col - dynamic params and external forces box sbs = wx.StaticBoxSizer( wx.StaticBox( @@ -217,7 +237,7 @@ def create_ui(self): 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) + self.params_grid(grid, params, 4, self.OnDynParamChanged, 1) sbs.Add(grid) sbs.AddSpacer(4) szr_right_col.AddSpacer(8) @@ -237,7 +257,7 @@ def create_ui(self): 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) + self.params_grid(grid, params, 3, self.OnBaseTwistChanged, 0) # right col - joint velocity and acceleration box sbs2 = wx.StaticBoxSizer( wx.StaticBox( diff --git a/symoroui/labels.py b/symoroui/labels.py index b40047c..d6216d9 100644 --- a/symoroui/labels.py +++ b/symoroui/labels.py @@ -36,27 +36,36 @@ joint_vel_acc = "Joint Velocity and Acceleration" ) -FieldEntry = namedtuple('FieldEntry', ['label', 'name']) - -ROBOT_TYPE = dict( - name = FieldEntry('Name of the robot:', 'name'), - num_links = FieldEntry('Number of moving links:', 'NL'), - num_joints = FieldEntry('Number of joints:', 'NJ'), - num_frames = FieldEntry('Number of frames:', 'NF'), - structure = FieldEntry('Type of structure:', 'type'), - is_mobile = FieldEntry('Is Mobile:', 'mobile'), - num_loops = FieldEntry('Number of closed loops:', 'loops') -) - -GRAVITY_CMPNTS = OrderedDict( - sorted( - dict( - gx = FieldEntry('GX', 'GX'), - gy = FieldEntry('GY', 'GY'), - gz = FieldEntry('GZ', 'GZ') - ).items(), key=lambda t: t[0] - ) -) +FieldEntry = namedtuple('FieldEntry', ['label', 'name', 'control']) + +GEOM_PARAMS = OrderedDict([ + ('frame', FieldEntry('Frame', 'frame', 'cmb')), + ('ant', FieldEntry('ant', 'ant', 'cmb')), + ('sigma', FieldEntry('sigma', 'sigma', 'cmb')), + ('mu', FieldEntry('mu', 'mu', 'cmb')), + ('gamma', FieldEntry('gamma', 'gamma', 'txt')), + ('b', FieldEntry('b', 'b', 'txt')), + ('alpha', FieldEntry('alpha', 'alpha', 'txt')), + ('d', FieldEntry('d', 'd', 'txt')), + ('theta', FieldEntry('theta', 'theta', 'txt')), + ('r', FieldEntry('r', 'r', 'txt')) +]) + +GRAVITY_CMPNTS = OrderedDict([ + ('gx', FieldEntry('GX', 'GX', 'txt')), + ('gy', FieldEntry('GY', 'GY', 'txt')), + ('gz', FieldEntry('GZ', 'GZ', 'txt')) +]) + +ROBOT_TYPE = OrderedDict([ + ('name', FieldEntry('Name of the robot:', 'name', 'lbl')), + ('num_links', FieldEntry('Number of moving links:', 'NL', 'lbl')), + ('num_joints', FieldEntry('Number of joints:', 'NJ', 'lbl')), + ('num_frames', FieldEntry('Number of frames:', 'NF', 'lbl')), + ('structure', FieldEntry('Type of structure:', 'type', 'lbl')), + ('is_mobile', FieldEntry('Is Mobile:', 'mobile', 'lbl')), + ('num_loops', FieldEntry('Number of closed loops:', 'loops', 'lbl')) +]) # menu bar From ae3c91521bb769cb0ef43ab342674cd64f3e491e Mon Sep 17 00:00:00 2001 From: aravind Date: Tue, 25 Mar 2014 23:01:18 +0100 Subject: [PATCH 025/273] Update labels with dict keys complete Update `bin/symoro-bin.py` with the correct dict keys for all the parameters of the UI -- completed. Add corresponding label, dict key pairs to `symoroui/labels.py`. --- bin/symoro-bin.py | 227 +++++++++++++++++++++++++++------------------ symoroui/labels.py | 81 +++++++++++++--- 2 files changed, 202 insertions(+), 106 deletions(-) diff --git a/bin/symoro-bin.py b/bin/symoro-bin.py index 82db3fe..54a88c5 100644 --- a/bin/symoro-bin.py +++ b/bin/symoro-bin.py @@ -8,7 +8,7 @@ """ -import os +from collections import OrderedDict import wx @@ -63,6 +63,10 @@ def params_in_grid( size=(width, -1), name=name ) ctrl.Bind(wx.EVT_COMBOBOX, handler) + elif control is 'lbl': + ctrl = wx.StaticText( + self.panel, size=(width, -1), name=name + ) else: ctrl = wx.TextCtrl( self.panel, size=(width, -1), name=name @@ -84,20 +88,6 @@ def params_in_grid( flag=wx.ALL | wx.ALIGN_RIGHT, border=2 ) - def params_grid(self, grid, elements, cols, handler=None, start_i=0, size=60): - for i, name in enumerate(elements): - horBox = wx.BoxSizer(wx.HORIZONTAL) - horBox.Add(wx.StaticText(self.panel, label=name, - size=(40, -1), style=wx.ALIGN_RIGHT), - 0, wx.ALL | wx.ALIGN_RIGHT, 5) - textBox = wx.TextCtrl(self.panel, size=(size, -1), - name=name, id=i%cols) - 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/cols, i % cols + start_i), - flag=wx.ALL, border=2) - def create_ui(self): """Method to create the contents of the user interface""" # main box - robot description box @@ -118,12 +108,12 @@ def create_ui(self): self.panel, label=ui_labels.BOX_TITLES['robot_type'] ), wx.HORIZONTAL ) - szr_grd_robot_type = wx.GridBagSizer(12, 10) + 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=(125, -1), + self.panel, size=(150, -1), name=ui_labels.ROBOT_TYPE[key].name ) szr_grd_robot_type.Add( @@ -145,19 +135,13 @@ def create_ui(self): self.panel, label=ui_labels.BOX_TITLES['gravity'] ), wx.HORIZONTAL ) - for idx, key in enumerate(ui_labels.GRAVITY_CMPNTS): - label = ui_labels.GRAVITY_CMPNTS[key].label - name = ui_labels.GRAVITY_CMPNTS[key].name - txt_gravity = wx.TextCtrl( - self.panel, name=name, size=(60, -1) - ) - self.widgets[name] = txt_gravity - szr_gravity.AddSpacer(5) - txt_gravity.Bind(wx.EVT_KILL_FOCUS, self.OnBaseTwistChanged) - szr_gravity.Add( - wx.StaticText(self.panel, label=label), 0, wx.ALL, 4 - ) - szr_gravity.Add(txt_gravity, 0, wx.ALL | wx.ALIGN_LEFT, 2) + szr_grd_gravity = wx.GridBagSizer(5, 5) + self.params_in_grid( + szr_grd_gravity, elements=ui_labels.GRAVITY_CMPNTS, + rows=1, cols=3, handler=self.OnBaseTwistChanged, + elplace=[(0,0), (0,1), (0,2)], start=0, 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 @@ -207,87 +191,151 @@ def create_ui(self): self.panel, label=ui_labels.BOX_TITLES['geom_params'] ), wx.HORIZONTAL ) + cmb_frame = wx.ComboBox( + self.panel, size=(70, -1), style=wx.CB_READONLY, + name=ui_labels.GEOM_PARAMS['frame'].name + ) + cmb_frame.Bind(wx.EVT_COMBOBOX, self.OnFrameChanged) + self.widgets[ui_labels.GEOM_PARAMS['frame'].name] = cmb_frame + szr_frame = wx.BoxSizer(wx.HORIZONTAL) + szr_frame.Add( + wx.StaticText( + self.panel, style=wx.ALIGN_RIGHT, + label=ui_labels.GEOM_PARAMS['frame'].label, + ), proportion=0, flag=wx.ALL | wx.ALIGN_RIGHT, border=5 + ) + szr_frame.Add( + cmb_frame, proportion=0, + flag=wx.ALL | wx.ALIGN_LEFT, border=1 + ) szr_grd_geom = wx.GridBagSizer(0, 5) + szr_grd_geom.Add( + szr_frame, pos=(0, 0), flag=wx.ALL | wx.ALIGN_RIGHT, border=2 + ) + elements = OrderedDict(ui_labels.GEOM_PARAMS.items()[1:]) self.params_in_grid( - szr_grd_geom, elements=ui_labels.GEOM_PARAMS, + szr_grd_geom, elements=elements, rows=2, cols=5, handler=self.OnGeoParamChanged, elplace=[ - (0,0), (1,0), (0,1), (1,1), (0,2), (1,2), + (1,0), (0,1), (1,1), (0,2), (1,2), (0,3), (1,3), (0,4), (1,4) ], start=0, 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 - sbs = wx.StaticBoxSizer( + szr_dyn_params = wx.StaticBoxSizer( wx.StaticBox( self.panel, label=ui_labels.BOX_TITLES['dyn_params'] - ), wx.HORIZONTAL + ), 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, self.OnLinkChanged) + self.widgets['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 inertial params to the grid + self.params_in_grid( + szr_grd_dyn, elements=ui_labels.DYN_PARAMS_I, + rows=1, cols=6, handler=self.OnDynParamChanged, + elplace=[(0,0), (0,1), (0,2), (0,3), (0,4), (0,5)], + start=0, width=75 + ) + # add mass tensor params to the grid + self.params_in_grid( + szr_grd_dyn, elements=ui_labels.DYN_PARAMS_M, + rows=1, cols=4, handler=self.OnDynParamChanged, + elplace=[(1,0), (1,1), (1,2), (1,3)], start=0, width=75 + ) + # add friction params to the grid + self.params_in_grid( + szr_grd_dyn, elements=ui_labels.DYN_PARAMS_X, + rows=1, cols=3, handler=self.OnDynParamChanged, + elplace=[(2,0), (2,1), (2,2)], start=0, width=75 ) - grid = wx.GridBagSizer(0, 0) - ver_sizer = wx.BoxSizer(wx.VERTICAL) - ver_sizer.Add(wx.StaticText(self.panel, label='Link'), - 0, wx.ALL | wx.ALIGN_CENTER_HORIZONTAL, 5) - combo_box = wx.ComboBox(self.panel, 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, params, 4, self.OnDynParamChanged, 1) - sbs.Add(grid) - sbs.AddSpacer(4) + # add external force params to the grid + self.params_in_grid( + szr_grd_dyn, elements=ui_labels.DYN_PARAMS_F, + rows=1, cols=6, handler=self.OnDynParamChanged, + elplace=[(3,0), (3,1), (3,2), (3,3), (3,4), (3,5)], + start=0, 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) - szr_right_col.Add(sbs, 0, wx.ALL | wx.EXPAND, 0) + # box sizer for the last row in right col + szr_velacc = wx.BoxSizer(wx.HORIZONTAL) # right col - velocity and acceleration of the base box - sbs = wx.StaticBoxSizer( + szr_base_velacc = wx.StaticBoxSizer( wx.StaticBox( - self.panel, label=ui_labels.BOX_TITLES['vel_acc_base'] + self.panel, label=ui_labels.BOX_TITLES['base_vel_acc'] ), 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, params, 3, self.OnBaseTwistChanged, 0) + 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, handler=self.OnBaseTwistChanged, + elplace=[ + (0,0), (1,0), (2,0), (0,1), (1,1), (2,1), + (0,2), (1,2), (2,2), (0,3), (1,3), (2,3) + ], start=0, 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 - sbs2 = wx.StaticBoxSizer( + szr_joint_velacc = wx.StaticBoxSizer( wx.StaticBox( self.panel, label=ui_labels.BOX_TITLES['joint_vel_acc'] ), wx.HORIZONTAL ) - sbs2.AddSpacer(5) - grid = wx.GridBagSizer(5, 5) - sbs2.Add(grid) - combo_box = wx.ComboBox(self.panel, 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.panel, 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.panel, 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) - szr_right_col.AddSpacer(8) - szr_right_col.Add(hor_sizer, 1, wx.ALL | wx.EXPAND, 0) + cmb_joint = wx.ComboBox( + self.panel, size=(70, -1), style=wx.CB_READONLY, + name=ui_labels.JOINT_VEL_ACC['joint'].name + ) + cmb_joint.Bind(wx.EVT_COMBOBOX, self.OnJointChanged) + self.widgets[ui_labels.JOINT_VEL_ACC['joint'].name] = cmb_joint + szr_joint = wx.BoxSizer(wx.HORIZONTAL) + szr_joint.Add( + wx.StaticText( + self.panel, style=wx.ALIGN_RIGHT, + label=ui_labels.JOINT_VEL_ACC['joint'].label, + ), proportion=0, flag=wx.ALL | wx.ALIGN_RIGHT, border=5 + ) + szr_joint.Add( + cmb_joint, proportion=0, + flag=wx.ALL | wx.ALIGN_LEFT, border=1 + ) + szr_grd_joint_velacc = wx.GridBagSizer(5, 5) + szr_grd_joint_velacc.Add( + szr_joint, pos=(0, 0), flag=wx.ALL | wx.ALIGN_RIGHT, border=2 + ) + elements = OrderedDict(ui_labels.JOINT_VEL_ACC.items()[1:]) + self.params_in_grid( + szr_grd_joint_velacc, elements=elements, + rows=3, cols=1, handler=self.OnSpeedChanged, + elplace=[(1,0), (2,0), (3,0), (4,0)], start=0, 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) - #szr_right_col.Add(sbs, 0, wx.ALL | wx.EXPAND, 0) def Change(self, index, name, event_object): prev_value = str(self.robo.get_val(index, name)) @@ -353,7 +401,6 @@ 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) @@ -384,7 +431,6 @@ def feed_data(self): 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)]), @@ -394,13 +440,11 @@ def feed_data(self): 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 = {} @@ -541,6 +585,7 @@ def create_menu(self): iden_menu, wx.ID_ANY, ui_labels.IDEN_MENU['m_energy_iden_model'] ) + # TODO: uncomment the 3 lines below to add the event # self.Bind( # wx.EVT_MENU, self.OnEnergyIdentifModel, m_energy_iden_model # ) diff --git a/symoroui/labels.py b/symoroui/labels.py index d6216d9..5ec3c10 100644 --- a/symoroui/labels.py +++ b/symoroui/labels.py @@ -16,15 +16,12 @@ # main window - MAIN_WIN = dict( prog_name = "SYMORO", window_title = "SYMORO - SYmbolic MOdelling of RObots" ) - # interface contents - BOX_TITLES = dict( robot_des = "Robot Description", robot_type = "Robot Type", @@ -32,12 +29,73 @@ location = "Robot Location", geom_params = "Geometric Parameters", dyn_params = "Dynamic Parameters and External Forces", - vel_acc_base = "Velocity and Acceleration of the base", + base_vel_acc = "Velocity and Acceleration of the base", joint_vel_acc = "Joint Velocity and Acceleration" ) - +# named tuple to hold the content field entries FieldEntry = namedtuple('FieldEntry', ['label', 'name', 'control']) - +# joint velocity and acceleration params +JOINT_VEL_ACC = OrderedDict([ + ('joint', FieldEntry('Joint', 'joint', 'cmb')), + ('qp', FieldEntry('QP', 'QP', 'txt')), + ('qdp', FieldEntry('QDP', 'QDP', 'txt')), + ('gam', FieldEntry('GAM', 'GAM', 'txt')), +]) +# base velocity and acceleration params +BASE_VEL_ACC = OrderedDict([ + ('wx', FieldEntry('W0X', 'W0X', 'txt')), + ('wy', FieldEntry('W0Y', 'W0Y', 'txt')), + ('wz', FieldEntry('W0Z', 'W0Z', 'txt')), + ('wpx', FieldEntry('WP0X', 'WP0X', 'txt')), + ('wpy', FieldEntry('WP0Y', 'WP0Y', 'txt')), + ('wpz', FieldEntry('WP0Z', 'WP0Z', 'txt')), + ('vx', FieldEntry('V0X', 'V0X', 'txt')), + ('vy', FieldEntry('V0Y', 'V0Y', 'txt')), + ('vz', FieldEntry('V0Z', 'V0Z', 'txt')), + ('vpx', FieldEntry('VP0X', 'VP0X', 'txt')), + ('vpy', FieldEntry('VP0Y', 'VP0Y', 'txt')), + ('vpz', FieldEntry('VP0Z', 'VP0Z', 'txt')) +]) +# inertial params +DYN_PARAMS_I = OrderedDict([ + ('xx', FieldEntry('XX', 'XX', 'txt')), + ('xy', FieldEntry('XY', 'XY', 'txt')), + ('xz', FieldEntry('XZ', 'XZ', 'txt')), + ('yy', FieldEntry('YY', 'YY', 'txt')), + ('yz', FieldEntry('YZ', 'YZ', 'txt')), + ('zz', FieldEntry('ZZ', 'ZZ', 'txt')) +]) +# mass tensor params +DYN_PARAMS_M = OrderedDict([ + ('mx', FieldEntry('MX', 'MX', 'txt')), + ('my', FieldEntry('MY', 'MY', 'txt')), + ('mz', FieldEntry('MZ', 'MZ', 'txt')), + ('m', FieldEntry('M', 'M', 'txt')) +]) +# friction and rotor inertia params +DYN_PARAMS_X = OrderedDict([ + ('ia', FieldEntry('IA', 'IA', 'txt')), + ('fc', FieldEntry('FS', 'FS', 'txt')), + ('fv', FieldEntry('FV', 'FV', 'txt')) +]) +# external force, moments params +DYN_PARAMS_F = OrderedDict([ + ('ex_fx', FieldEntry('FX', 'FX', 'txt')), + ('ex_fy', FieldEntry('FY', 'FY', 'txt')), + ('ex_fz', FieldEntry('FZ', 'FZ', 'txt')), + ('ex_mx', FieldEntry('CX', 'CX', 'txt')), + ('ex_my', FieldEntry('CY', 'CY', 'txt')), + ('ex_mz', FieldEntry('CZ', 'CZ', 'txt')) +]) +# dynamic params got by concatenation +DYN_PARAMS = OrderedDict( + [('link', FieldEntry('Link', 'link', 'cmb'))] + \ + 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')), ('ant', FieldEntry('ant', 'ant', 'cmb')), @@ -50,13 +108,13 @@ ('theta', FieldEntry('theta', 'theta', 'txt')), ('r', FieldEntry('r', 'r', 'txt')) ]) - +# gravity component params GRAVITY_CMPNTS = OrderedDict([ ('gx', FieldEntry('GX', 'GX', 'txt')), ('gy', FieldEntry('GY', 'GY', 'txt')), ('gz', FieldEntry('GZ', 'GZ', 'txt')) ]) - +# robot type params ROBOT_TYPE = OrderedDict([ ('name', FieldEntry('Name of the robot:', 'name', 'lbl')), ('num_links', FieldEntry('Number of moving links:', 'NL', 'lbl')), @@ -68,7 +126,6 @@ ]) # menu bar - MAIN_MENU = dict( file_menu = "&File", geom_menu = "&Geometric", @@ -78,22 +135,18 @@ optim_menu = "&Optimiser", viz_menu = "&Visualisation" ) - VIZ_MENU = dict(m_viz = "Visualisation") - IDEN_MENU = dict( m_base_inertial_params = "Base Inertial parameters", m_dyn_iden_model = "Dynamic Identification Model", m_energy_iden_model = "Energy Identification Model" ) - DYN_MENU = dict( m_idym = "Inverse Dynamic Model", m_inertia_matrix = "Inertia matrix", m_h_term = "Centrifugal, Coriolis & Gravity torques", m_ddym = "Direct Dynamic Model" ) - KIN_MENU = dict( m_jac_matrix = "Jacobian matrix", m_determinant = "Determinant of a Jacobian", @@ -102,14 +155,12 @@ m_acc = "Accelerations", m_jpqp = "Jpqp" ) - GEOM_MENU = dict( m_trans_matrix = "Transformation matrix", m_fast_dgm = "Fast Geometric model", m_igm_paul = "IGM - Paul method", m_geom_constraint = "Geometric constraint equation of loops" ) - FILE_MENU = dict( m_new = "&New", m_open = "&Open", From 59663821e97ff69839462e7d49678ddca9990491 Mon Sep 17 00:00:00 2001 From: aravind Date: Tue, 25 Mar 2014 23:08:57 +0100 Subject: [PATCH 026/273] Move contents related to UI creation Move contents relation to UI creation from `bin/symoro-bin.py` to `symoroui/gui.py` (new file). --- bin/symoro-bin.py | 852 +-------------------------------------------- symoroui/gui.py | 860 ++++++++++++++++++++++++++++++++++++++++++++++ 2 files changed, 865 insertions(+), 847 deletions(-) create mode 100644 symoroui/gui.py diff --git a/bin/symoro-bin.py b/bin/symoro-bin.py index 54a88c5..84041ba 100644 --- a/bin/symoro-bin.py +++ b/bin/symoro-bin.py @@ -3,865 +3,23 @@ """ -This module creates the main window user interface and draws the -interface on the screen for the SYMORO package. +This is the main executable script of SYMORO. """ -from collections import OrderedDict +import os +import sys import wx -from pysymoro.symoro import Robot, FAIL -from pysymoro import geometry, kinematics, dynamics, invgeom -from pysymoro.parfile import readpar, writepar -from symoroviz import graphics -from symoroui import definition as ui_definition -from symoroui import geometry as ui_geometry -from symoroui import kinematics as ui_kinematics +from symoroui import gui from symoroui import labels as ui_labels -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() - # set default robot - self.robo = Robot.RX90() - # object to store different ui elements - self.widgets = {} - # 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() - # update fields with data - self.feed_data() - # set status bar to Ready - self.statusbar.SetStatusText("Ready") - - def params_in_grid( - self, szr_grd, elements, rows, cols, elplace, - handler=None, start=0, width=60): - """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 - if control is 'cmb': - ctrl = wx.ComboBox( - self.panel, style=wx.CB_READONLY, - size=(width, -1), name=name - ) - ctrl.Bind(wx.EVT_COMBOBOX, handler) - elif control is 'lbl': - ctrl = wx.StaticText( - self.panel, size=(width, -1), name=name - ) - else: - ctrl = wx.TextCtrl( - self.panel, size=(width, -1), name=name - ) - ctrl.Bind(wx.EVT_KILL_FOCUS, handler) - self.widgets[name] = 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=(elplace[idx][0], elplace[idx][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, handler=self.OnBaseTwistChanged, - elplace=[(0,0), (0,1), (0,2)], start=0, 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( - self.panel, name=name, size=(60, -1) - ) - self.widgets[name] = 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 - ) - cmb_frame = wx.ComboBox( - self.panel, size=(70, -1), style=wx.CB_READONLY, - name=ui_labels.GEOM_PARAMS['frame'].name - ) - cmb_frame.Bind(wx.EVT_COMBOBOX, self.OnFrameChanged) - self.widgets[ui_labels.GEOM_PARAMS['frame'].name] = cmb_frame - szr_frame = wx.BoxSizer(wx.HORIZONTAL) - szr_frame.Add( - wx.StaticText( - self.panel, style=wx.ALIGN_RIGHT, - label=ui_labels.GEOM_PARAMS['frame'].label, - ), proportion=0, flag=wx.ALL | wx.ALIGN_RIGHT, border=5 - ) - szr_frame.Add( - cmb_frame, proportion=0, - flag=wx.ALL | wx.ALIGN_LEFT, border=1 - ) - szr_grd_geom = wx.GridBagSizer(0, 5) - szr_grd_geom.Add( - szr_frame, pos=(0, 0), flag=wx.ALL | wx.ALIGN_RIGHT, border=2 - ) - elements = OrderedDict(ui_labels.GEOM_PARAMS.items()[1:]) - self.params_in_grid( - szr_grd_geom, elements=elements, - rows=2, cols=5, handler=self.OnGeoParamChanged, - elplace=[ - (1,0), (0,1), (1,1), (0,2), (1,2), - (0,3), (1,3), (0,4), (1,4) - ], start=0, 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, self.OnLinkChanged) - self.widgets['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 inertial params to the grid - self.params_in_grid( - szr_grd_dyn, elements=ui_labels.DYN_PARAMS_I, - rows=1, cols=6, handler=self.OnDynParamChanged, - elplace=[(0,0), (0,1), (0,2), (0,3), (0,4), (0,5)], - start=0, width=75 - ) - # add mass tensor params to the grid - self.params_in_grid( - szr_grd_dyn, elements=ui_labels.DYN_PARAMS_M, - rows=1, cols=4, handler=self.OnDynParamChanged, - elplace=[(1,0), (1,1), (1,2), (1,3)], start=0, width=75 - ) - # add friction params to the grid - self.params_in_grid( - szr_grd_dyn, elements=ui_labels.DYN_PARAMS_X, - rows=1, cols=3, handler=self.OnDynParamChanged, - elplace=[(2,0), (2,1), (2,2)], start=0, width=75 - ) - # add external force params to the grid - self.params_in_grid( - szr_grd_dyn, elements=ui_labels.DYN_PARAMS_F, - rows=1, cols=6, handler=self.OnDynParamChanged, - elplace=[(3,0), (3,1), (3,2), (3,3), (3,4), (3,5)], - start=0, 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, handler=self.OnBaseTwistChanged, - elplace=[ - (0,0), (1,0), (2,0), (0,1), (1,1), (2,1), - (0,2), (1,2), (2,2), (0,3), (1,3), (2,3) - ], start=0, 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_vel_acc'] - ), wx.HORIZONTAL - ) - cmb_joint = wx.ComboBox( - self.panel, size=(70, -1), style=wx.CB_READONLY, - name=ui_labels.JOINT_VEL_ACC['joint'].name - ) - cmb_joint.Bind(wx.EVT_COMBOBOX, self.OnJointChanged) - self.widgets[ui_labels.JOINT_VEL_ACC['joint'].name] = cmb_joint - szr_joint = wx.BoxSizer(wx.HORIZONTAL) - szr_joint.Add( - wx.StaticText( - self.panel, style=wx.ALIGN_RIGHT, - label=ui_labels.JOINT_VEL_ACC['joint'].label, - ), proportion=0, flag=wx.ALL | wx.ALIGN_RIGHT, border=5 - ) - szr_joint.Add( - cmb_joint, proportion=0, - flag=wx.ALL | wx.ALIGN_LEFT, border=1 - ) - szr_grd_joint_velacc = wx.GridBagSizer(5, 5) - szr_grd_joint_velacc.Add( - szr_joint, pos=(0, 0), flag=wx.ALL | wx.ALIGN_RIGHT, border=2 - ) - elements = OrderedDict(ui_labels.JOINT_VEL_ACC.items()[1:]) - self.params_in_grid( - szr_grd_joint_velacc, elements=elements, - rows=3, cols=1, handler=self.OnSpeedChanged, - elplace=[(1,0), (2,0), (3,0), (4,0)], start=0, 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) == 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) - # print type(self.robo.get_val(link_index, event.EventObject.Name)) - - def OnSpeedChanged(self, event): - joint_index = int(self.widgets['joint'].Value) - self.Change(joint_index, event.EventObject.Name, event.EventObject) - - 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_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_menu(self): - """Method to 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_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) - #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) - 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) - 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) - m_energy_iden_model = wx.MenuItem( - iden_menu, wx.ID_ANY, - ui_labels.IDEN_MENU['m_energy_iden_model'] - ) - # TODO: uncomment the 3 lines below to add the event -# 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 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_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( - ui_labels.MAIN_WIN['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( - ui_labels.MAIN_WIN['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( - ui_labels.MAIN_WIN['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( - ui_labels.MAIN_WIN['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( - ui_labels.MAIN_WIN['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( - 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(_) == FAIL: - return - elif result == wx.CANCEL: - return - self.Destroy() - - def main(): app = wx.App(redirect=False) style = wx.DEFAULT_FRAME_STYLE ^ wx.MAXIMIZE_BOX ^ wx.RESIZE_BORDER - frame = MainFrame( + frame = gui.MainFrame( parent=None, id=wx.ID_ANY, title=ui_labels.MAIN_WIN['window_title'], diff --git a/symoroui/gui.py b/symoroui/gui.py new file mode 100644 index 0000000..ac0ec0e --- /dev/null +++ b/symoroui/gui.py @@ -0,0 +1,860 @@ +#!/usr/bin/env python +# -*- coding: utf-8 -*- + + +""" +This module creates the main window user interface and draws the +interface on the screen for the SYMORO package. +""" + + +from collections import OrderedDict + +import wx + +from pysymoro.symoro import Robot, FAIL +from pysymoro import geometry, kinematics, dynamics, invgeom +from pysymoro.parfile import readpar, writepar +from symoroviz import graphics +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 + + +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() + # set default robot + self.robo = Robot.RX90() + # object to store different ui elements + self.widgets = {} + # 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() + # update fields with data + self.feed_data() + # set status bar to Ready + self.statusbar.SetStatusText("Ready") + + def params_in_grid( + self, szr_grd, elements, rows, cols, elplace, + handler=None, start=0, width=60): + """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 + if control is 'cmb': + ctrl = wx.ComboBox( + self.panel, style=wx.CB_READONLY, + size=(width, -1), name=name + ) + ctrl.Bind(wx.EVT_COMBOBOX, handler) + elif control is 'lbl': + ctrl = wx.StaticText( + self.panel, size=(width, -1), name=name + ) + else: + ctrl = wx.TextCtrl( + self.panel, size=(width, -1), name=name + ) + ctrl.Bind(wx.EVT_KILL_FOCUS, handler) + self.widgets[name] = 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=(elplace[idx][0], elplace[idx][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, handler=self.OnBaseTwistChanged, + elplace=[(0,0), (0,1), (0,2)], start=0, 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( + self.panel, name=name, size=(60, -1) + ) + self.widgets[name] = 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 + ) + cmb_frame = wx.ComboBox( + self.panel, size=(70, -1), style=wx.CB_READONLY, + name=ui_labels.GEOM_PARAMS['frame'].name + ) + cmb_frame.Bind(wx.EVT_COMBOBOX, self.OnFrameChanged) + self.widgets[ui_labels.GEOM_PARAMS['frame'].name] = cmb_frame + szr_frame = wx.BoxSizer(wx.HORIZONTAL) + szr_frame.Add( + wx.StaticText( + self.panel, style=wx.ALIGN_RIGHT, + label=ui_labels.GEOM_PARAMS['frame'].label, + ), proportion=0, flag=wx.ALL | wx.ALIGN_RIGHT, border=5 + ) + szr_frame.Add( + cmb_frame, proportion=0, + flag=wx.ALL | wx.ALIGN_LEFT, border=1 + ) + szr_grd_geom = wx.GridBagSizer(0, 5) + szr_grd_geom.Add( + szr_frame, pos=(0, 0), flag=wx.ALL | wx.ALIGN_RIGHT, border=2 + ) + elements = OrderedDict(ui_labels.GEOM_PARAMS.items()[1:]) + self.params_in_grid( + szr_grd_geom, elements=elements, + rows=2, cols=5, handler=self.OnGeoParamChanged, + elplace=[ + (1,0), (0,1), (1,1), (0,2), (1,2), + (0,3), (1,3), (0,4), (1,4) + ], start=0, 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, self.OnLinkChanged) + self.widgets['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 inertial params to the grid + self.params_in_grid( + szr_grd_dyn, elements=ui_labels.DYN_PARAMS_I, + rows=1, cols=6, handler=self.OnDynParamChanged, + elplace=[(0,0), (0,1), (0,2), (0,3), (0,4), (0,5)], + start=0, width=75 + ) + # add mass tensor params to the grid + self.params_in_grid( + szr_grd_dyn, elements=ui_labels.DYN_PARAMS_M, + rows=1, cols=4, handler=self.OnDynParamChanged, + elplace=[(1,0), (1,1), (1,2), (1,3)], start=0, width=75 + ) + # add friction params to the grid + self.params_in_grid( + szr_grd_dyn, elements=ui_labels.DYN_PARAMS_X, + rows=1, cols=3, handler=self.OnDynParamChanged, + elplace=[(2,0), (2,1), (2,2)], start=0, width=75 + ) + # add external force params to the grid + self.params_in_grid( + szr_grd_dyn, elements=ui_labels.DYN_PARAMS_F, + rows=1, cols=6, handler=self.OnDynParamChanged, + elplace=[(3,0), (3,1), (3,2), (3,3), (3,4), (3,5)], + start=0, 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, handler=self.OnBaseTwistChanged, + elplace=[ + (0,0), (1,0), (2,0), (0,1), (1,1), (2,1), + (0,2), (1,2), (2,2), (0,3), (1,3), (2,3) + ], start=0, 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_vel_acc'] + ), wx.HORIZONTAL + ) + cmb_joint = wx.ComboBox( + self.panel, size=(70, -1), style=wx.CB_READONLY, + name=ui_labels.JOINT_VEL_ACC['joint'].name + ) + cmb_joint.Bind(wx.EVT_COMBOBOX, self.OnJointChanged) + self.widgets[ui_labels.JOINT_VEL_ACC['joint'].name] = cmb_joint + szr_joint = wx.BoxSizer(wx.HORIZONTAL) + szr_joint.Add( + wx.StaticText( + self.panel, style=wx.ALIGN_RIGHT, + label=ui_labels.JOINT_VEL_ACC['joint'].label, + ), proportion=0, flag=wx.ALL | wx.ALIGN_RIGHT, border=5 + ) + szr_joint.Add( + cmb_joint, proportion=0, + flag=wx.ALL | wx.ALIGN_LEFT, border=1 + ) + szr_grd_joint_velacc = wx.GridBagSizer(5, 5) + szr_grd_joint_velacc.Add( + szr_joint, pos=(0, 0), flag=wx.ALL | wx.ALIGN_RIGHT, border=2 + ) + elements = OrderedDict(ui_labels.JOINT_VEL_ACC.items()[1:]) + self.params_in_grid( + szr_grd_joint_velacc, elements=elements, + rows=3, cols=1, handler=self.OnSpeedChanged, + elplace=[(1,0), (2,0), (3,0), (4,0)], start=0, 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) == 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) + # print type(self.robo.get_val(link_index, event.EventObject.Name)) + + def OnSpeedChanged(self, event): + joint_index = int(self.widgets['joint'].Value) + self.Change(joint_index, event.EventObject.Name, event.EventObject) + + 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_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_menu(self): + """Method to 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_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) + #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) + 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) + 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) + m_energy_iden_model = wx.MenuItem( + iden_menu, wx.ID_ANY, + ui_labels.IDEN_MENU['m_energy_iden_model'] + ) + # TODO: uncomment the 3 lines below to add the event +# 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 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_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( + ui_labels.MAIN_WIN['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( + ui_labels.MAIN_WIN['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( + ui_labels.MAIN_WIN['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( + ui_labels.MAIN_WIN['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( + ui_labels.MAIN_WIN['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( + 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(_) == FAIL: + return + elif result == wx.CANCEL: + return + self.Destroy() + + From 5ba14f908f15e131c307f142f3776c1b3d92137a Mon Sep 17 00:00:00 2001 From: aravind Date: Wed, 26 Mar 2014 17:02:59 +0100 Subject: [PATCH 027/273] Minor modifications Minor changes to `pysymoro`, `symoroui` and `symoroviz` code. --- pysymoro/dynamics.py | 9 +-- pysymoro/geometry.py | 10 +-- pysymoro/invgeom.py | 7 +- pysymoro/kinematics.py | 8 +- pysymoro/parfile.py | 4 +- symoroui/definition.py | 164 ++++++++++++++++++++-------------------- symoroui/geometry.py | 27 +++---- symoroui/gui.py | 7 +- symoroui/kinematics.py | 42 +++++----- symoroui/labels.py | 2 + symoroviz/graphics.py | 9 +-- symoroviz/objects.py | 5 +- symoroviz/primitives.py | 4 +- 13 files changed, 143 insertions(+), 155 deletions(-) diff --git a/pysymoro/dynamics.py b/pysymoro/dynamics.py index f49b46e..6e1926e 100644 --- a/pysymoro/dynamics.py +++ b/pysymoro/dynamics.py @@ -3,12 +3,7 @@ """ 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 +modeling of robot dynamics. """ @@ -806,3 +801,5 @@ def group_param_prism_spec(robo, symo, j, antRj, antPj): 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/geometry.py b/pysymoro/geometry.py index 4acc530..95995f4 100644 --- a/pysymoro/geometry.py +++ b/pysymoro/geometry.py @@ -2,13 +2,7 @@ # -*- coding: utf-8 -*- """ -This module of SYMORO package provides geometric models' computation. - -The core symbolic library is sympy. - -Needed modules: symoro.py - -ECN - ARIA1 2013 +This module of SYMORO package computes the geometric models. """ from sympy import Matrix, zeros, eye, sin, cos @@ -489,3 +483,5 @@ def direct_geometric(robo, frames, trig_subs): symo.write_line() symo.file_close() return symo + + diff --git a/pysymoro/invgeom.py b/pysymoro/invgeom.py index 47ee9b1..a69149f 100644 --- a/pysymoro/invgeom.py +++ b/pysymoro/invgeom.py @@ -4,11 +4,6 @@ """ 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 @@ -496,3 +491,5 @@ def _get_coefs(eq, A1, A2, *xs): # print eq, 'i', A1, 'i', A2 # print xs return X, Y, Z, is_ok + + diff --git a/pysymoro/kinematics.py b/pysymoro/kinematics.py index d70bcea..ecd08d8 100644 --- a/pysymoro/kinematics.py +++ b/pysymoro/kinematics.py @@ -2,13 +2,7 @@ # -*- coding: utf-8 -*- """ -This module of SYMORO package provides kinematic models' computation. - -The core symbolic library is sympy. - -Needed modules : symoro.py, geometry.py - -ECN - ARIA1 2013 +This module of SYMORO package computes the kinematic models. """ diff --git a/pysymoro/parfile.py b/pysymoro/parfile.py index dc5b353..2e9e1c8 100644 --- a/pysymoro/parfile.py +++ b/pysymoro/parfile.py @@ -1,6 +1,8 @@ #!/usr/bin/env python # -*- coding: utf-8 -*- + +import os import re from pysymoro import symoro @@ -99,7 +101,7 @@ def readpar(directory, robo_name): robo: an instance of Robot, read from file flag: indicates if any errors accured. (symoro.FAIL) """ - fname = '%s\\%s.par' % (directory, robo_name) + fname = os.path.join(directory, robo_name) with open(fname, 'r') as f: #initialize the Robot instance f.seek(0) diff --git a/symoroui/definition.py b/symoroui/definition.py index bff37d6..661d45e 100644 --- a/symoroui/definition.py +++ b/symoroui/definition.py @@ -1,7 +1,12 @@ #!/usr/bin/env python # -*- coding: utf-8 -*- -__author__ = 'Izzat' + +""" +This module creates the dialog box to define a new robot and +visualisation parameters. +""" + import wx import wx.lib.scrolledpanel as scrolled @@ -10,85 +15,9 @@ from pysymoro.symoro import SIMPLE, TREE, CLOSED_LOOP -#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): + """Creates the dialog box to define a new robot.""" 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) @@ -97,11 +26,9 @@ def __init__(self, prefix, name, nl, nj, structure, is_mobile, parent=None): 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), @@ -115,14 +42,12 @@ def init_ui(self, name, nl, nj, is_mobile, structure): 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), @@ -132,7 +57,6 @@ def init_ui(self, name, nl, nj, is_mobile, 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 @@ -158,7 +82,6 @@ def init_ui(self, name, nl, nj, is_mobile, structure): 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, _): @@ -190,3 +113,76 @@ def get_values(self): 'keep_dyn': self.ch_keep_dyn.Value, 'keep_base': self.ch_keep_base.Value} + +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 = 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) + + 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/symoroui/geometry.py b/symoroui/geometry.py index fe351e6..4a29403 100644 --- a/symoroui/geometry.py +++ b/symoroui/geometry.py @@ -1,11 +1,18 @@ #!/usr/bin/env python # -*- coding: utf-8 -*- -__author__ = 'Izzat' + +""" +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) @@ -17,7 +24,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') @@ -41,7 +47,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) @@ -52,11 +57,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) @@ -67,7 +70,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, _): @@ -94,6 +96,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) @@ -103,10 +106,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') @@ -129,14 +130,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, _): @@ -150,6 +149,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) @@ -159,7 +159,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) @@ -170,7 +169,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'] @@ -192,10 +190,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") @@ -205,7 +201,6 @@ 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, _): @@ -236,3 +231,5 @@ def get_values(self): else: lst.append(widget.LabelText) return lst, int(self.cmb.Value) + + diff --git a/symoroui/gui.py b/symoroui/gui.py index ac0ec0e..3ef69c5 100644 --- a/symoroui/gui.py +++ b/symoroui/gui.py @@ -359,7 +359,6 @@ def OnGeoParamChanged(self, event): def OnDynParamChanged(self, event): link_index = int(self.widgets['link'].Value) self.Change(link_index, event.EventObject.Name, event.EventObject) - # print type(self.robo.get_val(link_index, event.EventObject.Name)) def OnSpeedChanged(self, event): joint_index = int(self.widgets['joint'].Value) @@ -672,8 +671,8 @@ def message_info(self, message): ).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) + msg = 'The model has been saved in %s_%s.txt' % \ + (os.path.joint(self.robo.directory, self.robo.name), model_name) self.message_info(msg) def OnOpen(self, _): @@ -829,7 +828,7 @@ def OnDynIdentifModel(self, _): self.model_success('dim') def OnVisualisation(self, _): - dialog = ui_definition.DialogConversion( + dialog = ui_definition.DialogVisualisation( ui_labels.MAIN_WIN['prog_name'], self.robo, self.par_dict ) if dialog.has_syms(): diff --git a/symoroui/kinematics.py b/symoroui/kinematics.py index 6850fed..ef8c8f2 100644 --- a/symoroui/kinematics.py +++ b/symoroui/kinematics.py @@ -1,25 +1,33 @@ #!/usr/bin/env python # -*- coding: utf-8 -*- -__author__ = 'Izzat' + +""" +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 )') @@ -29,7 +37,6 @@ 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 )') @@ -38,7 +45,6 @@ def InitUI(self): 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), @@ -46,7 +52,6 @@ def InitUI(self): 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) - hor_sizer = wx.BoxSizer(wx.HORIZONTAL) ok_btn = wx.Button(self, wx.ID_OK, "OK") ok_btn.Bind(wx.EVT_BUTTON, self.OnOK) @@ -55,7 +60,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, _): @@ -76,21 +80,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), @@ -101,7 +108,6 @@ 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 )') @@ -112,7 +118,6 @@ def InitUI(self): 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) @@ -122,7 +127,6 @@ def InitUI(self): self.cmb_proj.SetSelection(0) grid.Add(self.cmb_proj, 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) @@ -130,7 +134,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:") @@ -150,7 +153,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) @@ -177,4 +179,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 index 5ec3c10..765e540 100644 --- a/symoroui/labels.py +++ b/symoroui/labels.py @@ -22,6 +22,7 @@ ) # interface contents +# TODO: add event handlers to dict entries as well BOX_TITLES = dict( robot_des = "Robot Description", robot_type = "Robot Type", @@ -126,6 +127,7 @@ ]) # menu bar +# TODO: add event handlers to dict entries as well MAIN_MENU = dict( file_menu = "&File", geom_menu = "&Geometric", diff --git a/symoroviz/graphics.py b/symoroviz/graphics.py index 36e8d12..7a394d1 100644 --- a/symoroviz/graphics.py +++ b/symoroviz/graphics.py @@ -1,19 +1,18 @@ #!/usr/bin/env python # -*- coding: utf-8 -*- -__author__ = 'Izzat' from math import atan2 -import OpenGL.GL as gl -import OpenGL.GLU as glu 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 sympy import Expr from pysymoro.symoro import Symoro, CLOSED_LOOP diff --git a/symoroviz/objects.py b/symoroviz/objects.py index fb03453..2185691 100644 --- a/symoroviz/objects.py +++ b/symoroviz/objects.py @@ -1,8 +1,9 @@ #!/usr/bin/env python # -*- coding: utf-8 -*- -__author__ = 'Izzat' + import OpenGL.GL as gL + from numpy import degrees, identity, array from primitives import Primitives @@ -234,3 +235,5 @@ 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/symoroviz/primitives.py b/symoroviz/primitives.py index 35624c4..abcbad6 100644 --- a/symoroviz/primitives.py +++ b/symoroviz/primitives.py @@ -1,9 +1,9 @@ #!/usr/bin/env python # -*- coding: utf-8 -*- -__author__ = 'Izzat' from itertools import product + from numpy import sin, cos, pi @@ -133,3 +133,5 @@ def arr_array(cls, length): vertices, indices, normals """ return create_arrow_array(length) + + From 43c6ee6bfa71f84512aaf6ca083f8d71e81bb658 Mon Sep 17 00:00:00 2001 From: aravind Date: Thu, 27 Mar 2014 01:35:23 +0100 Subject: [PATCH 028/273] Add handler and grid placement detail Add handler and grid placement details within the dicts in `symoroui/labels.py` and update `symoroui/gui.py` accordingly. --- symoroui/gui.py | 104 +++++++--------------------------------- symoroui/labels.py | 116 +++++++++++++++++++++++---------------------- 2 files changed, 76 insertions(+), 144 deletions(-) diff --git a/symoroui/gui.py b/symoroui/gui.py index 3ef69c5..64807c0 100644 --- a/symoroui/gui.py +++ b/symoroui/gui.py @@ -49,14 +49,14 @@ def __init__(self, *args, **kwargs): # set status bar to Ready self.statusbar.SetStatusText("Ready") - def params_in_grid( - self, szr_grd, elements, rows, cols, elplace, - handler=None, start=0, width=60): + 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) if control is 'cmb': ctrl = wx.ComboBox( self.panel, style=wx.CB_READONLY, @@ -84,7 +84,7 @@ def params_in_grid( flag=wx.ALL | wx.ALIGN_LEFT, border=1 ) szr_grd.Add( - szr_ele, pos=(elplace[idx][0], elplace[idx][1]), + szr_ele, pos=(place[0], place[1]), flag=wx.ALL | wx.ALIGN_RIGHT, border=2 ) @@ -138,8 +138,7 @@ def create_ui(self): szr_grd_gravity = wx.GridBagSizer(5, 5) self.params_in_grid( szr_grd_gravity, elements=ui_labels.GRAVITY_CMPNTS, - rows=1, cols=3, handler=self.OnBaseTwistChanged, - elplace=[(0,0), (0,1), (0,2)], start=0, width=70 + rows=1, cols=3, width=70, ) szr_gravity.Add(szr_grd_gravity) szr_left_col.Add(szr_gravity, 0, wx.ALL | wx.EXPAND, 0) @@ -191,35 +190,10 @@ def create_ui(self): self.panel, label=ui_labels.BOX_TITLES['geom_params'] ), wx.HORIZONTAL ) - cmb_frame = wx.ComboBox( - self.panel, size=(70, -1), style=wx.CB_READONLY, - name=ui_labels.GEOM_PARAMS['frame'].name - ) - cmb_frame.Bind(wx.EVT_COMBOBOX, self.OnFrameChanged) - self.widgets[ui_labels.GEOM_PARAMS['frame'].name] = cmb_frame - szr_frame = wx.BoxSizer(wx.HORIZONTAL) - szr_frame.Add( - wx.StaticText( - self.panel, style=wx.ALIGN_RIGHT, - label=ui_labels.GEOM_PARAMS['frame'].label, - ), proportion=0, flag=wx.ALL | wx.ALIGN_RIGHT, border=5 - ) - szr_frame.Add( - cmb_frame, proportion=0, - flag=wx.ALL | wx.ALIGN_LEFT, border=1 - ) szr_grd_geom = wx.GridBagSizer(0, 5) - szr_grd_geom.Add( - szr_frame, pos=(0, 0), flag=wx.ALL | wx.ALIGN_RIGHT, border=2 - ) - elements = OrderedDict(ui_labels.GEOM_PARAMS.items()[1:]) self.params_in_grid( - szr_grd_geom, elements=elements, - rows=2, cols=5, handler=self.OnGeoParamChanged, - elplace=[ - (1,0), (0,1), (1,1), (0,2), (1,2), - (0,3), (1,3), (0,4), (1,4) - ], start=0, width=70 + 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) @@ -234,7 +208,10 @@ def create_ui(self): self.panel, style=wx.CB_READONLY, size=(100, -1), name=ui_labels.DYN_PARAMS['link'].name ) - cmb_link.Bind(wx.EVT_COMBOBOX, self.OnLinkChanged) + cmb_link.Bind( + wx.EVT_COMBOBOX, + getattr(self, ui_labels.DYN_PARAMS['link'].handler) + ) self.widgets['link'] = cmb_link szr_link = wx.BoxSizer(wx.HORIZONTAL) szr_link.Add( @@ -247,31 +224,10 @@ def create_ui(self): 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 inertial params to the grid - self.params_in_grid( - szr_grd_dyn, elements=ui_labels.DYN_PARAMS_I, - rows=1, cols=6, handler=self.OnDynParamChanged, - elplace=[(0,0), (0,1), (0,2), (0,3), (0,4), (0,5)], - start=0, width=75 - ) - # add mass tensor params to the grid + # add dynamic params to the grid + elements = OrderedDict(ui_labels.DYN_PARAMS.items()[1:]) self.params_in_grid( - szr_grd_dyn, elements=ui_labels.DYN_PARAMS_M, - rows=1, cols=4, handler=self.OnDynParamChanged, - elplace=[(1,0), (1,1), (1,2), (1,3)], start=0, width=75 - ) - # add friction params to the grid - self.params_in_grid( - szr_grd_dyn, elements=ui_labels.DYN_PARAMS_X, - rows=1, cols=3, handler=self.OnDynParamChanged, - elplace=[(2,0), (2,1), (2,2)], start=0, width=75 - ) - # add external force params to the grid - self.params_in_grid( - szr_grd_dyn, elements=ui_labels.DYN_PARAMS_F, - rows=1, cols=6, handler=self.OnDynParamChanged, - elplace=[(3,0), (3,1), (3,2), (3,3), (3,4), (3,5)], - start=0, width=75 + szr_grd_dyn, elements=elements, rows=4, cols=6, width=75 ) szr_dyn_params.Add(szr_grd_dyn) szr_dyn_params.AddSpacer(4) @@ -288,11 +244,7 @@ def create_ui(self): 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, handler=self.OnBaseTwistChanged, - elplace=[ - (0,0), (1,0), (2,0), (0,1), (1,1), (2,1), - (0,2), (1,2), (2,2), (0,3), (1,3), (2,3) - ], start=0, width=60 + rows=3, cols=4, width=60 ) szr_base_velacc.Add(szr_grd_base_velacc) szr_velacc.Add(szr_base_velacc) @@ -303,32 +255,10 @@ def create_ui(self): self.panel, label=ui_labels.BOX_TITLES['joint_vel_acc'] ), wx.HORIZONTAL ) - cmb_joint = wx.ComboBox( - self.panel, size=(70, -1), style=wx.CB_READONLY, - name=ui_labels.JOINT_VEL_ACC['joint'].name - ) - cmb_joint.Bind(wx.EVT_COMBOBOX, self.OnJointChanged) - self.widgets[ui_labels.JOINT_VEL_ACC['joint'].name] = cmb_joint - szr_joint = wx.BoxSizer(wx.HORIZONTAL) - szr_joint.Add( - wx.StaticText( - self.panel, style=wx.ALIGN_RIGHT, - label=ui_labels.JOINT_VEL_ACC['joint'].label, - ), proportion=0, flag=wx.ALL | wx.ALIGN_RIGHT, border=5 - ) - szr_joint.Add( - cmb_joint, proportion=0, - flag=wx.ALL | wx.ALIGN_LEFT, border=1 - ) szr_grd_joint_velacc = wx.GridBagSizer(5, 5) - szr_grd_joint_velacc.Add( - szr_joint, pos=(0, 0), flag=wx.ALL | wx.ALIGN_RIGHT, border=2 - ) - elements = OrderedDict(ui_labels.JOINT_VEL_ACC.items()[1:]) self.params_in_grid( - szr_grd_joint_velacc, elements=elements, - rows=3, cols=1, handler=self.OnSpeedChanged, - elplace=[(1,0), (2,0), (3,0), (4,0)], start=0, width=75 + szr_grd_joint_velacc, elements=ui_labels.JOINT_VEL_ACC, + rows=3, cols=1, width=75, ) szr_joint_velacc.Add(szr_grd_joint_velacc, flag=wx.ALL | wx.ALIGN_CENTER, border=2 diff --git a/symoroui/labels.py b/symoroui/labels.py index 765e540..9d5b687 100644 --- a/symoroui/labels.py +++ b/symoroui/labels.py @@ -34,63 +34,65 @@ joint_vel_acc = "Joint Velocity and Acceleration" ) # named tuple to hold the content field entries -FieldEntry = namedtuple('FieldEntry', ['label', 'name', 'control']) +FieldEntry = namedtuple( + 'FieldEntry', ['label', 'name', 'control', 'place', 'handler'] +) # joint velocity and acceleration params JOINT_VEL_ACC = OrderedDict([ - ('joint', FieldEntry('Joint', 'joint', 'cmb')), - ('qp', FieldEntry('QP', 'QP', 'txt')), - ('qdp', FieldEntry('QDP', 'QDP', 'txt')), - ('gam', FieldEntry('GAM', 'GAM', 'txt')), + ('joint', FieldEntry('Joint', 'joint', 'cmb', (0,0), 'OnJointChanged')), + ('qp', FieldEntry('QP', 'QP', 'txt', (1,0), 'OnSpeedChanged')), + ('qdp', FieldEntry('QDP', 'QDP', 'txt', (2,0), 'OnSpeedChanged')), + ('gam', FieldEntry('GAM', 'GAM', 'txt', (3,0), 'OnSpeedChanged')) ]) # base velocity and acceleration params BASE_VEL_ACC = OrderedDict([ - ('wx', FieldEntry('W0X', 'W0X', 'txt')), - ('wy', FieldEntry('W0Y', 'W0Y', 'txt')), - ('wz', FieldEntry('W0Z', 'W0Z', 'txt')), - ('wpx', FieldEntry('WP0X', 'WP0X', 'txt')), - ('wpy', FieldEntry('WP0Y', 'WP0Y', 'txt')), - ('wpz', FieldEntry('WP0Z', 'WP0Z', 'txt')), - ('vx', FieldEntry('V0X', 'V0X', 'txt')), - ('vy', FieldEntry('V0Y', 'V0Y', 'txt')), - ('vz', FieldEntry('V0Z', 'V0Z', 'txt')), - ('vpx', FieldEntry('VP0X', 'VP0X', 'txt')), - ('vpy', FieldEntry('VP0Y', 'VP0Y', 'txt')), - ('vpz', FieldEntry('VP0Z', 'VP0Z', 'txt')) + ('wx', FieldEntry('W0X', 'W0X', 'txt', (0,0), 'OnBaseTwistChanged')), + ('wy', FieldEntry('W0Y', 'W0Y', 'txt', (1,0), 'OnBaseTwistChanged')), + ('wz', FieldEntry('W0Z', 'W0Z', 'txt', (2,0), 'OnBaseTwistChanged')), + ('wpx', FieldEntry('WP0X', 'WP0X', 'txt', (0,1), 'OnBaseTwistChanged')), + ('wpy', FieldEntry('WP0Y', 'WP0Y', 'txt', (1,1), 'OnBaseTwistChanged')), + ('wpz', FieldEntry('WP0Z', 'WP0Z', 'txt', (2,1), 'OnBaseTwistChanged')), + ('vx', FieldEntry('V0X', 'V0X', 'txt', (0,2), 'OnBaseTwistChanged')), + ('vy', FieldEntry('V0Y', 'V0Y', 'txt', (1,2), 'OnBaseTwistChanged')), + ('vz', FieldEntry('V0Z', 'V0Z', 'txt', (2,2), 'OnBaseTwistChanged')), + ('vpx', FieldEntry('VP0X', 'VP0X', 'txt', (0,3), 'OnBaseTwistChanged')), + ('vpy', FieldEntry('VP0Y', 'VP0Y', 'txt', (1,3), 'OnBaseTwistChanged')), + ('vpz', FieldEntry('VP0Z', 'VP0Z', 'txt', (2,3), 'OnBaseTwistChanged')) ]) # inertial params DYN_PARAMS_I = OrderedDict([ - ('xx', FieldEntry('XX', 'XX', 'txt')), - ('xy', FieldEntry('XY', 'XY', 'txt')), - ('xz', FieldEntry('XZ', 'XZ', 'txt')), - ('yy', FieldEntry('YY', 'YY', 'txt')), - ('yz', FieldEntry('YZ', 'YZ', 'txt')), - ('zz', FieldEntry('ZZ', 'ZZ', 'txt')) + ('xx', FieldEntry('XX', 'XX', 'txt', (0,0), 'OnDynParamChanged')), + ('xy', FieldEntry('XY', 'XY', 'txt', (0,1), 'OnDynParamChanged')), + ('xz', FieldEntry('XZ', 'XZ', 'txt', (0,2), 'OnDynParamChanged')), + ('yy', FieldEntry('YY', 'YY', 'txt', (0,3), 'OnDynParamChanged')), + ('yz', FieldEntry('YZ', 'YZ', 'txt', (0,4), 'OnDynParamChanged')), + ('zz', FieldEntry('ZZ', 'ZZ', 'txt', (0,5), 'OnDynParamChanged')) ]) # mass tensor params DYN_PARAMS_M = OrderedDict([ - ('mx', FieldEntry('MX', 'MX', 'txt')), - ('my', FieldEntry('MY', 'MY', 'txt')), - ('mz', FieldEntry('MZ', 'MZ', 'txt')), - ('m', FieldEntry('M', 'M', 'txt')) + ('mx', FieldEntry('MX', 'MX', 'txt', (1,0), 'OnDynParamChanged')), + ('my', FieldEntry('MY', 'MY', 'txt', (1,1), 'OnDynParamChanged')), + ('mz', FieldEntry('MZ', 'MZ', 'txt', (1,2), 'OnDynParamChanged')), + ('m', FieldEntry('M', 'M', 'txt', (1,3), 'OnDynParamChanged')) ]) # friction and rotor inertia params DYN_PARAMS_X = OrderedDict([ - ('ia', FieldEntry('IA', 'IA', 'txt')), - ('fc', FieldEntry('FS', 'FS', 'txt')), - ('fv', FieldEntry('FV', 'FV', 'txt')) + ('ia', FieldEntry('IA', 'IA', 'txt', (2,0), 'OnDynParamChanged')), + ('fc', FieldEntry('FS', 'FS', 'txt', (2,1), 'OnDynParamChanged')), + ('fv', FieldEntry('FV', 'FV', 'txt', (2,2), 'OnDynParamChanged')) ]) # external force, moments params DYN_PARAMS_F = OrderedDict([ - ('ex_fx', FieldEntry('FX', 'FX', 'txt')), - ('ex_fy', FieldEntry('FY', 'FY', 'txt')), - ('ex_fz', FieldEntry('FZ', 'FZ', 'txt')), - ('ex_mx', FieldEntry('CX', 'CX', 'txt')), - ('ex_my', FieldEntry('CY', 'CY', 'txt')), - ('ex_mz', FieldEntry('CZ', 'CZ', 'txt')) + ('ex_fx', FieldEntry('FX', 'FX', 'txt', (3,0), 'OnDynParamChanged')), + ('ex_fy', FieldEntry('FY', 'FY', 'txt', (3,1), 'OnDynParamChanged')), + ('ex_fz', FieldEntry('FZ', 'FZ', 'txt', (3,2), 'OnDynParamChanged')), + ('ex_mx', FieldEntry('CX', 'CX', 'txt', (3,3), 'OnDynParamChanged')), + ('ex_my', FieldEntry('CY', 'CY', 'txt', (3,4), 'OnDynParamChanged')), + ('ex_mz', FieldEntry('CZ', 'CZ', 'txt', (3,5), 'OnDynParamChanged')) ]) # dynamic params got by concatenation DYN_PARAMS = OrderedDict( - [('link', FieldEntry('Link', 'link', 'cmb'))] + \ + [('link', FieldEntry('Link', 'link', 'cmb', None, 'OnLinkChanged'))] + \ DYN_PARAMS_I.items() + \ DYN_PARAMS_M.items() + \ DYN_PARAMS_X.items() + \ @@ -98,32 +100,32 @@ ) # geometric params GEOM_PARAMS = OrderedDict([ - ('frame', FieldEntry('Frame', 'frame', 'cmb')), - ('ant', FieldEntry('ant', 'ant', 'cmb')), - ('sigma', FieldEntry('sigma', 'sigma', 'cmb')), - ('mu', FieldEntry('mu', 'mu', 'cmb')), - ('gamma', FieldEntry('gamma', 'gamma', 'txt')), - ('b', FieldEntry('b', 'b', 'txt')), - ('alpha', FieldEntry('alpha', 'alpha', 'txt')), - ('d', FieldEntry('d', 'd', 'txt')), - ('theta', FieldEntry('theta', 'theta', 'txt')), - ('r', FieldEntry('r', 'r', 'txt')) + ('frame', FieldEntry('Frame', 'frame', 'cmb', (0,0), 'OnFrameChanged')), + ('ant', FieldEntry('ant', 'ant', 'cmb', (1,0), 'OnGeoParamChanged')), + ('sigma', FieldEntry('sigma', 'sigma', 'cmb', (0,1), 'OnGeoParamChanged')), + ('mu', FieldEntry('mu', 'mu', 'cmb', (1,1), 'OnGeoParamChanged')), + ('gamma', FieldEntry('gamma', 'gamma', 'txt', (0,2), 'OnGeoParamChanged')), + ('b', FieldEntry('b', 'b', 'txt', (1,2), 'OnGeoParamChanged')), + ('alpha', FieldEntry('alpha', 'alpha', 'txt', (0,3), 'OnGeoParamChanged')), + ('d', FieldEntry('d', 'd', 'txt', (1,3), 'OnGeoParamChanged')), + ('theta', FieldEntry('theta', 'theta', 'txt', (0,4), 'OnGeoParamChanged')), + ('r', FieldEntry('r', 'r', 'txt', (1,4), 'OnGeoParamChanged')) ]) # gravity component params GRAVITY_CMPNTS = OrderedDict([ - ('gx', FieldEntry('GX', 'GX', 'txt')), - ('gy', FieldEntry('GY', 'GY', 'txt')), - ('gz', FieldEntry('GZ', 'GZ', 'txt')) + ('gx', FieldEntry('GX', 'GX', 'txt', (0,0), 'OnBaseTwistChanged')), + ('gy', FieldEntry('GY', 'GY', 'txt', (0,1), 'OnBaseTwistChanged')), + ('gz', FieldEntry('GZ', 'GZ', 'txt', (0,2), 'OnBaseTwistChanged')) ]) # robot type params ROBOT_TYPE = OrderedDict([ - ('name', FieldEntry('Name of the robot:', 'name', 'lbl')), - ('num_links', FieldEntry('Number of moving links:', 'NL', 'lbl')), - ('num_joints', FieldEntry('Number of joints:', 'NJ', 'lbl')), - ('num_frames', FieldEntry('Number of frames:', 'NF', 'lbl')), - ('structure', FieldEntry('Type of structure:', 'type', 'lbl')), - ('is_mobile', FieldEntry('Is Mobile:', 'mobile', 'lbl')), - ('num_loops', FieldEntry('Number of closed loops:', 'loops', 'lbl')) + ('name', FieldEntry('Name of the robot:', 'name', 'lbl', (0,0), None)), + ('num_links', FieldEntry('Number of moving links:', 'NL', 'lbl', (1,0), None)), + ('num_joints', FieldEntry('Number of joints:', 'NJ', 'lbl', (2,0), None)), + ('num_frames', FieldEntry('Number of frames:', 'NF', 'lbl', (3,0), None)), + ('structure', FieldEntry('Type of structure:', 'type', 'lbl', (4,0), None)), + ('is_mobile', FieldEntry('Is Mobile:', 'mobile', 'lbl', (5,0), None)), + ('num_loops', FieldEntry('Number of closed loops:', 'loops', 'lbl', (6,0), None)) ]) # menu bar From c784f1c89b7487f889589856a77e1c8b1a400464 Mon Sep 17 00:00:00 2001 From: aravind Date: Thu, 27 Mar 2014 16:06:30 +0100 Subject: [PATCH 029/273] Rename file Rename `symoroui/gui.py` to `symoroui/layout.py` and update accordingly. --- bin/symoro-bin.py | 4 ++-- symoroui/{gui.py => layout.py} | 0 2 files changed, 2 insertions(+), 2 deletions(-) rename symoroui/{gui.py => layout.py} (100%) diff --git a/bin/symoro-bin.py b/bin/symoro-bin.py index 84041ba..b2d0584 100644 --- a/bin/symoro-bin.py +++ b/bin/symoro-bin.py @@ -12,14 +12,14 @@ import wx -from symoroui import gui +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 = gui.MainFrame( + frame = layout.MainFrame( parent=None, id=wx.ID_ANY, title=ui_labels.MAIN_WIN['window_title'], diff --git a/symoroui/gui.py b/symoroui/layout.py similarity index 100% rename from symoroui/gui.py rename to symoroui/layout.py From efe2a733a03f0be0061915e09fc030d070518b81 Mon Sep 17 00:00:00 2001 From: aravind Date: Thu, 27 Mar 2014 16:15:44 +0100 Subject: [PATCH 030/273] Move `parfile` module Move `pysymoro/parfile.py` to `symoroutils/parfile.py` and update module importing accordingly. --- pysymoro/tests/test.py | 2 +- symoroui/layout.py | 8 ++++---- {pysymoro => symoroutils}/parfile.py | 6 ++++++ 3 files changed, 11 insertions(+), 5 deletions(-) rename {pysymoro => symoroutils}/parfile.py (97%) diff --git a/pysymoro/tests/test.py b/pysymoro/tests/test.py index 3275597..c190fce 100644 --- a/pysymoro/tests/test.py +++ b/pysymoro/tests/test.py @@ -17,7 +17,7 @@ from pysymoro import kinematics from pysymoro import invgeom from pysymoro import dynamics -from pysymoro import parfile +from symoroutils import parfile class testMisc(unittest.TestCase): diff --git a/symoroui/layout.py b/symoroui/layout.py index 64807c0..500e8ee 100644 --- a/symoroui/layout.py +++ b/symoroui/layout.py @@ -14,7 +14,7 @@ from pysymoro.symoro import Robot, FAIL from pysymoro import geometry, kinematics, dynamics, invgeom -from pysymoro.parfile import readpar, writepar +from symoroutils import parfile from symoroviz import graphics from symoroui import definition as ui_definition from symoroui import geometry as ui_geometry @@ -626,7 +626,7 @@ def OnOpen(self, _): defaultFile='*.par' ) if dialog.ShowModal() == wx.ID_OK: - new_robo, flag = readpar( + new_robo, flag = parfile.readpar( dialog.GetDirectory(), dialog.GetFilename()[:-4] ) @@ -641,7 +641,7 @@ def OnOpen(self, _): self.feed_data() def OnSave(self, _): - writepar(self.robo) + parfile.writepar(self.robo) self.changed = False def OnSaveAs(self, _): @@ -657,7 +657,7 @@ def OnSaveAs(self, _): self.robo.directory = dialog.GetDirectory() self.robo.name = dialog.GetFilename()[:-4] - writepar(self.robo) + parfile.writepar(self.robo) self.widgets['name'].SetLabel(self.robo.name) self.changed = False diff --git a/pysymoro/parfile.py b/symoroutils/parfile.py similarity index 97% rename from pysymoro/parfile.py rename to symoroutils/parfile.py index 2e9e1c8..f95a137 100644 --- a/pysymoro/parfile.py +++ b/symoroutils/parfile.py @@ -2,6 +2,12 @@ # -*- coding: utf-8 -*- +""" +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 d5c6ba05082234d8f6aa2ee3786c21b2ea7bb8a0 Mon Sep 17 00:00:00 2001 From: aravind Date: Fri, 28 Mar 2014 18:19:47 +0100 Subject: [PATCH 031/273] Create `symoroutils/filemgr.py` file Create `symoroutils/filemgr.py` file. Move file management functions to this new file. Update function calls in appropriate places. --- pysymoro/symoro.py | 20 +++++--------- pysymoro/tests/test.py | 7 +++-- symoroui/layout.py | 58 ++++++++++++++++++-------------------- symoroutils/filemgr.py | 63 ++++++++++++++++++++++++++++++++++++++++++ symoroutils/parfile.py | 10 +++---- 5 files changed, 106 insertions(+), 52 deletions(-) create mode 100644 symoroutils/filemgr.py diff --git a/pysymoro/symoro.py b/pysymoro/symoro.py index 36a5e3a..0e121d2 100644 --- a/pysymoro/symoro.py +++ b/pysymoro/symoro.py @@ -20,6 +20,9 @@ from sympy import Symbol, Matrix, Expr, Integer from sympy import Mul, Add, factor, zeros, var, sympify, eye +from symoroutils import filemgr + + ZERO = Integer(0) ONE = Integer(1) CLOSED_LOOP = 'Closed loop' @@ -749,7 +752,7 @@ def product_combinations(cls, v): def hat(v): - """Generates vectorial preproduct matrix + """skew-symmetry : Generates vectorial preproduct matrix Parameters ========== @@ -813,17 +816,6 @@ 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): """ """ @@ -1330,7 +1322,7 @@ def file_open(self, robo, ext): ext: string provides the file name extention """ - fname = make_fname(robo, ext) + fname = filemgr.make_file_path(robo, ext) self.file_out = open(fname, 'w') def file_close(self): @@ -1488,3 +1480,5 @@ def gen_func(self, name, to_return, args, multival=False): # print fun_string # TODO: print is for debug pupuses, to be removed return eval('%s_func' % name) + + diff --git a/pysymoro/tests/test.py b/pysymoro/tests/test.py index c190fce..afe3118 100644 --- a/pysymoro/tests/test.py +++ b/pysymoro/tests/test.py @@ -5,6 +5,7 @@ Unit tests for SYMORO modules """ + import unittest from sympy import sympify, var, Matrix @@ -25,9 +26,9 @@ 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) + fname = '%s.par' % original_robo.name + file_path = os.path.join(original_robo.directory, fname) + new_robo, flag = parfile.readpar(original_robo.name, file_path) self.assertEqual(flag, symoro.OK) l1 = original_robo.get_geom_head() l2 = original_robo.get_dynam_head() diff --git a/symoroui/layout.py b/symoroui/layout.py index 500e8ee..79a5051 100644 --- a/symoroui/layout.py +++ b/symoroui/layout.py @@ -15,6 +15,7 @@ from pysymoro.symoro import Robot, FAIL from pysymoro import geometry, kinematics, dynamics, invgeom from symoroutils import parfile +from symoroutils import filemgr from symoroviz import graphics from symoroui import definition as ui_definition from symoroui import geometry as ui_geometry @@ -569,10 +570,7 @@ def OnNew(self, event): 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.robo.directory = filemgr.get_folder_path(self.robo.name) self.feed_data() dialog.Destroy() @@ -601,11 +599,12 @@ def message_info(self, message): ).ShowModal() def model_success(self, model_name): - msg = 'The model has been saved in %s_%s.txt' % \ - (os.path.joint(self.robo.directory, self.robo.name), model_name) + msg = 'The model has been saved in %s_%s.txt' % ( + os.path.join(self.robo.directory, self.robo.name), model_name + ) self.message_info(msg) - def OnOpen(self, _): + def OnOpen(self, event): if self.changed: dialog_res = wx.MessageBox( 'Do you want to save changes?', @@ -627,8 +626,7 @@ def OnOpen(self, _): ) if dialog.ShowModal() == wx.ID_OK: new_robo, flag = parfile.readpar( - dialog.GetDirectory(), - dialog.GetFilename()[:-4] + dialog.GetFilename()[:-4], dialog.GetPath() ) if new_robo is None: self.message_error('File could not be read!') @@ -640,28 +638,26 @@ def OnOpen(self, _): self.robo = new_robo self.feed_data() - def OnSave(self, _): + def OnSave(self, event): parfile.writepar(self.robo) self.changed = False - def OnSaveAs(self, _): + def OnSaveAs(self, event): dialog = wx.FileDialog( - self, - message="Save PAR file", + 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] parfile.writepar(self.robo) self.widgets['name'].SetLabel(self.robo.name) self.changed = False - def OnTransformationMatrix(self, _): + def OnTransformationMatrix(self, event): dialog = ui_geometry.DialogTrans( ui_labels.MAIN_WIN['prog_name'], self.robo.NF ) @@ -671,7 +667,7 @@ def OnTransformationMatrix(self, _): self.model_success('trm') dialog.Destroy() - def OnFastGeometricModel(self, _): + def OnFastGeometricModel(self, event): dialog = ui_geometry.DialogFast( ui_labels.MAIN_WIN['prog_name'], self.robo.NF ) @@ -681,7 +677,7 @@ def OnFastGeometricModel(self, _): self.model_success('fgm') dialog.Destroy() - def OnIgmPaul(self, _): + def OnIgmPaul(self, event): dialog = ui_geometry.DialogPaul( ui_labels.MAIN_WIN['prog_name'], self.robo.endeffectors, @@ -693,10 +689,10 @@ def OnIgmPaul(self, _): self.model_success('igm') dialog.Destroy() - def OnConstraintGeoEq(self, _): + def OnConstraintGeoEq(self, event): pass - def OnJacobianMatrix(self, _): + def OnJacobianMatrix(self, event): dialog = ui_kinematics.DialogJacobian( ui_labels.MAIN_WIN['prog_name'], self.robo ) @@ -706,7 +702,7 @@ def OnJacobianMatrix(self, _): self.model_success('jac') dialog.Destroy() - def OnDeterminant(self, _): + def OnDeterminant(self, event): dialog = ui_kinematics.DialogDeterminant( ui_labels.MAIN_WIN['prog_name'], self.robo ) @@ -715,49 +711,49 @@ def OnDeterminant(self, _): self.model_success('det') dialog.Destroy() - def OnCkel(self, _): + def OnCkel(self, event): if kinematics.kinematic_constraints(self.robo) == FAIL: self.message_warning('There are no loops') else: self.model_success('ckel') - def OnVelocities(self, _): + def OnVelocities(self, event): kinematics.velocities(self.robo) self.model_success('vlct') - def OnAccelerations(self, _): + def OnAccelerations(self, event): kinematics.accelerations(self.robo) self.model_success('aclr') - def OnJpqp(self, _): + def OnJpqp(self, event): kinematics.jdot_qdot(self.robo) self.model_success('jpqp') - def OnInverseDynamic(self, _): + def OnInverseDynamic(self, event): dynamics.inverse_dynamic_NE(self.robo) self.model_success('idm') - def OnInertiaMatrix(self, _): + def OnInertiaMatrix(self, event): dynamics.inertia_matrix(self.robo) self.model_success('inm') - def OnCentrCoriolGravTorq(self, _): + def OnCentrCoriolGravTorq(self, event): dynamics.pseudo_force_NE(self.robo) self.model_success('ccg') - def OnDirectDynamicModel(self, _): + def OnDirectDynamicModel(self, event): dynamics.direct_dynamic_NE(self.robo) self.model_success('ddm') - def OnBaseInertialParams(self, _): + def OnBaseInertialParams(self, event): dynamics.base_paremeters(self.robo) self.model_success('regp') - def OnDynIdentifModel(self, _): + def OnDynIdentifModel(self, event): dynamics.dynamic_identification_NE(self.robo) self.model_success('dim') - def OnVisualisation(self, _): + def OnVisualisation(self, event): dialog = ui_definition.DialogVisualisation( ui_labels.MAIN_WIN['prog_name'], self.robo, self.par_dict ) diff --git a/symoroutils/filemgr.py b/symoroutils/filemgr.py new file mode 100644 index 0000000..974daf0 --- /dev/null +++ b/symoroutils/filemgr.py @@ -0,0 +1,63 @@ +#!/usr/bin/env python +# -*- coding: utf-8 -*- + + +""" +This module aids with file management operations for the SYMORO +package. +""" + + +import os + + +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. + + Returns: + A string specifying the folder path. + """ + folder_path = os.path.join('robots', robot_name) + make_folders(folder_path) + return folder_path + + +def make_file_path(robot, ext=None): + """ + Create the file path with the appropriate extension appended to + the file name using an underscore. + + Args: + robot: 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 = '%s.par' % robot.name + else: + fname = '%s_%s.txt' % (robot.name, ext) + file_path = os.path.join(robot.directory, fname) + make_folders(robot.directory) + return file_path + + diff --git a/symoroutils/parfile.py b/symoroutils/parfile.py index f95a137..fd369c6 100644 --- a/symoroutils/parfile.py +++ b/symoroutils/parfile.py @@ -11,6 +11,7 @@ import os import re +from symoroutils import filemgr from pysymoro import symoro @@ -71,7 +72,7 @@ def _write_par_list(robo, f, key, N0, N): def writepar(robo): - fname = symoro.make_fname(robo) + fname = filemgr.make_file_path(robo) with open(fname, 'w') as f: f.write('(* Robotname = \'%s\' *)\n' % robo.name) f.write('NL = %s\n' % robo.nl) @@ -102,13 +103,12 @@ def writepar(robo): f.write('\n(* End of definition *)\n') -def readpar(directory, robo_name): +def readpar(robo_name, file_path): """Return: robo: an instance of Robot, read from file flag: indicates if any errors accured. (symoro.FAIL) """ - fname = os.path.join(directory, robo_name) - with open(fname, 'r') as f: + with open(file_path, 'r') as f: #initialize the Robot instance f.seek(0) d = {} @@ -127,7 +127,7 @@ def readpar(directory, robo_name): 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 + robo.directory = os.path.dirname(file_path) #fitting the data acc_line = '' key = '' From 4e5044d0140e10f2ae3a9eb46283985e46f55b76 Mon Sep 17 00:00:00 2001 From: aravind Date: Thu, 3 Apr 2014 04:41:11 +0200 Subject: [PATCH 032/273] Modify base folder path for file storage Modify base folder path for file storage such that it is now located in the HOME folder of the OS. Also indicate this base folder path in the status bar of the user interface. --- symoroui/layout.py | 5 ++++- symoroutils/filemgr.py | 21 ++++++++++++++++----- 2 files changed, 20 insertions(+), 6 deletions(-) diff --git a/symoroui/layout.py b/symoroui/layout.py index 79a5051..beafb20 100644 --- a/symoroui/layout.py +++ b/symoroui/layout.py @@ -48,7 +48,10 @@ def __init__(self, *args, **kwargs): # update fields with data self.feed_data() # set status bar to Ready - self.statusbar.SetStatusText("Ready") + self.statusbar.SetStatusText( + "Ready. The location of robot files is %s" + % filemgr.get_base_path() + ) def params_in_grid(self, szr_grd, elements, rows, cols, width=70): """Method to display a set of fields in a grid.""" diff --git a/symoroutils/filemgr.py b/symoroutils/filemgr.py index 974daf0..a8fedf3 100644 --- a/symoroutils/filemgr.py +++ b/symoroutils/filemgr.py @@ -2,15 +2,26 @@ # -*- coding: utf-8 -*- -""" -This module aids with file management operations for the SYMORO -package. -""" +"""Perform file management operations for the SYMORO package.""" import os +SYMORO_ROBOTS_FOLDER = "symoro-robots" + + +def get_base_path(): + """ + 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, SYMORO_ROBOTS_FOLDER) + + def make_folders(folder_path): """ Check if a specified folder path exists and create the folder path @@ -34,7 +45,7 @@ def get_folder_path(robot_name): Returns: A string specifying the folder path. """ - folder_path = os.path.join('robots', robot_name) + folder_path = os.path.join(get_base_path(), robot_name) make_folders(folder_path) return folder_path From 9d0ea6690a4e9b0418c5b06533d8a1a16fef7de4 Mon Sep 17 00:00:00 2001 From: aravind Date: Thu, 3 Apr 2014 05:51:46 +0200 Subject: [PATCH 033/273] Modify status bar to have two fields --- symoroui/layout.py | 9 ++++++--- 1 file changed, 6 insertions(+), 3 deletions(-) diff --git a/symoroui/layout.py b/symoroui/layout.py index beafb20..93cbea5 100644 --- a/symoroui/layout.py +++ b/symoroui/layout.py @@ -47,10 +47,13 @@ def __init__(self, *args, **kwargs): self.Fit() # update fields with data self.feed_data() - # set status bar to Ready + # configure status bar + self.statusbar.SetFieldsCount(number=2) + self.statusbar.SetStatusWidths(widths=[-1, -1]) + self.statusbar.SetStatusText(text="Ready", number=0) self.statusbar.SetStatusText( - "Ready. The location of robot files is %s" - % filemgr.get_base_path() + text="Location of robot files is %s" + % filemgr.get_base_path(), number = 1 ) def params_in_grid(self, szr_grd, elements, rows, cols, width=70): From af48e19727f8d3a3af30f6f2a47886e1200cc931 Mon Sep 17 00:00:00 2001 From: aravind Date: Thu, 3 Apr 2014 10:09:10 +0200 Subject: [PATCH 034/273] Remove `robots` folder Remove `robots` folder as it is now a separate repo. --- robots/AKR3000/AKR3000.par | 57 -- robots/RX90/RX90.par | 57 -- robots/RX90/RX90_ccg.txt | 260 --------- robots/RX90/RX90_ddm.txt | 937 --------------------------------- robots/RX90/RX90_dim.txt | 237 --------- robots/RX90/RX90_idm.txt | 273 ---------- robots/RX90/RX90_inm.txt | 320 ----------- robots/RX90/RX90_jac.txt | 63 --- robots/RX90/RX90_regp.txt | 102 ---- robots/SR400/SR400.par | 57 -- robots/Stanford/Stanford.par | 57 -- robots/Stanford4/Stanford4.par | 57 -- robots/TwoLoops/TwoLoops.par | 57 -- robots/testLoop/testLoop.par | 57 -- 14 files changed, 2591 deletions(-) delete mode 100644 robots/AKR3000/AKR3000.par delete mode 100644 robots/RX90/RX90.par delete mode 100644 robots/RX90/RX90_ccg.txt delete mode 100644 robots/RX90/RX90_ddm.txt delete mode 100644 robots/RX90/RX90_dim.txt delete mode 100644 robots/RX90/RX90_idm.txt delete mode 100644 robots/RX90/RX90_inm.txt delete mode 100644 robots/RX90/RX90_jac.txt delete mode 100644 robots/RX90/RX90_regp.txt delete mode 100644 robots/SR400/SR400.par delete mode 100644 robots/Stanford/Stanford.par delete mode 100644 robots/Stanford4/Stanford4.par delete mode 100644 robots/TwoLoops/TwoLoops.par delete mode 100644 robots/testLoop/testLoop.par 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 *) From b55dce8e1ebc96b06c42d17bd516686eecae8b40 Mon Sep 17 00:00:00 2001 From: aravind Date: Thu, 3 Apr 2014 10:10:01 +0200 Subject: [PATCH 035/273] Add new function `get_clean_name()` Add new function `get_clean_name()` in `symoroutils/filemgr.py`. This function cleans up the robot name before it is used as folder/file names. Fix minor issues in other two files. --- pysymoro/symoro.py | 2 +- symoroui/layout.py | 1 + symoroutils/filemgr.py | 22 ++++++++++++++++++++-- 3 files changed, 22 insertions(+), 3 deletions(-) diff --git a/pysymoro/symoro.py b/pysymoro/symoro.py index 0e121d2..e19d44f 100644 --- a/pysymoro/symoro.py +++ b/pysymoro/symoro.py @@ -47,7 +47,7 @@ def __init__(self, name, NL=0, NJ=0, NF=0, is_mobile=False, # member variables: self.name = name """ name of the robot: string""" - self.directory = os.path.join('robots', name) + self.directory = filemgr.get_folder_path(name) """ directory name""" self.is_mobile = is_mobile """ whethere the base frame is floating: bool""" diff --git a/symoroui/layout.py b/symoroui/layout.py index 93cbea5..82eb0b0 100644 --- a/symoroui/layout.py +++ b/symoroui/layout.py @@ -8,6 +8,7 @@ """ +import os from collections import OrderedDict import wx diff --git a/symoroutils/filemgr.py b/symoroutils/filemgr.py index a8fedf3..261b782 100644 --- a/symoroutils/filemgr.py +++ b/symoroutils/filemgr.py @@ -22,6 +22,23 @@ def get_base_path(): return os.path.join(home_folder, SYMORO_ROBOTS_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. + """ + return name.lower().replace(' ', char) + + def make_folders(folder_path): """ Check if a specified folder path exists and create the folder path @@ -45,6 +62,7 @@ def get_folder_path(robot_name): 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 @@ -64,9 +82,9 @@ def make_file_path(robot, ext=None): The file path (string) created. """ if ext is None: - fname = '%s.par' % robot.name + fname = '%s.par' % get_clean_name(robot.name) else: - fname = '%s_%s.txt' % (robot.name, ext) + fname = '%s_%s.txt' % (get_clean_name(robot.name), ext) file_path = os.path.join(robot.directory, fname) make_folders(robot.directory) return file_path From 22fa7bf7645fa2b04c30150228a1c595a954c559 Mon Sep 17 00:00:00 2001 From: aravind Date: Fri, 4 Apr 2014 15:58:22 +0200 Subject: [PATCH 036/273] Update docstrings in `symoroutils/filemgr.py` --- symoroutils/filemgr.py | 21 +++++++++++++++++++-- 1 file changed, 19 insertions(+), 2 deletions(-) diff --git a/symoroutils/filemgr.py b/symoroutils/filemgr.py index 261b782..5c73283 100644 --- a/symoroutils/filemgr.py +++ b/symoroutils/filemgr.py @@ -1,4 +1,3 @@ -#!/usr/bin/env python # -*- coding: utf-8 -*- @@ -17,6 +16,9 @@ def get_base_path(): Returns: A string specifying the base folder path. + + >>> get_base_path() + '/home/aravind/symoro-robots' """ home_folder = os.path.expanduser("~") return os.path.join(home_folder, SYMORO_ROBOTS_FOLDER) @@ -35,6 +37,11 @@ def get_clean_name(name, char='-'): 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) @@ -57,10 +64,15 @@ def get_folder_path(robot_name): folders if they are not already present. Args: - robot_name: The name of the robot. + robot_name: The name of the robot (string). Returns: A string specifying the folder path. + + >>> get_folder_path('RX 90') + '/home/aravind/symoro-robots/rx-90' + >>> get_folder_path('RX90') + '/home/aravind/symoro-robots/rx90' """ robot_name = get_clean_name(robot_name) folder_path = os.path.join(get_base_path(), robot_name) @@ -90,3 +102,8 @@ def make_file_path(robot, ext=None): return file_path +if __name__ == "__main__": + import doctest + doctest.testmod() + + From b3427bcb314423ac001d26bb516abcd5efe4f476 Mon Sep 17 00:00:00 2001 From: aravind Date: Mon, 7 Apr 2014 15:20:44 +0200 Subject: [PATCH 037/273] Add `pysymoro/manipulator.py` --- pysymoro/manipulator.py | 0 1 file changed, 0 insertions(+), 0 deletions(-) create mode 100644 pysymoro/manipulator.py diff --git a/pysymoro/manipulator.py b/pysymoro/manipulator.py new file mode 100644 index 0000000..e69de29 From d77d9c9a6852399d48e9a869c6e1507df7b04648 Mon Sep 17 00:00:00 2001 From: aravind Date: Wed, 9 Apr 2014 17:21:27 +0200 Subject: [PATCH 038/273] Fix event KILL_FOCUS bug Fix event KILL_FOCUS bug that was caused when some text fields lost focus. This was because the ID value of the textbox control was used to access individual elements from Matrix (read as list). Since these ID values were assigned using the default wxPython method they were mostly negative values that was causing an `IndexError`. The fix basically assigns custom ID values (mostly 0, 1, 2) to those textbox controls that require it and `-1` to those that don't depend on ID values for any functionality. These ID values were set for each field in the `symoroui/labels.py` file. --- pysymoro/symoro.py | 1 - symoroui/labels.py | 114 ++++++++++++++++++++++----------------------- symoroui/layout.py | 13 ++++-- 3 files changed, 65 insertions(+), 63 deletions(-) diff --git a/pysymoro/symoro.py b/pysymoro/symoro.py index e19d44f..66b89f7 100644 --- a/pysymoro/symoro.py +++ b/pysymoro/symoro.py @@ -1,4 +1,3 @@ -#!/usr/bin/env python # -*- coding: utf-8 -*- """ diff --git a/symoroui/labels.py b/symoroui/labels.py index 9d5b687..d762cd5 100644 --- a/symoroui/labels.py +++ b/symoroui/labels.py @@ -35,64 +35,64 @@ ) # named tuple to hold the content field entries FieldEntry = namedtuple( - 'FieldEntry', ['label', 'name', 'control', 'place', 'handler'] + 'FieldEntry', ['label', 'name', 'control', 'place', 'handler', 'id'] ) # joint velocity and acceleration params JOINT_VEL_ACC = OrderedDict([ - ('joint', FieldEntry('Joint', 'joint', 'cmb', (0,0), 'OnJointChanged')), - ('qp', FieldEntry('QP', 'QP', 'txt', (1,0), 'OnSpeedChanged')), - ('qdp', FieldEntry('QDP', 'QDP', 'txt', (2,0), 'OnSpeedChanged')), - ('gam', FieldEntry('GAM', 'GAM', 'txt', (3,0), 'OnSpeedChanged')) + ('joint', FieldEntry('Joint', 'joint', 'cmb', (0,0), 'OnJointChanged', -1)), + ('qp', FieldEntry('QP', 'QP', 'txt', (1,0), 'OnSpeedChanged', -1)), + ('qdp', FieldEntry('QDP', 'QDP', 'txt', (2,0), 'OnSpeedChanged', -1)), + ('gam', FieldEntry('GAM', 'GAM', 'txt', (3,0), 'OnSpeedChanged', -1)) ]) # base velocity and acceleration params BASE_VEL_ACC = OrderedDict([ - ('wx', FieldEntry('W0X', 'W0X', 'txt', (0,0), 'OnBaseTwistChanged')), - ('wy', FieldEntry('W0Y', 'W0Y', 'txt', (1,0), 'OnBaseTwistChanged')), - ('wz', FieldEntry('W0Z', 'W0Z', 'txt', (2,0), 'OnBaseTwistChanged')), - ('wpx', FieldEntry('WP0X', 'WP0X', 'txt', (0,1), 'OnBaseTwistChanged')), - ('wpy', FieldEntry('WP0Y', 'WP0Y', 'txt', (1,1), 'OnBaseTwistChanged')), - ('wpz', FieldEntry('WP0Z', 'WP0Z', 'txt', (2,1), 'OnBaseTwistChanged')), - ('vx', FieldEntry('V0X', 'V0X', 'txt', (0,2), 'OnBaseTwistChanged')), - ('vy', FieldEntry('V0Y', 'V0Y', 'txt', (1,2), 'OnBaseTwistChanged')), - ('vz', FieldEntry('V0Z', 'V0Z', 'txt', (2,2), 'OnBaseTwistChanged')), - ('vpx', FieldEntry('VP0X', 'VP0X', 'txt', (0,3), 'OnBaseTwistChanged')), - ('vpy', FieldEntry('VP0Y', 'VP0Y', 'txt', (1,3), 'OnBaseTwistChanged')), - ('vpz', FieldEntry('VP0Z', 'VP0Z', 'txt', (2,3), 'OnBaseTwistChanged')) + ('wx', FieldEntry('W0X', 'W0X', 'txt', (0,0), 'OnBaseTwistChanged', 0)), + ('wy', FieldEntry('W0Y', 'W0Y', 'txt', (1,0), 'OnBaseTwistChanged', 1)), + ('wz', FieldEntry('W0Z', 'W0Z', 'txt', (2,0), 'OnBaseTwistChanged', 2)), + ('wpx', FieldEntry('WP0X', 'WP0X', 'txt', (0,1), 'OnBaseTwistChanged', 0)), + ('wpy', FieldEntry('WP0Y', 'WP0Y', 'txt', (1,1), 'OnBaseTwistChanged', 1)), + ('wpz', FieldEntry('WP0Z', 'WP0Z', 'txt', (2,1), 'OnBaseTwistChanged', 2)), + ('vx', FieldEntry('V0X', 'V0X', 'txt', (0,2), 'OnBaseTwistChanged', 0)), + ('vy', FieldEntry('V0Y', 'V0Y', 'txt', (1,2), 'OnBaseTwistChanged', 1)), + ('vz', FieldEntry('V0Z', 'V0Z', 'txt', (2,2), 'OnBaseTwistChanged', 2)), + ('vpx', FieldEntry('VP0X', 'VP0X', 'txt', (0,3), 'OnBaseTwistChanged', 0)), + ('vpy', FieldEntry('VP0Y', 'VP0Y', 'txt', (1,3), 'OnBaseTwistChanged', 1)), + ('vpz', FieldEntry('VP0Z', 'VP0Z', 'txt', (2,3), 'OnBaseTwistChanged', 2)) ]) # inertial params DYN_PARAMS_I = OrderedDict([ - ('xx', FieldEntry('XX', 'XX', 'txt', (0,0), 'OnDynParamChanged')), - ('xy', FieldEntry('XY', 'XY', 'txt', (0,1), 'OnDynParamChanged')), - ('xz', FieldEntry('XZ', 'XZ', 'txt', (0,2), 'OnDynParamChanged')), - ('yy', FieldEntry('YY', 'YY', 'txt', (0,3), 'OnDynParamChanged')), - ('yz', FieldEntry('YZ', 'YZ', 'txt', (0,4), 'OnDynParamChanged')), - ('zz', FieldEntry('ZZ', 'ZZ', 'txt', (0,5), 'OnDynParamChanged')) + ('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([ - ('mx', FieldEntry('MX', 'MX', 'txt', (1,0), 'OnDynParamChanged')), - ('my', FieldEntry('MY', 'MY', 'txt', (1,1), 'OnDynParamChanged')), - ('mz', FieldEntry('MZ', 'MZ', 'txt', (1,2), 'OnDynParamChanged')), - ('m', FieldEntry('M', 'M', 'txt', (1,3), 'OnDynParamChanged')) + ('mx', FieldEntry('MX', 'MX', 'txt', (1,0), 'OnDynParamChanged', -1)), + ('my', FieldEntry('MY', 'MY', 'txt', (1,1), 'OnDynParamChanged', -1)), + ('mz', FieldEntry('MZ', 'MZ', 'txt', (1,2), 'OnDynParamChanged', -1)), + ('m', FieldEntry('M', 'M', 'txt', (1,3), 'OnDynParamChanged', -1)) ]) # friction and rotor inertia params DYN_PARAMS_X = OrderedDict([ - ('ia', FieldEntry('IA', 'IA', 'txt', (2,0), 'OnDynParamChanged')), - ('fc', FieldEntry('FS', 'FS', 'txt', (2,1), 'OnDynParamChanged')), - ('fv', FieldEntry('FV', 'FV', 'txt', (2,2), 'OnDynParamChanged')) + ('ia', FieldEntry('IA', 'IA', 'txt', (2,0), 'OnDynParamChanged', -1)), + ('fc', FieldEntry('FS', 'FS', 'txt', (2,1), 'OnDynParamChanged', -1)), + ('fv', FieldEntry('FV', 'FV', 'txt', (2,2), 'OnDynParamChanged', -1)) ]) # external force, moments params DYN_PARAMS_F = OrderedDict([ - ('ex_fx', FieldEntry('FX', 'FX', 'txt', (3,0), 'OnDynParamChanged')), - ('ex_fy', FieldEntry('FY', 'FY', 'txt', (3,1), 'OnDynParamChanged')), - ('ex_fz', FieldEntry('FZ', 'FZ', 'txt', (3,2), 'OnDynParamChanged')), - ('ex_mx', FieldEntry('CX', 'CX', 'txt', (3,3), 'OnDynParamChanged')), - ('ex_my', FieldEntry('CY', 'CY', 'txt', (3,4), 'OnDynParamChanged')), - ('ex_mz', FieldEntry('CZ', 'CZ', 'txt', (3,5), 'OnDynParamChanged')) + ('ex_fx', FieldEntry('FX', 'FX', 'txt', (3,0), 'OnDynParamChanged', -1)), + ('ex_fy', FieldEntry('FY', 'FY', 'txt', (3,1), 'OnDynParamChanged', -1)), + ('ex_fz', FieldEntry('FZ', 'FZ', 'txt', (3,2), 'OnDynParamChanged', -1)), + ('ex_mx', FieldEntry('CX', 'CX', 'txt', (3,3), 'OnDynParamChanged', -1)), + ('ex_my', FieldEntry('CY', 'CY', 'txt', (3,4), 'OnDynParamChanged', -1)), + ('ex_mz', FieldEntry('CZ', 'CZ', 'txt', (3,5), 'OnDynParamChanged', -1)) ]) # dynamic params got by concatenation DYN_PARAMS = OrderedDict( - [('link', FieldEntry('Link', 'link', 'cmb', None, 'OnLinkChanged'))] + \ + [('link', FieldEntry('Link', 'link', 'cmb', None, 'OnLinkChanged', -1))] + \ DYN_PARAMS_I.items() + \ DYN_PARAMS_M.items() + \ DYN_PARAMS_X.items() + \ @@ -100,32 +100,32 @@ ) # geometric params GEOM_PARAMS = OrderedDict([ - ('frame', FieldEntry('Frame', 'frame', 'cmb', (0,0), 'OnFrameChanged')), - ('ant', FieldEntry('ant', 'ant', 'cmb', (1,0), 'OnGeoParamChanged')), - ('sigma', FieldEntry('sigma', 'sigma', 'cmb', (0,1), 'OnGeoParamChanged')), - ('mu', FieldEntry('mu', 'mu', 'cmb', (1,1), 'OnGeoParamChanged')), - ('gamma', FieldEntry('gamma', 'gamma', 'txt', (0,2), 'OnGeoParamChanged')), - ('b', FieldEntry('b', 'b', 'txt', (1,2), 'OnGeoParamChanged')), - ('alpha', FieldEntry('alpha', 'alpha', 'txt', (0,3), 'OnGeoParamChanged')), - ('d', FieldEntry('d', 'd', 'txt', (1,3), 'OnGeoParamChanged')), - ('theta', FieldEntry('theta', 'theta', 'txt', (0,4), 'OnGeoParamChanged')), - ('r', FieldEntry('r', 'r', 'txt', (1,4), 'OnGeoParamChanged')) + ('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')), - ('gy', FieldEntry('GY', 'GY', 'txt', (0,1), 'OnBaseTwistChanged')), - ('gz', FieldEntry('GZ', 'GZ', 'txt', (0,2), 'OnBaseTwistChanged')) + ('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)), - ('num_links', FieldEntry('Number of moving links:', 'NL', 'lbl', (1,0), None)), - ('num_joints', FieldEntry('Number of joints:', 'NJ', 'lbl', (2,0), None)), - ('num_frames', FieldEntry('Number of frames:', 'NF', 'lbl', (3,0), None)), - ('structure', FieldEntry('Type of structure:', 'type', 'lbl', (4,0), None)), - ('is_mobile', FieldEntry('Is Mobile:', 'mobile', 'lbl', (5,0), None)), - ('num_loops', FieldEntry('Number of closed loops:', 'loops', 'lbl', (6,0), None)) + ('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_mobile', FieldEntry('Is Mobile:', 'mobile', 'lbl', (5,0), None, -1)), + ('num_loops', FieldEntry('Number of closed loops:', 'loops', 'lbl', (6,0), None, -1)) ]) # menu bar diff --git a/symoroui/layout.py b/symoroui/layout.py index 82eb0b0..d4661d3 100644 --- a/symoroui/layout.py +++ b/symoroui/layout.py @@ -65,19 +65,21 @@ def params_in_grid(self, szr_grd, elements, rows, cols, width=70): 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( - self.panel, style=wx.CB_READONLY, + 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( - self.panel, size=(width, -1), name=name + parent=self.panel, size=(width, -1), name=name ) else: ctrl = wx.TextCtrl( - self.panel, size=(width, -1), name=name + parent=self.panel, size=(width, -1), + name=name, id=field_id ) ctrl.Bind(wx.EVT_KILL_FOCUS, handler) self.widgets[name] = ctrl @@ -161,10 +163,11 @@ def create_ui(self): szr_grd_loc = wx.GridBagSizer(1, 1) for i in range(4): for j in range(3): - idx = j*4+i + idx = (j*4) + i name = 'Z'+str(idx) txt_z_element = wx.TextCtrl( - self.panel, name=name, size=(60, -1) + parent=self.panel, name=name, + id=idx, size=(60, -1) ) self.widgets[name] = txt_z_element txt_z_element.Bind( From 447df013a2967e261d27cedc383091c2598fb7b6 Mon Sep 17 00:00:00 2001 From: aravind Date: Thu, 10 Apr 2014 15:04:21 +0200 Subject: [PATCH 039/273] Add `symoroutils/symbolmgr.py` file --- pysymoro/manipulator.py | 5 +++++ symoroutils/symbolmgr.py | 10 ++++++++++ 2 files changed, 15 insertions(+) create mode 100644 symoroutils/symbolmgr.py diff --git a/pysymoro/manipulator.py b/pysymoro/manipulator.py index e69de29..3039a03 100644 --- a/pysymoro/manipulator.py +++ b/pysymoro/manipulator.py @@ -0,0 +1,5 @@ +# -*- coding: utf-8 -*- + + +"""This module contains the Robot data structure""" + diff --git a/symoroutils/symbolmgr.py b/symoroutils/symbolmgr.py new file mode 100644 index 0000000..d3aba82 --- /dev/null +++ b/symoroutils/symbolmgr.py @@ -0,0 +1,10 @@ +# -*- coding: utf-8 -*- + + +"""This module contains the Symbol Manager tools.""" + + +import os + + + From acb98ec632fd6a77ac9b2c80c87bf7c43c96504d Mon Sep 17 00:00:00 2001 From: aravind Date: Thu, 10 Apr 2014 15:35:18 +0200 Subject: [PATCH 040/273] Update `Symoro` with `SymbolManager` Rename `Symoro` class to `SymbolManager` and place it in `symoroutils/symbolmgr.py`. Update in all places to call `SymbolManager` instead of `Symoro`. --- pysymoro/dynamics.py | 19 +- pysymoro/geometry.py | 15 +- pysymoro/invgeom.py | 5 +- pysymoro/kinematics.py | 24 +- pysymoro/symoro.py | 559 --------------------------------------- pysymoro/tests/test.py | 7 +- symoroutils/symbolmgr.py | 558 ++++++++++++++++++++++++++++++++++++++ symoroviz/graphics.py | 7 +- 8 files changed, 600 insertions(+), 594 deletions(-) diff --git a/pysymoro/dynamics.py b/pysymoro/dynamics.py index 6e1926e..0eb1a52 100644 --- a/pysymoro/dynamics.py +++ b/pysymoro/dynamics.py @@ -11,11 +11,12 @@ from sympy import Matrix from copy import copy, deepcopy -from pysymoro.symoro import Symoro, Init, hat, ZERO +from pysymoro.symoro import Init, hat, ZERO from pysymoro.geometry import compute_screw_transform from pysymoro.geometry import compute_rot_trans, Transform from pysymoro.kinematics import compute_vel_acc from pysymoro.kinematics import compute_omega +from symoroutils import symbolmgr chars = 'ABCDEFGHJKLMNPQRSTUVWXYZ' @@ -31,7 +32,7 @@ def Newton_Euler(robo, symo): ========== robo : Robot Instance of robot description container - symo : Symoro + symo : symbolmgr.SymbolManager Instance of symbolic manager """ # init external forces @@ -74,7 +75,7 @@ def dynamic_identification_NE(robo): Fjnt = Init.init_vec(robo) Njnt = Init.init_vec(robo) # init file output, writing the robot description - symo = Symoro() + symo = symbolmgr.SymbolManager() symo.file_open(robo, 'dim') title = "Dynamic identification model using Newton - Euler Algorith" symo.write_params_table(robo, title, inert=True, dynam=True) @@ -152,7 +153,7 @@ def direct_dynamic_NE(robo): Tau = Init.init_scalar(robo) grandVp = Init.init_vec(robo, 6) grandVp.append(Matrix([robo.vdot0 - robo.G, robo.w0])) - symo = Symoro() + symo = symbolmgr.SymbolManager() symo.file_open(robo, 'ddm') title = 'Direct dynamic model using Newton - Euler Algorith' symo.write_params_table(robo, title, inert=True, dynam=True) @@ -203,7 +204,7 @@ def inertia_matrix(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 = symbolmgr.SymbolManager() symo.file_open(robo, 'inm') title = 'Inertia Matrix using composite links' symo.write_params_table(robo, title, inert=True, dynam=True) @@ -243,7 +244,7 @@ def inverse_dynamic_NE(robo): symo.sydi : dictionary Dictionary with the information of all the sybstitution """ - symo = Symoro() + symo = symbolmgr.SymbolManager() symo.file_open(robo, 'idm') title = 'Inverse dynamic model using Newton - Euler Algorith' symo.write_params_table(robo, title, inert=True, dynam=True) @@ -268,7 +269,7 @@ def pseudo_force_NE(robo): """ robo_pseudo = deepcopy(robo) robo_pseudo.qddot = sympy.zeros(robo_pseudo.NL, 1) - symo = Symoro() + symo = symbolmgr.SymbolManager() symo.file_open(robo, 'ccg') title = 'Pseudo forces using Newton - Euler Algorith' symo.write_params_table(robo, title, inert=True, dynam=True) @@ -330,7 +331,7 @@ def compute_joint_torque_deriv(symo, param, arg, index): Parameters ========== - symo : Symoro + symo : symbolmgr.SymbolManager symbol manager param : var Dynamic parameter @@ -547,7 +548,7 @@ def base_paremeters(robo_orig): """ robo = copy(robo_orig) lam = [0 for i in xrange(robo.NL)] - symo = Symoro() + symo = symbolmgr.SymbolManager() symo.file_open(robo, 'regp') title = 'Base parameters computation' symo.write_params_table(robo, title, inert=True, dynam=True) diff --git a/pysymoro/geometry.py b/pysymoro/geometry.py index 95995f4..5ece55d 100644 --- a/pysymoro/geometry.py +++ b/pysymoro/geometry.py @@ -7,7 +7,8 @@ from sympy import Matrix, zeros, eye, sin, cos -from pysymoro.symoro import Symoro, Init, hat +from pysymoro.symoro import Init, hat +from symoroutils import symbolmgr Z_AXIS = Matrix([0, 0, 1]) @@ -303,8 +304,8 @@ def dgm(robo, symo, i, j, key='one', fast_form=True, forced=False, Parameters ========== - symo: Symoro - Instance of Symoro. All the substitutions will + symo: symbolmgr.SymbolManager + Instance of symbolmgr.SymbolManager. All the substitutions will be put into symo.sydi i: int To-frame index. @@ -444,10 +445,10 @@ def direct_geometric_fast(robo, i, j): Returns ======= - symo: Symoro + symo: symbolmgr.SymbolManager Instance that contains all the relations of the computed model """ - symo = Symoro() + symo = symbolmgr.SymbolManager() symo.file_open(robo, 'fgm') symo.write_params_table(robo, 'Direct Geometrix model') dgm(robo, symo, i, j, fast_form=True, forced=True) @@ -470,10 +471,10 @@ def direct_geometric(robo, frames, trig_subs): Returns ======= - symo: Symoro + symo: symbolmgr.SymbolManager Instance that contains all the relations of the computed model """ - symo = Symoro() + symo = symbolmgr.SymbolManager() symo.file_open(robo, 'trm') symo.write_params_table(robo, 'Direct Geometrix model') for i, j in frames: diff --git a/pysymoro/invgeom.py b/pysymoro/invgeom.py index a69149f..e64da5b 100644 --- a/pysymoro/invgeom.py +++ b/pysymoro/invgeom.py @@ -10,8 +10,9 @@ from sympy import var, sin, cos, eye, atan2, sqrt, pi from sympy import Matrix, Symbol, Expr -from pysymoro.symoro import Symoro, ZERO, ONE, get_max_coef +from pysymoro.symoro import ZERO, ONE, get_max_coef from pysymoro.geometry import dgm +from symoroutils import symbolmgr EMPTY = var("EMPTY") @@ -123,7 +124,7 @@ def loop_solve(robo, symo, knowns=None): 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) _paul_solve(robo, symo, T_ref, 0, n) diff --git a/pysymoro/kinematics.py b/pysymoro/kinematics.py index ecd08d8..25bbdeb 100644 --- a/pysymoro/kinematics.py +++ b/pysymoro/kinematics.py @@ -8,10 +8,11 @@ from sympy import Matrix, zeros -from pysymoro.symoro import Symoro, Init, hat +from pysymoro.symoro import Init, hat from pysymoro.symoro import FAIL, ZERO from pysymoro.geometry import dgm, Transform from pysymoro.geometry import compute_rot_trans, Z_AXIS +from symoroutils import symbolmgr TERMINAL = 0 @@ -207,7 +208,7 @@ def compute_vel_acc(robo, symo, antRj, antPj, forced=False, gravity=True): ========== robo : Robot Instance of robot description container - symo : Symoro + symo : symbolmgr.SymbolManager Instance of symbolic manager """ #init velocities and accelerations @@ -230,7 +231,7 @@ def compute_vel_acc(robo, symo, antRj, antPj, forced=False, gravity=True): 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) @@ -248,7 +249,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) @@ -259,7 +260,7 @@ def accelerations(robo): #very simial to comute_vel_acc def jdot_qdot(robo): - symo = Symoro(None) + symo = symbolmgr.SymbolManager(None) symo.file_open(robo, 'jpqp') symo.write_params_table(robo, 'JdotQdot') antRj, antPj = compute_rot_trans(robo, symo) @@ -282,7 +283,7 @@ 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" @@ -293,7 +294,7 @@ def jacobian(robo, n, i, j): 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): @@ -307,7 +308,7 @@ 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 @@ -333,8 +334,9 @@ def kinematic_constraints(robo): return symo -#symo = Symoro() -#from symoro import Symoro, Robot +#symo = symbolmgr.SymbolManager() +#from symoro import Robot +#from symoroutils import symbolmgr #kinematic_constraints(Robot.SR400()) ##jacobian_determinant(robo, 6, range(6), range(6)) ###print _jac(robo, symo, 2, 5, 5) @@ -346,7 +348,7 @@ def kinematic_constraints(robo): ###print _jac_inv(Robot.RX90(), symo, 2, 5, 5) ## #def b(): -# symo = Symoro() +# symo = symbolmgr.SymbolManager() # print _jac_inv(Robot.RX90(), symo, 6, 3, 3) ####from timeit import timeit #####print timeit(a, number=10) diff --git a/pysymoro/symoro.py b/pysymoro/symoro.py index 66b89f7..fc14372 100644 --- a/pysymoro/symoro.py +++ b/pysymoro/symoro.py @@ -922,562 +922,3 @@ def trigonometric_info(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 = filemgr.make_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_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/tests/test.py b/pysymoro/tests/test.py index afe3118..661ee4d 100644 --- a/pysymoro/tests/test.py +++ b/pysymoro/tests/test.py @@ -19,6 +19,7 @@ from pysymoro import invgeom from pysymoro import dynamics from symoroutils import parfile +from symoroutils import symbolmgr class testMisc(unittest.TestCase): @@ -71,7 +72,7 @@ def test_robo_misc(self): class testSymoroTrig(unittest.TestCase): def setUp(self): - self.symo = symoro.Symoro() + self.symo = symbolmgr.SymbolManager() def test_GetMaxCoef(self): print "######## test_GetMaxCoef ##########" @@ -186,7 +187,7 @@ def test_trig_simp(self): class testGeometry(unittest.TestCase): def setUp(self): - self.symo = symoro.Symoro() + self.symo = symbolmgr.SymbolManager() self.robo = symoro.Robot.RX90() # def test_misc(self): @@ -277,7 +278,7 @@ def test_loop(self): class testKinematics(unittest.TestCase): def setUp(self): - self.symo = symoro.Symoro() + self.symo = symbolmgr.SymbolManager() self.robo = symoro.Robot.RX90() def test_speeds(self): diff --git a/symoroutils/symbolmgr.py b/symoroutils/symbolmgr.py index d3aba82..d635764 100644 --- a/symoroutils/symbolmgr.py +++ b/symoroutils/symbolmgr.py @@ -7,4 +7,562 @@ import os +class SymbolManager: + """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 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 = 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 = filemgr.make_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_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/symoroviz/graphics.py b/symoroviz/graphics.py index 7a394d1..a5785af 100644 --- a/symoroviz/graphics.py +++ b/symoroviz/graphics.py @@ -15,9 +15,10 @@ from sympy import Expr -from pysymoro.symoro import Symoro, CLOSED_LOOP +from pysymoro.symoro import CLOSED_LOOP from pysymoro.invgeom import loop_solve from pysymoro.geometry import dgm +from symoroutils import symbolmgr from objects import Frame, RevoluteJoint, FixedJoint, PrismaticJoint @@ -167,7 +168,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] @@ -210,7 +211,7 @@ def solve(self): 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) From 5394492504ba385fa379d17560ab809114e36178 Mon Sep 17 00:00:00 2001 From: aravind Date: Fri, 11 Apr 2014 16:55:31 +0200 Subject: [PATCH 041/273] Add `symoroutils/tools.py` Move functions from `pysymoro/symoro.py` to `symoroutils/tools.py`. These functions are helper functions that are called by other modules. --- symoroutils/tools.py | 198 +++++++++++++++++++++++++++++++++++++++++++ 1 file changed, 198 insertions(+) create mode 100644 symoroutils/tools.py diff --git a/symoroutils/tools.py b/symoroutils/tools.py new file mode 100644 index 0000000..8767cd1 --- /dev/null +++ b/symoroutils/tools.py @@ -0,0 +1,198 @@ +# -*- coding: utf-8 -*- + + +""" +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(v): + """skew-symmetry : 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 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): + 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 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 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 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 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 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 + + From 3874d131e816b71544c1d5ae1d9c90f43fae75a4 Mon Sep 17 00:00:00 2001 From: aravind Date: Fri, 11 Apr 2014 16:57:02 +0200 Subject: [PATCH 042/273] Fix `NameError` from function calls Fix `NameError` arising from different function calls in `symoroutils/symbolmgr.py` and `symoroutils/tools.py`. --- pysymoro/dynamics.py | 35 +++---- pysymoro/geometry.py | 5 +- pysymoro/invgeom.py | 9 +- pysymoro/kinematics.py | 13 +-- pysymoro/symoro.py | 188 +------------------------------------ pysymoro/tests/test.py | 29 +++--- symoroui/labels.py | 194 +++++++++++++++++++-------------------- symoroutils/filemgr.py | 6 +- symoroutils/symbolmgr.py | 61 ++++++------ 9 files changed, 184 insertions(+), 356 deletions(-) diff --git a/pysymoro/dynamics.py b/pysymoro/dynamics.py index 0eb1a52..5a4a84a 100644 --- a/pysymoro/dynamics.py +++ b/pysymoro/dynamics.py @@ -11,12 +11,13 @@ from sympy import Matrix from copy import copy, deepcopy -from pysymoro.symoro import Init, hat, ZERO +from pysymoro.symoro import Init, ZERO from pysymoro.geometry import compute_screw_transform from pysymoro.geometry import compute_rot_trans, Transform from pysymoro.kinematics import compute_vel_acc from pysymoro.kinematics import compute_omega from symoroutils import symbolmgr +from symoroutils import tools chars = 'ABCDEFGHJKLMNPQRSTUVWXYZ' @@ -289,7 +290,7 @@ def compute_wrench(robo, symo, j, w, wdot, U, vdot, F, N): 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 + N[j] = robo.J[j]*wdot[j] + tools.skew(w[j])*Psi symo.mat_replace(N[j], 'No', j) @@ -303,12 +304,12 @@ def compute_joint_wrench(robo, symo, j, antRj, antPj, vdot, 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] + Njnt[j] = N[j] + Nex[j] + tools.skew(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 + Nex[robo.ant[j]] += antRj[j]*Njnt[j] + tools.skew(antPj[j])*f_ant def compute_torque(robo, symo, j, Fjnt, Njnt, name='GAM'): @@ -322,7 +323,7 @@ def compute_torque(robo, symo, j, Fjnt, Njnt, name='GAM'): def inertia_spatial(J, MS, M): - return Matrix([(M*sympy.eye(3)).row_join(hat(MS).T), hat(MS).row_join(J)]) + return Matrix([(M*sympy.eye(3)).row_join(tools.skew(MS).T), tools.skew(MS).row_join(J)]) def compute_joint_torque_deriv(symo, param, arg, index): @@ -354,9 +355,9 @@ def compute_beta(robo, symo, j, w, beta_star): 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) + E2 = symo.mat_replace(tools.skew(w[j])*E1, 'KW', j) + E3 = tools.skew(w[j])*robo.MS[j] + E4 = symo.mat_replace(tools.skew(w[j])*E3, 'SW', j) E5 = -robo.Nex[j] - E2 E6 = -robo.Fex[j] - E4 beta_star[j] = Matrix([E6, E5]) @@ -370,12 +371,12 @@ def compute_link_acc(robo, symo, j, antRj, antPj, link_acc, w, wi): ===== link_acc is the output parameter """ - E1 = symo.mat_replace(hat(wi[j])*Matrix([0, 0, robo.qdot[j]]), + E1 = symo.mat_replace(tools.skew(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 + E4 = tools.skew(w[robo.ant[j]])*antPj[j] + E5 = tools.skew(w[robo.ant[j]])*E4 E6 = antRj[j].T*E5 E7 = symo.mat_replace(E6 + E3, 'LW', j) link_acc[j] = Matrix([E7, E2]) @@ -474,12 +475,12 @@ def compute_Jplus(robo, symo, j, antRj, antPj, Jplus, MSplus, Mplus, AJE1): ===== Jplus, MSplus, Mplus are the output parameters """ - hat_antPj = hat(antPj[j]) + hat_antPj = tools.skew(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) + E3 = symo.mat_replace(hat_antPj*tools.skew(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] @@ -515,9 +516,9 @@ def compute_A_triangle(robo, symo, j, k, ka, antRj, antPj, f, n, A, AJE1): """ f[ka] = antRj[k]*f[k] if k == j and robo.sigma[j] == 0: - n[ka] = AJE1[k] + hat(antPj[k])*f[k] + n[ka] = AJE1[k] + tools.skew(antPj[k])*f[k] else: - n[ka] = antRj[k]*n[k] + hat(antPj[k])*f[k] + n[ka] = antRj[k]*n[k] + tools.skew(antPj[k])*f[k] if ka == - 1: symo.mat_replace(f[ka], 'AV0') symo.mat_replace(n[ka], 'AW0') @@ -606,7 +607,7 @@ def vec_mut_MS(v, P): Returns : Matrix 6x1 """ - U = - hat(v)*hat(P) + 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]]) @@ -621,7 +622,7 @@ def vec_mut_M(P): Returns : Matrix 6x1 """ - U = -hat(P)*hat(P) + 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]]) diff --git a/pysymoro/geometry.py b/pysymoro/geometry.py index 5ece55d..0dda983 100644 --- a/pysymoro/geometry.py +++ b/pysymoro/geometry.py @@ -7,8 +7,9 @@ from sympy import Matrix, zeros, eye, sin, cos -from pysymoro.symoro import Init, hat +from pysymoro.symoro import Init from symoroutils import symbolmgr +from symoroutils import tools Z_AXIS = Matrix([0, 0, 1]) @@ -182,7 +183,7 @@ def compute_screw_transform(robo, symo, j, antRj, antPj, jTant): jTant is an output parameter """ jRant = antRj[j].T - ET = symo.mat_replace(-jRant*hat(antPj[j]), 'JPR', j) + ET = symo.mat_replace(-jRant*tools.skew(antPj[j]), 'JPR', j) jTant[j] = (Matrix([jRant.row_join(ET), zeros(3, 3).row_join(jRant)])) diff --git a/pysymoro/invgeom.py b/pysymoro/invgeom.py index e64da5b..eb5fefc 100644 --- a/pysymoro/invgeom.py +++ b/pysymoro/invgeom.py @@ -10,9 +10,10 @@ from sympy import var, sin, cos, eye, atan2, sqrt, pi from sympy import Matrix, Symbol, Expr -from pysymoro.symoro import ZERO, ONE, get_max_coef +from pysymoro.symoro import ZERO, ONE from pysymoro.geometry import dgm from symoroutils import symbolmgr +from symoroutils import tools EMPTY = var("EMPTY") @@ -136,7 +137,7 @@ def igm_Paul(robo, T_ref, n): def _try_solve_0(symo, eq_sys, known): 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" @@ -479,9 +480,9 @@ def _check_const(consts, *xs): def _get_coefs(eq, A1, A2, *xs): eqe = eq.expand() - X = get_max_coef(eqe, A1) + X = tools.get_max_coef(eqe, A1) eqe = eqe.xreplace({A1: ZERO}) - Y = get_max_coef(eqe, A2) + Y = tools.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) is_ok = True diff --git a/pysymoro/kinematics.py b/pysymoro/kinematics.py index 25bbdeb..65b3cee 100644 --- a/pysymoro/kinematics.py +++ b/pysymoro/kinematics.py @@ -8,11 +8,12 @@ from sympy import Matrix, zeros -from pysymoro.symoro import Init, hat +from pysymoro.symoro import Init from pysymoro.symoro import FAIL, ZERO from pysymoro.geometry import dgm, Transform from pysymoro.geometry import compute_rot_trans, Z_AXIS from symoroutils import symbolmgr +from symoroutils import tools TERMINAL = 0 @@ -30,13 +31,13 @@ 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] @@ -48,13 +49,13 @@ def _v_dot_j(robo, symo, j, jRant, antPj, w, wi, wdot, U, vdot, qdj, qddj): 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] @@ -113,7 +114,7 @@ def _jac(robo, symo, n, i, j, chain=None, forced=False, trig_subs=False): 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) if forced: symo.mat_replace(Jac, 'J', '', forced) L = symo.mat_replace(L, 'L', '', forced) diff --git a/pysymoro/symoro.py b/pysymoro/symoro.py index fc14372..46f2e96 100644 --- a/pysymoro/symoro.py +++ b/pysymoro/symoro.py @@ -20,17 +20,9 @@ from sympy import Mul, Add, factor, zeros, var, sympify, eye from symoroutils import filemgr - - -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'] +from symoroutils import tools +from symoroutils.tools import ZERO, ONE, FAIL, OK +from symoroutils.tools import CLOSED_LOOP, SIMPLE, TREE, TYPES, INT_KEYS #TODO: write consistency check @@ -729,7 +721,7 @@ 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)) + U.append(tools.skew(robo.w0)**2 + tools.skew(robo.wdot0)) return U @classmethod @@ -750,175 +742,3 @@ def product_combinations(cls, v): v[1]*v[1], v[1]*v[2], v[2]*v[2]]) -def hat(v): - """skew-symmetry : 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 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 - - diff --git a/pysymoro/tests/test.py b/pysymoro/tests/test.py index 661ee4d..ec0841a 100644 --- a/pysymoro/tests/test.py +++ b/pysymoro/tests/test.py @@ -20,6 +20,7 @@ from pysymoro import dynamics from symoroutils import parfile from symoroutils import symbolmgr +from symoroutils import tools class testMisc(unittest.TestCase): @@ -78,35 +79,35 @@ 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) + 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(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(tools.get_max_coef(expr3*expr4, expr4), expr3) + self.assertEqual(tools.get_max_coef(expr3x, expr4), symoro.ZERO) + res = tools.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'}) + self.assertEqual(tools.get_trig_couple_names(expr1), {'2', '3'}) expr2 = sympify("CG2*S3*R + SG2*C1*R") - self.assertEqual(symoro.get_trig_couple_names(expr2), {'G2'}) + self.assertEqual(tools.get_trig_couple_names(expr2), {'G2'}) expr2 = sympify("CA2*SA3*R + SG2*C3*R") - self.assertEqual(symoro.get_trig_couple_names(expr2), set()) + self.assertEqual(tools.get_trig_couple_names(expr2), set()) expr3 = sympify("C2*S3*R + S1*C4*R") - self.assertEqual(symoro.get_trig_couple_names(expr3), set()) + self.assertEqual(tools.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')) + 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 "######## test_try_opt ##########" diff --git a/symoroui/labels.py b/symoroui/labels.py index d762cd5..8797a68 100644 --- a/symoroui/labels.py +++ b/symoroui/labels.py @@ -6,7 +6,7 @@ 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 +maintaining the labels easily since all the labels are placed in the same place. """ @@ -17,21 +17,21 @@ # main window MAIN_WIN = dict( - prog_name = "SYMORO", - window_title = "SYMORO - SYmbolic MOdelling of RObots" + 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_vel_acc = "Joint Velocity and Acceleration" + 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_vel_acc="Joint Velocity and Acceleration" ) # named tuple to hold the content field entries FieldEntry = namedtuple( @@ -39,56 +39,56 @@ ) # joint velocity and acceleration params JOINT_VEL_ACC = OrderedDict([ - ('joint', FieldEntry('Joint', 'joint', 'cmb', (0,0), 'OnJointChanged', -1)), - ('qp', FieldEntry('QP', 'QP', 'txt', (1,0), 'OnSpeedChanged', -1)), - ('qdp', FieldEntry('QDP', 'QDP', 'txt', (2,0), 'OnSpeedChanged', -1)), - ('gam', FieldEntry('GAM', 'GAM', 'txt', (3,0), 'OnSpeedChanged', -1)) + ('joint', FieldEntry('Joint', 'joint', 'cmb', (0, 0), 'OnJointChanged', -1)), + ('qp', FieldEntry('QP', 'QP', 'txt', (1, 0), 'OnSpeedChanged', -1)), + ('qdp', FieldEntry('QDP', 'QDP', 'txt', (2, 0), 'OnSpeedChanged', -1)), + ('gam', FieldEntry('GAM', 'GAM', 'txt', (3, 0), 'OnSpeedChanged', -1)) ]) # base velocity and acceleration params BASE_VEL_ACC = OrderedDict([ - ('wx', FieldEntry('W0X', 'W0X', 'txt', (0,0), 'OnBaseTwistChanged', 0)), - ('wy', FieldEntry('W0Y', 'W0Y', 'txt', (1,0), 'OnBaseTwistChanged', 1)), - ('wz', FieldEntry('W0Z', 'W0Z', 'txt', (2,0), 'OnBaseTwistChanged', 2)), - ('wpx', FieldEntry('WP0X', 'WP0X', 'txt', (0,1), 'OnBaseTwistChanged', 0)), - ('wpy', FieldEntry('WP0Y', 'WP0Y', 'txt', (1,1), 'OnBaseTwistChanged', 1)), - ('wpz', FieldEntry('WP0Z', 'WP0Z', 'txt', (2,1), 'OnBaseTwistChanged', 2)), - ('vx', FieldEntry('V0X', 'V0X', 'txt', (0,2), 'OnBaseTwistChanged', 0)), - ('vy', FieldEntry('V0Y', 'V0Y', 'txt', (1,2), 'OnBaseTwistChanged', 1)), - ('vz', FieldEntry('V0Z', 'V0Z', 'txt', (2,2), 'OnBaseTwistChanged', 2)), - ('vpx', FieldEntry('VP0X', 'VP0X', 'txt', (0,3), 'OnBaseTwistChanged', 0)), - ('vpy', FieldEntry('VP0Y', 'VP0Y', 'txt', (1,3), 'OnBaseTwistChanged', 1)), - ('vpz', FieldEntry('VP0Z', 'VP0Z', 'txt', (2,3), 'OnBaseTwistChanged', 2)) + ('wx', FieldEntry('W0X', 'W0X', 'txt', (0, 0), 'OnBaseTwistChanged', 0)), + ('wy', FieldEntry('W0Y', 'W0Y', 'txt', (1, 0), 'OnBaseTwistChanged', 1)), + ('wz', FieldEntry('W0Z', 'W0Z', 'txt', (2, 0), 'OnBaseTwistChanged', 2)), + ('wpx', FieldEntry('WP0X', 'WP0X', 'txt', (0, 1), 'OnBaseTwistChanged', 0)), + ('wpy', FieldEntry('WP0Y', 'WP0Y', 'txt', (1, 1), 'OnBaseTwistChanged', 1)), + ('wpz', FieldEntry('WP0Z', 'WP0Z', 'txt', (2, 1), 'OnBaseTwistChanged', 2)), + ('vx', FieldEntry('V0X', 'V0X', 'txt', (0, 2), 'OnBaseTwistChanged', 0)), + ('vy', FieldEntry('V0Y', 'V0Y', 'txt', (1, 2), 'OnBaseTwistChanged', 1)), + ('vz', FieldEntry('V0Z', 'V0Z', 'txt', (2, 2), 'OnBaseTwistChanged', 2)), + ('vpx', FieldEntry('VP0X', 'VP0X', 'txt', (0, 3), 'OnBaseTwistChanged', 0)), + ('vpy', FieldEntry('VP0Y', 'VP0Y', 'txt', (1, 3), 'OnBaseTwistChanged', 1)), + ('vpz', FieldEntry('VP0Z', 'VP0Z', '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)) + ('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([ - ('mx', FieldEntry('MX', 'MX', 'txt', (1,0), 'OnDynParamChanged', -1)), - ('my', FieldEntry('MY', 'MY', 'txt', (1,1), 'OnDynParamChanged', -1)), - ('mz', FieldEntry('MZ', 'MZ', 'txt', (1,2), 'OnDynParamChanged', -1)), - ('m', FieldEntry('M', 'M', 'txt', (1,3), 'OnDynParamChanged', -1)) + ('mx', FieldEntry('MX', 'MX', 'txt', (1, 0), 'OnDynParamChanged', -1)), + ('my', FieldEntry('MY', 'MY', 'txt', (1, 1), 'OnDynParamChanged', -1)), + ('mz', FieldEntry('MZ', 'MZ', 'txt', (1, 2), 'OnDynParamChanged', -1)), + ('m', FieldEntry('M', 'M', 'txt', (1, 3), 'OnDynParamChanged', -1)) ]) # friction and rotor inertia params DYN_PARAMS_X = OrderedDict([ - ('ia', FieldEntry('IA', 'IA', 'txt', (2,0), 'OnDynParamChanged', -1)), - ('fc', FieldEntry('FS', 'FS', 'txt', (2,1), 'OnDynParamChanged', -1)), - ('fv', FieldEntry('FV', 'FV', 'txt', (2,2), 'OnDynParamChanged', -1)) + ('ia', FieldEntry('IA', 'IA', 'txt', (2, 0), 'OnDynParamChanged', -1)), + ('fc', FieldEntry('FS', 'FS', 'txt', (2, 1), 'OnDynParamChanged', -1)), + ('fv', FieldEntry('FV', 'FV', 'txt', (2, 2), 'OnDynParamChanged', -1)) ]) # external force, moments params DYN_PARAMS_F = OrderedDict([ - ('ex_fx', FieldEntry('FX', 'FX', 'txt', (3,0), 'OnDynParamChanged', -1)), - ('ex_fy', FieldEntry('FY', 'FY', 'txt', (3,1), 'OnDynParamChanged', -1)), - ('ex_fz', FieldEntry('FZ', 'FZ', 'txt', (3,2), 'OnDynParamChanged', -1)), - ('ex_mx', FieldEntry('CX', 'CX', 'txt', (3,3), 'OnDynParamChanged', -1)), - ('ex_my', FieldEntry('CY', 'CY', 'txt', (3,4), 'OnDynParamChanged', -1)), - ('ex_mz', FieldEntry('CZ', 'CZ', 'txt', (3,5), 'OnDynParamChanged', -1)) + ('ex_fx', FieldEntry('FX', 'FX', 'txt', (3, 0), 'OnDynParamChanged', -1)), + ('ex_fy', FieldEntry('FY', 'FY', 'txt', (3, 1), 'OnDynParamChanged', -1)), + ('ex_fz', FieldEntry('FZ', 'FZ', 'txt', (3, 2), 'OnDynParamChanged', -1)), + ('ex_mx', FieldEntry('CX', 'CX', 'txt', (3, 3), 'OnDynParamChanged', -1)), + ('ex_my', FieldEntry('CY', 'CY', 'txt', (3, 4), 'OnDynParamChanged', -1)), + ('ex_mz', FieldEntry('CZ', 'CZ', 'txt', (3, 5), 'OnDynParamChanged', -1)) ]) # dynamic params got by concatenation DYN_PARAMS = OrderedDict( @@ -100,78 +100,78 @@ ) # 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)) + ('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)) + ('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_mobile', FieldEntry('Is Mobile:', 'mobile', 'lbl', (5,0), None, -1)), - ('num_loops', FieldEntry('Number of closed loops:', 'loops', 'lbl', (6,0), None, -1)) + ('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_mobile', FieldEntry('Is Mobile:', 'mobile', 'lbl', (5, 0), None, -1)), + ('num_loops', FieldEntry('Number of closed loops:', 'loops', 'lbl', (6, 0), None, -1)) ]) # menu bar # TODO: add event handlers to dict entries as well MAIN_MENU = dict( - file_menu = "&File", - geom_menu = "&Geometric", - kin_menu = "&Kinematic", - dyn_menu = "&Dynamic", - iden_menu = "&Identification", - optim_menu = "&Optimiser", - viz_menu = "&Visualisation" + file_menu="&File", + geom_menu="&Geometric", + kin_menu="&Kinematic", + dyn_menu="&Dynamic", + iden_menu="&Identification", + optim_menu="&Optimiser", + viz_menu="&Visualisation" ) -VIZ_MENU = dict(m_viz = "Visualisation") +VIZ_MENU = dict(m_viz="Visualisation") IDEN_MENU = dict( - m_base_inertial_params = "Base Inertial parameters", - m_dyn_iden_model = "Dynamic Identification Model", - m_energy_iden_model = "Energy Identification Model" + m_base_inertial_params="Base Inertial parameters", + m_dyn_iden_model="Dynamic Identification Model", + m_energy_iden_model="Energy Identification Model" ) DYN_MENU = dict( - m_idym = "Inverse Dynamic Model", - m_inertia_matrix = "Inertia matrix", - m_h_term = "Centrifugal, Coriolis & Gravity torques", - m_ddym = "Direct Dynamic Model" + m_idym="Inverse Dynamic Model", + m_inertia_matrix="Inertia matrix", + m_h_term="Centrifugal, Coriolis & Gravity torques", + m_ddym="Direct Dynamic Model" ) KIN_MENU = dict( - 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" + 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 = dict( - m_trans_matrix = "Transformation matrix", - m_fast_dgm = "Fast Geometric model", - m_igm_paul = "IGM - Paul method", - m_geom_constraint = "Geometric constraint equation of loops" + m_trans_matrix="Transformation matrix", + m_fast_dgm="Fast Geometric model", + m_igm_paul="IGM - Paul method", + m_geom_constraint="Geometric constraint equation of loops" ) FILE_MENU = dict( - m_new = "&New", - m_open = "&Open", - m_save = "&Save", - m_save_as = "Save &As", - m_pref = "Preferences -- (unavailable)", - m_exit = "E&xit" + 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/symoroutils/filemgr.py b/symoroutils/filemgr.py index 5c73283..5e9778a 100644 --- a/symoroutils/filemgr.py +++ b/symoroutils/filemgr.py @@ -60,9 +60,9 @@ def make_folders(folder_path): def get_folder_path(robot_name): """ - Return the folder path to store the robot data. Also create the + 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). @@ -82,7 +82,7 @@ def get_folder_path(robot_name): def make_file_path(robot, ext=None): """ - Create the file path with the appropriate extension appended to + Create the file path with the appropriate extension appended to the file name using an underscore. Args: diff --git a/symoroutils/symbolmgr.py b/symoroutils/symbolmgr.py index d635764..dc58d37 100644 --- a/symoroutils/symbolmgr.py +++ b/symoroutils/symbolmgr.py @@ -3,8 +3,14 @@ """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 class SymbolManager: @@ -24,7 +30,7 @@ def __init__(self, file_out='disp', sydi={}): def simp(self, sym): sym = factor(sym) - new_sym = ONE + new_sym = tools.ONE for e in Mul.make_args(sym): if e.is_Pow: e, p = e.args @@ -48,13 +54,13 @@ def C2S2_simp(self, sym): repl_dict[term] = self.C2S2_simp(term) sym = sym.xreplace(repl_dict) return sym - names, short_form = trigonometric_info(sym) + names, short_form = tools.trignometric_info(sym) for name in names: if short_form: - C, S = CS_syms(name) + C, S = tools.CS_syms(name) else: C, S = cos(name), sin(name) - sym = self.try_opt(ONE, None, S**2, C**2, sym) + sym = self.try_opt(tools.ONE, None, S**2, C**2, sym) return sym def CS12_simp(self, sym, silent=False): @@ -74,20 +80,20 @@ def CS12_simp(self, sym, silent=False): repl_dict[term] = self.CS12_simp(term) sym = sym.xreplace(repl_dict) return sym - names, short_form = trigonometric_info(sym) + names, short_form = tools.trignometric_info(sym) names = list(names) names.sort() sym2 = sym - for n1, n2 in combinations(names, 2): + for n1, n2 in itertools.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) + C1, S1 = tools.CS_syms(n1) + C2, S2 = tools.CS_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.CS_syms(n12) + C1m2, S1m2 = tools.CS_syms(nm12) else: C1, S1 = cos(n1), sin(n1) C2, S2 = cos(n2), sin(n2) @@ -104,16 +110,16 @@ 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) + 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 sym_less(Res_tmp, Res): + if tools.sym_less(Res_tmp, Res): Res = Res_tmp - if sym_less(Res, old_sym) and Am is None: + 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 @@ -122,13 +128,13 @@ def try_opt(self, A, Am, B, C, old_sym, silent=False): Res_tmp = Res2 for coef in Bcfs: Res_tmp += Am*coef - B*coef + C*coef - if sym_less(Res_tmp, Res2): + if tools.sym_less(Res_tmp, Res2): Res2 = Res_tmp - if sym_less(Res2, Res) and sym_less(Res2, old_sym): + 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 sym_less(Res, old_sym): + elif tools.sym_less(Res, old_sym): if not A.is_number and not silent: self.add_to_dict(A, B + C) return Res @@ -139,7 +145,7 @@ def add_to_dict(self, new_sym, old_sym): Extends symbol dictionary by (new_sym, old_sym) pair """ new_sym = sympify(new_sym) - if new_sym.as_coeff_Mul()[0] == -ONE: + if new_sym.as_coeff_Mul()[0] == -tools.ONE: new_sym = -new_sym old_sym = -old_sym if new_sym not in self.sydi: @@ -168,7 +174,7 @@ def trig_replace(self, M, angle, name): """ if not isinstance(angle, Expr) or angle.is_number: return M - cos_sym, sin_sym = CS_syms(name) + cos_sym, sin_sym = tools.CS_syms(name) sym_list = [(cos_sym, cos(angle)), (sin_sym, sin(angle))] subs_dict = {} for sym, sym_old in sym_list: @@ -303,13 +309,12 @@ def write_param(self, name, header, robo, N): Indices for which parameter rows will be written """ self.write_line(name) - self.write_line(l2str(header)) + self.write_line(tools.l2str(header)) for j in N: params = robo.get_param_vec(header, j) - self.write_line(l2str(params)) + self.write_line(tools.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'): @@ -561,8 +566,6 @@ def gen_func(self, name, to_return, args, multival=False): 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) From 0ac6f33c0b7990ede9cb57e766036db45b781dd4 Mon Sep 17 00:00:00 2001 From: aravind Date: Mon, 14 Apr 2014 14:14:11 +0200 Subject: [PATCH 043/273] Add `symoroutils/dyninit.py` --- symoroutils/dyninit.py | 138 +++++++++++++++++++++++++++++++++++++++++ 1 file changed, 138 insertions(+) create mode 100644 symoroutils/dyninit.py diff --git a/symoroutils/dyninit.py b/symoroutils/dyninit.py new file mode 100644 index 0000000..3e91163 --- /dev/null +++ b/symoroutils/dyninit.py @@ -0,0 +1,138 @@ +# -*- coding: utf-8 -*- + + +""" +This module contains the parameters used to initialise the dynamic +model of a robot. +""" + + +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(tools.skew(robo.w0)**2 + tools.skew(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]]) From e7f50d30fc40c9f15647009362f59b09856ff3f1 Mon Sep 17 00:00:00 2001 From: aravind Date: Mon, 14 Apr 2014 15:20:45 +0200 Subject: [PATCH 044/273] Move initialisation class to new file Move initialisation class to `symoroutils/paramsinit.py`. Rename class `Init` to `ParamsInit`. Rename `symoroutils/dyninit.py` to `symoroutils/paramsinit.py`. Update to call `ParamsInit` class instead in all relevant parts. --- pysymoro/dynamics.py | 48 ++++---- pysymoro/geometry.py | 6 +- pysymoro/kinematics.py | 20 ++-- pysymoro/symoro.py | 131 ---------------------- symoroutils/{dyninit.py => paramsinit.py} | 82 ++++++++------ symoroutils/symbolmgr.py | 12 +- symoroutils/tools.py | 106 ++++++++--------- 7 files changed, 147 insertions(+), 258 deletions(-) rename symoroutils/{dyninit.py => paramsinit.py} (62%) diff --git a/pysymoro/dynamics.py b/pysymoro/dynamics.py index 5a4a84a..f6cb1c4 100644 --- a/pysymoro/dynamics.py +++ b/pysymoro/dynamics.py @@ -11,13 +11,13 @@ from sympy import Matrix from copy import copy, deepcopy -from pysymoro.symoro import Init, ZERO from pysymoro.geometry import compute_screw_transform from pysymoro.geometry import compute_rot_trans, Transform from pysymoro.kinematics import compute_vel_acc from pysymoro.kinematics import compute_omega from symoroutils import symbolmgr from symoroutils import tools +from symoroutils.paramsinit import ParamsInit chars = 'ABCDEFGHJKLMNPQRSTUVWXYZ' @@ -44,10 +44,10 @@ def Newton_Euler(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) + F = ParamsInit.init_vec(robo) + N = ParamsInit.init_vec(robo) + Fjnt = ParamsInit.init_vec(robo) + Njnt = ParamsInit.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)): @@ -73,8 +73,8 @@ def dynamic_identification_NE(robo): """ # init forces vectors - Fjnt = Init.init_vec(robo) - Njnt = Init.init_vec(robo) + Fjnt = ParamsInit.init_vec(robo) + Njnt = ParamsInit.init_vec(robo) # init file output, writing the robot description symo = symbolmgr.SymbolManager() symo.file_open(robo, 'dim') @@ -91,8 +91,8 @@ def dynamic_identification_NE(robo): 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) + F = ParamsInit.init_vec(robo) + N = ParamsInit.init_vec(robo) for i in xrange(10): if param_vec[i] == ZERO: continue @@ -141,18 +141,18 @@ def direct_dynamic_NE(robo): symo.sydi : dictionary Dictionary with the information of all the sybstitution """ - wi = Init.init_vec(robo) + wi = ParamsInit.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) + w = ParamsInit.init_w(robo) + jaj = ParamsInit.init_vec(robo, 6) + jTant = ParamsInit.init_mat(robo, 6) # Twist transform list of Matrices 6x6 + beta_star = ParamsInit.init_vec(robo, 6) + grandJ = ParamsInit.init_mat(robo, 6) + link_acc = ParamsInit.init_vec(robo, 6) + H_inv = ParamsInit.init_scalar(robo) + juj = ParamsInit.init_vec(robo, 6) # Jj*aj / Hj + Tau = ParamsInit.init_scalar(robo) + grandVp = ParamsInit.init_vec(robo, 6) grandVp.append(Matrix([robo.vdot0 - robo.G, robo.w0])) symo = symbolmgr.SymbolManager() symo.file_open(robo, 'ddm') @@ -200,10 +200,10 @@ def inertia_matrix(robo): 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) + Jplus, MSplus, Mplus = ParamsInit.init_jplus(robo) + AJE1 = ParamsInit.init_vec(robo) + f = ParamsInit.init_vec(robo, ext=1) + n = ParamsInit.init_vec(robo, ext=1) A = sympy.zeros(robo.NL, robo.NL) symo = symbolmgr.SymbolManager() symo.file_open(robo, 'inm') diff --git a/pysymoro/geometry.py b/pysymoro/geometry.py index 0dda983..9d3ae86 100644 --- a/pysymoro/geometry.py +++ b/pysymoro/geometry.py @@ -7,9 +7,9 @@ from sympy import Matrix, zeros, eye, sin, cos -from pysymoro.symoro import Init from symoroutils import symbolmgr from symoroutils import tools +from symoroutils.paramsinit import ParamsInit Z_AXIS = Matrix([0, 0, 1]) @@ -424,8 +424,8 @@ def _rot_trans(axis='z', th=0, p=0): def compute_rot_trans(robo, symo): #init transformation - antRj = Init.init_mat(robo) - antPj = Init.init_vec(robo) + 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 diff --git a/pysymoro/kinematics.py b/pysymoro/kinematics.py index 65b3cee..29dfac2 100644 --- a/pysymoro/kinematics.py +++ b/pysymoro/kinematics.py @@ -8,12 +8,12 @@ from sympy import Matrix, zeros -from pysymoro.symoro import Init from pysymoro.symoro import FAIL, ZERO 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 @@ -44,7 +44,7 @@ def _v_j(robo, j, antPj, jRant, v, w, qdj, forced=False): def _v_dot_j(robo, symo, j, jRant, antPj, w, wi, wdot, U, vdot, qdj, qddj): - DV = Init.product_combinations(w[j]) + 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]], @@ -213,10 +213,10 @@ def compute_vel_acc(robo, symo, antRj, antPj, forced=False, gravity=True): 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) #init auxilary matrix - U = Init.init_U(robo) + U = ParamsInit.init_u(robo) for j in xrange(1, robo.NL): jRant = antRj[j].T qdj = Z_AXIS * robo.qdot[j] @@ -236,8 +236,8 @@ def velocities(robo): 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] @@ -265,9 +265,9 @@ def jdot_qdot(robo): 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] diff --git a/pysymoro/symoro.py b/pysymoro/symoro.py index 46f2e96..6f8ed9c 100644 --- a/pysymoro/symoro.py +++ b/pysymoro/symoro.py @@ -611,134 +611,3 @@ def RX90(cls): 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(tools.skew(robo.w0)**2 + tools.skew(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]]) - - diff --git a/symoroutils/dyninit.py b/symoroutils/paramsinit.py similarity index 62% rename from symoroutils/dyninit.py rename to symoroutils/paramsinit.py index 3e91163..2a5a2f7 100644 --- a/symoroutils/dyninit.py +++ b/symoroutils/paramsinit.py @@ -2,14 +2,26 @@ """ -This module contains the parameters used to initialise the dynamic -model of a robot. +This module contains the methods used to initialise the different +matrices and parameters for various models. """ -class Init: +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): + def init_jplus(cls, robo): """Copies the inertia parameters. Used for composed link inertia computation @@ -19,16 +31,16 @@ def init_Jplus(cls, robo): 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 + 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, N=3): + def init_mat(cls, robo, num=3): """Generates a list of Matrices.Size of the list is number of links. @@ -36,17 +48,17 @@ def init_mat(cls, robo, N=3): ========== robo: Robot Instance of robot description container - N: int, optional + num: int, optional size of the matries, default is 3 Returns ======= - list of Matrices NxN + list of Matrices numxnum """ - return [zeros(N, N) for i in xrange(robo.NL)] + return [zeros(num, num) for i in xrange(robo.NL)] @classmethod - def init_vec(cls, robo, N=3, ext=0): + def init_vec(cls, robo, num=3, ext=0): """Generates a list of vectors. Size of the list is number of links. @@ -54,7 +66,7 @@ def init_vec(cls, robo, N=3, ext=0): ========== robo: Robot Instance of robot description container - N: int, optional + num: int, optional size of the vectors, default is 3 ext: int, optional additional vector instances over number of links @@ -63,7 +75,7 @@ def init_vec(cls, robo, N=3, ext=0): ======= list of Matrices Nx1 """ - return [zeros(N, 1) for i in xrange(robo.NL+ext)] + return [zeros(num, 1) for i in xrange(robo.NL+ext)] @classmethod def init_scalar(cls, robo): @@ -78,9 +90,9 @@ def init_w(cls, robo): 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 + omega = cls.init_vec(robo) + omega[0] = robo.w0 + return omega @classmethod def init_v(cls, robo): @@ -88,9 +100,9 @@ def init_v(cls, robo): 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 + vel = cls.init_vec(robo) + vel[0] = robo.v0 + return vel @classmethod def init_wv_dot(cls, robo, gravity=True): @@ -113,26 +125,32 @@ def init_wv_dot(cls, robo, gravity=True): return wdot, vdot @classmethod - def init_U(cls, robo): + def init_u(cls, robo): """Generates a list of auxiliary U matrices""" - U = Init.init_mat(robo) + u_aux_matrix = ParamsInit.init_mat(robo) # the value for the -1th base frame - U.append(tools.skew(robo.w0)**2 + tools.skew(robo.wdot0)) - return U + u_aux_matrix.append( + tools.skew(robo.w0)**2 + tools.skew(robo.wdot0) + ) + return u_aux_matrix @classmethod - def product_combinations(cls, v): + def product_combinations(cls, vec): """Generates 6-vector of different v elements' product combinations Parameters ========== - v: Matrix 3x1 + vec: 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]]) + 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/symbolmgr.py b/symoroutils/symbolmgr.py index dc58d37..4bd0ae3 100644 --- a/symoroutils/symbolmgr.py +++ b/symoroutils/symbolmgr.py @@ -57,7 +57,7 @@ def C2S2_simp(self, sym): names, short_form = tools.trignometric_info(sym) for name in names: if short_form: - C, S = tools.CS_syms(name) + C, S = tools.cos_sin_syms(name) else: C, S = cos(name), sin(name) sym = self.try_opt(tools.ONE, None, S**2, C**2, sym) @@ -86,14 +86,14 @@ def CS12_simp(self, sym, silent=False): sym2 = sym for n1, n2 in itertools.combinations(names, 2): if short_form: - C1, S1 = tools.CS_syms(n1) - C2, S2 = tools.CS_syms(n2) + 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.CS_syms(n12) - C1m2, S1m2 = tools.CS_syms(nm12) + 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) @@ -174,7 +174,7 @@ def trig_replace(self, M, angle, name): """ if not isinstance(angle, Expr) or angle.is_number: return M - cos_sym, sin_sym = tools.CS_syms(name) + 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: diff --git a/symoroutils/tools.py b/symoroutils/tools.py index 8767cd1..85263ed 100644 --- a/symoroutils/tools.py +++ b/symoroutils/tools.py @@ -26,7 +26,7 @@ INT_KEYS = ['ant', 'sigma', 'mu'] -def skew(v): +def skew(vec): """skew-symmetry : Generates vectorial preproduct matrix Parameters @@ -38,9 +38,9 @@ def skew(v): ======= hat: Matrix 3x3 """ - return Matrix([[0, -v[2], v[1]], - [v[2], 0, -v[0]], - [-v[1], v[0], 0]]) + return Matrix([[0, -vec[2], vec[1]], + [vec[2], 0, -vec[0]], + [-vec[1], vec[0], 0]]) def l2str(list_var, spacing=8): @@ -56,17 +56,17 @@ def l2str(list_var, spacing=8): Returns ======= - s: string + ret_str: string String representation Notes ===== l2str([1, 2, 3]) will be converted into '1 2 3 ' """ - s = '' + ret_str = '' for i in list_var: - s += str(i) + ' '*(spacing-len(str(i))) - return s + 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): @@ -83,65 +83,67 @@ def get_trig_couple_names(sym): return names_c & names_s -def get_max_coef_mul(sym, x): - k, ex = x.as_coeff_Mul() +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 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: + 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): - return [get_max_coef_mul(s, x) for s in Add.make_args(sym)] +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): - return Add.fromiter(get_max_coef_mul(s, x) 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(s): - if s.find('m') != -1: - s_split = s.split('m') +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 s, '' + return str_term, '' -def reduce_str(s1, s2): +def reduce_str(str1, str2): while True: - for j, char in enumerate(s1): + for j, char in enumerate(str1): if char in 'AG': - i = s2.find(s1[j:j+2]) + i = str2.find(str1[j:j+2]) k = 2 else: - i = s2.find(char) + i = str2.find(char) k = 1 if i != -1: - if i+k < len(s2): - s2_tail = s2[i+k:] + if i+k < len(str2): + str2_tail = str2[i+k:] else: - s2_tail = '' - if j+k < len(s1): - s1_tail = s1[j+k:] + str2_tail = '' + if j+k < len(str1): + str1_tail = str1[j+k:] else: - s1_tail = '' - s2 = s2[:i] + s2_tail - s1 = s1[:j] + s1_tail + str1_tail = '' + str2 = str2[:i] + str2_tail + str1 = str1[:j] + str1_tail break else: break - return s1, s2 + return str1, str2 def ang_sum(np1, np2, nm1, nm2): @@ -153,37 +155,37 @@ def ang_sum(np1, np2, nm1, nm2): return np1 + np2 + 'm' + nm1 + nm2 -def CS_syms(name): +def cos_sin_syms(name): if isinstance(name, str) and name[0] == 'm': - C, S = var('C{0}, S{0}'.format(name[1:])) - return C, -S + 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(A, B): - A_measure = A.count_ops() - B_measure = B.count_ops() - return A_measure < B_measure +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 s in expr.atoms(sin): - angles_s |= set(s.args) + for sin_term in expr.atoms(sin): + angles_s |= set(sin_term.args) angles_c = set() - for c in expr.atoms(cos): - angles_c |= set(c.args) + for cos_term in expr.atoms(cos): + angles_c |= set(cos_term.args) return angles_s & angles_c -def cancel_terms(sym, X, coef): +def cancel_terms(sym, x_term, coef): if coef.is_Add: for arg_c in coef.args: - sym = cancel_terms(sym, X, arg_c) + 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*coef) + return Add.fromiter(t for t in terms if t != x_term*coef) def trignometric_info(sym): From cc01e47312a0b9887b3dc900f8d53df6d0cdf202 Mon Sep 17 00:00:00 2001 From: aravind Date: Mon, 14 Apr 2014 15:37:11 +0200 Subject: [PATCH 045/273] Minor changes in `symoroutils/symbolmgr.py` --- symoroutils/symbolmgr.py | 26 ++++++++++++++------------ 1 file changed, 14 insertions(+), 12 deletions(-) diff --git a/symoroutils/symbolmgr.py b/symoroutils/symbolmgr.py index 4bd0ae3..fbc3cfa 100644 --- a/symoroutils/symbolmgr.py +++ b/symoroutils/symbolmgr.py @@ -13,9 +13,9 @@ from symoroutils import tools -class SymbolManager: +class SymbolManager(object): """Symbol manager, responsible for symbol replacing, file writing.""" - def __init__(self, file_out='disp', sydi={}): + def __init__(self, file_out='disp', sydi=dict()): """Default values correspond to empty dictionary and screen output. """ self.file_out = file_out @@ -31,14 +31,14 @@ def __init__(self, file_out='disp', sydi={}): def simp(self, sym): sym = factor(sym) new_sym = tools.ONE - for e in Mul.make_args(sym): - if e.is_Pow: - e, p = e.args + for expr in Mul.make_args(sym): + if expr.is_Pow: + expr, pow_val = expr.args else: - p = 1 - e = self.C2S2_simp(e) - e = self.CS12_simp(e, silent=True) - new_sym *= e**p + 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): @@ -57,10 +57,12 @@ def C2S2_simp(self, sym): names, short_form = tools.trignometric_info(sym) for name in names: if short_form: - C, S = tools.cos_sin_syms(name) + cos_term, sin_term = tools.cos_sin_syms(name) else: - C, S = cos(name), sin(name) - sym = self.try_opt(tools.ONE, None, S**2, C**2, sym) + 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): From 8bb676278d0e81dd9e936afed70ab53e9d78dfea Mon Sep 17 00:00:00 2001 From: aravind Date: Mon, 14 Apr 2014 16:02:35 +0200 Subject: [PATCH 046/273] Update test file Update test file to call the right functions and objects. --- pysymoro/dynamics.py | 8 ++++---- pysymoro/tests/test.py | 4 +++- 2 files changed, 7 insertions(+), 5 deletions(-) diff --git a/pysymoro/dynamics.py b/pysymoro/dynamics.py index f6cb1c4..845cb66 100644 --- a/pysymoro/dynamics.py +++ b/pysymoro/dynamics.py @@ -94,7 +94,7 @@ def dynamic_identification_NE(robo): F = ParamsInit.init_vec(robo) N = ParamsInit.init_vec(robo) for i in xrange(10): - if param_vec[i] == ZERO: + if param_vec[i] == tools.ZERO: continue # change link names according to current non-zero parameter robo_tmp.num = [str(l) + str(param_vec[i]) @@ -341,7 +341,7 @@ def compute_joint_torque_deriv(symo, param, arg, index): index : strig identifies the parameter in the sybstituted symbol's name """ - if param != ZERO and arg != ZERO: + if param != tools.ZERO and arg != tools.ZERO: index = str(index) + str(param) symo.replace(arg, 'DG', index, forced=True) @@ -795,8 +795,8 @@ def group_param_prism_spec(robo, symo, j, antRj, antPj): 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 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 diff --git a/pysymoro/tests/test.py b/pysymoro/tests/test.py index ec0841a..43cea86 100644 --- a/pysymoro/tests/test.py +++ b/pysymoro/tests/test.py @@ -6,6 +6,7 @@ """ +import os import unittest from sympy import sympify, var, Matrix @@ -18,6 +19,7 @@ from pysymoro import kinematics from pysymoro import invgeom from pysymoro import dynamics +from symoroutils import filemgr from symoroutils import parfile from symoroutils import symbolmgr from symoroutils import tools @@ -28,7 +30,7 @@ def test_readwrite(self): print "######## test_readwrite ##########" original_robo = symoro.Robot.RX90() parfile.writepar(original_robo) - fname = '%s.par' % original_robo.name + fname = filemgr.get_clean_name(original_robo.name) + ".par" file_path = os.path.join(original_robo.directory, fname) new_robo, flag = parfile.readpar(original_robo.name, file_path) self.assertEqual(flag, symoro.OK) From ff30f8ce18419da92f0e3a89ed6a3b00f3317391 Mon Sep 17 00:00:00 2001 From: aravind Date: Mon, 14 Apr 2014 16:43:46 +0200 Subject: [PATCH 047/273] Refactor test from `pysymoro/tests/test.py` --- pysymoro/tests/test.py | 114 ------------------------------ symoroutils/tests/test_filemgr.py | 0 2 files changed, 114 deletions(-) create mode 100644 symoroutils/tests/test_filemgr.py diff --git a/pysymoro/tests/test.py b/pysymoro/tests/test.py index 43cea86..3ecbca7 100644 --- a/pysymoro/tests/test.py +++ b/pysymoro/tests/test.py @@ -73,120 +73,6 @@ def test_robo_misc(self): self.assertEqual(self.robo.get_val(i, Name), v) -class testSymoroTrig(unittest.TestCase): - def setUp(self): - self.symo = symbolmgr.SymbolManager() - - def test_GetMaxCoef(self): - print "######## test_GetMaxCoef ##########" - 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), symoro.ZERO) - res = tools.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(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 "######## test_name_operations ##########" - 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 "######## 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): diff --git a/symoroutils/tests/test_filemgr.py b/symoroutils/tests/test_filemgr.py new file mode 100644 index 0000000..e69de29 From cbf88296b319618c96bef8188f8eb0318ba9c5d3 Mon Sep 17 00:00:00 2001 From: aravind Date: Tue, 15 Apr 2014 14:18:04 +0200 Subject: [PATCH 048/273] Add unittest for `symoroutils/filemgr.py` Add unittest for `symoroutils/filemgr.py` in `symoroutils/tests/test_filemgr.py`. Remove some of the doctest code from `symoroutils/filemgr.py` and move to the corresponding unittest file. --- symoroutils/filemgr.py | 17 +--- symoroutils/tests/test_filemgr.py | 89 ++++++++++++++++++ symoroutils/tests/test_symbolmgr.py | 141 ++++++++++++++++++++++++++++ 3 files changed, 232 insertions(+), 15 deletions(-) create mode 100644 symoroutils/tests/test_symbolmgr.py diff --git a/symoroutils/filemgr.py b/symoroutils/filemgr.py index 5e9778a..1f5f987 100644 --- a/symoroutils/filemgr.py +++ b/symoroutils/filemgr.py @@ -10,18 +10,15 @@ SYMORO_ROBOTS_FOLDER = "symoro-robots" -def get_base_path(): +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. - - >>> get_base_path() - '/home/aravind/symoro-robots' """ home_folder = os.path.expanduser("~") - return os.path.join(home_folder, SYMORO_ROBOTS_FOLDER) + return os.path.join(home_folder, base_folder) def get_clean_name(name, char='-'): @@ -68,11 +65,6 @@ def get_folder_path(robot_name): Returns: A string specifying the folder path. - - >>> get_folder_path('RX 90') - '/home/aravind/symoro-robots/rx-90' - >>> get_folder_path('RX90') - '/home/aravind/symoro-robots/rx90' """ robot_name = get_clean_name(robot_name) folder_path = os.path.join(get_base_path(), robot_name) @@ -102,8 +94,3 @@ def make_file_path(robot, ext=None): return file_path -if __name__ == "__main__": - import doctest - doctest.testmod() - - diff --git a/symoroutils/tests/test_filemgr.py b/symoroutils/tests/test_filemgr.py index e69de29..67327d3 100644 --- a/symoroutils/tests/test_filemgr.py +++ b/symoroutils/tests/test_filemgr.py @@ -0,0 +1,89 @@ +#!/usr/bin/env python +# -*- coding: utf-8 -*- + + +"""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_make_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.make_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.make_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_symbolmgr.py b/symoroutils/tests/test_symbolmgr.py new file mode 100644 index 0000000..ab86513 --- /dev/null +++ b/symoroutils/tests/test_symbolmgr.py @@ -0,0 +1,141 @@ +#!/usr/bin/env python +# -*- coding: utf-8 -*- + + +"""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() + + From 198bc270353627460cfa7052ef349f254e64fb5b Mon Sep 17 00:00:00 2001 From: aravind Date: Tue, 15 Apr 2014 23:36:18 +0200 Subject: [PATCH 049/273] Fix module import and calls --- pysymoro/invgeom.py | 37 ++++++++++++++++++------------------- pysymoro/kinematics.py | 15 +++++++-------- symoroui/definition.py | 11 ++++++----- symoroui/layout.py | 22 +++++++++++++--------- symoroviz/graphics.py | 6 +++--- 5 files changed, 47 insertions(+), 44 deletions(-) diff --git a/pysymoro/invgeom.py b/pysymoro/invgeom.py index eb5fefc..b7e96c1 100644 --- a/pysymoro/invgeom.py +++ b/pysymoro/invgeom.py @@ -10,7 +10,6 @@ from sympy import var, sin, cos, eye, atan2, sqrt, pi from sympy import Matrix, Symbol, Expr -from pysymoro.symoro import ZERO, ONE from pysymoro.geometry import dgm from symoroutils import symbolmgr from symoroutils import tools @@ -156,7 +155,7 @@ def _try_solve_1(symo, eq_sys, known): if th_i in known: 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 + i_ok &= sum([Xi == tools.ZERO, Yi == tools.ZERO, Zi == tools.ZERO]) <= 1 if not i_ok: continue j_ok = False @@ -199,8 +198,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 @@ -317,21 +316,21 @@ def _solve_type_2(symo, X, Y, Z, th): Y = symo.replace(symo.CS12_simp(Y), 'Y', th) Z = symo.replace(symo.CS12_simp(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)) @@ -350,9 +349,9 @@ def _solve_type_3(symo, X1, Y1, Z1, X2, Y2, Z2, 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: + 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) @@ -373,7 +372,7 @@ def _solve_type_4(symo, X1, Y1, X2, Y2, th, r): X2 = symo.replace(symo.CS12_simp(X2), 'X2', th) Y2 = symo.replace(symo.CS12_simp(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))) @@ -396,7 +395,7 @@ def _solve_type_5(symo, X1, Y1, Z1, X2, Y2, Z2, th, 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): @@ -441,7 +440,7 @@ def _solve_type_8(symo, X, Y, Z1, Z2, th_i, th_j): Z2 = symo.replace(symo.CS12_simp(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) @@ -460,7 +459,7 @@ 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)) @@ -481,9 +480,9 @@ def _check_const(consts, *xs): def _get_coefs(eq, A1, A2, *xs): eqe = eq.expand() X = tools.get_max_coef(eqe, A1) - eqe = eqe.xreplace({A1: ZERO}) + eqe = eqe.xreplace({A1: tools.ZERO}) Y = tools.get_max_coef(eqe, A2) - Z = eqe.xreplace({A2: ZERO}) + 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) diff --git a/pysymoro/kinematics.py b/pysymoro/kinematics.py index 29dfac2..3ed70f3 100644 --- a/pysymoro/kinematics.py +++ b/pysymoro/kinematics.py @@ -8,7 +8,6 @@ from sympy import Matrix, zeros -from pysymoro.symoro import FAIL, ZERO from pysymoro.geometry import dgm, Transform from pysymoro.geometry import compute_rot_trans, Z_AXIS from symoroutils import symbolmgr @@ -134,7 +133,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 @@ -165,7 +164,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 @@ -184,9 +183,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: @@ -271,7 +270,7 @@ def jdot_qdot(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) @@ -311,8 +310,8 @@ def jacobian_determinant(robo, n, i, j, rows, cols): def kinematic_constraints(robo): 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', diff --git a/symoroui/definition.py b/symoroui/definition.py index 661d45e..ed0f5de 100644 --- a/symoroui/definition.py +++ b/symoroui/definition.py @@ -13,7 +13,7 @@ from sympy import Expr, Symbol -from pysymoro.symoro import SIMPLE, TREE, CLOSED_LOOP +from symoroutils import tools class DialogDefinition(wx.Dialog): @@ -50,10 +50,11 @@ def init_ui(self, name, nl, nj, is_mobile, structure): 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) + 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 + ) grid.Add(self.cmb_structure, pos=(3, 1)) self.cmb_structure.Bind(wx.EVT_COMBOBOX, self.OnTypeChanged) self.OnTypeChanged(None) diff --git a/symoroui/layout.py b/symoroui/layout.py index d4661d3..49a222e 100644 --- a/symoroui/layout.py +++ b/symoroui/layout.py @@ -13,15 +13,19 @@ import wx -from pysymoro.symoro import Robot, FAIL -from pysymoro import geometry, kinematics, dynamics, invgeom +from pysymoro.symoro import Robot +from pysymoro import geometry +from pysymoro import kinematics +from pysymoro import dynamics +from pysymoro import invgeom from symoroutils import parfile from symoroutils import filemgr -from symoroviz import graphics +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): @@ -281,7 +285,7 @@ def create_ui(self): 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) == FAIL: + 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) @@ -625,7 +629,7 @@ def OnOpen(self, event): if dialog_res == wx.CANCEL: return elif dialog_res == wx.YES: - if self.OnSave(None) == FAIL: + if self.OnSave(None) == tools.FAIL: return dialog = wx.FileDialog( self, @@ -641,7 +645,7 @@ def OnOpen(self, event): if new_robo is None: self.message_error('File could not be read!') else: - if flag == FAIL: + if flag == tools.FAIL: self.message_warning( "While reading file an error occured." ) @@ -660,7 +664,7 @@ def OnSaveAs(self, event): wildcard='*.par' ) if dialog.ShowModal() == wx.ID_CANCEL: - return FAIL + return tools.FAIL self.robo.directory = dialog.GetDirectory() self.robo.name = dialog.GetFilename()[:-4] parfile.writepar(self.robo) @@ -722,7 +726,7 @@ def OnDeterminant(self, event): dialog.Destroy() def OnCkel(self, event): - if kinematics.kinematic_constraints(self.robo) == FAIL: + if kinematics.kinematic_constraints(self.robo) == tools.FAIL: self.message_warning('There are no loops') else: self.model_success('ckel') @@ -786,7 +790,7 @@ def OnClose(self, event): '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: + if self.OnSave(_) == tools.FAIL: return elif result == wx.CANCEL: return diff --git a/symoroviz/graphics.py b/symoroviz/graphics.py index a5785af..f7f7582 100644 --- a/symoroviz/graphics.py +++ b/symoroviz/graphics.py @@ -15,10 +15,10 @@ from sympy import Expr -from pysymoro.symoro import CLOSED_LOOP from pysymoro.invgeom import loop_solve from pysymoro.geometry import dgm from symoroutils import symbolmgr +from symoroutils import tools from objects import Frame, RevoluteJoint, FixedJoint, PrismaticJoint @@ -194,7 +194,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() @@ -432,7 +432,7 @@ def init_ui(self): gridJnts.Add(s, pos=(p_index, 1), flag=wx.ALIGN_CENTER_VERTICAL) p_index += 1 - if self.robo.structure == CLOSED_LOOP: + if self.robo.structure == tools.CLOSED_LOOP: choise_list = ['Break Loops', 'Make Loops'] self.radioBox = wx.RadioBox(self.p, choices=choise_list, style=wx.RA_SPECIFY_ROWS) From d822ab60aa32d94d991f8a7cd5d9e74f052f5863 Mon Sep 17 00:00:00 2001 From: aravind Date: Thu, 17 Apr 2014 14:02:16 +0200 Subject: [PATCH 050/273] Add unittest - `symoroutils/tests/test_parfile.py` --- symoroutils/tests/test_parfile.py | 0 1 file changed, 0 insertions(+), 0 deletions(-) create mode 100644 symoroutils/tests/test_parfile.py diff --git a/symoroutils/tests/test_parfile.py b/symoroutils/tests/test_parfile.py new file mode 100644 index 0000000..e69de29 From ae36944b94f91fbc41ae187e3bb0a73fb9c2bd50 Mon Sep 17 00:00:00 2001 From: aravind Date: Thu, 17 Apr 2014 17:01:24 +0200 Subject: [PATCH 051/273] Refactor `parfile` module unit test code --- pysymoro/tests/test.py | 48 ++++++++++++------------------- symoroutils/parfile.py | 19 ++++++------ symoroutils/tests/test_parfile.py | 48 +++++++++++++++++++++++++++++++ 3 files changed, 76 insertions(+), 39 deletions(-) diff --git a/pysymoro/tests/test.py b/pysymoro/tests/test.py index 3ecbca7..984f283 100644 --- a/pysymoro/tests/test.py +++ b/pysymoro/tests/test.py @@ -26,22 +26,6 @@ class testMisc(unittest.TestCase): - def test_readwrite(self): - print "######## test_readwrite ##########" - original_robo = symoro.Robot.RX90() - parfile.writepar(original_robo) - fname = filemgr.get_clean_name(original_robo.name) + ".par" - file_path = os.path.join(original_robo.directory, fname) - new_robo, flag = parfile.readpar(original_robo.name, file_path) - 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() @@ -57,20 +41,20 @@ def test_robo_misc(self): 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 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) + 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), symoro.OK) - for Name in l3[1:]+l2[1:]+l1[1:]: + 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 symoro.INT_KEYS: - self.assertEqual(self.robo.get_val(i, Name), i) + 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) + v = var(name + str(i)) + self.assertEqual(self.robo.get_val(i, name), v) class testGeometry(unittest.TestCase): @@ -235,12 +219,16 @@ def test_dynamics(self): print 'Base parameters computation' dynamics.base_paremeters(robo) + if __name__ == '__main__': - unittest.main() -######################### suite = unittest.TestSuite() -## suite.addTest(testSymoroTrig('test_trig_simp')) + 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')) -# unittest.TextTestRunner().run(suite) + suite.addTest(testKinematics('test_jac2')) + unittest.TextTestRunner(verbosity=2).run(suite) diff --git a/symoroutils/parfile.py b/symoroutils/parfile.py index fd369c6..3fd969d 100644 --- a/symoroutils/parfile.py +++ b/symoroutils/parfile.py @@ -12,6 +12,7 @@ import re from symoroutils import filemgr +from symoroutils import tools from pysymoro import symoro @@ -60,8 +61,8 @@ def _extract_vals(robo, key, line): 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 + if robo.put_val(i+k, key, v.strip()) == tools.FAIL: + return tools.FAIL def _write_par_list(robo, f, key, N0, N): @@ -78,7 +79,7 @@ def writepar(robo): 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('Type = %s\n' % tools.TYPES.index(robo.structure)) f.write('is_mobile = %s\n' % int(robo.is_mobile)) f.write('\n(* Geometric parameters *)\n') if robo.is_mobile: @@ -106,7 +107,7 @@ def writepar(robo): def readpar(robo_name, file_path): """Return: robo: an instance of Robot, read from file - flag: indicates if any errors accured. (symoro.FAIL) + flag: indicates if any errors accured. (tools.FAIL) """ with open(file_path, 'r') as f: #initialize the Robot instance @@ -123,16 +124,16 @@ def readpar(robo_name, file_path): if match: is_mobile = _bool_dict[(match.group(1).strip())] if len(d) < 2: - return None, symoro.FAIL + return None, tools.FAIL NF = d['NJ']*2 - d['NL'] robo = symoro.Robot(robo_name, d['NL'], d['NJ'], NF, - is_mobile, symoro.TYPES[d['Type']]) + is_mobile, tools.TYPES[d['Type']]) robo.directory = os.path.dirname(file_path) #fitting the data acc_line = '' key = '' f.seek(0) - flag = symoro.OK + flag = tools.OK for line in f.readlines(): if line.find('(*') != -1: continue @@ -148,8 +149,8 @@ def readpar(robo_name, file_path): 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 + if _extract_vals(robo, key, acc_line) == tools.FAIL: + flag = tools.FAIL acc_line = '' key = '' return robo, flag diff --git a/symoroutils/tests/test_parfile.py b/symoroutils/tests/test_parfile.py index e69de29..aae4095 100644 --- a/symoroutils/tests/test_parfile.py +++ b/symoroutils/tests/test_parfile.py @@ -0,0 +1,48 @@ +#!/usr/bin/env python +# -*- coding: utf-8 -*- + + +"""Unit test module for functions in parfile.py file.""" + + +import os +import unittest + +from symoroutils import filemgr +from symoroutils import parfile +from symoroutils import tools +from pysymoro import symoro + + +class TestParfile(unittest.TestCase): + def setUp(self): + self.orig_robo = symoro.Robot.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() + + From e48eb4cfbd46274a91340b2f2e83ad14e2c4b5fd Mon Sep 17 00:00:00 2001 From: aravind Date: Thu, 17 Apr 2014 17:06:49 +0200 Subject: [PATCH 052/273] Rename `pysymoro/symoro.py` to `pysymoro/robot.py` --- pysymoro/{symoro.py => robot.py} | 0 1 file changed, 0 insertions(+), 0 deletions(-) rename pysymoro/{symoro.py => robot.py} (100%) diff --git a/pysymoro/symoro.py b/pysymoro/robot.py similarity index 100% rename from pysymoro/symoro.py rename to pysymoro/robot.py From 1346fe7a4f5a7b4a8ef50b1212836e231ff9ad1b Mon Sep 17 00:00:00 2001 From: aravind Date: Thu, 17 Apr 2014 17:29:02 +0200 Subject: [PATCH 053/273] Update module calls Update calls to `pysymoro.symoro` module with `pysymoro.robot` module. --- pysymoro/tests/test.py | 26 +++++++++++++------------- symoroui/layout.py | 2 +- symoroutils/parfile.py | 4 ++-- symoroutils/tests/test_parfile.py | 4 ++-- symoroviz/graphics.py | 4 ++-- 5 files changed, 20 insertions(+), 20 deletions(-) diff --git a/pysymoro/tests/test.py b/pysymoro/tests/test.py index 984f283..10c73fc 100644 --- a/pysymoro/tests/test.py +++ b/pysymoro/tests/test.py @@ -13,7 +13,7 @@ from sympy.abc import A, B, C, X, Y, Z from numpy import random, amax, matrix, eye, zeros -from pysymoro import symoro +from pysymoro import robot from pysymoro import geometry from pysymoro.geometry import Transform as trns from pysymoro import kinematics @@ -28,7 +28,7 @@ class testMisc(unittest.TestCase): def test_robo_misc(self): print "######## test_robo_misc ##########" - self.robo = symoro.Robot.SR400() + self.robo = robot.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]) @@ -61,16 +61,16 @@ class testGeometry(unittest.TestCase): def setUp(self): self.symo = symbolmgr.SymbolManager() - self.robo = symoro.Robot.RX90() + self.robo = robot.Robot.RX90() # def test_misc(self): -# self.assertEqual(self.robo.structure, symoro.SIMPLE) +# self.assertEqual(self.robo.structure, tools.SIMPLE) # self.robo.ant[3] = 0 -# self.assertEqual(self.robo.type_of_structure, symoro.TREE) +# self.assertEqual(self.robo.type_of_structure, tools.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) +# self.assertEqual(self.robo.type_of_structure, tools.SIMPLE) +# robo2 = robot.Robot.SR400() +# self.assertEqual(robo2.type_of_structure, tools.CLOSED_LOOP) def test_dgm_RX90(self): print "######## test_dgm_RX90 ##########" @@ -103,7 +103,7 @@ def test_dgm_RX90(self): def test_dgm_SR400(self): print "######## test_dgm_SR400 ##########" - self.robo = symoro.Robot.SR400() + self.robo = robot.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) @@ -135,7 +135,7 @@ def test_igm(self): def test_loop(self): print "######## test_loop ##########" - self.robo = symoro.Robot.SR400() + self.robo = robot.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) @@ -152,14 +152,14 @@ def test_loop(self): class testKinematics(unittest.TestCase): def setUp(self): self.symo = symbolmgr.SymbolManager() - self.robo = symoro.Robot.RX90() + self.robo = robot.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()) + kinematics.kinematic_constraints(robot.Robot.SR400()) def test_jac(self): print "######## test_jac ##########" @@ -199,7 +199,7 @@ def test_jac2(self): class testDynamics(unittest.TestCase): def test_dynamics(self): - robo = symoro.Robot.RX90() + robo = robot.Robot.RX90() print 'Inverse dynamic model using Newton - Euler Algorith' dynamics.inverse_dynamic_NE(robo) diff --git a/symoroui/layout.py b/symoroui/layout.py index 49a222e..4515ddb 100644 --- a/symoroui/layout.py +++ b/symoroui/layout.py @@ -13,7 +13,7 @@ import wx -from pysymoro.symoro import Robot +from pysymoro.robot import Robot from pysymoro import geometry from pysymoro import kinematics from pysymoro import dynamics diff --git a/symoroutils/parfile.py b/symoroutils/parfile.py index 3fd969d..cd2abd7 100644 --- a/symoroutils/parfile.py +++ b/symoroutils/parfile.py @@ -13,7 +13,7 @@ from symoroutils import filemgr from symoroutils import tools -from pysymoro import symoro +from pysymoro import robot _keywords = ['ant', 'sigma', 'b', 'd', 'r', @@ -126,7 +126,7 @@ def readpar(robo_name, file_path): if len(d) < 2: return None, tools.FAIL NF = d['NJ']*2 - d['NL'] - robo = symoro.Robot(robo_name, d['NL'], d['NJ'], NF, + robo = robot.Robot(robo_name, d['NL'], d['NJ'], NF, is_mobile, tools.TYPES[d['Type']]) robo.directory = os.path.dirname(file_path) #fitting the data diff --git a/symoroutils/tests/test_parfile.py b/symoroutils/tests/test_parfile.py index aae4095..713d0d0 100644 --- a/symoroutils/tests/test_parfile.py +++ b/symoroutils/tests/test_parfile.py @@ -11,12 +11,12 @@ from symoroutils import filemgr from symoroutils import parfile from symoroutils import tools -from pysymoro import symoro +from pysymoro import robot class TestParfile(unittest.TestCase): def setUp(self): - self.orig_robo = symoro.Robot.RX90() + self.orig_robo = robot.Robot.RX90() def test_readwrite(self): parfile.writepar(self.orig_robo) diff --git a/symoroviz/graphics.py b/symoroviz/graphics.py index f7f7582..b838e6f 100644 --- a/symoroviz/graphics.py +++ b/symoroviz/graphics.py @@ -556,8 +556,8 @@ def OnSliderChanged(self, _): if __name__ == '__main__': app = wx.PySimpleApp() - import symoro - robo = symoro.Robot.RX90() + from pysymoro import robot + robo = robot.Robot.RX90() robo.d[3] = 1. robo.r[4] = 1. frame = MainWindow(prefix='', robo=robo) From 3ccb0c0691db5106dbdda84d96c81d5e674d26fc Mon Sep 17 00:00:00 2001 From: aravind Date: Thu, 17 Apr 2014 17:30:35 +0200 Subject: [PATCH 054/273] Update URL in `setup.py` file --- setup.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/setup.py b/setup.py index 972e83e..bd25d4c 100644 --- a/setup.py +++ b/setup.py @@ -29,7 +29,7 @@ def apply_folder_join(item): name='symoro', version='0.1alpha', description='SYmoblic MOdelling of RObots software', - url='http://github.com/vijaravind/symoro', + url='http://github.com/symoro/symoro', scripts=bin_scripts, packages=find_packages(), zip_safe=False From fe3b4816c8bc6c5cb6148292b997efe16e7014dd Mon Sep 17 00:00:00 2001 From: aravind Date: Fri, 18 Apr 2014 12:39:38 +0200 Subject: [PATCH 055/273] Add `symoroutils/samplerobots.py` --- symoroutils/samplerobots.py | 0 1 file changed, 0 insertions(+), 0 deletions(-) create mode 100644 symoroutils/samplerobots.py diff --git a/symoroutils/samplerobots.py b/symoroutils/samplerobots.py new file mode 100644 index 0000000..e69de29 From 42edcc400824d2b02e36dd355a1a607982b38ab8 Mon Sep 17 00:00:00 2001 From: aravind Date: Fri, 18 Apr 2014 13:23:49 +0200 Subject: [PATCH 056/273] Move sample robot definitions to a new module Move sample robot definitions from `pysymoro/robot.py` module `Robot` class to a new module `symoroutils/samplerobots.py`. Update function calls in all places. --- pysymoro/kinematics.py | 7 +- pysymoro/robot.py | 100 +------------------------- pysymoro/tests/test.py | 29 ++++---- symoroui/layout.py | 3 +- symoroutils/samplerobots.py | 113 ++++++++++++++++++++++++++++++ symoroutils/tests/test_parfile.py | 3 +- symoroviz/graphics.py | 3 +- 7 files changed, 139 insertions(+), 119 deletions(-) diff --git a/pysymoro/kinematics.py b/pysymoro/kinematics.py index 3ed70f3..db9c5e5 100644 --- a/pysymoro/kinematics.py +++ b/pysymoro/kinematics.py @@ -12,6 +12,7 @@ from pysymoro.geometry import compute_rot_trans, Z_AXIS from symoroutils import symbolmgr from symoroutils import tools +from symoroutils import samplerobots from symoroutils.paramsinit import ParamsInit @@ -337,7 +338,7 @@ def kinematic_constraints(robo): #symo = symbolmgr.SymbolManager() #from symoro import Robot #from symoroutils import symbolmgr -#kinematic_constraints(Robot.SR400()) +#kinematic_constraints(samplerobots.sr400()) ##jacobian_determinant(robo, 6, range(6), range(6)) ###print _jac(robo, symo, 2, 5, 5) ###print _jac_det(robo, symo, 5) @@ -345,11 +346,11 @@ def kinematic_constraints(robo): ###print W[0] ###print W[1] ###speeds_accelerations(robo, symo) -###print _jac_inv(Robot.RX90(), symo, 2, 5, 5) +###print _jac_inv(samplerobots.rx90(), symo, 2, 5, 5) ## #def b(): # symo = symbolmgr.SymbolManager() -# print _jac_inv(Robot.RX90(), symo, 6, 3, 3) +# print _jac_inv(samplerobots.rx90(), symo, 6, 3, 3) ####from timeit import timeit #####print timeit(a, number=10) #####print timeit(b, number=10) diff --git a/pysymoro/robot.py b/pysymoro/robot.py index 6f8ed9c..e0f4d52 100644 --- a/pysymoro/robot.py +++ b/pysymoro/robot.py @@ -28,7 +28,7 @@ #TODO: write consistency check #TODO: Ask about QP QDP file writing. Number of joints is different #from number of links -class Robot: +class Robot(object): """Container of the robot parametric description. Responsible for low-level geometric transformation and direct geometric model generation. @@ -512,102 +512,4 @@ def get_param_vec(self, head, j): 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 - diff --git a/pysymoro/tests/test.py b/pysymoro/tests/test.py index 10c73fc..ea5c707 100644 --- a/pysymoro/tests/test.py +++ b/pysymoro/tests/test.py @@ -21,6 +21,7 @@ from pysymoro import dynamics from symoroutils import filemgr from symoroutils import parfile +from symoroutils import samplerobots from symoroutils import symbolmgr from symoroutils import tools @@ -28,7 +29,7 @@ class testMisc(unittest.TestCase): def test_robo_misc(self): print "######## test_robo_misc ##########" - self.robo = robot.Robot.SR400() + 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]) @@ -61,7 +62,7 @@ class testGeometry(unittest.TestCase): def setUp(self): self.symo = symbolmgr.SymbolManager() - self.robo = robot.Robot.RX90() + self.robo = samplerobots.rx90() # def test_misc(self): # self.assertEqual(self.robo.structure, tools.SIMPLE) @@ -69,11 +70,11 @@ def setUp(self): # self.assertEqual(self.robo.type_of_structure, tools.TREE) # self.robo.ant[3] = 2 # self.assertEqual(self.robo.type_of_structure, tools.SIMPLE) -# robo2 = robot.Robot.SR400() +# robo2 = samplerobots.sr400() # self.assertEqual(robo2.type_of_structure, tools.CLOSED_LOOP) - def test_dgm_RX90(self): - print "######## test_dgm_RX90 ##########" + 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) @@ -101,9 +102,9 @@ def test_dgm_RX90(self): [0, 0, 0, 1]]) self.assertEqual(T36, T_true36) - def test_dgm_SR400(self): - print "######## test_dgm_SR400 ##########" - self.robo = robot.Robot.SR400() + 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) @@ -135,7 +136,7 @@ def test_igm(self): def test_loop(self): print "######## test_loop ##########" - self.robo = robot.Robot.SR400() + self.robo = samplerobots.sr400() invgeom.loop_solve(self.robo, self.symo) l_solver = self.symo.gen_func('IGM_gen', self.robo.q_vec, self.robo.q_active) @@ -152,14 +153,14 @@ def test_loop(self): class testKinematics(unittest.TestCase): def setUp(self): self.symo = symbolmgr.SymbolManager() - self.robo = robot.Robot.RX90() + self.robo = samplerobots.rx90() def test_speeds(self): print 'Speeds and accelerations' kinematics.speeds_accelerations(self.robo) print 'Kinematic constraint equations' - kinematics.kinematic_constraints(robot.Robot.SR400()) + kinematics.kinematic_constraints(samplerobots.sr400()) def test_jac(self): print "######## test_jac ##########" @@ -199,7 +200,7 @@ def test_jac2(self): class testDynamics(unittest.TestCase): def test_dynamics(self): - robo = robot.Robot.RX90() + robo = samplerobots.rx90() print 'Inverse dynamic model using Newton - Euler Algorith' dynamics.inverse_dynamic_NE(robo) @@ -223,8 +224,8 @@ def test_dynamics(self): 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_dgm_rx90')) + suite.addTest(testGeometry('test_dgm_sr400')) suite.addTest(testGeometry('test_igm')) suite.addTest(testGeometry('test_loop')) suite.addTest(testKinematics('test_jac')) diff --git a/symoroui/layout.py b/symoroui/layout.py index 4515ddb..6e469a2 100644 --- a/symoroui/layout.py +++ b/symoroui/layout.py @@ -20,6 +20,7 @@ from pysymoro import invgeom from symoroutils import parfile from symoroutils import filemgr +from symoroutils import samplerobots from symoroutils import tools from symoroui import definition as ui_definition from symoroui import geometry as ui_geometry @@ -39,7 +40,7 @@ def __init__(self, *args, **kwargs): # create menu bar self.create_menu() # set default robot - self.robo = Robot.RX90() + self.robo = samplerobots.rx90() # object to store different ui elements self.widgets = {} # object to store parameter values got from dialog box input diff --git a/symoroutils/samplerobots.py b/symoroutils/samplerobots.py index e69de29..c04aed9 100644 --- a/symoroutils/samplerobots.py +++ b/symoroutils/samplerobots.py @@ -0,0 +1,113 @@ +# -*- coding: utf-8 -*- + + +""" +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() + 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 + + +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, 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) +# 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 + + + diff --git a/symoroutils/tests/test_parfile.py b/symoroutils/tests/test_parfile.py index 713d0d0..744f969 100644 --- a/symoroutils/tests/test_parfile.py +++ b/symoroutils/tests/test_parfile.py @@ -10,13 +10,14 @@ 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 = robot.Robot.RX90() + self.orig_robo = samplerobots.rx90() def test_readwrite(self): parfile.writepar(self.orig_robo) diff --git a/symoroviz/graphics.py b/symoroviz/graphics.py index b838e6f..38bd4c4 100644 --- a/symoroviz/graphics.py +++ b/symoroviz/graphics.py @@ -17,6 +17,7 @@ from pysymoro.invgeom import loop_solve from pysymoro.geometry import dgm +from symoroutils import samplerobots from symoroutils import symbolmgr from symoroutils import tools @@ -557,7 +558,7 @@ def OnSliderChanged(self, _): if __name__ == '__main__': app = wx.PySimpleApp() from pysymoro import robot - robo = robot.Robot.RX90() + robo = samplerobots.rx90() robo.d[3] = 1. robo.r[4] = 1. frame = MainWindow(prefix='', robo=robo) From 2ac052c8a1740c64644f1a2b28fb21a622788e0e Mon Sep 17 00:00:00 2001 From: aravind Date: Tue, 22 Apr 2014 15:59:24 +0200 Subject: [PATCH 057/273] Add `planar2r` to `symoroutils/samplerobots.py` --- symoroui/layout.py | 2 +- symoroutils/samplerobots.py | 15 +++++++++++++++ 2 files changed, 16 insertions(+), 1 deletion(-) diff --git a/symoroui/layout.py b/symoroui/layout.py index 6e469a2..6bc3cef 100644 --- a/symoroui/layout.py +++ b/symoroui/layout.py @@ -40,7 +40,7 @@ def __init__(self, *args, **kwargs): # create menu bar self.create_menu() # set default robot - self.robo = samplerobots.rx90() + self.robo = samplerobots.planar2r() # object to store different ui elements self.widgets = {} # object to store parameter values got from dialog box input diff --git a/symoroutils/samplerobots.py b/symoroutils/samplerobots.py index c04aed9..7529100 100644 --- a/symoroutils/samplerobots.py +++ b/symoroutils/samplerobots.py @@ -54,6 +54,21 @@ def cart_pole(): return robo +def planar2r(): + """Generate Robot instance of 2R Planar robot""" + robo = Robot('Planar2R', 2, 2, 3, False) + robo.structure = tools.SIMPLE + robo.sigma = [2, 0, 0, 2] + robo.mu = [0, 1, 1, 0] + robo.gamma = [0, 0, 0, 0] + robo.b = [0, 0, 0, 0] + robo.alpha = [0, 0, 0, 0] + robo.d = [0, 0, var('L1'), var('L2')] + robo.theta = [0, var('th1'), var('th2'), 0] + robo.r = [0, 0, 0, 0] + return robo + + def sr400(): #TODO: bring it to the new notation with 0-frame """Generate Robot instance of SR400""" From dcf1df83d28903a89e94cb8ccbda0ed67be4d3f8 Mon Sep 17 00:00:00 2001 From: aravind Date: Wed, 23 Apr 2014 01:45:36 +0200 Subject: [PATCH 058/273] Add `Screw` class and corresponding unit test file --- pysymoro/floatr.py | 133 +++++++++++++++++++++++++++++++++++ pysymoro/tests/test_screw.py | 82 +++++++++++++++++++++ 2 files changed, 215 insertions(+) create mode 100644 pysymoro/floatr.py create mode 100644 pysymoro/tests/test_screw.py diff --git a/pysymoro/floatr.py b/pysymoro/floatr.py new file mode 100644 index 0000000..b73c63b --- /dev/null +++ b/pysymoro/floatr.py @@ -0,0 +1,133 @@ +# -*- 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 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 __init__(self, value=zeros(6, 1)): +# """ +# Another Constructor. +# +# Args: +# value: A 6x1 Matrix set to 0 by default. +# """ +# if value.rows != 6 or value.cols != 1: +# raise ShapeError("Matrix size has to be 6x1.") +# self._val = value + + @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 + +# @val.setter +# def val(self, lin, ang): +# """ +# Set the linear and angular terms individually. +# +# Args: +# lin: A 3x1 Matrix - linear term +# ang: A 3X1 Matrix - angular term +# """ +# 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 + + @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 + + diff --git a/pysymoro/tests/test_screw.py b/pysymoro/tests/test_screw.py new file mode 100644 index 0000000..25d05b6 --- /dev/null +++ b/pysymoro/tests/test_screw.py @@ -0,0 +1,82 @@ +#!/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.floatr 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.lin = Matrix([3, 3]) + + +def main(): + """Main function.""" + # load and run the unittests + unit_suite = unittest.TestLoader().loadTestsFromTestCase(TestScrew) + unittest.TextTestRunner(verbosity=2).run(unit_suite) + + +if __name__ == '__main__': + main() + + From 1c02628edebf52dc5ab7cac5d8eb5bb55c131ee4 Mon Sep 17 00:00:00 2001 From: aravind Date: Wed, 23 Apr 2014 06:31:16 +0200 Subject: [PATCH 059/273] Move `Screw` class to `pysymoro/screw.py` file Update unit test file accordingly. --- pysymoro/screw.py | 131 +++++++++++++++++++++++++++++++++++ pysymoro/tests/test_screw.py | 2 +- 2 files changed, 132 insertions(+), 1 deletion(-) create mode 100644 pysymoro/screw.py diff --git a/pysymoro/screw.py b/pysymoro/screw.py new file mode 100644 index 0000000..0414231 --- /dev/null +++ b/pysymoro/screw.py @@ -0,0 +1,131 @@ +# -*- 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 __init__(self, value=zeros(6, 1)): +# """ +# Another Constructor. +# +# Args: +# value: A 6x1 Matrix set to 0 by default. +# """ +# if value.rows != 6 or value.cols != 1: +# raise ShapeError("Matrix size has to be 6x1.") +# self._val = value + + @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 + +# @val.setter +# def val(self, lin, ang): +# """ +# Set the linear and angular terms individually. +# +# Args: +# lin: A 3x1 Matrix - linear term +# ang: A 3X1 Matrix - angular term +# """ +# 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 + + @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 + + diff --git a/pysymoro/tests/test_screw.py b/pysymoro/tests/test_screw.py index 25d05b6..191065b 100644 --- a/pysymoro/tests/test_screw.py +++ b/pysymoro/tests/test_screw.py @@ -11,7 +11,7 @@ from sympy import ShapeError from sympy import zeros -from pysymoro.floatr import Screw +from pysymoro.screw import Screw class TestScrew(unittest.TestCase): From c47d6bc8d43728bd64a5e5ebb80a8c3fc20d63aa Mon Sep 17 00:00:00 2001 From: aravind Date: Wed, 23 Apr 2014 06:32:12 +0200 Subject: [PATCH 060/273] Create `FloatingRobot` class --- pysymoro/floatr.py | 120 ++------------------------------------------- 1 file changed, 5 insertions(+), 115 deletions(-) diff --git a/pysymoro/floatr.py b/pysymoro/floatr.py index b73c63b..a4ead41 100644 --- a/pysymoro/floatr.py +++ b/pysymoro/floatr.py @@ -12,122 +12,12 @@ from sympy import ShapeError -class Screw(object): +class FloatingRobot(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 :) + Represent the FloatingRobot data structure and provide methods that + act as the gateway for robot modelling. """ - 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 __init__(self, value=zeros(6, 1)): -# """ -# Another Constructor. -# -# Args: -# value: A 6x1 Matrix set to 0 by default. -# """ -# if value.rows != 6 or value.cols != 1: -# raise ShapeError("Matrix size has to be 6x1.") -# self._val = value - - @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 - -# @val.setter -# def val(self, lin, ang): -# """ -# Set the linear and angular terms individually. -# -# Args: -# lin: A 3x1 Matrix - linear term -# ang: A 3X1 Matrix - angular term -# """ -# 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 - - @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 __init__(self, *args, **kwargs): + pass From 96ca06b93aa33fbe779a6e7c6c85439e5b62b1a8 Mon Sep 17 00:00:00 2001 From: aravind Date: Wed, 23 Apr 2014 22:02:53 +0200 Subject: [PATCH 061/273] Add `pysymoro/screw6.py` --- pysymoro/screw6.py | 133 +++++++++++++++++++++++++++++++++++++++++++++ 1 file changed, 133 insertions(+) create mode 100644 pysymoro/screw6.py diff --git a/pysymoro/screw6.py b/pysymoro/screw6.py new file mode 100644 index 0000000..3d13c48 --- /dev/null +++ b/pysymoro/screw6.py @@ -0,0 +1,133 @@ +# -*- 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.""" + self._val = zeros(6, 6) + + @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 + + From 7b9889fefe2cf9597d72a1395255d1aba2dfc44c Mon Sep 17 00:00:00 2001 From: aravind Date: Wed, 23 Apr 2014 23:25:00 +0200 Subject: [PATCH 062/273] Add unit test corresponding to `Screw6` class --- pysymoro/tests/test_screw.py | 12 ++-- pysymoro/tests/test_screw6.py | 126 ++++++++++++++++++++++++++++++++++ 2 files changed, 134 insertions(+), 4 deletions(-) create mode 100644 pysymoro/tests/test_screw6.py diff --git a/pysymoro/tests/test_screw.py b/pysymoro/tests/test_screw.py index 191065b..f6e8b48 100644 --- a/pysymoro/tests/test_screw.py +++ b/pysymoro/tests/test_screw.py @@ -66,16 +66,20 @@ def test_ang(self): self.indiv.ang = Matrix([4, 5, 6]) self.assertEqual(self.indiv.ang, Matrix([4, 5, 6])) with self.assertRaises(ShapeError): - self.empty.lin = Matrix([3, 3]) + self.empty.ang = Matrix([3, 3]) -def main(): - """Main function.""" - # load and run the unittests +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..a5ba5e7 --- /dev/null +++ b/pysymoro/tests/test_screw6.py @@ -0,0 +1,126 @@ +#!/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) + + 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 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() + + From bde4fc773f5610b95de010ef359d8a797fc9e74f Mon Sep 17 00:00:00 2001 From: aravind Date: Wed, 23 Apr 2014 23:35:03 +0200 Subject: [PATCH 063/273] Add `pysymoro/tests/run_all_tests.py` file --- pysymoro/tests/run_all_tests.py | 18 ++++++++++++++++++ 1 file changed, 18 insertions(+) create mode 100644 pysymoro/tests/run_all_tests.py diff --git a/pysymoro/tests/run_all_tests.py b/pysymoro/tests/run_all_tests.py new file mode 100644 index 0000000..6e4fc53 --- /dev/null +++ b/pysymoro/tests/run_all_tests.py @@ -0,0 +1,18 @@ +#!/usr/bin/env python +# -*- coding: utf-8 -*- + + +import test_screw +import test_screw6 + + +def main(): + """Main function.""" + test_screw.run_tests() + test_screw6.run_tests() + + +if __name__ == '__main__': + main() + + From e24ef8210ab6fa3e9792ed9819db3076041ba006 Mon Sep 17 00:00:00 2001 From: aravind Date: Thu, 24 Apr 2014 02:47:56 +0200 Subject: [PATCH 064/273] Modify `Screw6` constructor and unit test --- pysymoro/screw6.py | 42 ++++++++++++++++++++++++++++++++++- pysymoro/tests/test_screw6.py | 17 ++++++++++++++ 2 files changed, 58 insertions(+), 1 deletion(-) diff --git a/pysymoro/screw6.py b/pysymoro/screw6.py index 3d13c48..294c182 100644 --- a/pysymoro/screw6.py +++ b/pysymoro/screw6.py @@ -17,8 +17,48 @@ class Screw6(object): which in turn contains four 3x3 matrices. """ def __init__(self, *args, **kwargs): - """Constructor period.""" + """ + 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))) + ) @property def val(self): diff --git a/pysymoro/tests/test_screw6.py b/pysymoro/tests/test_screw6.py index a5ba5e7..a31db94 100644 --- a/pysymoro/tests/test_screw6.py +++ b/pysymoro/tests/test_screw6.py @@ -56,6 +56,23 @@ def test_init(self): # 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()""" From 68ecee2e7b430e693bfe8dcc857cfbd473032d4a Mon Sep 17 00:00:00 2001 From: aravind Date: Fri, 25 Apr 2014 15:49:29 +0200 Subject: [PATCH 065/273] Add `DynParams` class and its unittest module Add `DynParams` class which is a data structure to hold all the inertial parameters, mass tensor parameters, friction parameters and external force parameters. --- pysymoro/dynparams.py | 163 +++++++++++++++++++++++++++++++ pysymoro/tests/test_dynparams.py | 124 +++++++++++++++++++++++ 2 files changed, 287 insertions(+) create mode 100644 pysymoro/dynparams.py create mode 100644 pysymoro/tests/test_dynparams.py diff --git a/pysymoro/dynparams.py b/pysymoro/dynparams.py new file mode 100644 index 0000000..dd5fd62 --- /dev/null +++ b/pysymoro/dynparams.py @@ -0,0 +1,163 @@ +# -*- coding: utf-8 -*- + + +""" +This module contains the DynParams data structure. +""" + + +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): + """ + Constructor period. + + Usage: + DynParams(link=) + """ + 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() + + @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(): + value = term + str(self.link) + setattr(self, key, var(value)) + + def _init_ms_terms(self): + """Initialise mass tensor terms and mass of the link.""" + for key, term in self._ms_terms.iteritems(): + value = term + str(self.link) + setattr(self, key, var(value)) + + def _init_fr_terms(self): + """Initialise rotor inertia and friction parameters.""" + for key, term in self._fr_terms.iteritems(): + value = term + str(self.link) + setattr(self, key, var(value)) + + def _init_ext_force_terms(self): + """Initialise external force terms.""" + for key, term in self._ext_force_terms.iteritems(): + value = term + str(self.link) + setattr(self, key, var(value)) + + diff --git a/pysymoro/tests/test_dynparams.py b/pysymoro/tests/test_dynparams.py new file mode 100644 index 0000000..8a77174 --- /dev/null +++ b/pysymoro/tests/test_dynparams.py @@ -0,0 +1,124 @@ +#!/usr/bin/env python +# -*- coding: utf-8 -*- + + +"""Unit test module for Screw6 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 Screw6 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]) + ) + + 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 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() + + From 4766012edf7da547c850fb8331d1054902443309 Mon Sep 17 00:00:00 2001 From: aravind Date: Fri, 25 Apr 2014 15:51:57 +0200 Subject: [PATCH 066/273] Add `==`, `!=` operators to `Screw` and `Screw6` Add `==` and `!=` operators to `Screw` and `Screw6` classes. Add relevant unit test code for these operators. --- pysymoro/screw.py | 12 ++++++++++++ pysymoro/screw6.py | 12 ++++++++++++ pysymoro/tests/run_all_tests.py | 2 ++ pysymoro/tests/test_screw.py | 12 ++++++++++++ pysymoro/tests/test_screw6.py | 12 ++++++++++++ symoroutils/tools.py | 8 +++++--- 6 files changed, 55 insertions(+), 3 deletions(-) diff --git a/pysymoro/screw.py b/pysymoro/screw.py index 0414231..4d681ab 100644 --- a/pysymoro/screw.py +++ b/pysymoro/screw.py @@ -128,4 +128,16 @@ def ang(self, value): 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 index 294c182..16aad01 100644 --- a/pysymoro/screw6.py +++ b/pysymoro/screw6.py @@ -170,4 +170,16 @@ def botright(self, value): 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/run_all_tests.py b/pysymoro/tests/run_all_tests.py index 6e4fc53..22f6d95 100644 --- a/pysymoro/tests/run_all_tests.py +++ b/pysymoro/tests/run_all_tests.py @@ -4,12 +4,14 @@ import test_screw import test_screw6 +import test_dynparams def main(): """Main function.""" test_screw.run_tests() test_screw6.run_tests() + test_dynparams.run_tests() if __name__ == '__main__': diff --git a/pysymoro/tests/test_screw.py b/pysymoro/tests/test_screw.py index f6e8b48..73039c1 100644 --- a/pysymoro/tests/test_screw.py +++ b/pysymoro/tests/test_screw.py @@ -68,6 +68,18 @@ def test_ang(self): 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""" diff --git a/pysymoro/tests/test_screw6.py b/pysymoro/tests/test_screw6.py index a31db94..088b293 100644 --- a/pysymoro/tests/test_screw6.py +++ b/pysymoro/tests/test_screw6.py @@ -125,6 +125,18 @@ def test_botright(self): 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""" diff --git a/symoroutils/tools.py b/symoroutils/tools.py index 85263ed..444a84c 100644 --- a/symoroutils/tools.py +++ b/symoroutils/tools.py @@ -38,9 +38,11 @@ def skew(vec): ======= hat: Matrix 3x3 """ - return Matrix([[0, -vec[2], vec[1]], - [vec[2], 0, -vec[0]], - [-vec[1], vec[0], 0]]) + return Matrix([ + [0, -vec[2], vec[1]], + [vec[2], 0, -vec[0]], + [-vec[1], vec[0], 0] + ]) def l2str(list_var, spacing=8): From 6b861363670f57a9f9223e344667cecaf4253843 Mon Sep 17 00:00:00 2001 From: aravind Date: Tue, 29 Apr 2014 13:46:59 +0200 Subject: [PATCH 067/273] Update `symoroui/labels.py` with new keys Update `symoroui/labels.py` with new keys and include a new attribute to store the widget keys in `symoroui/layout.py`. --- symoroui/labels.py | 24 ++++++++++++------------ symoroui/layout.py | 12 +++++++++--- 2 files changed, 21 insertions(+), 15 deletions(-) diff --git a/symoroui/labels.py b/symoroui/labels.py index 8797a68..867596a 100644 --- a/symoroui/labels.py +++ b/symoroui/labels.py @@ -70,25 +70,25 @@ ]) # mass tensor params DYN_PARAMS_M = OrderedDict([ - ('mx', FieldEntry('MX', 'MX', 'txt', (1, 0), 'OnDynParamChanged', -1)), - ('my', FieldEntry('MY', 'MY', 'txt', (1, 1), 'OnDynParamChanged', -1)), - ('mz', FieldEntry('MZ', 'MZ', 'txt', (1, 2), 'OnDynParamChanged', -1)), - ('m', FieldEntry('M', 'M', 'txt', (1, 3), 'OnDynParamChanged', -1)) + ('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', (2, 0), 'OnDynParamChanged', -1)), - ('fc', FieldEntry('FS', 'FS', 'txt', (2, 1), 'OnDynParamChanged', -1)), - ('fv', FieldEntry('FV', 'FV', 'txt', (2, 2), 'OnDynParamChanged', -1)) + ('frc', FieldEntry('FS', 'FS', 'txt', (2, 1), 'OnDynParamChanged', -1)), + ('frv', FieldEntry('FV', 'FV', 'txt', (2, 2), 'OnDynParamChanged', -1)) ]) # external force, moments params DYN_PARAMS_F = OrderedDict([ - ('ex_fx', FieldEntry('FX', 'FX', 'txt', (3, 0), 'OnDynParamChanged', -1)), - ('ex_fy', FieldEntry('FY', 'FY', 'txt', (3, 1), 'OnDynParamChanged', -1)), - ('ex_fz', FieldEntry('FZ', 'FZ', 'txt', (3, 2), 'OnDynParamChanged', -1)), - ('ex_mx', FieldEntry('CX', 'CX', 'txt', (3, 3), 'OnDynParamChanged', -1)), - ('ex_my', FieldEntry('CY', 'CY', 'txt', (3, 4), 'OnDynParamChanged', -1)), - ('ex_mz', FieldEntry('CZ', 'CZ', 'txt', (3, 5), 'OnDynParamChanged', -1)) + ('fx_ext', FieldEntry('FX', 'FX', 'txt', (3, 0), 'OnDynParamChanged', -1)), + ('fy_ext', FieldEntry('FY', 'FY', 'txt', (3, 1), 'OnDynParamChanged', -1)), + ('fz_ext', FieldEntry('FZ', 'FZ', 'txt', (3, 2), 'OnDynParamChanged', -1)), + ('mx_ext', FieldEntry('CX', 'CX', 'txt', (3, 3), 'OnDynParamChanged', -1)), + ('my_ext', FieldEntry('CY', 'CY', 'txt', (3, 4), 'OnDynParamChanged', -1)), + ('mz_ext', FieldEntry('CZ', 'CZ', 'txt', (3, 5), 'OnDynParamChanged', -1)) ]) # dynamic params got by concatenation DYN_PARAMS = OrderedDict( diff --git a/symoroui/layout.py b/symoroui/layout.py index 6bc3cef..22c53bf 100644 --- a/symoroui/layout.py +++ b/symoroui/layout.py @@ -41,8 +41,9 @@ def __init__(self, *args, **kwargs): self.create_menu() # set default robot self.robo = samplerobots.planar2r() - # object to store different ui elements + # 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 @@ -88,6 +89,7 @@ def params_in_grid(self, szr_grd, elements, rows, cols, width=70): ) 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( @@ -127,16 +129,18 @@ def create_ui(self): 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( + ctrl = wx.StaticText( self.panel, size=(150, -1), name=ui_labels.ROBOT_TYPE[key].name ) + self.widgets[name] = ctrl + self.widget_keys[key] = ctrl 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), + ctrl, pos=(idx, 1), flag=wx.LEFT | wx.RIGHT, border=10 ) szr_robot_type.Add( @@ -175,6 +179,7 @@ def create_ui(self): 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 ) @@ -229,6 +234,7 @@ def create_ui(self): 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( From 8f588f9a0afae9832f7322ca3ffa9a550140c736 Mon Sep 17 00:00:00 2001 From: aravind Date: Tue, 29 Apr 2014 15:23:38 +0200 Subject: [PATCH 068/273] Add `update_params()` to `DynParams` class Add `update_params()` to `DynParams` class. This method receives a dict as input argument and updates the parameter values mentioned in the dict input. Update corresponding unittest module to test the method. --- pysymoro/dynparams.py | 28 +++++++++++++++++++++++++--- pysymoro/tests/test_dynparams.py | 26 ++++++++++++++++++++++++-- 2 files changed, 49 insertions(+), 5 deletions(-) diff --git a/pysymoro/dynparams.py b/pysymoro/dynparams.py index dd5fd62..fabbb3d 100644 --- a/pysymoro/dynparams.py +++ b/pysymoro/dynparams.py @@ -25,12 +25,13 @@ class DynParams(object): 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): + def __init__(self, link, params=None): """ Constructor period. Usage: DynParams(link=) + DynParams(link=, params=) """ self.link = link """Inertia matrix terms""" @@ -49,7 +50,7 @@ def __init__(self, link): """Rotor inertia term""" self.ia = None """Coulomb friction parameter""" - self.frc = None + self.frc = None """Viscous friction parameter""" self.frv = None """External forces and moments""" @@ -62,7 +63,7 @@ def __init__(self, link): # lists to hold the string representation for the prefix of # different terms self._inertial_terms = { - 'xx': 'XX', + 'xx': 'XX', 'xy': 'XY', 'xz': 'XZ', 'yy': 'YY', @@ -93,6 +94,27 @@ def __init__(self, link): 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 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 + ) @property def inertia(self): diff --git a/pysymoro/tests/test_dynparams.py b/pysymoro/tests/test_dynparams.py index 8a77174..b0222e9 100644 --- a/pysymoro/tests/test_dynparams.py +++ b/pysymoro/tests/test_dynparams.py @@ -41,7 +41,7 @@ def setUp(self): # rotor inertia term self.ia = var('IA33') # coulomb friction parameter - self.frc = var('FS33') + self.frc = var('FS33') # viscous friction parameter self.frv = var('FV33') # external forces and moments @@ -68,6 +68,16 @@ def setUp(self): 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.""" @@ -98,12 +108,24 @@ 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 run_tests(): """Load and run the unittests""" From a2dc897d8e431d23a9531b0c7fe117bf6e0c7969 Mon Sep 17 00:00:00 2001 From: aravind Date: Tue, 29 Apr 2014 16:10:29 +0200 Subject: [PATCH 069/273] Add `GeoParams` class and its unittest module Add `GeoParams` class which is data structure to hold the geometric parameters - frame, antecedant, sigma, mu, gamma, b, alpha, d, theta, r. --- pysymoro/geoparams.py | 68 ++++++++++++++++++++++++++ pysymoro/robot.py | 56 ++++++++++----------- pysymoro/tests/run_all_tests.py | 2 + pysymoro/tests/test_dynparams.py | 4 +- pysymoro/tests/test_geoparams.py | 84 ++++++++++++++++++++++++++++++++ 5 files changed, 184 insertions(+), 30 deletions(-) create mode 100644 pysymoro/geoparams.py create mode 100644 pysymoro/tests/test_geoparams.py diff --git a/pysymoro/geoparams.py b/pysymoro/geoparams.py new file mode 100644 index 0000000..627a960 --- /dev/null +++ b/pysymoro/geoparams.py @@ -0,0 +1,68 @@ +# -*- coding: utf-8 -*- + + +""" +This module contains the GeoParams data structure. +""" + + +class GeoParams(object): + """ + Data structure: + Represent the data structure to hold the geometric parameters. + """ + def __init__(self, frame, params=None): + """ + Constructor period. + + Usage: + GeoParams(frame=) + GeoParams(frame=, params=) + """ + self.frame = frame + self.ant = 0 + self.sigma = 0 + self.mu = 0 + self.gamma = 0 + self.b = 0 + self.alpha = 0 + self.d = 0 + self.theta = 0 + self.r = 0 + # initialise with values if available + if params is not None: + self.update_params(params) + + 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 + ) + + @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/robot.py b/pysymoro/robot.py index e0f4d52..351ff28 100644 --- a/pysymoro/robot.py +++ b/pysymoro/robot.py @@ -36,26 +36,27 @@ class Robot(object): 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 = filemgr.get_folder_path(name) + self.name = name """ directory name""" + self.directory = filemgr.get_folder_path(name) + """ whether the base frame is floating: bool""" self.is_mobile = is_mobile - """ whethere the base frame is floating: bool""" - self.nl = NL """ number of links: int""" - self.nj = NJ + self.nl = NL """ number of joints: int""" - self.nf = NF + self.nj = NJ """ number of frames: int""" - self.structure = structure + self.nf = NF """ type of robot's structure""" - self.sigma = [0 for i in xrange(NF + 1)] + self.structure = structure """ joint type: list of int""" - self.ant = range(-1, self.NF - 1) + self.sigma = [0 for i in xrange(NF + 1)] """ index of antecedent joint: list of int""" - self.mu = [0 for i in xrange(NF + 1)] + self.ant = range(-1, self.NF - 1) """motorization, if 1, then the joint im motorized""" + self.mu = [0 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)] @@ -67,46 +68,45 @@ def __init__(self, name, NL=0, NJ=0, NF=0, is_mobile=False, 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""" + self.Z = eye(4) num = range(self.NL) numj = range(self.NJ) - self.w0 = zeros(3, 1) """ base angular velocity: 3x1 matrix""" - self.wdot0 = zeros(3, 1) + self.w0 = zeros(3, 1) """ base angular acceleration: 3x1 matrix""" - self.v0 = zeros(3, 1) + self.wdot0 = zeros(3, 1) """ base linear velocity: 3x1 matrix""" - self.vdot0 = zeros(3, 1) + self.v0 = zeros(3, 1) """ base linear acceleration: 3x1 matrix""" - self.qdot = [var('QP{0}'.format(i)) for i in numj] + self.vdot0 = zeros(3, 1) """ joint speed: list of var""" - self.qddot = [var('QDP{0}'.format(i)) for i in numj] + self.qdot = [var('QP{0}'.format(i)) for i in numj] """ joint acceleration: list of var""" - self.Nex = [zeros(3, 1) for i in num] + 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))) - self.Fex = [zeros(3, 1) for i in num] """ 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))) - 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] + self.FS = [var('FS{0}'.format(i)) for i in num] """ joint actuator inertia: list of var""" - self.FV = [var('FV{0}'.format(i)) for i in num] + self.IA = [var('IA{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] + self.FV = [var('FV{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] + self.MS = [Matrix(var('MX{0}, MY{0}, MZ{0}'.format(i))) for i in num] """ mass of link: list of var""" - self.GAM = [var('GAM{0}'.format(i)) for i in numj] + 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] 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')]) + self.J = [Matrix(3, 3, var(J_str.format(i))) for i in num] """ gravity vector: 3x1 matrix""" + self.G = Matrix([0, 0, var('G3')]) # member methods: def put_val(self, j, name, val): diff --git a/pysymoro/tests/run_all_tests.py b/pysymoro/tests/run_all_tests.py index 22f6d95..3714827 100644 --- a/pysymoro/tests/run_all_tests.py +++ b/pysymoro/tests/run_all_tests.py @@ -4,6 +4,7 @@ import test_screw import test_screw6 +import test_geoparams import test_dynparams @@ -11,6 +12,7 @@ def main(): """Main function.""" test_screw.run_tests() test_screw6.run_tests() + test_geoparams.run_tests() test_dynparams.run_tests() diff --git a/pysymoro/tests/test_dynparams.py b/pysymoro/tests/test_dynparams.py index b0222e9..85e0841 100644 --- a/pysymoro/tests/test_dynparams.py +++ b/pysymoro/tests/test_dynparams.py @@ -2,7 +2,7 @@ # -*- coding: utf-8 -*- -"""Unit test module for Screw6 class.""" +"""Unit test module for DynParams class.""" import unittest @@ -18,7 +18,7 @@ class TestDynParams(unittest.TestCase): - """Unit test for Screw6 class.""" + """Unit test for DynParams class.""" def setUp(self): link = 33 # setup an instance of DynParams diff --git a/pysymoro/tests/test_geoparams.py b/pysymoro/tests/test_geoparams.py new file mode 100644 index 0000000..e047188 --- /dev/null +++ b/pysymoro/tests/test_geoparams.py @@ -0,0 +1,84 @@ +#!/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()""" + # 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() + + From a674f946189e4becb45165e69f7e6f37fbf09ec4 Mon Sep 17 00:00:00 2001 From: aravind Date: Tue, 29 Apr 2014 16:18:51 +0200 Subject: [PATCH 070/273] Add scaffolding code to `pysymoro/floatr.py` --- pysymoro/dynparams.py | 14 ++--- pysymoro/floatr.py | 118 ++++++++++++++++++++++++++++++++++++++++-- 2 files changed, 121 insertions(+), 11 deletions(-) diff --git a/pysymoro/dynparams.py b/pysymoro/dynparams.py index fabbb3d..c602baa 100644 --- a/pysymoro/dynparams.py +++ b/pysymoro/dynparams.py @@ -34,26 +34,26 @@ def __init__(self, link, params=None): DynParams(link=, params=) """ self.link = link - """Inertia matrix terms""" + # Inertia matrix terms""" self.xx = None self.xy = None self.xz = None self.yy = None self.yz = None self.zz = None - """Mass tensor terms""" + # Mass tensor terms""" self.msx = None self.msy = None self.msz = None - """Link mass""" + # Link mass""" self.mass = None - """Rotor inertia term""" + # Rotor inertia term""" self.ia = None - """Coulomb friction parameter""" + # Coulomb friction parameter""" self.frc = None - """Viscous friction parameter""" + # Viscous friction parameter""" self.frv = None - """External forces and moments""" + # External forces and moments""" self.fx_ext = None self.fy_ext = None self.fz_ext = None diff --git a/pysymoro/floatr.py b/pysymoro/floatr.py index a4ead41..617cddc 100644 --- a/pysymoro/floatr.py +++ b/pysymoro/floatr.py @@ -8,8 +8,12 @@ """ -from sympy import zeros -from sympy import ShapeError +from sympy import eye, var +from sympy import Matrix + +from pysymoro.dynparams import DynParams +from pysymoro.geoparams import GeoParams +from pysymoro.screw import Screw class FloatingRobot(object): @@ -17,7 +21,113 @@ class FloatingRobot(object): Represent the FloatingRobot data structure and provide methods that act as the gateway for robot modelling. """ - def __init__(self, *args, **kwargs): - pass + def __init__( + self, name, links=0, joints=0, frames=0, + is_floating=True, structure=TREE + ): + """ + 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 + # 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 joint_nums] + """Joint velocities.""" + self.qdots = [var('QD{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_acc = Screw() + """Transformation matrix of base wrt a reference frame.""" + self.base_tmat = eye(4) + + @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) From 26969c1a9ab5a7a074d32e283e8c628e04c77483 Mon Sep 17 00:00:00 2001 From: aravind Date: Thu, 1 May 2014 08:33:41 +0200 Subject: [PATCH 071/273] Fix return value --- pysymoro/geoparams.py | 2 +- pysymoro/tests/test_geoparams.py | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/pysymoro/geoparams.py b/pysymoro/geoparams.py index 627a960..ba94136 100644 --- a/pysymoro/geoparams.py +++ b/pysymoro/geoparams.py @@ -62,7 +62,7 @@ def q(self): returned. """ if self.sigma == 2: - return 0 + return None return ((1 - self.sigma) * self.theta) + (self.sigma * self.r) diff --git a/pysymoro/tests/test_geoparams.py b/pysymoro/tests/test_geoparams.py index e047188..7aa1262 100644 --- a/pysymoro/tests/test_geoparams.py +++ b/pysymoro/tests/test_geoparams.py @@ -62,7 +62,7 @@ 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) + self.assertEqual(self.data_fixed.q, None) def run_tests(): From 0f446241a5c626a92369ae6eb574636b5359b726 Mon Sep 17 00:00:00 2001 From: aravind Date: Thu, 1 May 2014 11:00:26 +0200 Subject: [PATCH 072/273] Add `TransformationMatrix` class Add `TransformationMatrix` class and a function to compute the transformation between any two frames - `get_transformation_matrix()`. Add unittest module for `get_transformation_matrix()`. Add `pysymoro/tests/transformationmatrix.py` file. Update `pysymoro/tests/run_all_tests.py` to discover all the unittest modules in the folder and run the tests. --- pysymoro/tests/run_all_tests.py | 11 +- pysymoro/tests/test_transform.py | 101 ++++++++++ pysymoro/tests/test_transformationmatrix.py | 34 ++++ pysymoro/transform.py | 204 ++++++++++++++++++++ 4 files changed, 342 insertions(+), 8 deletions(-) create mode 100644 pysymoro/tests/test_transform.py create mode 100644 pysymoro/tests/test_transformationmatrix.py create mode 100644 pysymoro/transform.py diff --git a/pysymoro/tests/run_all_tests.py b/pysymoro/tests/run_all_tests.py index 3714827..0e9aedc 100644 --- a/pysymoro/tests/run_all_tests.py +++ b/pysymoro/tests/run_all_tests.py @@ -2,18 +2,13 @@ # -*- coding: utf-8 -*- -import test_screw -import test_screw6 -import test_geoparams -import test_dynparams +import unittest def main(): """Main function.""" - test_screw.run_tests() - test_screw6.run_tests() - test_geoparams.run_tests() - test_dynparams.run_tests() + all_tests = unittest.TestLoader().discover('tests', pattern='test_*.py') + unittest.TextTestRunner(verbosity=2).run(all_tests) if __name__ == '__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..a36db71 --- /dev/null +++ b/pysymoro/tests/test_transformationmatrix.py @@ -0,0 +1,34 @@ +#!/usr/bin/env python +# -*- coding: utf-8 -*- + + +"""Unit test module for TransformationMatrix class.""" + + +import unittest + +from pysymoro.transform import TransformationMatrix + + +class TestTransformationMatrix(unittest.TestCase): + """Unit test for TransformationMatrix class.""" + pass + + +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..0b99e7d --- /dev/null +++ b/pysymoro/transform.py @@ -0,0 +1,204 @@ +# -*- coding: utf-8 -*- + + +""" +This module contains the TransformationMatrix data structure. +""" + + +import sympy + + +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, frame_i, frame_j, params=None): + """ + Constructor period. + + Args: + frame_i: Frame with respect to which the transformation is + computed (int). This is usually the antecedent frame. + frame_j: Frame whose transformation is to be computed (int). + params: A dict containing the geometric parameters. + + Usage: + """ + self._frame_i = frame_i + self._frame_j = frame_j + self._gamma = 0 + self._b = 0 + self._alpha = 0 + self._d = 0 + self._theta = 0 + self._r = 0 + if params is not None: + self.update(params) + + 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(): + if key in self._params_terms: + attr = '_' + key + setattr(self, attr, value) + else: + raise AttributeError( + "%s is not a geometric parameter" % key + ) + self._compute_tmat() + self._compute_tinv() + + @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 + + def _compute_tmat(self): + """ + Call (proxy method) the actual function that computes the + transformation matrix between any two frames. + """ + self._tmat = 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 = 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 + + From 23a51b5ebeee785410aae0ba61fd4e83eb3556f2 Mon Sep 17 00:00:00 2001 From: aravind Date: Thu, 1 May 2014 11:12:39 +0200 Subject: [PATCH 073/273] Fix name errors --- pysymoro/transform.py | 46 +++++++++++++++++++++---------------------- 1 file changed, 23 insertions(+), 23 deletions(-) diff --git a/pysymoro/transform.py b/pysymoro/transform.py index 0b99e7d..294e8b2 100644 --- a/pysymoro/transform.py +++ b/pysymoro/transform.py @@ -34,18 +34,18 @@ def get_transformation_matrix(gamma, b, alpha, d, theta, r): 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; + 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], @@ -96,8 +96,8 @@ def update(self, params): to be updated. """ for key, value in params.iteritems(): - if key in self._params_terms: - attr = '_' + key + attr = '_' + key + if hasattr(self, attr): setattr(self, attr, value) else: raise AttributeError( @@ -114,7 +114,7 @@ def val(self): Returns: A 4x4 Matrix """ - if not hasattr(self, _tmat): + if not hasattr(self, '_tmat'): raise AttributeError("tmat is yet to be computed") return self._tmat @@ -126,7 +126,7 @@ def rot(self): Returns: A 3x3 Matrix """ - if not hasattr(self, _tmat): + if not hasattr(self, '_tmat'): raise AttributeError("tmat is yet to be computed") return self._tmat[0:3, 0:3] @@ -138,7 +138,7 @@ def trans(self): Returns: A 3x1 Matrix """ - if not hasattr(self, _tmat): + if not hasattr(self, '_tmat'): raise AttributeError("tmat is yet to be computed") return self._tmat[0:3, 3:4] @@ -150,7 +150,7 @@ def inv(self): Returns: A 4x4 Matrix """ - if not hasattr(self, _tinv): + if not hasattr(self, '_tinv'): raise AttributeError("tinv is yet to be computed") return self._tinv @@ -162,7 +162,7 @@ def inv_rot(self): Returns: A 3x3 Matrix """ - if not hasattr(self, _tinv): + if not hasattr(self, '_tinv'): raise AttributeError("tinv is yet to be computed") return self._tinv[0:3, 0:3] @@ -174,7 +174,7 @@ def inv_trans(self): Returns: A 3x1 Matrix """ - if not hasattr(self, _tinv): + if not hasattr(self, '_tinv'): raise AttributeError("tinv is yet to be computed") return self._tinv @@ -183,7 +183,7 @@ def _compute_tmat(self): Call (proxy method) the actual function that computes the transformation matrix between any two frames. """ - self._tmat = transformation_matrix( + self._tmat = get_transformation_matrix( self._gamma, self._b, self._alpha, self._d, self._theta, self._r @@ -193,9 +193,9 @@ def _compute_tinv(self): """ Compute inverse of the transformation matrix. """ - if not hasattr(self, _tmat): + if not hasattr(self, '_tmat'): raise AttributeError("tmat is yet to be computed") - self._tinv = eye(4) + self._tinv = sympy.eye(4) rot_inv = self.rot.transpose() trans_inv = -rot_inv * self.trans self._tinv[0:3, 0:3] = rot_inv From 9935c19c1fc147ab1a4d4978e2af82cf676eec8c Mon Sep 17 00:00:00 2001 From: aravind Date: Thu, 1 May 2014 16:05:18 +0200 Subject: [PATCH 074/273] Rename file Rename `pysymoro/tests/test.py` to `pysymoro/tests/oldtest.py`. --- pysymoro/tests/{test.py => oldtest.py} | 0 1 file changed, 0 insertions(+), 0 deletions(-) rename pysymoro/tests/{test.py => oldtest.py} (100%) diff --git a/pysymoro/tests/test.py b/pysymoro/tests/oldtest.py similarity index 100% rename from pysymoro/tests/test.py rename to pysymoro/tests/oldtest.py From 176ad8ba5de546aa5a22be0fad98d41a40671684 Mon Sep 17 00:00:00 2001 From: aravind Date: Thu, 1 May 2014 20:19:59 +0200 Subject: [PATCH 075/273] Add unittest module for `TransformationMatrix` class Add unittest module for `TransformationMatrix` class. Modify `TransformationMatrix` class constructor to accept 3 forms of input parameters. --- pysymoro/tests/test_dynparams.py | 2 +- pysymoro/tests/test_geoparams.py | 2 +- pysymoro/tests/test_transformationmatrix.py | 124 ++++++++++++++++- pysymoro/transform.py | 143 +++++++++++++++----- 4 files changed, 231 insertions(+), 40 deletions(-) diff --git a/pysymoro/tests/test_dynparams.py b/pysymoro/tests/test_dynparams.py index 85e0841..d5ffd01 100644 --- a/pysymoro/tests/test_dynparams.py +++ b/pysymoro/tests/test_dynparams.py @@ -80,7 +80,7 @@ def setUp(self): self.wrong_param = {'rand': 'some-value'} def test_init(self): - """Test constructor.""" + """Test constructor""" # test instance type self.assertIsInstance(DynParams(self.link), DynParams) diff --git a/pysymoro/tests/test_geoparams.py b/pysymoro/tests/test_geoparams.py index 7aa1262..a499c43 100644 --- a/pysymoro/tests/test_geoparams.py +++ b/pysymoro/tests/test_geoparams.py @@ -40,7 +40,7 @@ def setUp(self): self.wrong_param = {'rand': 'some-value'} def test_init(self): - """Test constructor.""" + """Test constructor""" # test instance type self.assertIsInstance(GeoParams(self.frame), GeoParams) diff --git a/pysymoro/tests/test_transformationmatrix.py b/pysymoro/tests/test_transformationmatrix.py index a36db71..47a42f7 100644 --- a/pysymoro/tests/test_transformationmatrix.py +++ b/pysymoro/tests/test_transformationmatrix.py @@ -7,12 +7,134 @@ import unittest +from sympy import eye, var, zeros +from sympy import cos, sin +from sympy import Matrix + from pysymoro.transform import TransformationMatrix class TestTransformationMatrix(unittest.TestCase): """Unit test for TransformationMatrix class.""" - pass + 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]) + # 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_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(): diff --git a/pysymoro/transform.py b/pysymoro/transform.py index 294e8b2..81a0dfb 100644 --- a/pysymoro/transform.py +++ b/pysymoro/transform.py @@ -62,49 +62,54 @@ class TransformationMatrix(object): Represent the data structure to hold a transformation matrix between any two frames. """ - def __init__(self, frame_i, frame_j, params=None): + def __init__(self, *args, **kwargs): """ Constructor period. - Args: - frame_i: Frame with respect to which the transformation is - computed (int). This is usually the antecedent frame. - frame_j: Frame whose transformation is to be computed (int). - params: A dict containing the geometric parameters. - 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=) """ - self._frame_i = frame_i - self._frame_j = frame_j - self._gamma = 0 - self._b = 0 - self._alpha = 0 - self._d = 0 - self._theta = 0 - self._r = 0 - if params is not None: - self.update(params) + if len(kwargs) >= 1 and len(kwargs) <= 3: + self._init_default() + self._compute_tmat() + self._compute_tinv() + 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 update(self, params): - """ - Update the parameter values and all the matrices accordingly. + def __str__(self): + pass - 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) - else: - raise AttributeError( - "%s is not a geometric parameter" % key - ) - self._compute_tmat() - self._compute_tinv() + def __repr__(self): + pass @property def val(self): @@ -176,7 +181,34 @@ def inv_trans(self): """ if not hasattr(self, '_tinv'): raise AttributeError("tinv is yet to be computed") - return self._tinv + return self._tinv[0:3, 3:4] + + 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() def _compute_tmat(self): """ @@ -201,4 +233,41 @@ def _compute_tinv(self): self._tinv[0:3, 0:3] = rot_inv self._tinv[0:3, 3:4] = trans_inv + 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 + From 5204bf7621ebc64bbf3fab19a5f2b36e5a249c14 Mon Sep 17 00:00:00 2001 From: Bogdan Date: Thu, 1 May 2014 20:44:15 +0200 Subject: [PATCH 076/273] DGM with transform chains Also small bugs are fixen in the symbolmrg --- pysymoro/geometry.py | 212 +++++++++++++++++++++++++++++++++++++- pysymoro/invgeom.py | 3 + pysymoro/kinematics.py | 1 + pysymoro/tests/oldtest.py | 2 +- symoroutils/symbolmgr.py | 11 +- 5 files changed, 221 insertions(+), 8 deletions(-) diff --git a/pysymoro/geometry.py b/pysymoro/geometry.py index 9d3ae86..910b6e3 100644 --- a/pysymoro/geometry.py +++ b/pysymoro/geometry.py @@ -99,6 +99,212 @@ 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 + """ + return Matrix([self.rot().row_join(self.trans), + [0, 0, 0, 1]]) + + def rot(self): + _rot = 0 + _trans = 1 + _x = 0 + _y = 1 + _z = 2 + if self.type == _rot: + if self.axis == _x: + return Matrix([[1, 0, 0], + [0, cos(self.val), -sin(self.val)], + [0, sin(self.val), cos(self.val)]]) + elif self.axis == _y: + return Matrix([[cos(self.val), 0, sin(self.val)], + [0, 1, 0], + [-sin(self.val), 0, cos(self.val)]]) + elif self.axis == _z: + return Matrix([[cos(self.val), -sin(self.val), 0], + [sin(self.val), cos(self.val), 0], + [0, 0, 1]]) + elif self.type == _trans: + return eye(3) + + def trans(self): + v = zeros(3, 1) + if self.type == 1: + v[self.axis] = self.val + return v + + +class TransConvolve: + def __init__(self, symo, trig_subs=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 + self.T_tmp = eye(4) + + def process(self, tr): + if tr.type == 0: # rotation + if self.rot.axis == tr.axis: + 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 = 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 = 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 + """ + _rot = 0 + _trans = 1 + _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(_rot, _z, -robo.theta[indx], indx, ant)) + tr_list.append(CompTransf(_trans, _z, -robo.r[indx], indx, ant)) + tr_list.append(CompTransf(_rot, _x, -robo.alpha[indx], indx, ant, 'A')) + tr_list.append(CompTransf(_trans, _x, -robo.d[indx], indx, ant)) + tr_list.append(CompTransf(_rot, _z, -robo.gamma[indx], indx, ant, 'G')) + tr_list.append(CompTransf(_trans, _z, -robo.b[indx], indx, ant)) + for indx in chain2: + ant = robo.ant[indx] + tr_list.append(CompTransf(_rot, _z, robo.gamma[indx], ant, indx, 'G')) + tr_list.append(CompTransf(_trans, _z, robo.b[indx], ant, indx)) + tr_list.append(CompTransf(_rot, _x, robo.alpha[indx], ant, indx, 'A')) + tr_list.append(CompTransf(_trans, _x, robo.d[indx], ant, indx)) + tr_list.append(CompTransf(_rot, _z, robo.theta[indx], ant, indx)) + tr_list.append(CompTransf(_trans, _z, robo.r[indx], ant, indx)) + return [tr for tr in tr_list if tr.val != 0] + + +def to_matrix_fast(symo, tr_list): + 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) + return T + + +def to_matrix(symo, tr_list, trig_subs=True): + conv = TransConvolve(symo, trig_subs) + for tr in tr_list: + conv.process(tr) + return conv.result() + + +def to_matrices_right(symo, tr_list, trig_subs=True): + 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(symo, tr_list, trig_subs=True): + 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): +# tr_list = transform_list(robo, i, j) +# if key == 'one' and fast_form: +# return to_matrix_fast(symo, tr_list) +# else: +# if key == 'left': +# return to_matrices_left(symo, tr_list, trig_subs) +# elif key == 'right': +# return to_matrices_right(symo, tr_list, trig_subs) +# elif key == 'one': +# return to_matrix(symo, tr_list, trig_subs) + + def _transform(robo, j, invert=False): """Transform matrix between frames j and ant[j] @@ -286,10 +492,10 @@ def _dgm_one(robo, symo, i, j, fast_form=True, def _dgm_rename(robo, symo, T_res, x, i, j, inverted, forced): if inverted: name = _trans_name(robo, x, j) - forced_now = x == i + forced_now = (x == i) else: name = _trans_name(robo, robo.ant[x], j) - forced_now = robo.ant[x] == i + forced_now = (robo.ant[x] == i) symo.mat_replace(T_res, name, forced=forced and forced_now, skip=1) @@ -299,6 +505,8 @@ def _dgm_rename(robo, symo, T_res, x, i, j, inverted, forced): # 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' + +#set([sin(th2 + th3), cos(th2 + th3), sin(th7 + th8 + th9), cos(th7 + th8 + th9)]) def dgm(robo, symo, i, j, key='one', fast_form=True, forced=False, trig_subs=True): """must be the final DGM function diff --git a/pysymoro/invgeom.py b/pysymoro/invgeom.py index b7e96c1..7cc4b08 100644 --- a/pysymoro/invgeom.py +++ b/pysymoro/invgeom.py @@ -46,6 +46,8 @@ def _paul_solve(robo, symo, nTm, n, m, known=set()): while True: repeat = False for i in reversed(chain): + print iTn.keys() + print iTm.keys() M_eq = iTn[(i, n)]*nTm - iTm[(i, m)] while True: found = _look_for_eq(symo, M_eq, known, th_all, r_all) @@ -79,6 +81,7 @@ def _look_for_eq(symo, M_eq, known, th_all, r_all): eq = symo.unknown_sep(M_eq[e1, e2], known) th_vars = (eq.atoms(Symbol) & th_all) - known if th_vars: + print '*******', eq.atoms(sin, cos) arg_sum = max(at.count_ops()-1 for at in eq.atoms(sin, cos) if not at.atoms(Symbol) & known) else: diff --git a/pysymoro/kinematics.py b/pysymoro/kinematics.py index db9c5e5..cba610e 100644 --- a/pysymoro/kinematics.py +++ b/pysymoro/kinematics.py @@ -95,6 +95,7 @@ def _jac(robo, symo, n, i, j, chain=None, forced=False, trig_subs=False): iTk_dict = dgm(robo, symo, i, chain[0], key='right', trig_subs=trig_subs) iTk_tmp = dgm(robo, symo, i, chain[-1], key='right', trig_subs=trig_subs) iTk_dict.update(iTk_tmp) + print iTk_dict for k in chain: kTj = kTj_dict[k, j] iTk = iTk_dict[i, k] diff --git a/pysymoro/tests/oldtest.py b/pysymoro/tests/oldtest.py index ea5c707..93066db 100644 --- a/pysymoro/tests/oldtest.py +++ b/pysymoro/tests/oldtest.py @@ -227,7 +227,7 @@ def test_dynamics(self): suite.addTest(testGeometry('test_dgm_rx90')) suite.addTest(testGeometry('test_dgm_sr400')) suite.addTest(testGeometry('test_igm')) - suite.addTest(testGeometry('test_loop')) + # suite.addTest(testGeometry('test_loop')) suite.addTest(testKinematics('test_jac')) suite.addTest(testKinematics('test_jac2')) unittest.TextTestRunner(verbosity=2).run(suite) diff --git a/symoroutils/symbolmgr.py b/symoroutils/symbolmgr.py index fbc3cfa..9cdb639 100644 --- a/symoroutils/symbolmgr.py +++ b/symoroutils/symbolmgr.py @@ -180,10 +180,9 @@ def trig_replace(self, M, angle, 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 + 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]): @@ -543,8 +542,9 @@ def gen_fbody(self, name, to_return, wr_syms, multival): return fun_body def gen_func(self, name, to_return, args, multival=False): + #TODO self, name, toret, *args, **kwargs """ Returns function that computes what is in to_return - using *args as arguments + using args as arguments Parameters ========== @@ -563,6 +563,7 @@ def gen_func(self, name, to_return, args, multival=False): -This function must be called only after the model that computes symbols in to_return have been generated. """ + #if kwargs.get 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) From 553a5013b0a9e8d7db2396f35ce59747bc7b9d22 Mon Sep 17 00:00:00 2001 From: aravind Date: Fri, 2 May 2014 01:49:43 +0200 Subject: [PATCH 077/273] Add str and repr overload Add `__str__` and `__repr__` overload to `TransformationMatrix` class. --- pysymoro/transform.py | 17 +++++++++++++++-- 1 file changed, 15 insertions(+), 2 deletions(-) diff --git a/pysymoro/transform.py b/pysymoro/transform.py index 81a0dfb..1b95afc 100644 --- a/pysymoro/transform.py +++ b/pysymoro/transform.py @@ -106,10 +106,23 @@ def __init__(self, *args, **kwargs): ) def __str__(self): - pass + str_format = ( + "T matrix of frame %d wrt frame %d:\n" + "----------------------------------\n" + "\tgamma=%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): - pass + return "T of %d wrt %d" % (self._frame_j, self._frame_i) @property def val(self): From d1fd6909a15d4dbe687ff055045bb97520342ba7 Mon Sep 17 00:00:00 2001 From: aravind Date: Fri, 2 May 2014 12:06:16 +0200 Subject: [PATCH 078/273] Add computation of Screw transformation matrix Add computation of transformation matrix in Screw form in `TransformationMatrix` class. Add cooresponding unittest code. --- pysymoro/tests/test_transformationmatrix.py | 22 ++++++++ pysymoro/transform.py | 58 ++++++++++++++++++--- 2 files changed, 74 insertions(+), 6 deletions(-) diff --git a/pysymoro/tests/test_transformationmatrix.py b/pysymoro/tests/test_transformationmatrix.py index 47a42f7..d29df97 100644 --- a/pysymoro/tests/test_transformationmatrix.py +++ b/pysymoro/tests/test_transformationmatrix.py @@ -11,6 +11,9 @@ from sympy import cos, sin from sympy import Matrix +from pysymoro.screw6 import Screw6 +from symoroutils import tools + from pysymoro.transform import TransformationMatrix @@ -50,6 +53,15 @@ def setUp(self): [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, @@ -123,6 +135,16 @@ def test_inv_trans(self): 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 diff --git a/pysymoro/transform.py b/pysymoro/transform.py index 1b95afc..d24616c 100644 --- a/pysymoro/transform.py +++ b/pysymoro/transform.py @@ -8,6 +8,9 @@ import sympy +from pysymoro.screw6 import Screw6 +from symoroutils import tools + def get_transformation_matrix(gamma, b, alpha, d, theta, r): """ @@ -84,6 +87,8 @@ def __init__(self, *args, **kwargs): 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']) @@ -130,7 +135,7 @@ def val(self): Get the value of the transformation matrix. Returns: - A 4x4 Matrix + A 4x4 Matrix. """ if not hasattr(self, '_tmat'): raise AttributeError("tmat is yet to be computed") @@ -142,7 +147,7 @@ def rot(self): Get the value of the rotation matrix. Returns: - A 3x3 Matrix + A 3x3 Matrix. """ if not hasattr(self, '_tmat'): raise AttributeError("tmat is yet to be computed") @@ -154,7 +159,7 @@ def trans(self): Get the value of the translation vector. Returns: - A 3x1 Matrix + A 3x1 Matrix. """ if not hasattr(self, '_tmat'): raise AttributeError("tmat is yet to be computed") @@ -166,7 +171,7 @@ def inv(self): Get the inverse of the transformation matrix. Returns: - A 4x4 Matrix + A 4x4 Matrix. """ if not hasattr(self, '_tinv'): raise AttributeError("tinv is yet to be computed") @@ -178,7 +183,7 @@ def inv_rot(self): Get the inverse of the rotation matrix. Returns: - A 3x3 Matrix + A 3x3 Matrix. """ if not hasattr(self, '_tinv'): raise AttributeError("tinv is yet to be computed") @@ -190,12 +195,32 @@ def inv_trans(self): Get the inverse of the translation vector. Returns: - A 3x1 Matrix + 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. @@ -222,6 +247,8 @@ def update(self, params): ) self._compute_tmat() self._compute_tinv() + self._compute_smat() + self._compute_sinv() def _compute_tmat(self): """ @@ -246,6 +273,25 @@ def _compute_tinv(self): 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 From 3fcea2020e6e7742a32b2c3fe72f7f17ee1e7814 Mon Sep 17 00:00:00 2001 From: aravind Date: Fri, 2 May 2014 15:02:03 +0200 Subject: [PATCH 079/273] Include `TransformationMatrix` instance in `GeoParams` Include an instance of `TransformationMatrix` class in `GeoParams` class. This keeps track of the transformation matrix for each frame wrt its antecedent. --- pysymoro/geoparams.py | 14 +++++++++++++- pysymoro/screw.py | 18 +----------------- pysymoro/tests/run_all_tests.py | 5 +++++ pysymoro/tests/test_geoparams.py | 2 ++ pysymoro/transform.py | 4 ++-- 5 files changed, 23 insertions(+), 20 deletions(-) diff --git a/pysymoro/geoparams.py b/pysymoro/geoparams.py index ba94136..0d4f9f3 100644 --- a/pysymoro/geoparams.py +++ b/pysymoro/geoparams.py @@ -6,6 +6,9 @@ """ +from pysymoro import transform + + class GeoParams(object): """ Data structure: @@ -15,12 +18,17 @@ 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 = 0 + self.ant = frame - 1 self.sigma = 0 self.mu = 0 self.gamma = 0 @@ -29,6 +37,9 @@ def __init__(self, frame, params=None): 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) @@ -50,6 +61,7 @@ def update_params(self, params): raise AttributeError( "%s is not an attribute of GeoParams" % key ) + self.tmat.update(params) @property def q(self): diff --git a/pysymoro/screw.py b/pysymoro/screw.py index 4d681ab..6f27c59 100644 --- a/pysymoro/screw.py +++ b/pysymoro/screw.py @@ -2,7 +2,7 @@ """ -This module contains the Screw data structure. +This module contains the Screw data structure. """ @@ -67,22 +67,6 @@ def val(self, value): if value.rows != 6 or value.cols != 1: raise ShapeError("Matrix size has to be 6x1.") self._val = value - -# @val.setter -# def val(self, lin, ang): -# """ -# Set the linear and angular terms individually. -# -# Args: -# lin: A 3x1 Matrix - linear term -# ang: A 3X1 Matrix - angular term -# """ -# 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 @property def lin(self): diff --git a/pysymoro/tests/run_all_tests.py b/pysymoro/tests/run_all_tests.py index 0e9aedc..ae29d8a 100644 --- a/pysymoro/tests/run_all_tests.py +++ b/pysymoro/tests/run_all_tests.py @@ -2,6 +2,11 @@ # -*- coding: utf-8 -*- +""" +This module discovers all the unittest code in the folder and runs them. +""" + + import unittest diff --git a/pysymoro/tests/test_geoparams.py b/pysymoro/tests/test_geoparams.py index a499c43..e06d608 100644 --- a/pysymoro/tests/test_geoparams.py +++ b/pysymoro/tests/test_geoparams.py @@ -46,6 +46,8 @@ def test_init(self): 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, diff --git a/pysymoro/transform.py b/pysymoro/transform.py index d24616c..7c58407 100644 --- a/pysymoro/transform.py +++ b/pysymoro/transform.py @@ -65,7 +65,7 @@ class TransformationMatrix(object): Represent the data structure to hold a transformation matrix between any two frames. """ - def __init__(self, *args, **kwargs): + def __init__(self, **kwargs): """ Constructor period. @@ -305,7 +305,7 @@ 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. From 2d4fc298af985050e2d47ffe6e09d173609ca8df Mon Sep 17 00:00:00 2001 From: aravind Date: Fri, 2 May 2014 16:21:07 +0200 Subject: [PATCH 080/273] Add str and repr overload for `GeoParams` class Add `__str__()` and `__repr__()` overload for `GeoParams` class. --- pysymoro/geoparams.py | 18 ++++++++++++++++++ pysymoro/transform.py | 2 +- 2 files changed, 19 insertions(+), 1 deletion(-) diff --git a/pysymoro/geoparams.py b/pysymoro/geoparams.py index 0d4f9f3..3515cd1 100644 --- a/pysymoro/geoparams.py +++ b/pysymoro/geoparams.py @@ -44,6 +44,24 @@ def __init__(self, frame, params=None): if params is not None: self.update_params(params) + def __str__(self): + str_format = ( + "\t\t%s\t%s\t%s\t%s\t%s\t%s\t%s\t%s\t%s\t%s\t%s" + ) % ( + 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().replace('\t', ', ') + repr_format = '(' + repr_format + ')' + return repr_format + def update_params(self, params): """ Update the geometric parameter values. diff --git a/pysymoro/transform.py b/pysymoro/transform.py index 7c58407..84ae937 100644 --- a/pysymoro/transform.py +++ b/pysymoro/transform.py @@ -114,7 +114,7 @@ def __str__(self): str_format = ( "T matrix of frame %d wrt frame %d:\n" "----------------------------------\n" - "\tgamma=%s, b=%s, alpha=%s, d=%s, theta=%s, r=%s\n" + "gamma=%s, b=%s, alpha=%s, d=%s, theta=%s, r=%s\n" "%s\n" "**********************************\n" ) % ( From 21ff77b95e03ba220a60df80c218a18ace69c4aa Mon Sep 17 00:00:00 2001 From: aravind Date: Fri, 2 May 2014 16:50:02 +0200 Subject: [PATCH 081/273] Add str and repr overload to `DynParams` class --- pysymoro/dynparams.py | 19 +++++++++++++++++++ pysymoro/geoparams.py | 5 ++--- 2 files changed, 21 insertions(+), 3 deletions(-) diff --git a/pysymoro/dynparams.py b/pysymoro/dynparams.py index c602baa..7daf32a 100644 --- a/pysymoro/dynparams.py +++ b/pysymoro/dynparams.py @@ -98,6 +98,25 @@ def __init__(self, link, params=None): if params is not None: self.update_params(params) + def __str__(self): + pattern = ' %s' + str_format = (pattern * 20) % ( + 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) + ) + str_format = '\t' + str_format.lstrip() + return str_format + + def __repr__(self): + repr_format = str(self).lstrip().replace(' ', ', ') + repr_format = '(' + repr_format + ')' + return repr_format + def update_params(self, params): """ Update the dynamic parameter values. diff --git a/pysymoro/geoparams.py b/pysymoro/geoparams.py index 3515cd1..705609f 100644 --- a/pysymoro/geoparams.py +++ b/pysymoro/geoparams.py @@ -45,9 +45,8 @@ def __init__(self, frame, params=None): self.update_params(params) def __str__(self): - str_format = ( - "\t\t%s\t%s\t%s\t%s\t%s\t%s\t%s\t%s\t%s\t%s\t%s" - ) % ( + pattern = '\t%s' + str_format = (pattern * 11) % ( str(self.frame), str(self.ant), str(self.sigma), str(self.mu), str(self.gamma), str(self.b), From 25897a26a6209f1317007da5dcb0383c58b298ef Mon Sep 17 00:00:00 2001 From: Bogdan Date: Fri, 2 May 2014 17:14:24 +0200 Subject: [PATCH 082/273] DGM with transform chains -- IGM fix IGM is working as before, and even better. Still some improvments in IGM Paul method are possible --- pysymoro/geometry.py | 352 +++++++++----------------------------- pysymoro/invgeom.py | 51 ++++-- pysymoro/kinematics.py | 1 - pysymoro/tests/oldtest.py | 24 +-- 4 files changed, 129 insertions(+), 299 deletions(-) diff --git a/pysymoro/geometry.py b/pysymoro/geometry.py index 910b6e3..7db391a 100644 --- a/pysymoro/geometry.py +++ b/pysymoro/geometry.py @@ -106,7 +106,7 @@ def __init__(self, transform_type, axis, val, i=0, j=0, name=''): self.val = val self.i = i self.j = j - self.name = "%s%s" % (name, max(i,j)) + self.name = "%s%s" % (name, max(i, j)) def __str__(self): axes = ('x', 'y', 'z') @@ -121,50 +121,37 @@ def matrix(self): """ Homogeneous transformation matrix """ - return Matrix([self.rot().row_join(self.trans), - [0, 0, 0, 1]]) + 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): - _rot = 0 - _trans = 1 - _x = 0 - _y = 1 - _z = 2 - if self.type == _rot: - if self.axis == _x: - return Matrix([[1, 0, 0], - [0, cos(self.val), -sin(self.val)], - [0, sin(self.val), cos(self.val)]]) - elif self.axis == _y: - return Matrix([[cos(self.val), 0, sin(self.val)], - [0, 1, 0], - [-sin(self.val), 0, cos(self.val)]]) - elif self.axis == _z: - return Matrix([[cos(self.val), -sin(self.val), 0], - [sin(self.val), cos(self.val), 0], - [0, 0, 1]]) - elif self.type == _trans: + if self.type == 0: + return _rot(self.axis, self.val) + elif self.type == 1: return eye(3) def trans(self): - v = zeros(3, 1) if self.type == 1: - v[self.axis] = self.val - return v + return _trans_vec(self.axis, self.val) + else: + return zeros(3, 1) class TransConvolve: - def __init__(self, symo, trig_subs=True): + 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 + self.trig_subs = trig_subs and symo is not None self.T_tmp = eye(4) + self.siplify = simplify def process(self, tr): if tr.type == 0: # rotation - if self.rot.axis == tr.axis: + if self.rot.axis == tr.axis and self.siplify: self.rot.val += tr.val self.rot.name += tr.name else: # translation @@ -212,8 +199,6 @@ def transform_list(robo, i, j): """ Computes the chain of transformations for iTj """ - _rot = 0 - _trans = 1 _x = 0 _z = 2 k = robo.common_root(i, j) @@ -223,20 +208,20 @@ def transform_list(robo, i, j): tr_list = [] for indx in chain1: ant = robo.ant[indx] - tr_list.append(CompTransf(_rot, _z, -robo.theta[indx], indx, ant)) - tr_list.append(CompTransf(_trans, _z, -robo.r[indx], indx, ant)) - tr_list.append(CompTransf(_rot, _x, -robo.alpha[indx], indx, ant, 'A')) - tr_list.append(CompTransf(_trans, _x, -robo.d[indx], indx, ant)) - tr_list.append(CompTransf(_rot, _z, -robo.gamma[indx], indx, ant, 'G')) - tr_list.append(CompTransf(_trans, _z, -robo.b[indx], indx, ant)) + 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(_rot, _z, robo.gamma[indx], ant, indx, 'G')) - tr_list.append(CompTransf(_trans, _z, robo.b[indx], ant, indx)) - tr_list.append(CompTransf(_rot, _x, robo.alpha[indx], ant, indx, 'A')) - tr_list.append(CompTransf(_trans, _x, robo.d[indx], ant, indx)) - tr_list.append(CompTransf(_rot, _z, robo.theta[indx], ant, indx)) - tr_list.append(CompTransf(_trans, _z, robo.r[indx], 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] @@ -257,14 +242,14 @@ def to_matrix_fast(symo, tr_list): return T -def to_matrix(symo, tr_list, trig_subs=True): - conv = TransConvolve(symo, trig_subs) +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(symo, tr_list, trig_subs=True): +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 @@ -278,7 +263,7 @@ def to_matrices_right(symo, tr_list, trig_subs=True): return res -def to_matrices_left(symo, tr_list, trig_subs=True): +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 @@ -292,49 +277,47 @@ def to_matrices_left(symo, tr_list, trig_subs=True): return res -#def dgm(robo, symo, i, j, key='one', fast_form=True, trig_subs=True): -# tr_list = transform_list(robo, i, j) -# if key == 'one' and fast_form: -# return to_matrix_fast(symo, tr_list) -# else: -# if key == 'left': -# return to_matrices_left(symo, tr_list, trig_subs) -# elif key == 'right': -# return to_matrices_right(symo, tr_list, trig_subs) -# elif key == 'one': -# return to_matrix(symo, tr_list, trig_subs) - - -def _transform(robo, j, invert=False): - """Transform matrix between frames j and ant[j] +def dgm(robo, symo, i, j, key='one', fast_form=True, trig_subs=True): + """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 - Frame index. - invert: bool, optional - Defines the transformation direction + 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 - 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 + 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) 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 + 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) -#TODO: rewrite the description -def _transform_const_sep(robo, j, invert=False): +def _transform(robo, j, invert=False): """Transform matrix between frames j and ant[j] Parameters @@ -351,17 +334,15 @@ def _transform_const_sep(robo, j, invert=False): 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 + 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('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 + 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): @@ -394,161 +375,12 @@ def compute_screw_transform(robo, symo, j, antRj, antPj, jTant): 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' - -#set([sin(th2 + th3), cos(th2 + th3), sin(th7 + th8 + th9), cos(th7 + th8 + th9)]) -def dgm(robo, symo, i, j, key='one', fast_form=True, forced=False, - trig_subs=True): - """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) - 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): +def _rot(axis=2, th=0): """Rotation matrix about axis Parameters ========== - axis: {'x', 'y', 'z'} + axis: {0, 1, 2} Rotation axis th: var Rotation angle @@ -557,11 +389,12 @@ def _rot(axis='z', th=0): ======= rot: Matrix 3x3 """ - if axis == 'x': + 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 == 'y': + elif axis == 1: return Matrix([[cos(th), 0, sin(th)], [0, 1, 0], [-sin(th), 0, cos(th)]]) @@ -571,12 +404,12 @@ def _rot(axis='z', th=0): [0, 0, 1]]) -def _trans_vect(axis='z', p=0): +def _trans_vec(axis=2, p=0): """Translation vector along axis Parameters ========== - axis: {'x', 'y', 'z'} + axis: {0, 1, 2} Translation axis p: var Translation distance @@ -585,31 +418,13 @@ def _trans_vect(axis='z', p=0): ======= v: Matrix 3x1 """ - axis_dict = {'x': 0, 'y': 1, 'z': 2} + assert axis in {0, 1, 2} v = zeros(3, 1) - v[axis_dict[axis]] = p + v[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): +def _rot_trans(axis=2, th=0, p=0): """Transformation matrix with rotation about and translation along axis @@ -626,7 +441,8 @@ def _rot_trans(axis='z', th=0, p=0): ======= rot_trans: Matrix 4x4 """ - return Matrix([_rot(axis, th).row_join(_trans_vect(axis, p)), + assert axis in {0, 1, 2} + return Matrix([_rot(axis, th).row_join(_trans_vec(axis, p)), [0, 0, 0, 1]]) @@ -693,5 +509,3 @@ def direct_geometric(robo, frames, trig_subs): symo.write_line() symo.file_close() return symo - - diff --git a/pysymoro/invgeom.py b/pysymoro/invgeom.py index 7cc4b08..92d7d70 100644 --- a/pysymoro/invgeom.py +++ b/pysymoro/invgeom.py @@ -10,7 +10,7 @@ from sympy import var, sin, cos, eye, atan2, sqrt, pi from sympy import Matrix, Symbol, Expr -from pysymoro.geometry import dgm +from pysymoro.geometry import transform_list, to_matrix from symoroutils import symbolmgr from symoroutils import tools @@ -27,10 +27,11 @@ def _paul_solve(robo, symo, nTm, n, m, known=set()): 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) +# 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() for i in chain: @@ -45,15 +46,29 @@ def _paul_solve(robo, symo, nTm, n, m, known=set()): known |= robo.alpha[i].atoms(Symbol) while True: repeat = False - for i in reversed(chain): - print iTn.keys() - print iTm.keys() - 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 + iTm = nTm.copy() + tr_list = transform_list(robo, n, m) + while True: #bring all the known transforms to the left hand side + tr = tr_list.pop() + print tr + if tr.val.atoms(Symbol) <= known: + print 'ololo' + tr.val = -tr.val + iTm = iTm*tr.matrix() + else: + tr_list.append(tr) + break + while tr_list: + tr = tr_list.pop(0) + if tr.val.atoms(Symbol) - known: + M_eq = tr.matrix() * to_matrix(tr_list, simplify=False) - iTm + 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 + tr.val = -tr.val + iTm = tr.matrix() * iTm if th_all | r_all <= known: break # if th_all | r_all <= known: @@ -78,12 +93,13 @@ def _look_for_eq(symo, M_eq, known, th_all, r_all): for e2 in xrange(4): if M_eq[e1, e2].has(EMPTY): continue - eq = symo.unknown_sep(M_eq[e1, e2], known) + #eq = symo.unknown_sep(M_eq[e1, e2], known) + eq = M_eq[e1, e2] th_vars = (eq.atoms(Symbol) & th_all) - known - if th_vars: - print '*******', eq.atoms(sin, cos) - arg_sum = max(at.count_ops()-1 for at in eq.atoms(sin, cos) - if not at.atoms(Symbol) & known) + arg_ops = [at.count_ops()-1 for at in eq.atoms(sin, cos) + if not at.atoms(Symbol) & known] + if th_vars and arg_ops: + arg_sum = max(arg_ops) else: arg_sum = 0 rs_s = (eq.atoms(Symbol) & r_all) - known @@ -100,7 +116,6 @@ def _look_for_eq(symo, M_eq, known, th_all, r_all): cont_search |= _try_solve_4(symo, eq_candidates[4], known) 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)] diff --git a/pysymoro/kinematics.py b/pysymoro/kinematics.py index cba610e..db9c5e5 100644 --- a/pysymoro/kinematics.py +++ b/pysymoro/kinematics.py @@ -95,7 +95,6 @@ def _jac(robo, symo, n, i, j, chain=None, forced=False, trig_subs=False): iTk_dict = dgm(robo, symo, i, chain[0], key='right', trig_subs=trig_subs) iTk_tmp = dgm(robo, symo, i, chain[-1], key='right', trig_subs=trig_subs) iTk_dict.update(iTk_tmp) - print iTk_dict for k in chain: kTj = kTj_dict[k, j] iTk = iTk_dict[i, k] diff --git a/pysymoro/tests/oldtest.py b/pysymoro/tests/oldtest.py index 93066db..951b1f5 100644 --- a/pysymoro/tests/oldtest.py +++ b/pysymoro/tests/oldtest.py @@ -157,8 +157,9 @@ def setUp(self): def test_speeds(self): print 'Speeds and accelerations' - kinematics.speeds_accelerations(self.robo) - + kinematics.velocities(self.robo) + kinematics.accelerations(self.robo) + kinematics.jdot_qdot(self.robo) print 'Kinematic constraint equations' kinematics.kinematic_constraints(samplerobots.sr400()) @@ -222,14 +223,15 @@ def test_dynamics(self): 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) +# 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() From c81805eecdac132a03fa7792eb7a8089e87a9ce2 Mon Sep 17 00:00:00 2001 From: aravind Date: Fri, 2 May 2014 17:30:49 +0200 Subject: [PATCH 083/273] Add str and repr overload to `FloatingRobot` class --- pysymoro/floatr.py | 41 +++++++++++++++++++++++++++++++++++++++-- 1 file changed, 39 insertions(+), 2 deletions(-) diff --git a/pysymoro/floatr.py b/pysymoro/floatr.py index 617cddc..54e0bdc 100644 --- a/pysymoro/floatr.py +++ b/pysymoro/floatr.py @@ -14,6 +14,8 @@ from pysymoro.dynparams import DynParams from pysymoro.geoparams import GeoParams from pysymoro.screw import Screw +from symoroutils import filemgr +from symoroutils import tools class FloatingRobot(object): @@ -23,7 +25,7 @@ class FloatingRobot(object): """ def __init__( self, name, links=0, joints=0, frames=0, - is_floating=True, structure=TREE + is_floating=True, structure=tools.TREE ): """ Constructor period. @@ -58,7 +60,7 @@ def __init__( corresponds to a virtual joint of the base. This joint is usually rigid. """ - self.etas = [0 for j in joint_nums] + self.etas = [0 for j in self.joint_nums] """Joint velocities.""" self.qdots = [var('QD{0}'.format(j)) for j in self.joint_nums] """Joint accelerations.""" @@ -86,6 +88,41 @@ def __init__( """Transformation matrix of base wrt a reference frame.""" self.base_tmat = eye(4) + 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 + ("\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" + 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" + for dyn in self.dyns: + str_format = str_format + str(dyn) + '\n' + str_format = str_format + '\n' + ('=*' * 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 + @property def link_nums(self): """ From 278b8741d843fc2f1ef33d7ee3aa2582a2b7ac58 Mon Sep 17 00:00:00 2001 From: aravind Date: Sat, 3 May 2014 00:36:51 +0200 Subject: [PATCH 084/273] Fix ImportError Fix `ImportError` in `pysymoro/tests/run_all_tests.py` caused by wrong file search path for the test files. --- pysymoro/tests/run_all_tests.py | 9 ++++++++- 1 file changed, 8 insertions(+), 1 deletion(-) diff --git a/pysymoro/tests/run_all_tests.py b/pysymoro/tests/run_all_tests.py index ae29d8a..9739990 100644 --- a/pysymoro/tests/run_all_tests.py +++ b/pysymoro/tests/run_all_tests.py @@ -7,12 +7,19 @@ """ +import inspect +import os import unittest def main(): """Main function.""" - all_tests = unittest.TestLoader().discover('tests', pattern='test_*.py') + 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) From b6f58e1d1d8c6a87dd51fa1c0b1c7ddf5f6153a3 Mon Sep 17 00:00:00 2001 From: Bogdan Date: Sat, 3 May 2014 19:31:09 +0200 Subject: [PATCH 085/273] Paul IGM IGM efficiently deals with constant transformations in the transformation chain --- pysymoro/geometry.py | 9 +++ pysymoro/invgeom.py | 164 ++++++++++++++++++-------------------- pysymoro/tests/oldtest.py | 4 +- 3 files changed, 90 insertions(+), 87 deletions(-) diff --git a/pysymoro/geometry.py b/pysymoro/geometry.py index 7db391a..1c6bec9 100644 --- a/pysymoro/geometry.py +++ b/pysymoro/geometry.py @@ -126,6 +126,15 @@ def matrix(self): 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) diff --git a/pysymoro/invgeom.py b/pysymoro/invgeom.py index 92d7d70..c6e6ba5 100644 --- a/pysymoro/invgeom.py +++ b/pysymoro/invgeom.py @@ -25,115 +25,124 @@ (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, knowns=set()): 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 iTm = nTm.copy() tr_list = transform_list(robo, n, m) - while True: #bring all the known transforms to the left hand side - tr = tr_list.pop() - print tr - if tr.val.atoms(Symbol) <= known: - print 'ololo' - tr.val = -tr.val - iTm = iTm*tr.matrix() - else: - tr_list.append(tr) - break + 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) - known: + if tr.val.atoms(Symbol) - knowns: M_eq = tr.matrix() * to_matrix(tr_list, simplify=False) - iTm while True: - found = _look_for_eq(symo, M_eq, known, th_all, r_all) + found = _look_for_eq(symo, M_eq, knowns, th_all, r_all) repeat |= found - if not found or th_all | r_all <= known: + if not found or th_all | r_all <= knowns: break - tr.val = -tr.val - iTm = tr.matrix() * iTm - if th_all | r_all <= known: + 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 knowns + + +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 known + return tr_list[:var_idx], tr_list[var_idx:] -def _look_for_eq(symo, M_eq, known, th_all, r_all): +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): continue - #eq = symo.unknown_sep(M_eq[e1, e2], known) eq = M_eq[e1, e2] - th_vars = (eq.atoms(Symbol) & th_all) - 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) & known] + 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): + +def loop_solve(robo, symo, know=None): #TODO: rewrite; Add parallelogram detection q_vec = 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]) @@ -151,7 +160,7 @@ def igm_Paul(robo, T_ref, n): #TODO: think about smarter way of matching -def _try_solve_0(symo, eq_sys, known): +def _try_solve_0(symo, eq_sys, knowns): res = False for eq, [r], th_names in eq_sys: X = tools.get_max_coef(eq, r) @@ -161,19 +170,20 @@ def _try_solve_0(symo, eq_sys, known): X = symo.replace(symo.CS12_simp(X), 'X', r) Y = symo.replace(symo.CS12_simp(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 == tools.ZERO, Yi == tools.ZERO, Zi == tools.ZERO]) <= 1 + zero = tools.ZERO + i_ok &= sum([Xi == zero, Yi == zero, Zi == zero]) <= 1 if not i_ok: continue j_ok = False @@ -190,12 +200,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)): @@ -233,7 +243,7 @@ def _try_solve_2(symo, eq_sys, known): _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} + knowns |= {th, r} return True return False @@ -242,7 +252,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)): @@ -285,13 +295,13 @@ 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): +def _try_solve_4(symo, eq_sys, knowns): res = False for i in xrange(len(eq_sys)): all_ok = False @@ -320,7 +330,7 @@ def _try_solve_4(symo, eq_sys, known): continue symo.write_line("# Solving type 8") _solve_type_8(symo, X1, Y1, Z1, Z2, th1, th2) - known |= {th1, th2} + knowns |= {th1, th2} res = True return res @@ -438,11 +448,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): @@ -481,12 +486,6 @@ def _solve_square(symo, A, B, C, x): 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: @@ -504,11 +503,4 @@ def _get_coefs(eq, A1, A2, *xs): # 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/tests/oldtest.py b/pysymoro/tests/oldtest.py index 951b1f5..60b7670 100644 --- a/pysymoro/tests/oldtest.py +++ b/pysymoro/tests/oldtest.py @@ -121,6 +121,8 @@ def test_dgm_sr400(self): 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) igm_f = self.symo.gen_func('IGM_gen', self.robo.q_vec, invgeom.T_GENERAL) @@ -228,7 +230,7 @@ def test_dynamics(self): # suite.addTest(testGeometry('test_dgm_rx90')) # suite.addTest(testGeometry('test_dgm_sr400')) # suite.addTest(testGeometry('test_igm')) -# suite.addTest(testGeometry('test_loop')) + # suite.addTest(testGeometry('test_loop')) # suite.addTest(testKinematics('test_jac')) # suite.addTest(testKinematics('test_jac2')) # unittest.TextTestRunner(verbosity=2).run(suite) From 3c2d971f3dcdcbada60be6eb1c1ac93416d0105b Mon Sep 17 00:00:00 2001 From: aravind Date: Tue, 6 May 2014 15:04:15 +0200 Subject: [PATCH 086/273] Add enum module Add enum module in `symoroutils/enum.py`. For details on the module refer https://pypi.python.org/pypi/enum34 --- symoroutils/enum.py | 812 ++++++++++++++++++++++++++++++++++++++++++++ 1 file changed, 812 insertions(+) create mode 100644 symoroutils/enum.py 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 From 9e8700e19c470c9695897c8c2653af2d7427b18e Mon Sep 17 00:00:00 2001 From: aravind Date: Tue, 6 May 2014 15:06:13 +0200 Subject: [PATCH 087/273] Add properties to `FloatingRobot` class --- pysymoro/floatr.py | 79 ++++++++++++++++++++++++++++++++++++++++++++++ 1 file changed, 79 insertions(+) diff --git a/pysymoro/floatr.py b/pysymoro/floatr.py index 54e0bdc..ff143be 100644 --- a/pysymoro/floatr.py +++ b/pysymoro/floatr.py @@ -99,6 +99,8 @@ def __str__(self): 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 + ("\tStructure: %s\n" % str(self.structure)) + # add joint type - rigid or flexible + str_format = str_format + "\tJoint Type: " + str(self.etas) + '\n' str_format = str_format + '\n' # add geometric params str_format = str_format + "Geometric Parameters:\n" @@ -167,4 +169,81 @@ def frame_nums(self): """ 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 + From 6da625ba1767d9cfada626d83ec5bad6b52be6aa Mon Sep 17 00:00:00 2001 From: Bogdan Date: Tue, 6 May 2014 15:40:56 +0200 Subject: [PATCH 088/273] DGM term change --- pysymoro/geometry.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/pysymoro/geometry.py b/pysymoro/geometry.py index 1c6bec9..62fb36f 100644 --- a/pysymoro/geometry.py +++ b/pysymoro/geometry.py @@ -247,7 +247,7 @@ def to_matrix_fast(symo, tr_list): conv = TransConvolve(symo, trig_subs=True) conv.process(tr) T *= conv.result() - symo.mat_replace(T, 'T%sT%s' % (i, j), skip=1) + symo.mat_replace(T, 'T%sT%s' % (i, j), skip=1, forced=True) return T From bdb455a2a7f00696a06981c5e28de331ee59668b Mon Sep 17 00:00:00 2001 From: Bogdan Date: Tue, 6 May 2014 15:42:33 +0200 Subject: [PATCH 089/273] Start Matlab function output module --- pysymoro/tests/oldtest.py | 18 ++++-- symoroutils/genfunc.py | 127 ++++++++++++++++++++++++++++++++++++++ symoroutils/symbolmgr.py | 64 +++++++++++++------ 3 files changed, 183 insertions(+), 26 deletions(-) create mode 100644 symoroutils/genfunc.py diff --git a/pysymoro/tests/oldtest.py b/pysymoro/tests/oldtest.py index 60b7670..0a1be5a 100644 --- a/pysymoro/tests/oldtest.py +++ b/pysymoro/tests/oldtest.py @@ -77,6 +77,8 @@ 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) @@ -124,6 +126,8 @@ def test_igm(self): 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, @@ -140,6 +144,8 @@ 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, @@ -225,15 +231,15 @@ def test_dynamics(self): if __name__ == '__main__': -# suite = unittest.TestSuite() + suite = unittest.TestSuite() # suite.addTest(testMisc('test_robo_misc')) -# suite.addTest(testGeometry('test_dgm_rx90')) + suite.addTest(testGeometry('test_dgm_rx90')) # suite.addTest(testGeometry('test_dgm_sr400')) -# suite.addTest(testGeometry('test_igm')) - # suite.addTest(testGeometry('test_loop')) + 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() + unittest.TextTestRunner(verbosity=2).run(suite) +# unittest.main() diff --git a/symoroutils/genfunc.py b/symoroutils/genfunc.py new file mode 100644 index 0000000..79af41d --- /dev/null +++ b/symoroutils/genfunc.py @@ -0,0 +1,127 @@ +# -*- coding: utf-8 -*- +""" +This module provides MATLAB function generation +""" + +from sympy import Matrix, Symbol +from sympy import sympify + + +def gen_fheader_matlab(symo, name, args, to_return, multival=False): + func_head = ['function ['] + if multival: + func_head.append(convert_syms_matlab(to_return, 'r')) + else: + func_head.append(convert_syms_matlab(to_return)) + func_head.append('] = %s_func (' % name) + func_head.append(convert_syms_matlab(args)) + func_head.append(')\n'); + return func_head + +def convert_syms_matlab(syms, extension=''): + """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 = [convert_syms_matlab(item, rpl_liter) for item in syms] +# res = [] +# for i, s in enumerate(syms): +# res.append(s) +# if i < len(syms) - 1: +# res += ',' +# return res +# elif isinstance(syms, Matrix): +# res = '' +# for i in xrange(syms.shape[0]): +# res += convert_syms_matlab(list(syms[i, :]), rpl_liter) +# if i < syms.shape[0] - 1: +# res += ',' +# return res +# elif rpl_liter and sympify(syms).is_number: +# return '' +# else: +# return '%s%s' % (syms, extension) + cond1 = isinstance(syms, tuple) + cond2 = isinstance(syms, list) + cond3 = isinstance(syms, Matrix) + if cond1 or cond2 or cond3: + res = [] + for item in iter(syms): + s = convert_syms_matlab(item, extension) + if s is not None: + res.append(s) + res.append(',') + res.pop() + return "".join(res) + elif isinstance(syms, Symbol): + return '%s%s' % (syms, extension) + else: + return None + +def convert_to_list(syms): + 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)) + return res + elif isinstance(syms, Symbol): + return syms + else: + return [] + + +def gen_fbody_matlab(symo, 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 = symo.extract_syms(args) + # final symbols to be compute + syms = symo.extract_syms(to_return) + syms_list = list(syms) + # defines order of computation + order_list = symo.sift_syms(syms, wr_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 + for s in order_list: + if s not in symo.sydi: + item = '%s%s=1.;\n' % (space * folded, s) + elif 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.replace('**','^') + func_body.append(item) + if multival: + for sym in syms_list: + func_body.insert(0, '%s%sr=[];\n' % (space, sym)) + item = '%s%sr=[%sr;%s];\n' % (space*folded, sym, sym, sym) + func_body.append(item) + + for f in xrange(folded-1, 0, -1): + func_body.append('%send\n' % (space * f)) + func_body.append('end\n') + + return func_body, multival diff --git a/symoroutils/symbolmgr.py b/symoroutils/symbolmgr.py index 9cdb639..f8a19da 100644 --- a/symoroutils/symbolmgr.py +++ b/symoroutils/symbolmgr.py @@ -11,7 +11,7 @@ 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.""" @@ -220,10 +220,7 @@ def replace(self, old_sym, name, index='', forced=False): 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 + return new_sym def mat_replace(self, M, name, index='', forced=False, skip=0, symmet=False): @@ -386,7 +383,7 @@ def write_equation(self, A, B): B: expression or var right-hand side of the equation """ - self.write_line(str(A) + ' = ' + str(B)) + self.write_line(str(A) + ' = ' + str(B) + ';') def write_line(self, line=''): """Writes string data into tha output with new line symbol @@ -470,11 +467,10 @@ def convert_syms(self, syms, rpl_liter=False): res += ',' res += ']' return res + elif rpl_liter and sympify(syms).is_number: + return '_' else: - if rpl_liter and sympify(syms).is_number: - return '_' - else: - return str(syms) + return str(syms) def extract_syms(self, syms): """ returns set of all symbols from list or matrix @@ -506,11 +502,13 @@ def sift_syms(self, rq_syms, wr_syms): # will be set to '1.' return rq_vals + order_list - def gen_fbody(self, name, to_return, wr_syms, multival): + 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 @@ -520,7 +518,7 @@ def gen_fbody(self, name, to_return, wr_syms, multival): # 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) @@ -541,10 +539,10 @@ def gen_fbody(self, name, to_return, wr_syms, multival): fun_body.append(' return %s_result\n' % (name)) return fun_body - def gen_func(self, name, to_return, args, multival=False): + def gen_func_string(self, name, to_return, args, syntax = 'python'): #TODO self, name, toret, *args, **kwargs - """ Returns function that computes what is in to_return - using args as arguments + """ Returns function string. The rest is the same as for + gen_func Parameters ========== @@ -564,11 +562,37 @@ def gen_func(self, name, to_return, args, multival=False): computes symbols in to_return have been generated. """ #if kwargs.get - 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) + if syntax == 'python': + fun_head = self.gen_fheader(name, args) + fun_body = self.gen_fbody(name, to_return, args) + elif syntax == 'matlab': + fun_body, mval = gen_fbody_matlab(self, name, to_return, args) + fun_head = gen_fheader_matlab(self, name, args, to_return, mval) fun_string = "".join(fun_head + fun_body) - exec fun_string - return eval('%s_func' % name) + print fun_string + 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_func' % name) \ No newline at end of file From 57185e4fac7b2dba3efe06ec818b52c1f7e0033b Mon Sep 17 00:00:00 2001 From: Bogdan Date: Tue, 6 May 2014 17:10:32 +0200 Subject: [PATCH 090/273] Matlab function generator --- pysymoro/geometry.py | 2 +- pysymoro/tests/oldtest.py | 12 ++--- symoroutils/genfunc.py | 101 +++++++++++++++++--------------------- symoroutils/symbolmgr.py | 9 ++-- symoroviz/graphics.py | 2 +- 5 files changed, 56 insertions(+), 70 deletions(-) diff --git a/pysymoro/geometry.py b/pysymoro/geometry.py index 62fb36f..1c6bec9 100644 --- a/pysymoro/geometry.py +++ b/pysymoro/geometry.py @@ -247,7 +247,7 @@ def to_matrix_fast(symo, tr_list): conv = TransConvolve(symo, trig_subs=True) conv.process(tr) T *= conv.result() - symo.mat_replace(T, 'T%sT%s' % (i, j), skip=1, forced=True) + symo.mat_replace(T, 'T%sT%s' % (i, j), skip=1) return T diff --git a/pysymoro/tests/oldtest.py b/pysymoro/tests/oldtest.py index 0a1be5a..1c3905a 100644 --- a/pysymoro/tests/oldtest.py +++ b/pysymoro/tests/oldtest.py @@ -231,15 +231,15 @@ def test_dynamics(self): if __name__ == '__main__': - suite = unittest.TestSuite() +# suite = unittest.TestSuite() # suite.addTest(testMisc('test_robo_misc')) - suite.addTest(testGeometry('test_dgm_rx90')) +# suite.addTest(testGeometry('test_dgm_rx90')) # suite.addTest(testGeometry('test_dgm_sr400')) - suite.addTest(testGeometry('test_igm')) - suite.addTest(testGeometry('test_loop')) +# 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() +# unittest.TextTestRunner(verbosity=2).run(suite) + unittest.main() diff --git a/symoroutils/genfunc.py b/symoroutils/genfunc.py index 79af41d..0027f9b 100644 --- a/symoroutils/genfunc.py +++ b/symoroutils/genfunc.py @@ -4,21 +4,31 @@ """ from sympy import Matrix, Symbol -from sympy import sympify -def gen_fheader_matlab(symo, name, args, to_return, multival=False): - func_head = ['function ['] - if multival: - func_head.append(convert_syms_matlab(to_return, 'r')) - else: - func_head.append(convert_syms_matlab(to_return)) - func_head.append('] = %s_func (' % name) +def gen_fheader_matlab(symo, name, args, + multival=False): + func_head = [] + func_head.append('function RESULT=%s_func (' % name) func_head.append(convert_syms_matlab(args)) - func_head.append(')\n'); + func_head.append(')\n') return func_head -def convert_syms_matlab(syms, extension=''): + +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 sintactically correct string Parameters @@ -29,43 +39,16 @@ def convert_syms_matlab(syms, extension=''): 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 = [convert_syms_matlab(item, rpl_liter) for item in syms] -# res = [] -# for i, s in enumerate(syms): -# res.append(s) -# if i < len(syms) - 1: -# res += ',' -# return res -# elif isinstance(syms, Matrix): -# res = '' -# for i in xrange(syms.shape[0]): -# res += convert_syms_matlab(list(syms[i, :]), rpl_liter) -# if i < syms.shape[0] - 1: -# res += ',' -# return res -# elif rpl_liter and sympify(syms).is_number: -# return '' -# else: -# return '%s%s' % (syms, extension) - cond1 = isinstance(syms, tuple) - cond2 = isinstance(syms, list) - cond3 = isinstance(syms, Matrix) - if cond1 or cond2 or cond3: - res = [] - for item in iter(syms): - s = convert_syms_matlab(item, extension) - if s is not None: - res.append(s) - res.append(',') - res.pop() - return "".join(res) - elif isinstance(syms, Symbol): - return '%s%s' % (syms, extension) - else: - return None + 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): + +def convert_to_list(syms, keep_const=True): cond1 = isinstance(syms, tuple) cond2 = isinstance(syms, list) cond3 = isinstance(syms, Matrix) @@ -74,13 +57,13 @@ def convert_to_list(syms): for item in syms: res.extend(convert_to_list(item)) return res - elif isinstance(syms, Symbol): - return syms + elif isinstance(syms, Symbol) or keep_const: + return [syms] else: return [] -def gen_fbody_matlab(symo, name, to_return, args): +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. wr_syms are considered to be known @@ -89,7 +72,10 @@ def gen_fbody_matlab(symo, name, to_return, args): wr_syms = symo.extract_syms(args) # final symbols to be compute syms = symo.extract_syms(to_return) - syms_list = list(syms) + 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(syms, wr_syms) # list of instructions in final function @@ -112,16 +98,17 @@ def gen_fbody_matlab(symo, name, to_return, args): folded += 1 else: item = '%s%s=%s;\n' % (space * folded, s, symo.sydi[s]) - item.replace('**','^') + item.replace('**', '^') func_body.append(item) if multival: - for sym in syms_list: - func_body.insert(0, '%s%sr=[];\n' % (space, sym)) - item = '%s%sr=[%sr;%s];\n' % (space*folded, sym, sym, sym) - func_body.append(item) - + 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') - return func_body, multival + return func_body diff --git a/symoroutils/symbolmgr.py b/symoroutils/symbolmgr.py index f8a19da..7901da9 100644 --- a/symoroutils/symbolmgr.py +++ b/symoroutils/symbolmgr.py @@ -539,7 +539,7 @@ def gen_fbody(self, name, to_return, args): fun_body.append(' return %s_result\n' % (name)) return fun_body - def gen_func_string(self, name, to_return, args, syntax = 'python'): + 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 @@ -566,13 +566,12 @@ def gen_func_string(self, name, to_return, args, syntax = 'python'): fun_head = self.gen_fheader(name, args) fun_body = self.gen_fbody(name, to_return, args) elif syntax == 'matlab': - fun_body, mval = gen_fbody_matlab(self, name, to_return, args) - fun_head = gen_fheader_matlab(self, name, args, to_return, mval) + 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) print fun_string return fun_string - def gen_func(self, name, to_return, args): """ Returns function that computes what is in to_return using args as arguments @@ -595,4 +594,4 @@ def gen_func(self, name, to_return, args): computes symbols in to_return have been generated. """ exec self.gen_func_string(name, to_return, args) - return eval('%s_func' % name) \ No newline at end of file + return eval('%s_func' % name) diff --git a/symoroviz/graphics.py b/symoroviz/graphics.py index 38bd4c4..2a07843 100644 --- a/symoroviz/graphics.py +++ b/symoroviz/graphics.py @@ -215,7 +215,7 @@ def generate_loop_fcn(self): 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.q_act_sym) def centralize_to_frame(self, index): q_vec = [self.jnt_dict[sym].q for sym in self.q_sym] From 9f23400bf4598bebec9d6a4331365dfeca05ffcc Mon Sep 17 00:00:00 2001 From: Bogdan Date: Tue, 6 May 2014 23:12:59 +0200 Subject: [PATCH 091/273] Fix in the inertia matrix computation --- pysymoro/dynamics.py | 31 +++++------ pysymoro/tests/oldtest.py | 24 ++++++++- symoroutils/samplerobots.py | 102 +++++++++++++++++++++--------------- 3 files changed, 98 insertions(+), 59 deletions(-) diff --git a/pysymoro/dynamics.py b/pysymoro/dynamics.py index 845cb66..ecb1c15 100644 --- a/pysymoro/dynamics.py +++ b/pysymoro/dynamics.py @@ -141,11 +141,12 @@ def direct_dynamic_NE(robo): symo.sydi : dictionary Dictionary with the information of all the sybstitution """ + # antecedent angular velocity, projected into jth frame wi = ParamsInit.init_vec(robo) - # antecedent angular velocity, projected into jth frame w = ParamsInit.init_w(robo) jaj = ParamsInit.init_vec(robo, 6) - jTant = ParamsInit.init_mat(robo, 6) # Twist transform list of Matrices 6x6 + # Twist transform list of Matrices 6x6 + jTant = ParamsInit.init_mat(robo, 6) beta_star = ParamsInit.init_vec(robo, 6) grandJ = ParamsInit.init_mat(robo, 6) link_acc = ParamsInit.init_vec(robo, 6) @@ -204,28 +205,28 @@ def inertia_matrix(robo): AJE1 = ParamsInit.init_vec(robo) f = ParamsInit.init_vec(robo, ext=1) n = ParamsInit.init_vec(robo, ext=1) - A = sympy.zeros(robo.NL, robo.NL) + A = sympy.zeros(robo.nl, robo.nl) symo = symbolmgr.SymbolManager() 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)): + for j in reversed(xrange(robo.NL)): replace_Jplus(robo, symo, j, Jplus, MSplus, Mplus) - if j != - 1: + if j != 0: 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: + while ka != 0: 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]) + J_base = inertia_spatial(Jplus[0], MSplus[0], Mplus[0]) symo.mat_replace(J_base, 'JP', 0, forced=True, symmet=True) symo.file_close() return symo @@ -497,11 +498,11 @@ def compute_A_diagonal(robo, symo, j, Jplus, MSplus, Mplus, f, n, A): 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] + A[j-1, j-1] = 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] + A[j-1, j-1] = Mplus[j] + robo.IA[j] symo.mat_replace(f[j], 'E' + chars[j], j) symo.mat_replace(n[j], 'N' + chars[j], j) @@ -516,20 +517,20 @@ def compute_A_triangle(robo, symo, j, k, ka, antRj, antPj, f, n, A, AJE1): """ f[ka] = antRj[k]*f[k] if k == j and robo.sigma[j] == 0: - n[ka] = AJE1[k] + tools.skew(antPj[k])*f[k] + n[ka] = AJE1[k] + tools.skew(antPj[k])*antRj[k]*f[k] else: - n[ka] = antRj[k]*n[k] + tools.skew(antPj[k])*f[k] - if ka == - 1: + n[ka] = antRj[k]*n[k] + tools.skew(antPj[k])*antRj[k]*f[k] + if ka == 0: 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] + A[j-1, ka-1] = n[ka][2] elif robo.sigma[ka] == 1: - A[j, ka] = f[ka][2] - A[ka, j] = A[j, ka] + A[j-1, ka-1] = f[ka][2] + A[ka-1, j-1] = A[j-1, ka-1] # TODO:Finish base parameters computation diff --git a/pysymoro/tests/oldtest.py b/pysymoro/tests/oldtest.py index 1c3905a..b84e8f2 100644 --- a/pysymoro/tests/oldtest.py +++ b/pysymoro/tests/oldtest.py @@ -209,7 +209,7 @@ def test_jac2(self): class testDynamics(unittest.TestCase): def test_dynamics(self): - robo = samplerobots.rx90() + robo = samplerobots.cart_pole() print 'Inverse dynamic model using Newton - Euler Algorith' dynamics.inverse_dynamic_NE(robo) @@ -240,6 +240,26 @@ def test_dynamics(self): # suite.addTest(testKinematics('test_jac')) # suite.addTest(testKinematics('test_jac2')) # unittest.TextTestRunner(verbosity=2).run(suite) - unittest.main() +# unittest.main() + robo = samplerobots.planar2r() + + 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) + diff --git a/symoroutils/samplerobots.py b/symoroutils/samplerobots.py index 7529100..0077c53 100644 --- a/symoroutils/samplerobots.py +++ b/symoroutils/samplerobots.py @@ -17,20 +17,17 @@ def cart_pole(): """Generate 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 = 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] @@ -40,17 +37,17 @@ def cart_pole(): 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.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 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 = var('R1, Th2') - robo.qdot = var('R1d, Th2d') - robo.qddot = var('R1dd, Th2dd') - robo.num.append(0) + robo.q = var('O, R1, Th2') + robo.qdot = var('O, R1d, Th2d') + robo.qddot = var('O, R1dd, Th2dd') return robo @@ -63,9 +60,29 @@ def planar2r(): robo.gamma = [0, 0, 0, 0] robo.b = [0, 0, 0, 0] robo.alpha = [0, 0, 0, 0] - robo.d = [0, 0, var('L1'), var('L2')] + robo.d = [0, 0, var('L2'), var('L3')] robo.theta = [0, var('th1'), var('th2'), 0] robo.r = [0, 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] + 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 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 = var('O, th1, th2') + robo.qdot = var('O, th1d, th2d') + robo.qddot = var('O, th1dd, th2dd') return robo @@ -101,27 +118,28 @@ def rx90(): 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) -# 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) + 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] + 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 From 3e4d1dfddb77290688609a45222da579a41b4cf7 Mon Sep 17 00:00:00 2001 From: Bogdan Date: Wed, 7 May 2014 08:39:00 +0200 Subject: [PATCH 092/273] Fix the identification model Problem was in the naming system, another bug due to the changing of numeration --- pysymoro/dynamics.py | 41 ++++++++++++++++++++-------------------- symoroutils/symbolmgr.py | 1 + 2 files changed, 21 insertions(+), 21 deletions(-) diff --git a/pysymoro/dynamics.py b/pysymoro/dynamics.py index ecb1c15..2bdaa98 100644 --- a/pysymoro/dynamics.py +++ b/pysymoro/dynamics.py @@ -97,22 +97,21 @@ def dynamic_identification_NE(robo): if param_vec[i] == tools.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)] + name = '%s' + 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_wrench(robo_tmp, symo, k, w, wdot, U, vdot, F, N) + compute_wrench(robo_tmp, symo, k, w, wdot, U, vdot, F, N, name) # init external forces - Fex = copy(robo.Fex) - Nex = copy(robo.Nex) - for j in reversed(xrange(k + 1)): + Fex = ParamsInit.init_vec(robo) + Nex = ParamsInit.init_vec(robo) + for j in reversed(xrange(1, 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') + vdot, Fjnt, Njnt, F, N, Fex, Nex, name) + for j in xrange(1, k + 1): + compute_torque(robo_tmp, symo, j, Fjnt, Njnt, 'DG' + name) # reset all the parameters to zero robo_tmp.put_inert_param(sympy.zeros(10, 1), k) # compute model for the joint parameters @@ -279,8 +278,8 @@ def pseudo_force_NE(robo): symo.file_close() return symo - -def compute_wrench(robo, symo, j, w, wdot, U, vdot, F, N): +#TODO naming stuff +def compute_wrench(robo, symo, j, w, wdot, U, vdot, F, N, name='%s'): """Internal function. Computes total wrench (torques and forces) of the link j @@ -289,14 +288,14 @@ def compute_wrench(robo, symo, j, w, wdot, U, vdot, F, N): 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) + symo.mat_replace(F[j], ('F' + name) % j) + Psi = symo.mat_replace(robo.J[j]*w[j], ('PSI' + name) % j) N[j] = robo.J[j]*wdot[j] + tools.skew(w[j])*Psi - symo.mat_replace(N[j], 'No', j) - + symo.mat_replace(N[j], ('No' + name) % j) +#TODO refabrish the naming system def compute_joint_wrench(robo, symo, j, antRj, antPj, vdot, - Fjnt, Njnt, F, N, Fex, Nex): + Fjnt, Njnt, F, N, Fex, Nex, name='%s'): """Internal function. Computes wrench (torques and forces) of the joint j @@ -304,23 +303,23 @@ def compute_joint_wrench(robo, symo, j, antRj, antPj, vdot, ===== Fjnt, Njnt, Fex, Nex are the output parameters """ - Fjnt[j] = symo.mat_replace(F[j] + Fex[j], 'E', j) + Fjnt[j] = symo.mat_replace(F[j] + Fex[j], ('E' + name) % j) Njnt[j] = N[j] + Nex[j] + tools.skew(robo.MS[j])*vdot[j] - symo.mat_replace(Njnt[j], 'N', j) - f_ant = symo.mat_replace(antRj[j]*Fjnt[j], 'FDI', j) + symo.mat_replace(Njnt[j], ('N' + name) % j) + f_ant = symo.mat_replace(antRj[j]*Fjnt[j], ('FDI' + name) % j) if robo.ant[j] != - 1: Fex[robo.ant[j]] += f_ant Nex[robo.ant[j]] += antRj[j]*Njnt[j] + tools.skew(antPj[j])*f_ant -def compute_torque(robo, symo, j, Fjnt, Njnt, name='GAM'): +def compute_torque(robo, symo, j, Fjnt, Njnt, name='GAM%s'): """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) + symo.replace(tau_total, name % j, forced=True) def inertia_spatial(J, MS, M): diff --git a/symoroutils/symbolmgr.py b/symoroutils/symbolmgr.py index 7901da9..15c0944 100644 --- a/symoroutils/symbolmgr.py +++ b/symoroutils/symbolmgr.py @@ -189,6 +189,7 @@ def trig_replace(self, M, angle, name): 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. From b3a855131c67c154a69c5d1737f04b34193f6c60 Mon Sep 17 00:00:00 2001 From: Bogdan Date: Wed, 7 May 2014 19:14:27 +0200 Subject: [PATCH 093/273] Matlab functions fix, small dynamic fix Matlab functions will write all the unknown constants as global, In the dynamic model the computation parts of DDM and Inertia matrix were separeted from the interface functions --- pysymoro/dynamics.py | 77 +++++++++++++++++++++++----------------- pysymoro/geometry.py | 9 ++--- symoroutils/genfunc.py | 52 ++++++++++++++++----------- symoroutils/symbolmgr.py | 7 ++-- 4 files changed, 84 insertions(+), 61 deletions(-) diff --git a/pysymoro/dynamics.py b/pysymoro/dynamics.py index 2bdaa98..5994b5c 100644 --- a/pysymoro/dynamics.py +++ b/pysymoro/dynamics.py @@ -126,20 +126,7 @@ def dynamic_identification_NE(robo): 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 - """ +def compute_direct_dynamic_NE(symo, robo): # antecedent angular velocity, projected into jth frame wi = ParamsInit.init_vec(robo) w = ParamsInit.init_w(robo) @@ -153,12 +140,7 @@ def direct_dynamic_NE(robo): juj = ParamsInit.init_vec(robo, 6) # Jj*aj / Hj Tau = ParamsInit.init_scalar(robo) grandVp = ParamsInit.init_vec(robo, 6) - grandVp.append(Matrix([robo.vdot0 - robo.G, robo.w0])) - symo = symbolmgr.SymbolManager() - symo.file_open(robo, 'ddm') - title = 'Direct dynamic model using Newton - Euler Algorith' - symo.write_params_table(robo, title, inert=True, dynam=True) - + grandVp[0] = Matrix([robo.vdot0 - robo.G, robo.w0]) # init transformation antRj, antPj = compute_rot_trans(robo, symo) for j in xrange(1, robo.NL): @@ -175,7 +157,7 @@ def direct_dynamic_NE(robo): 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: + if robo.ant[j] != 0: compute_beta_J_star(robo, symo, j, grandJ, jaj, juj, Tau, beta_star, jTant, link_acc) for j in xrange(1, robo.NL): @@ -183,12 +165,12 @@ def direct_dynamic_NE(robo): 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 + return robo.qddot -def inertia_matrix(robo): - """Computes Inertia Matrix using composed link +def direct_dynamic_NE(robo): + """Computes Direct Dynamic Model using + Newton-Euler formulation Parameters ========== @@ -200,15 +182,21 @@ def inertia_matrix(robo): symo.sydi : dictionary Dictionary with the information of all the sybstitution """ + symo = symbolmgr.SymbolManager() + symo.file_open(robo, 'ddm') + title = 'Direct dynamic model using Newton - Euler Algorith' + symo.write_params_table(robo, title, inert=True, dynam=True) + compute_direct_dynamic_NE(symo, robo) + symo.file_close() + return symo + + +def compute_inertia_matrix(symo, robo, forced=False): Jplus, MSplus, Mplus = ParamsInit.init_jplus(robo) AJE1 = ParamsInit.init_vec(robo) f = ParamsInit.init_vec(robo, ext=1) n = ParamsInit.init_vec(robo, ext=1) A = sympy.zeros(robo.nl, robo.nl) - symo = symbolmgr.SymbolManager() - 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(robo.NL)): @@ -224,9 +212,30 @@ def inertia_matrix(robo): 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) + symo.mat_replace(A, 'A', forced=forced, symmet=True) J_base = inertia_spatial(Jplus[0], MSplus[0], Mplus[0]) - symo.mat_replace(J_base, 'JP', 0, forced=True, symmet=True) + symo.mat_replace(J_base, 'JP', 0, forced=forced, symmet=True) + return A + + +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 + """ + symo = symbolmgr.SymbolManager() + symo.file_open(robo, 'inm') + title = 'Inertia Matrix using composite links' + symo.write_params_table(robo, title, inert=True, dynam=True) + compute_inertia_matrix(symo, robo, forced=True) symo.file_close() return symo @@ -312,10 +321,12 @@ def compute_joint_wrench(robo, symo, j, antRj, antPj, vdot, Nex[robo.ant[j]] += antRj[j]*Njnt[j] + tools.skew(antPj[j])*f_ant -def compute_torque(robo, symo, j, Fjnt, Njnt, name='GAM%s'): +def compute_torque(robo, symo, j, Fjnt, Njnt, name=None): """Internal function. Computes actuation torques - projection of joint wrench on the joint axis """ + if name is None: + name = str(robo.GAM[j]) + '%s' 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) @@ -445,7 +456,7 @@ def compute_acceleration(robo, symo, j, jTant, grandVp, qddot = 0 else: qddot = H_inv[j]*Tau[j] - E1 - qddot = symo.replace(qddot, "QDP", j, forced=True) + qddot = symo.replace(qddot, str(robo.qddot[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) diff --git a/pysymoro/geometry.py b/pysymoro/geometry.py index 1c6bec9..b654dc0 100644 --- a/pysymoro/geometry.py +++ b/pysymoro/geometry.py @@ -234,7 +234,7 @@ def transform_list(robo, i, j): return [tr for tr in tr_list if tr.val != 0] -def to_matrix_fast(symo, tr_list): +def to_matrix_fast(symo, tr_list, forced=False): conv = TransConvolve(symo, trig_subs=True) T = eye(4) i = tr_list[0].i @@ -247,7 +247,7 @@ def to_matrix_fast(symo, tr_list): conv = TransConvolve(symo, trig_subs=True) conv.process(tr) T *= conv.result() - symo.mat_replace(T, 'T%sT%s' % (i, j), skip=1) + symo.mat_replace(T, 'T%sT%s' % (i, j), skip=1, forced=forced) return T @@ -286,7 +286,8 @@ def to_matrices_left(tr_list, symo=None, trig_subs=False): return res -def dgm(robo, symo, i, j, key='one', fast_form=True, trig_subs=True): +def dgm(robo, symo, i, j, key='one', fast_form=True, + trig_subs=True, forced=False): """must be the final DGM function Parameters @@ -316,7 +317,7 @@ def dgm(robo, symo, i, j, key='one', fast_form=True, trig_subs=True): 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) + return to_matrix_fast(symo, tr_list, forced) else: if key == 'left': return to_matrices_left(tr_list, symo, trig_subs) diff --git a/symoroutils/genfunc.py b/symoroutils/genfunc.py index 0027f9b..67cf713 100644 --- a/symoroutils/genfunc.py +++ b/symoroutils/genfunc.py @@ -29,7 +29,7 @@ def convert_mat_matlab(to_return): def convert_syms_matlab(syms): - """Converts 'syms' structure to sintactically correct string + """Converts 'syms' structure to a string Parameters ========== @@ -55,7 +55,7 @@ def convert_to_list(syms, keep_const=True): if cond1 or cond2 or cond3: res = [] for item in syms: - res.extend(convert_to_list(item)) + res.extend(convert_to_list(item, keep_const)) return res elif isinstance(syms, Symbol) or keep_const: return [syms] @@ -65,41 +65,53 @@ def convert_to_list(syms, keep_const=True): 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. wr_syms are considered to + computes symbolf from to_return. arg_syms are considered to be known """ # set of defined symbols - wr_syms = symo.extract_syms(args) + arg_syms = symo.extract_syms(args) # final symbols to be compute - syms = symo.extract_syms(to_return) + 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(syms, wr_syms) + print arg_syms, res_syms + 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: - item = '%s%s=1.;\n' % (space * folded, s) - elif 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 + 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: - item = '%s%s=%s;\n' % (space * folded, s, symo.sydi[s]) - item.replace('**', '^') - func_body.append(item) + 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.replace('**', '^') + func_body.append(item) if multival: func_body.insert(0, '%sRESULT=[];\n' % (space)) item = '%sRESULT=[RESULT;%s];\n' % (space * folded, to_ret_str) @@ -110,5 +122,5 @@ def gen_fbody_matlab(symo, name, to_return, args, ret_name=''): 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/symbolmgr.py b/symoroutils/symbolmgr.py index 15c0944..3c3ed79 100644 --- a/symoroutils/symbolmgr.py +++ b/symoroutils/symbolmgr.py @@ -264,9 +264,9 @@ def mat_replace(self, M, name, index='', 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: + for i1 in xrange(M.shape[0] - skip): + for i2 in xrange(M.shape[1]): + if symmet and i1 > i2: M[i1, i2] = M[i2, i1] continue if M.shape[1] > 1: @@ -570,7 +570,6 @@ def gen_func_string(self, name, to_return, args, syntax='python'): 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) - print fun_string return fun_string def gen_func(self, name, to_return, args): From c5ef8495d1079563417277679008b3ae0357875d Mon Sep 17 00:00:00 2001 From: aravind Date: Sat, 10 May 2014 23:31:14 +0200 Subject: [PATCH 094/273] Fix __str__() to have correct alignment Fix __str__() to have correct alignment in `DynParams` and `GeoParams` classes. Update corresponding __repr__() accordingly. --- pysymoro/dynparams.py | 12 +++++++----- pysymoro/geoparams.py | 11 +++++++---- 2 files changed, 14 insertions(+), 9 deletions(-) diff --git a/pysymoro/dynparams.py b/pysymoro/dynparams.py index 7daf32a..dab5050 100644 --- a/pysymoro/dynparams.py +++ b/pysymoro/dynparams.py @@ -6,6 +6,8 @@ """ +import re + from sympy import eye, var from sympy import Matrix @@ -99,8 +101,8 @@ def __init__(self, link, params=None): self.update_params(params) def __str__(self): - pattern = ' %s' - str_format = (pattern * 20) % ( + 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), @@ -108,12 +110,12 @@ def __str__(self): 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) - ) - str_format = '\t' + str_format.lstrip() + )) return str_format def __repr__(self): - repr_format = str(self).lstrip().replace(' ', ', ') + repr_format = str(self).lstrip().rstrip() + repr_format = re.sub('\s+', ', ', repr_format) repr_format = '(' + repr_format + ')' return repr_format diff --git a/pysymoro/geoparams.py b/pysymoro/geoparams.py index 705609f..c986df5 100644 --- a/pysymoro/geoparams.py +++ b/pysymoro/geoparams.py @@ -6,6 +6,8 @@ """ +import re + from pysymoro import transform @@ -45,19 +47,20 @@ def __init__(self, frame, params=None): self.update_params(params) def __str__(self): - pattern = '\t%s' - str_format = (pattern * 11) % ( + 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().replace('\t', ', ') + repr_format = str(self).lstrip().rstrip() + repr_format = re.sub('\s+', ', ', repr_format) repr_format = '(' + repr_format + ')' return repr_format From 400983696d5917a750818d7a0b812e07ad0cf138 Mon Sep 17 00:00:00 2001 From: aravind Date: Sat, 10 May 2014 23:35:23 +0200 Subject: [PATCH 095/273] Add methods to update geometric and dynamic params Add methods to update the geometric and dynamic parameters of the robot. The method is called update_params(). Add put_val() method to communicate with the user interface. The put_val() method updates the parameters by calling the update_params() method. NOTE: Currently only geometric and dynamic parameters can be updated. --- pysymoro/floatr.py | 106 ++++++++++++++++++++++++++++++++++++++++++++- 1 file changed, 105 insertions(+), 1 deletion(-) diff --git a/pysymoro/floatr.py b/pysymoro/floatr.py index ff143be..3c1b049 100644 --- a/pysymoro/floatr.py +++ b/pysymoro/floatr.py @@ -85,8 +85,10 @@ def __init__( self.base_vel = Screw() """Base acceleration 6x1 column vector - a Screw.""" self.base_acc = Screw() - """Transformation matrix of base wrt a reference frame.""" + """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 = "" @@ -105,12 +107,20 @@ def __str__(self): # 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' + ('=*' * 60) + '=' @@ -125,6 +135,77 @@ def __repr__(self): ) return repr_format + 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. + """ + # update Z matrix + if name is 'Z': + return tools.OK + #return self.base_tmat[idx] = val + 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) + return tools.FAIL + + def update_params(self, attr, params): + """ + Update the parameter values of the robot. + + Args: + attr: A string metioning the attribute. More specifically + indicating whether the parameters correspond to geometric + (`geos`), dynamic (`dyns`) 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 Example 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} + } + """ + if attr in ['dyns', 'geos']: + param = getattr(self, attr) + else: + raise NotImplementedError( + "Only geometric and dynamic params are supported." + ) + for key in params: + try: + idx = int(key) + param[idx].update_params(params[key]) + except IndexError: + raise IndexError( + "geos doesnt have %s index value." % str(idx) + ) + return tools.OK + @property def link_nums(self): """ @@ -246,4 +327,27 @@ def active_joints(self): joints.append(j) return joints + 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._misc_params_map = [ + 'axis', 'W0', 'WP0', 'V0', 'VP0', 'G', 'QP', 'QDP', 'GAM' + ] + From a87481d06c4447fe2912732bb935fffb83f953c8 Mon Sep 17 00:00:00 2001 From: aravind Date: Sun, 11 May 2014 03:22:48 +0200 Subject: [PATCH 096/273] Modify order of base parameters in UI --- symoroui/labels.py | 24 ++++++++++++------------ 1 file changed, 12 insertions(+), 12 deletions(-) diff --git a/symoroui/labels.py b/symoroui/labels.py index 867596a..7ff7747 100644 --- a/symoroui/labels.py +++ b/symoroui/labels.py @@ -46,18 +46,18 @@ ]) # base velocity and acceleration params BASE_VEL_ACC = OrderedDict([ - ('wx', FieldEntry('W0X', 'W0X', 'txt', (0, 0), 'OnBaseTwistChanged', 0)), - ('wy', FieldEntry('W0Y', 'W0Y', 'txt', (1, 0), 'OnBaseTwistChanged', 1)), - ('wz', FieldEntry('W0Z', 'W0Z', 'txt', (2, 0), 'OnBaseTwistChanged', 2)), - ('wpx', FieldEntry('WP0X', 'WP0X', 'txt', (0, 1), 'OnBaseTwistChanged', 0)), - ('wpy', FieldEntry('WP0Y', 'WP0Y', 'txt', (1, 1), 'OnBaseTwistChanged', 1)), - ('wpz', FieldEntry('WP0Z', 'WP0Z', 'txt', (2, 1), 'OnBaseTwistChanged', 2)), - ('vx', FieldEntry('V0X', 'V0X', 'txt', (0, 2), 'OnBaseTwistChanged', 0)), - ('vy', FieldEntry('V0Y', 'V0Y', 'txt', (1, 2), 'OnBaseTwistChanged', 1)), - ('vz', FieldEntry('V0Z', 'V0Z', 'txt', (2, 2), 'OnBaseTwistChanged', 2)), - ('vpx', FieldEntry('VP0X', 'VP0X', 'txt', (0, 3), 'OnBaseTwistChanged', 0)), - ('vpy', FieldEntry('VP0Y', 'VP0Y', 'txt', (1, 3), 'OnBaseTwistChanged', 1)), - ('vpz', FieldEntry('VP0Z', 'VP0Z', 'txt', (2, 3), 'OnBaseTwistChanged', 2)) + ('wx', FieldEntry('W0X', 'W0X', 'txt', (0, 1), 'OnBaseTwistChanged', 0)), + ('wy', FieldEntry('W0Y', 'W0Y', 'txt', (1, 1), 'OnBaseTwistChanged', 1)), + ('wz', FieldEntry('W0Z', 'W0Z', 'txt', (2, 1), 'OnBaseTwistChanged', 2)), + ('wpx', FieldEntry('WP0X', 'WP0X', 'txt', (0, 3), 'OnBaseTwistChanged', 0)), + ('wpy', FieldEntry('WP0Y', 'WP0Y', 'txt', (1, 3), 'OnBaseTwistChanged', 1)), + ('wpz', FieldEntry('WP0Z', 'WP0Z', 'txt', (2, 3), 'OnBaseTwistChanged', 2)), + ('vx', FieldEntry('V0X', 'V0X', 'txt', (0, 0), 'OnBaseTwistChanged', 0)), + ('vy', FieldEntry('V0Y', 'V0Y', 'txt', (1, 0), 'OnBaseTwistChanged', 1)), + ('vz', FieldEntry('V0Z', 'V0Z', 'txt', (2, 0), 'OnBaseTwistChanged', 2)), + ('vpx', FieldEntry('VP0X', 'VP0X', 'txt', (0, 2), 'OnBaseTwistChanged', 0)), + ('vpy', FieldEntry('VP0Y', 'VP0Y', 'txt', (1, 2), 'OnBaseTwistChanged', 1)), + ('vpz', FieldEntry('VP0Z', 'VP0Z', 'txt', (2, 2), 'OnBaseTwistChanged', 2)) ]) # inertial params DYN_PARAMS_I = OrderedDict([ From 29f768c49e02c81c66d8ac8825789b9b5b2aff4d Mon Sep 17 00:00:00 2001 From: aravind Date: Sun, 11 May 2014 03:51:58 +0200 Subject: [PATCH 097/273] Add method to update miscellaneous parameters Modify `update_params()` method to update some other parameter values. Move update of geometric and dynamic parameters to separate private method - `_update_dyns_geos()`. Add `_update_misc()` method to do the actual update of miscellaneous parameter values. Add `stiffness` attribute to `FloatingRobot` class. --All changes in `FloatingRobot` class-- --- pysymoro/dynparams.py | 28 +++++++---- pysymoro/floatr.py | 105 ++++++++++++++++++++++++++++++++++-------- pysymoro/geoparams.py | 2 +- 3 files changed, 106 insertions(+), 29 deletions(-) diff --git a/pysymoro/dynparams.py b/pysymoro/dynparams.py index dab5050..1e927b4 100644 --- a/pysymoro/dynparams.py +++ b/pysymoro/dynparams.py @@ -182,25 +182,37 @@ def moment(self): def _init_inertial_terms(self): """Initialise inertial terms.""" for key, term in self._inertial_terms.iteritems(): - value = term + str(self.link) - setattr(self, key, var(value)) + 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(): - value = term + str(self.link) - setattr(self, key, var(value)) + 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(): - value = term + str(self.link) - setattr(self, key, var(value)) + 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(): - value = term + str(self.link) - setattr(self, key, var(value)) + if self.link != 0: + value = var(term + str(self.link)) + else: + value = 0 + setattr(self, key, value) diff --git a/pysymoro/floatr.py b/pysymoro/floatr.py index 3c1b049..cf3309b 100644 --- a/pysymoro/floatr.py +++ b/pysymoro/floatr.py @@ -61,6 +61,8 @@ def __init__( 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('QD{0}'.format(j)) for j in self.joint_nums] """Joint accelerations.""" @@ -102,7 +104,8 @@ def __str__(self): str_format = str_format + ("\tFloating: %s\n" % str(self.is_floating)) str_format = str_format + ("\tStructure: %s\n" % str(self.structure)) # add joint type - rigid or flexible - str_format = str_format + "\tJoint Type: " + str(self.etas) + '\n' + str_format = str_format + "\tJoint Type: %s\n" % str(self.etas) + str_format = str_format + "\tStiffness: %s\n" % str(self.stiffness) str_format = str_format + '\n' # add geometric params str_format = str_format + "Geometric Parameters:\n" @@ -161,19 +164,26 @@ def put_val(self, idx, name, value): value = int(value) param = {int(idx): {self._geo_params_map[name]: value}} return self.update_params('geos', param) + elif name in self._base_params_map: + return self.update_params('base', 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) return tools.FAIL - def update_params(self, attr, params): + def update_params(self, kind, params): """ Update the parameter values of the robot. Args: - attr: A string metioning the attribute. More specifically + kind: A string metioning the paramter type. More specifically indicating whether the parameters correspond to geometric - (`geos`), dynamic (`dyns`) or others (`misc`). + (`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 Example for a valid dict + values as the value. See Examples for a valid dict argument that is accepted by this method. Example 1: @@ -189,21 +199,30 @@ def update_params(self, attr, params): 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 attr in ['dyns', 'geos']: - param = getattr(self, attr) - else: + 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( - "Only geometric and dynamic params are supported." + "Yet to be supported." ) - for key in params: - try: - idx = int(key) - param[idx].update_params(params[key]) - except IndexError: - raise IndexError( - "geos doesnt have %s index value." % str(idx) - ) + else: + errmsg = "`kind` can be ['dyns', 'geos', 'misc', 'base']. " + errmsg = errmsg + ("Current input: {0}").format(kind) + raise ValueError(errmsg) return tools.OK @property @@ -327,6 +346,47 @@ def active_joints(self): 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 @@ -346,8 +406,13 @@ def _init_maps(self): ('mu', 'mu'), ('gamma', 'gamma'), ('b', 'b'), ('alpha', 'alpha'), ('d', 'd'), ('theta', 'theta'), ('r', 'r') ]) - self._misc_params_map = [ - 'axis', 'W0', 'WP0', 'V0', 'VP0', 'G', 'QP', 'QDP', 'GAM' - ] + self._base_params_map = dict([ + ('V0', 'base_vel'), ('VP0', 'base_acc'), + ('W0', 'base_vel'), ('WP0', 'base_acc') + ]) + self._misc_params_map = dict([ + ('QP', 'qdots'), ('QDP', 'qddots'), ('GAM', 'torques'), + ('ETA', 'etas'), ('K', 'stiffness'), ('G', 'gravity'), + ]) diff --git a/pysymoro/geoparams.py b/pysymoro/geoparams.py index c986df5..bbcca8b 100644 --- a/pysymoro/geoparams.py +++ b/pysymoro/geoparams.py @@ -31,7 +31,7 @@ def __init__(self, frame, params=None): """ self.frame = frame self.ant = frame - 1 - self.sigma = 0 + self.sigma = 0 if frame != 0 else 2 self.mu = 0 self.gamma = 0 self.b = 0 From f983d843507c1f0827f5b322fdf9851e5a73ae81 Mon Sep 17 00:00:00 2001 From: aravind Date: Sun, 11 May 2014 04:16:43 +0200 Subject: [PATCH 098/273] Remove `pysymoro/manipulator.py` --- pysymoro/manipulator.py | 5 ----- 1 file changed, 5 deletions(-) delete mode 100644 pysymoro/manipulator.py diff --git a/pysymoro/manipulator.py b/pysymoro/manipulator.py deleted file mode 100644 index 3039a03..0000000 --- a/pysymoro/manipulator.py +++ /dev/null @@ -1,5 +0,0 @@ -# -*- coding: utf-8 -*- - - -"""This module contains the Robot data structure""" - From 7694b8d3e5277472c70ba21a39a401015807223f Mon Sep 17 00:00:00 2001 From: aravind Date: Sun, 11 May 2014 05:33:45 +0200 Subject: [PATCH 099/273] Add UI fields for joint flexibility and stiffness --- pysymoro/floatr.py | 8 ++-- symoroui/labels.py | 30 ++++++++------- symoroui/layout.py | 91 ++++++++++++++++++++++++---------------------- 3 files changed, 68 insertions(+), 61 deletions(-) diff --git a/pysymoro/floatr.py b/pysymoro/floatr.py index cf3309b..02b4a70 100644 --- a/pysymoro/floatr.py +++ b/pysymoro/floatr.py @@ -24,7 +24,7 @@ class FloatingRobot(object): act as the gateway for robot modelling. """ def __init__( - self, name, links=0, joints=0, frames=0, + self, name, links=0, joints=0, frames=0, is_floating=True, structure=tools.TREE ): """ @@ -50,7 +50,7 @@ def __init__( """ 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). + link of the base). """ self.dyns = [DynParams(j) for j in self.link_nums] # properties dependent on number of joints @@ -232,7 +232,7 @@ def link_nums(self): Returns: An iteratable object with the link numbers. - Note: + 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 @@ -412,7 +412,7 @@ def _init_maps(self): ]) self._misc_params_map = dict([ ('QP', 'qdots'), ('QDP', 'qddots'), ('GAM', 'torques'), - ('ETA', 'etas'), ('K', 'stiffness'), ('G', 'gravity'), + ('eta', 'etas'), ('K', 'stiffness'), ('G', 'gravity'), ]) diff --git a/symoroui/labels.py b/symoroui/labels.py index 7ff7747..aa78ac2 100644 --- a/symoroui/labels.py +++ b/symoroui/labels.py @@ -31,18 +31,20 @@ geom_params="Geometric Parameters", dyn_params="Dynamic Parameters and External Forces", base_vel_acc="Velocity and Acceleration of the base", - joint_vel_acc="Joint Velocity and Acceleration" + 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_VEL_ACC = OrderedDict([ +JOINT_PARAMS = OrderedDict([ ('joint', FieldEntry('Joint', 'joint', 'cmb', (0, 0), 'OnJointChanged', -1)), - ('qp', FieldEntry('QP', 'QP', 'txt', (1, 0), 'OnSpeedChanged', -1)), - ('qdp', FieldEntry('QDP', 'QDP', 'txt', (2, 0), 'OnSpeedChanged', -1)), - ('gam', FieldEntry('GAM', 'GAM', 'txt', (3, 0), 'OnSpeedChanged', -1)) + ('qp', FieldEntry('QP', 'QP', 'txt', (2, 0), 'OnSpeedChanged', -1)), + ('qdp', FieldEntry('QDP', 'QDP', 'txt', (2, 1), 'OnSpeedChanged', -1)), + ('gam', FieldEntry('GAM', 'GAM', 'txt', (1, 1), 'OnSpeedChanged', -1)), + ('eta', FieldEntry('eta', 'eta', 'cmb', (0, 1), 'OnSpeedChanged', -1)), + ('stiff', FieldEntry('k', 'K', 'txt', (1, 0), 'OnSpeedChanged', -1)), ]) # base velocity and acceleration params BASE_VEL_ACC = OrderedDict([ @@ -77,18 +79,18 @@ ]) # friction and rotor inertia params DYN_PARAMS_X = OrderedDict([ - ('ia', FieldEntry('IA', 'IA', 'txt', (2, 0), 'OnDynParamChanged', -1)), - ('frc', FieldEntry('FS', 'FS', 'txt', (2, 1), 'OnDynParamChanged', -1)), - ('frv', FieldEntry('FV', 'FV', 'txt', (2, 2), 'OnDynParamChanged', -1)) + ('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', (3, 0), 'OnDynParamChanged', -1)), - ('fy_ext', FieldEntry('FY', 'FY', 'txt', (3, 1), 'OnDynParamChanged', -1)), - ('fz_ext', FieldEntry('FZ', 'FZ', 'txt', (3, 2), 'OnDynParamChanged', -1)), - ('mx_ext', FieldEntry('CX', 'CX', 'txt', (3, 3), 'OnDynParamChanged', -1)), - ('my_ext', FieldEntry('CY', 'CY', 'txt', (3, 4), 'OnDynParamChanged', -1)), - ('mz_ext', FieldEntry('CZ', 'CZ', 'txt', (3, 5), 'OnDynParamChanged', -1)) + ('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( diff --git a/symoroui/layout.py b/symoroui/layout.py index 22c53bf..622beb9 100644 --- a/symoroui/layout.py +++ b/symoroui/layout.py @@ -4,7 +4,7 @@ """ This module creates the main window user interface and draws the -interface on the screen for the SYMORO package. +interface on the screen for the SYMORO package. """ @@ -59,7 +59,7 @@ def __init__(self, *args, **kwargs): self.statusbar.SetStatusWidths(widths=[-1, -1]) self.statusbar.SetStatusText(text="Ready", number=0) self.statusbar.SetStatusText( - text="Location of robot files is %s" + text="Location of robot files is %s" % filemgr.get_base_path(), number = 1 ) @@ -74,7 +74,7 @@ def params_in_grid(self, szr_grd, elements, rows, cols, width=70): field_id = int(elements[key].id) if control is 'cmb': ctrl = wx.ComboBox( - parent=self.panel, style=wx.CB_READONLY, + parent=self.panel, style=wx.CB_READONLY, size=(width, -1), name=name ) ctrl.Bind(wx.EVT_COMBOBOX, handler) @@ -84,7 +84,7 @@ def params_in_grid(self, szr_grd, elements, rows, cols, width=70): ) else: ctrl = wx.TextCtrl( - parent=self.panel, size=(width, -1), + parent=self.panel, size=(width, -1), name=name, id=field_id ) ctrl.Bind(wx.EVT_KILL_FOCUS, handler) @@ -97,11 +97,11 @@ def params_in_grid(self, szr_grd, elements, rows, cols, width=70): ), proportion=0, flag=wx.ALL | wx.ALIGN_RIGHT, border=5 ) szr_ele.Add( - ctrl, proportion=0, + ctrl, proportion=0, flag=wx.ALL | wx.ALIGN_LEFT, border=1 ) szr_grd.Add( - szr_ele, pos=(place[0], place[1]), + szr_ele, pos=(place[0], place[1]), flag=wx.ALL | wx.ALIGN_RIGHT, border=2 ) @@ -136,11 +136,11 @@ def create_ui(self): self.widgets[name] = ctrl self.widget_keys[key] = ctrl szr_grd_robot_type.Add( - wx.StaticText(self.panel, label=label), + wx.StaticText(self.panel, label=label), pos=(idx, 0), flag=wx.LEFT, border=10 ) szr_grd_robot_type.Add( - ctrl, pos=(idx, 1), + ctrl, pos=(idx, 1), flag=wx.LEFT | wx.RIGHT, border=10 ) szr_robot_type.Add( @@ -156,8 +156,8 @@ def create_ui(self): ) 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_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) @@ -175,7 +175,7 @@ def create_ui(self): idx = (j*4) + i name = 'Z'+str(idx) txt_z_element = wx.TextCtrl( - parent=self.panel, name=name, + parent=self.panel, name=name, id=idx, size=(60, -1) ) self.widgets[name] = txt_z_element @@ -184,7 +184,7 @@ def create_ui(self): wx.EVT_KILL_FOCUS, self.OnZParamChanged ) szr_grd_loc.Add( - txt_z_element, pos=(j + 1, i + 1), + 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)) @@ -196,11 +196,11 @@ def create_ui(self): lbl_row, pos=(i+1, 0), flag=wx.RIGHT, border=3 ) szr_grd_loc.Add( - lbl_col, pos=(0, i+1), + lbl_col, pos=(0, i+1), flag=wx.ALIGN_CENTER_HORIZONTAL, border=3 ) szr_grd_loc.Add( - lbl_last_row, pos=(4, i+1), + 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) @@ -213,7 +213,7 @@ def create_ui(self): ) szr_grd_geom = wx.GridBagSizer(0, 5) self.params_in_grid( - szr_grd_geom, elements=ui_labels.GEOM_PARAMS, + szr_grd_geom, elements=ui_labels.GEOM_PARAMS, rows=2, cols=5, width=70 ) szr_geom_params.Add(szr_grd_geom) @@ -230,7 +230,7 @@ def create_ui(self): name=ui_labels.DYN_PARAMS['link'].name ) cmb_link.Bind( - wx.EVT_COMBOBOX, + wx.EVT_COMBOBOX, getattr(self, ui_labels.DYN_PARAMS['link'].handler) ) self.widgets['link'] = cmb_link @@ -239,7 +239,7 @@ def create_ui(self): szr_link.Add( wx.StaticText( self.panel, label=ui_labels.DYN_PARAMS['link'].label - ), proportion=0, + ), proportion=0, flag=wx.ALL | wx.ALIGN_LEFT, border=5 ) szr_link.AddSpacer((4,4)) @@ -265,7 +265,7 @@ def create_ui(self): ) szr_grd_base_velacc = wx.GridBagSizer(0, 0) self.params_in_grid( - szr_grd_base_velacc, elements=ui_labels.BASE_VEL_ACC, + szr_grd_base_velacc, elements=ui_labels.BASE_VEL_ACC, rows=3, cols=4, width=60 ) szr_base_velacc.Add(szr_grd_base_velacc) @@ -274,15 +274,15 @@ def create_ui(self): # right col - joint velocity and acceleration box szr_joint_velacc = wx.StaticBoxSizer( wx.StaticBox( - self.panel, label=ui_labels.BOX_TITLES['joint_vel_acc'] + 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_VEL_ACC, - rows=3, cols=1, width=75, + szr_grd_joint_velacc, elements=ui_labels.JOINT_PARAMS, + rows=3, cols=2, width=75, ) - szr_joint_velacc.Add(szr_grd_joint_velacc, + 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) @@ -382,11 +382,16 @@ def feed_data(self): 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'])] + 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']), ('eta', ['0', '1']) + ] for name, lst in lsts: cmb = self.widgets[name] cmb.SetItems(lst) @@ -402,7 +407,7 @@ def feed_data(self): def create_menu(self): """Method to create the menu bar""" menu_bar = wx.MenuBar() - # menu item - file + # menu item - file file_menu = wx.Menu() m_new = wx.MenuItem( file_menu, wx.ID_NEW, ui_labels.FILE_MENU['m_new'] @@ -520,7 +525,7 @@ def create_menu(self): # menu item - identification iden_menu = wx.Menu() m_base_inertial_params = wx.MenuItem( - iden_menu, wx.ID_ANY, + iden_menu, wx.ID_ANY, ui_labels.IDEN_MENU['m_base_inertial_params'] ) self.Bind( @@ -533,7 +538,7 @@ def create_menu(self): self.Bind(wx.EVT_MENU, self.OnDynIdentifModel, m_dyn_iden_model) iden_menu.AppendItem(m_dyn_iden_model) m_energy_iden_model = wx.MenuItem( - iden_menu, wx.ID_ANY, + iden_menu, wx.ID_ANY, ui_labels.IDEN_MENU['m_energy_iden_model'] ) # TODO: uncomment the 3 lines below to add the event @@ -555,7 +560,7 @@ def create_menu(self): def OnNew(self, event): dialog = ui_definition.DialogDefinition( - ui_labels.MAIN_WIN['prog_name'], + ui_labels.MAIN_WIN['prog_name'], self.robo.name, self.robo.nl, self.robo.nj, self.robo.structure, self.robo.is_mobile ) @@ -597,24 +602,24 @@ def OnNew(self, event): def message_error(self, message): wx.MessageDialog( - None, + None, message, - 'Error', + 'Error', wx.OK | wx.ICON_ERROR ).ShowModal() def message_warning(self, message): wx.MessageDialog( - None, + None, message, - 'Error', + 'Error', wx.OK | wx.ICON_WARNING ).ShowModal() def message_info(self, message): wx.MessageDialog( - None, - message, + None, + message, 'Information', wx.OK | wx.ICON_INFORMATION ).ShowModal() @@ -639,10 +644,10 @@ def OnOpen(self, event): if self.OnSave(None) == tools.FAIL: return dialog = wx.FileDialog( - self, - message="Choose PAR file", + self, + message="Choose PAR file", style=wx.OPEN, - wildcard='*.par', + wildcard='*.par', defaultFile='*.par' ) if dialog.ShowModal() == wx.ID_OK: @@ -700,7 +705,7 @@ def OnFastGeometricModel(self, event): def OnIgmPaul(self, event): dialog = ui_geometry.DialogPaul( - ui_labels.MAIN_WIN['prog_name'], + ui_labels.MAIN_WIN['prog_name'], self.robo.endeffectors, str(invgeom.EMPTY) ) @@ -782,12 +787,12 @@ def OnVisualisation(self, event): if dialog.ShowModal() == wx.ID_OK: self.par_dict = dialog.get_values() graphics.MainWindow( - ui_labels.MAIN_WIN['prog_name'], self.robo, + ui_labels.MAIN_WIN['prog_name'], self.robo, self.par_dict, self ) else: graphics.MainWindow( - ui_labels.MAIN_WIN['prog_name'], self.robo, + ui_labels.MAIN_WIN['prog_name'], self.robo, self.par_dict, self ) From 9b658528995d11d1b1a26765ff9a3c819ad88178 Mon Sep 17 00:00:00 2001 From: aravind Date: Sun, 11 May 2014 06:34:08 +0200 Subject: [PATCH 100/273] Remove dependency on `*_head` terms from `Robot` class --- symoroui/definition.py | 2 +- symoroui/labels.py | 2 +- symoroui/layout.py | 43 ++++++++++++++++++++++++++++-------------- 3 files changed, 31 insertions(+), 16 deletions(-) diff --git a/symoroui/definition.py b/symoroui/definition.py index ed0f5de..ac6817f 100644 --- a/symoroui/definition.py +++ b/symoroui/definition.py @@ -129,7 +129,7 @@ def has_syms(self): return len(self.syms) > 0 def construct_sym(self): - params = self.robo.get_geom_head()[4:] + params = ['gamma', 'b', 'alpha', 'd', 'theta', 'r'] q_vec = self.robo.q_vec self.syms = set() for par in params: diff --git a/symoroui/labels.py b/symoroui/labels.py index aa78ac2..2769433 100644 --- a/symoroui/labels.py +++ b/symoroui/labels.py @@ -43,8 +43,8 @@ ('qp', FieldEntry('QP', 'QP', 'txt', (2, 0), 'OnSpeedChanged', -1)), ('qdp', FieldEntry('QDP', 'QDP', 'txt', (2, 1), 'OnSpeedChanged', -1)), ('gam', FieldEntry('GAM', 'GAM', 'txt', (1, 1), 'OnSpeedChanged', -1)), - ('eta', FieldEntry('eta', 'eta', 'cmb', (0, 1), 'OnSpeedChanged', -1)), ('stiff', FieldEntry('k', 'K', 'txt', (1, 0), 'OnSpeedChanged', -1)), + ('eta', FieldEntry('eta', 'eta', 'cmb', (0, 1), 'OnSpeedChanged', -1)) ]) # base velocity and acceleration params BASE_VEL_ACC = OrderedDict([ diff --git a/symoroui/layout.py b/symoroui/layout.py index 622beb9..1cf682c 100644 --- a/symoroui/layout.py +++ b/symoroui/layout.py @@ -335,7 +335,7 @@ def OnLinkChanged(self, event): self.update_dyn_params() def OnJointChanged(self, event): - self.update_vel_params() + self.update_joint_params() def update_params(self, index, pars): for par in pars: @@ -343,28 +343,36 @@ def update_params(self, index, pars): widget.ChangeValue(str(self.robo.get_val(index, par))) def update_geo_params(self): + pars = self._extract_param_names(ui_labels.GEOM_PARAMS) index = int(self.widgets['frame'].Value) - for par in self.robo.get_geom_head()[1:4]: + for par in pars[1:4]: self.widgets[par].SetValue(str(self.robo.get_val(index, par))) - self.update_params(index, self.robo.get_geom_head()[4:]) + self.update_params(index, pars[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] + pars = self._extract_param_names(ui_labels.DYN_PARAMS) index = int(self.widgets['link'].Value) self.update_params(index, pars) - def update_vel_params(self): - pars = self.robo.get_ext_dynam_head()[-3:] + def update_joint_params(self): + pars = self._extract_param_names(ui_labels.JOINT_PARAMS) index = int(self.widgets['joint'].Value) - self.update_params(index, pars) + self.widgets[pars[-1]].SetValue( + str(self.robo.get_val(index, pars[-1])) + ) + self.update_params(index, pars[:-1]) 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))) + 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 @@ -372,6 +380,13 @@ def update_z_params(self): 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), @@ -398,7 +413,7 @@ def feed_data(self): cmb.SetSelection(0) self.update_geo_params() self.update_dyn_params() - self.update_vel_params() + self.update_joint_params() self.update_base_twist_params() self.update_z_params() self.changed = False From 5489bcf80a34211501eadf4a4b05cbb7f5f1bc8d Mon Sep 17 00:00:00 2001 From: aravind Date: Sun, 11 May 2014 06:45:03 +0200 Subject: [PATCH 101/273] Add temp support for flexible joints in `Robot` class Add temporary support for flexible joint parameters in `Robot` class. This is mainly not to break the communication with the UI. --- pysymoro/floatr.py | 2 +- pysymoro/robot.py | 14 +++++++++++++- symoroui/labels.py | 2 +- 3 files changed, 15 insertions(+), 3 deletions(-) diff --git a/pysymoro/floatr.py b/pysymoro/floatr.py index 02b4a70..664cd24 100644 --- a/pysymoro/floatr.py +++ b/pysymoro/floatr.py @@ -412,7 +412,7 @@ def _init_maps(self): ]) self._misc_params_map = dict([ ('QP', 'qdots'), ('QDP', 'qddots'), ('GAM', 'torques'), - ('eta', 'etas'), ('K', 'stiffness'), ('G', 'gravity'), + ('eta', 'etas'), ('k', 'stiffness'), ('G', 'gravity'), ]) diff --git a/pysymoro/robot.py b/pysymoro/robot.py index 351ff28..3310eb5 100644 --- a/pysymoro/robot.py +++ b/pysymoro/robot.py @@ -102,11 +102,15 @@ def __init__(self, name, NL=0, NJ=0, NF=0, is_mobile=False, 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] - J_str = 'XX{0},XY{0},XZ{0},XY{0},YY{0},YZ{0},XZ{0},YZ{0},ZZ{0}' """ 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('G3')]) + """ eta - rigid or flexible""" + self.eta = [0 for j in numj] + """ k - joint stiffness""" + self.k = [0 for j in numj] # member methods: def put_val(self, j, name, val): @@ -139,6 +143,10 @@ def put_val(self, j, name, val): 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 @@ -162,6 +170,10 @@ def get_val(self, j, name): 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] diff --git a/symoroui/labels.py b/symoroui/labels.py index 2769433..0e57be7 100644 --- a/symoroui/labels.py +++ b/symoroui/labels.py @@ -43,7 +43,7 @@ ('qp', FieldEntry('QP', 'QP', 'txt', (2, 0), 'OnSpeedChanged', -1)), ('qdp', FieldEntry('QDP', 'QDP', 'txt', (2, 1), 'OnSpeedChanged', -1)), ('gam', FieldEntry('GAM', 'GAM', 'txt', (1, 1), 'OnSpeedChanged', -1)), - ('stiff', FieldEntry('k', 'K', 'txt', (1, 0), 'OnSpeedChanged', -1)), + ('stiff', FieldEntry('k', 'k', 'txt', (1, 0), 'OnSpeedChanged', -1)), ('eta', FieldEntry('eta', 'eta', 'cmb', (0, 1), 'OnSpeedChanged', -1)) ]) # base velocity and acceleration params From 19cda823b7096e181ea763ab890895539d90ab21 Mon Sep 17 00:00:00 2001 From: aravind Date: Mon, 12 May 2014 01:14:17 +0200 Subject: [PATCH 102/273] Add `get_val()` method to `FloatingRobot` class --- pysymoro/floatr.py | 37 ++++++++++++++++++++++++++++++++----- 1 file changed, 32 insertions(+), 5 deletions(-) diff --git a/pysymoro/floatr.py b/pysymoro/floatr.py index 664cd24..548a459 100644 --- a/pysymoro/floatr.py +++ b/pysymoro/floatr.py @@ -64,7 +64,7 @@ def __init__( """Joint stiffness usually indicated by k.""" self.stiffness = [0 for j in self.joint_nums] """Joint velocities.""" - self.qdots = [var('QD{0}'.format(j)) for j in self.joint_nums] + 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.""" @@ -138,6 +138,35 @@ def __repr__(self): ) 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 @@ -152,10 +181,8 @@ def put_val(self, idx, name, value): Returns: A `OK` if successful and `FAIL` otherwise. """ - # update Z matrix if name is 'Z': return tools.OK - #return self.base_tmat[idx] = val elif name in self._dyn_params_map: param = {int(idx): {self._dyn_params_map[name]: value}} return self.update_params('dyns', param) @@ -164,12 +191,12 @@ def put_val(self, idx, name, value): value = int(value) param = {int(idx): {self._geo_params_map[name]: value}} return self.update_params('geos', param) - elif name in self._base_params_map: - return self.update_params('base', 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): From 92f5d1f5ba552e373a7d3f76fd1acd908315b9c3 Mon Sep 17 00:00:00 2001 From: aravind Date: Tue, 13 May 2014 15:36:07 +0200 Subject: [PATCH 103/273] Add floating base and WMR fields to UI --- symoroui/definition.py | 12 +++++++++--- symoroui/labels.py | 5 +++-- symoroui/layout.py | 15 ++++++++------- symoroviz/graphics.py | 1 + 4 files changed, 21 insertions(+), 12 deletions(-) diff --git a/symoroui/definition.py b/symoroui/definition.py index ac6817f..2514612 100644 --- a/symoroui/definition.py +++ b/symoroui/definition.py @@ -59,15 +59,21 @@ def init_ui(self, name, nl, nj, is_mobile, structure): 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_is_floating = wx.CheckBox(self, label=' Is Floating Base') + self.ch_is_floating.Value = is_mobile + self.ch_is_wmr = wx.CheckBox( + self, label=' Is Wheeled Mobile Robot' + ) + self.ch_is_wmr.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, + main_sizer.Add(self.ch_is_floating, 0, + wx.LEFT | wx.RIGHT | wx.ALIGN_LEFT, 15) + main_sizer.Add(self.ch_is_wmr, 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) diff --git a/symoroui/labels.py b/symoroui/labels.py index 0e57be7..a55fe5e 100644 --- a/symoroui/labels.py +++ b/symoroui/labels.py @@ -126,8 +126,9 @@ ('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_mobile', FieldEntry('Is Mobile:', 'mobile', 'lbl', (5, 0), None, -1)), - ('num_loops', FieldEntry('Number of closed loops:', 'loops', 'lbl', (6, 0), None, -1)) + ('is_floating', FieldEntry('Is Floating Base:', 'floating', 'lbl', (5, 0), None, -1)), + ('is_wmr', FieldEntry('Is Wheeled Mobile Robot:', 'wmr', 'lbl', (6, 0), None, -1)), + ('num_loops', FieldEntry('Number of closed loops:', 'loops', 'lbl', (7, 0), None, -1)) ]) # menu bar diff --git a/symoroui/layout.py b/symoroui/layout.py index 1cf682c..ee899e9 100644 --- a/symoroui/layout.py +++ b/symoroui/layout.py @@ -130,8 +130,7 @@ def create_ui(self): label = ui_labels.ROBOT_TYPE[key].label name = ui_labels.ROBOT_TYPE[key].name ctrl = wx.StaticText( - self.panel, size=(150, -1), - name=ui_labels.ROBOT_TYPE[key].name + self.panel, size=(150, -1), name=name ) self.widgets[name] = ctrl self.widget_keys[key] = ctrl @@ -389,11 +388,13 @@ def _extract_param_names(self, params): 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)] + 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_mobile), ('wmr', False), + ('loops', self.robo.nj-self.robo.nl) + ] for name, info in names: label = self.widgets[name] label.SetLabel(str(info)) diff --git a/symoroviz/graphics.py b/symoroviz/graphics.py index 2a07843..4976ac8 100644 --- a/symoroviz/graphics.py +++ b/symoroviz/graphics.py @@ -453,6 +453,7 @@ def init_ui(self): q_box = wx.StaticBoxSizer(wx.StaticBox(self.p, label='Joint variables')) q_box.Add(gridJnts, 0, wx.ALL, 10) + q_box.FitInside(self.p) ver_sizer = wx.BoxSizer(wx.VERTICAL) ver_sizer.Add(q_box) From c3e4d05210163306feec4faa41d46459df32d125 Mon Sep 17 00:00:00 2001 From: aravind Date: Wed, 14 May 2014 14:32:49 +0200 Subject: [PATCH 104/273] Add printing of joint params in table format Add printing of joint params in table format in __str__() of `FloatingRobot` class. --- pysymoro/floatr.py | 21 +++++++++++++++++---- 1 file changed, 17 insertions(+), 4 deletions(-) diff --git a/pysymoro/floatr.py b/pysymoro/floatr.py index 548a459..20dc72d 100644 --- a/pysymoro/floatr.py +++ b/pysymoro/floatr.py @@ -103,9 +103,6 @@ def __str__(self): 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 + ("\tStructure: %s\n" % str(self.structure)) - # add joint type - rigid or flexible - str_format = str_format + "\tJoint Type: %s\n" % str(self.etas) - str_format = str_format + "\tStiffness: %s\n" % str(self.stiffness) str_format = str_format + '\n' # add geometric params str_format = str_format + "Geometric Parameters:\n" @@ -126,7 +123,23 @@ def __str__(self): )) for dyn in self.dyns: str_format = str_format + str(dyn) + '\n' - str_format = str_format + '\n' + ('=*' * 60) + '=' + 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): From 8bd8b40bb748b28d9ca9ad27ce6146330bde3483 Mon Sep 17 00:00:00 2001 From: aravind Date: Wed, 14 May 2014 15:47:54 +0200 Subject: [PATCH 105/273] Fix scrollable area in Visualisation Fix scrollable area for joint variables display in Visualisation window. --- symoroviz/graphics.py | 7 ++++--- 1 file changed, 4 insertions(+), 3 deletions(-) diff --git a/symoroviz/graphics.py b/symoroviz/graphics.py index 4976ac8..6ed87a3 100644 --- a/symoroviz/graphics.py +++ b/symoroviz/graphics.py @@ -378,8 +378,10 @@ def __init__(self, prefix, robo, params=None, parent=None, identifier=-1): self.solve_loops = False self.canvas = myGLCanvas(self, robo, self.params_dict, size=(600, 600)) - self.p = wx.Panel(self) + self.p = wx.lib.scrolledpanel.ScrolledPanel(self, -1) + self.p.SetMinSize((350,600)) self.init_ui() + self.p.SetupScrolling() self.update_spin_controls() self.sizer = wx.BoxSizer(wx.HORIZONTAL) @@ -453,7 +455,6 @@ def init_ui(self): q_box = wx.StaticBoxSizer(wx.StaticBox(self.p, label='Joint variables')) q_box.Add(gridJnts, 0, wx.ALL, 10) - q_box.FitInside(self.p) ver_sizer = wx.BoxSizer(wx.VERTICAL) ver_sizer.Add(q_box) @@ -481,7 +482,7 @@ def init_ui(self): # border = wx.BoxSizer() # border.Add(sizer, flag=wx.ALL | wx.EXPAND, border=5) - self.p.SetSizerAndFit(top_sizer) + self.p.SetSizer(top_sizer) def OnChangeRepresentation(self, evt): self.canvas.representation(evt.EventObject.GetValue()) From 0587eada5c94dc73c608e90db77b4f353ecfe48a Mon Sep 17 00:00:00 2001 From: aravind Date: Wed, 14 May 2014 16:13:57 +0200 Subject: [PATCH 106/273] Rename `pysymoro/floatr.py` to `pysymoro/robotf.py` --- pysymoro/{floatr.py => robotf.py} | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) rename pysymoro/{floatr.py => robotf.py} (99%) diff --git a/pysymoro/floatr.py b/pysymoro/robotf.py similarity index 99% rename from pysymoro/floatr.py rename to pysymoro/robotf.py index 20dc72d..50c47e1 100644 --- a/pysymoro/floatr.py +++ b/pysymoro/robotf.py @@ -25,7 +25,7 @@ class FloatingRobot(object): """ def __init__( self, name, links=0, joints=0, frames=0, - is_floating=True, structure=tools.TREE + is_floating=True, structure=tools.TREE, is_wmr=False ): """ Constructor period. @@ -46,6 +46,8 @@ def __init__( 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_wmr = is_wmr # properties dependent on number of links """ List to hold the dynamic parameters. The indices of the list From e41458f3ca2d727ae36d5479d453e7df51660fdf Mon Sep 17 00:00:00 2001 From: aravind Date: Thu, 15 May 2014 10:07:49 +0200 Subject: [PATCH 107/273] Add methods to return axis values Add method to return axis values in `GeoParams` class. The `zunit` method returns the unit vector along z-axis and the `axisa` method returns the joint axis in screw form. --- pysymoro/geoparams.py | 25 +++++++++++++++++++++++++ 1 file changed, 25 insertions(+) diff --git a/pysymoro/geoparams.py b/pysymoro/geoparams.py index bbcca8b..a8c810e 100644 --- a/pysymoro/geoparams.py +++ b/pysymoro/geoparams.py @@ -8,6 +8,8 @@ import re +from sympy import Matrix + from pysymoro import transform @@ -83,6 +85,29 @@ def update_params(self, params): ) 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. + """ + return Matrix([0, 0, 1]) + + @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): """ From 4db563d75c47da25173f8c3d3cf02e8f3106e938 Mon Sep 17 00:00:00 2001 From: aravind Date: Thu, 15 May 2014 10:09:40 +0200 Subject: [PATCH 108/273] Add `pysymoro/dynmodel.py` --- pysymoro/dynmodel.py | 55 ++++++++++++++++++++++++++++++++++++++++++++ 1 file changed, 55 insertions(+) create mode 100644 pysymoro/dynmodel.py diff --git a/pysymoro/dynmodel.py b/pysymoro/dynmodel.py new file mode 100644 index 0000000..3d1fcb9 --- /dev/null +++ b/pysymoro/dynmodel.py @@ -0,0 +1,55 @@ +# -*- coding: utf-8 -*- + + +""" +This module contains the functions and classes for the computation of +dynamic models (inverse and direct). +""" + + +def _compute_link_velocity(robo, j, i): + if i == 0: robo.vels.append(robo.base_vel.val) + j_s_i = robo.geos[j].tmat.s_i_wrt_j + i_v_i = robo.vels[i] + qdot = robo.qdots[j] + return (j_s_i * i_v_i) + (qdot * robo.geos[j].axisa) + pass + + +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 new attributes for the robot + robo.vels = list() + # some book keeping variables + nums = robo.joint_nums + # first forward recursion + for j in nums: + if j == 0: continue + # antecedent index + i = robo.geos[j].ant + # compute j^S_i : screw transformation matrix + j_s_i = robo.geos[j].tmat.s_i_wrt_j + # compute j^V_j : link velocity (6x1) + # compute j^gamma_j : gyroscopic acceleration (6x1) + # compute j^beta_j : external+coriolis+centrifugal wrench (6x1) + # compute j^zeta_j : relative acceleration (6x1) + pass + # backward recursion + for j in reversed(nums): + if j == 0: continue + pass + # second forward recursion + for j in robo.nums: + if j == 0: continue + pass + pass + From 252c5bdd96fd4d3c41ee8e89efcfd7564f438c0c Mon Sep 17 00:00:00 2001 From: aravind Date: Thu, 15 May 2014 11:29:56 +0200 Subject: [PATCH 109/273] Add __str__() and __repr__() for `Screw` class --- pysymoro/screw.py | 12 ++++++++++++ 1 file changed, 12 insertions(+) diff --git a/pysymoro/screw.py b/pysymoro/screw.py index 6f27c59..aff5b29 100644 --- a/pysymoro/screw.py +++ b/pysymoro/screw.py @@ -35,6 +35,18 @@ def __init__(self, lin=zeros(3, 1), ang=zeros(3, 1)): 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 + # def __init__(self, value=zeros(6, 1)): # """ # Another Constructor. From 0ff49ca4e0dcc293ec168c3dca5f4d65b47708ec Mon Sep 17 00:00:00 2001 From: Bogdan Date: Thu, 15 May 2014 12:55:28 +0200 Subject: [PATCH 110/273] Fix for Inertia matrix, matlab functions Now Inertia matrix is generated for more than 24 links, Element-wise initialization of result in matlab generated function --- pysymoro/dynamics.py | 21 ++++++++++++++++----- pysymoro/geometry.py | 4 ++-- symoroutils/genfunc.py | 32 ++++++++++++++++++++++++-------- symoroutils/symbolmgr.py | 25 ++++++++++++++++++------- 4 files changed, 60 insertions(+), 22 deletions(-) diff --git a/pysymoro/dynamics.py b/pysymoro/dynamics.py index 5994b5c..ffcb18c 100644 --- a/pysymoro/dynamics.py +++ b/pysymoro/dynamics.py @@ -20,7 +20,15 @@ from symoroutils.paramsinit import ParamsInit -chars = 'ABCDEFGHJKLMNPQRSTUVWXYZ' +chars = ('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') + inert_names = ('XXR', 'XYR', 'XZR', 'YYR', 'YZR', 'ZZR', 'MXR', 'MYR', 'MZR', 'MR') @@ -325,16 +333,19 @@ def compute_torque(robo, symo, j, Fjnt, Njnt, name=None): """Internal function. Computes actuation torques - projection of joint wrench on the joint axis """ - if name is None: - name = str(robo.GAM[j]) + '%s' 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) + if name is None: + name = str(robo.GAM[j]) + else: + name = name % j + symo.replace(tau_total, name, forced=True) def inertia_spatial(J, MS, M): - return Matrix([(M*sympy.eye(3)).row_join(tools.skew(MS).T), tools.skew(MS).row_join(J)]) + return Matrix([(M*sympy.eye(3)).row_join(tools.skew(MS).T), + tools.skew(MS).row_join(J)]) def compute_joint_torque_deriv(symo, param, arg, index): diff --git a/pysymoro/geometry.py b/pysymoro/geometry.py index b654dc0..78598d7 100644 --- a/pysymoro/geometry.py +++ b/pysymoro/geometry.py @@ -514,8 +514,8 @@ def direct_geometric(robo, frames, trig_subs): 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) + 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/symoroutils/genfunc.py b/symoroutils/genfunc.py index 67cf713..13ce1d5 100644 --- a/symoroutils/genfunc.py +++ b/symoroutils/genfunc.py @@ -9,7 +9,7 @@ def gen_fheader_matlab(symo, name, args, multival=False): func_head = [] - func_head.append('function RESULT=%s_func (' % name) + func_head.append('function RESULT=%s (' % name) func_head.append(convert_syms_matlab(args)) func_head.append(')\n') return func_head @@ -71,13 +71,18 @@ def gen_fbody_matlab(symo, name, to_return, args, ret_name=''): # set of defined symbols arg_syms = symo.extract_syms(args) # final symbols to be compute - res_syms = symo.extract_syms(to_return) - if isinstance(to_return, Matrix): - to_ret_str = convert_mat_matlab(to_return) + 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: - to_ret_str = convert_syms_matlab(to_return) + 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 - print arg_syms, res_syms order_list = symo.sift_syms(res_syms, arg_syms) # list of instructions in final function func_body = [] @@ -110,9 +115,20 @@ def gen_fbody_matlab(symo, name, to_return, args, ret_name=''): folded += 1 else: item = '%s%s=%s;\n' % (space * folded, s, symo.sydi[s]) - item.replace('**', '^') + item = item.replace('**', '^') func_body.append(item) - if multival: + 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) diff --git a/symoroutils/symbolmgr.py b/symoroutils/symbolmgr.py index 3c3ed79..ad85574 100644 --- a/symoroutils/symbolmgr.py +++ b/symoroutils/symbolmgr.py @@ -264,13 +264,17 @@ def mat_replace(self, M, name, index='', 2) >>> A = symo.mat_replace(B+C+..., 'A') # for the case when B+C+... is small enough """ - for i1 in xrange(M.shape[0] - skip): - for i2 in xrange(M.shape[1]): - if symmet and i1 > i2: + 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 + str(i1 + 1) + str(i2 + 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) @@ -289,10 +293,17 @@ def unfold(self, expr): expr: symbolic expression Unfolded expression """ - while self.sydi.keys() & expr.atoms(): + 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 @@ -430,7 +441,7 @@ def file_close(self): def gen_fheader(self, name, *args): fun_head = [] - fun_head.append('def %s_func(*args):\n' % name) + 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) @@ -594,4 +605,4 @@ def gen_func(self, name, to_return, args): computes symbols in to_return have been generated. """ exec self.gen_func_string(name, to_return, args) - return eval('%s_func' % name) + return eval('%s' % name) From f6dde5c6e587190303bc96e5572a5b5738efdc6d Mon Sep 17 00:00:00 2001 From: aravind Date: Thu, 15 May 2014 16:54:06 +0200 Subject: [PATCH 111/273] Add methods in `FloatingRobot` class Add methods in `FloatingRobot` class for the purpose dynamic model computation. --- pysymoro/geoparams.py | 5 ++++- pysymoro/robotf.py | 17 ++++++++++++++++- pysymoro/screw.py | 2 +- 3 files changed, 21 insertions(+), 3 deletions(-) diff --git a/pysymoro/geoparams.py b/pysymoro/geoparams.py index a8c810e..caca755 100644 --- a/pysymoro/geoparams.py +++ b/pysymoro/geoparams.py @@ -93,7 +93,10 @@ def zunit(self): Returns: A (3x1) Matrix. """ - return Matrix([0, 0, 1]) + if self.sigma != 2: + return Matrix([0, 0, 1]) + else: + return Matrix([0, 0, 0]) @property def axisa(self): diff --git a/pysymoro/robotf.py b/pysymoro/robotf.py index 50c47e1..de879db 100644 --- a/pysymoro/robotf.py +++ b/pysymoro/robotf.py @@ -11,9 +11,10 @@ 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.screw import Screw +from pysymoro import dynmodel from symoroutils import filemgr from symoroutils import tools @@ -267,6 +268,20 @@ def update_params(self, kind, params): raise ValueError(errmsg) return tools.OK + 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): """ diff --git a/pysymoro/screw.py b/pysymoro/screw.py index aff5b29..8779062 100644 --- a/pysymoro/screw.py +++ b/pysymoro/screw.py @@ -36,7 +36,7 @@ def __init__(self, lin=zeros(3, 1), ang=zeros(3, 1)): self._val[3:6, 0] = ang def __str__(self): - row_format = '[' + ('{:}; ' * 5) + ('{:}') + ']' + 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]) From 97f1933548174ae79fbb713786e6534e6b1fcb36 Mon Sep 17 00:00:00 2001 From: aravind Date: Thu, 15 May 2014 17:10:11 +0200 Subject: [PATCH 112/273] Add first forward recursion for inverse dynmodel Add first forward recursion for inverse dynamic model computation. Add helper functions to perform the various steps of inverse dynamic model computation. Note that these helper functions maybe used by the direct dynamic model as well. Add `DynModel` data structure class to hold the various components of dynamic model computation. This class is to be further extended. --- pysymoro/dynmodel.py | 219 ++++++++++++++++++++++++++++++++++++++++--- 1 file changed, 206 insertions(+), 13 deletions(-) diff --git a/pysymoro/dynmodel.py b/pysymoro/dynmodel.py index 3d1fcb9..0fcb6e9 100644 --- a/pysymoro/dynmodel.py +++ b/pysymoro/dynmodel.py @@ -7,13 +7,186 @@ """ -def _compute_link_velocity(robo, j, i): - if i == 0: robo.vels.append(robo.base_vel.val) +from pysymoro.screw import Screw +from symoroutil.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): + """ + Constructor period. + + Args: + joints: An iterative object containing all the joint + numbers. This is usually the `joint_nums` attribute in + `Robot` class. + """ + self.vels = list(None for j in joints) + self.gammas = list(None for j in joints) + self.betas = list(None for j in joints) + self.zetas = list(None for j in joints) + + def __str__(self): + str_format = "" + # add header + str_format = str_format + "DynModel:\n" + str_format = str_format + "---------\n" + # add link velocity + str_format = str_format + "vels: \n" + str_format = str_format + self._str_items(self.vels) + # add gyroscopic acceleration + str_format = str_format + "gammas: \n" + str_format = str_format + self._str_items(self.gammas) + # add wrench - external + coriolis + centrifugal + str_format = str_format + "betas: \n" + str_format = str_format + self._str_items(self.betas) + # add relative acceleration + str_format = str_format + "zetas: \n" + str_format = str_format + self._str_items(self.zetas) + 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 _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 - i_v_i = robo.vels[i] - qdot = robo.qdots[j] - return (j_s_i * i_v_i) + (qdot * robo.geos[j].axisa) - pass + i_v_i = models.vels[i].val + qdot_j = robo.qdots[j] + j_a_j = robo.geos[j].axisa + # 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_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.rot_inv + 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 + qddot_j = robo.qddots[j] + j_gamma_j = model.gammas[j].val + # 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 inverse_dynamic_model(robo): @@ -27,29 +200,49 @@ def inverse_dynamic_model(robo): Returns: The inverse dynamic model of the robot. """ - # some new attributes for the robot - robo.vels = list() # some book keeping variables nums = robo.joint_nums + model = DynModel(nums) # first forward recursion for j in nums: if j == 0: continue # antecedent index i = robo.geos[j].ant - # compute j^S_i : screw transformation matrix - j_s_i = robo.geos[j].tmat.s_i_wrt_j # 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) - pass + # TODO: check joint flexibility + model = _compute_relative_acceleration(model, robo, j) # backward recursion for j in reversed(nums): - if j == 0: continue - pass + # antecedent index + i = robo.geos[j].ant + if j != 0: + pass + else: + pass # second forward recursion for j in robo.nums: if j == 0: continue pass + 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. + """ pass + From d74cb39b5a6b397040a21b14d652420946b42c6e Mon Sep 17 00:00:00 2001 From: aravind Date: Fri, 16 May 2014 17:50:51 +0200 Subject: [PATCH 113/273] Add backward and 2nd forward recursion for IDyM Add backward recursion for inverse dynamic model computation. Add second forward recursion for inverse dynamic model computation. Add more helper functions to compute the inverse dynamic model. Expand `DynModel` to include more attributes. --- pysymoro/dynmodel.py | 254 +++++++++++++++++++++++++++++++++++++++++-- pysymoro/robotf.py | 6 +- 2 files changed, 246 insertions(+), 14 deletions(-) diff --git a/pysymoro/dynmodel.py b/pysymoro/dynmodel.py index 0fcb6e9..3060758 100644 --- a/pysymoro/dynmodel.py +++ b/pysymoro/dynmodel.py @@ -7,7 +7,10 @@ """ +from sympy import sign + from pysymoro.screw import Screw +from pysymoro.screw6 import Screw6 from symoroutil.tools import skew @@ -26,10 +29,24 @@ def __init__(self, joints): numbers. This is usually the `joint_nums` attribute in `Robot` class. """ + # link velocity self.vels = list(None for j in joints) + # gyroscopic acceleration self.gammas = list(None for j in joints) + # wrench - external+coriolis+centrifugal self.betas = list(None for j in joints) + # relative acceleration self.zetas = list(None for j in joints) + # composite spatial inertia matrix + self.composite_inertias = list(None for j in joints) + # composite wrench + self.composite_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) + # joint torque + self.torques = list(None for j in joints) def __str__(self): str_format = "" @@ -72,6 +89,38 @@ def _str_items(self, items): 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 _compute_link_velocity(model, robo, j, i): """ Compute the velocity of link j whose antecedent is i. @@ -99,6 +148,33 @@ def _compute_link_velocity(model, robo, j, i): 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() + if i == 0 and not robo.is_floating: + model.accels[i] = robo.base_accel + # local variables + j_s_i = robo.geos[j].tmat.s_j_wrt_i + 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. @@ -189,6 +265,145 @@ def _compute_beta_wrench(model, robo, 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 = ia_j * qddot_j + coriolis_friction_term = f_cj * sign(qdot_j) + viscous_friction_term = 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 + 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. + + 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 + o_inertia_o_c = model.composite_inertias[0].val + o_beta_o_c = model.composite_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_no_gravity = o_vdot_o + # compute base acceleration taking gravity into account + 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 @@ -201,10 +416,9 @@ def inverse_dynamic_model(robo): The inverse dynamic model of the robot. """ # some book keeping variables - nums = robo.joint_nums - model = DynModel(nums) + model = DynModel(robo.joint_nums) # first forward recursion - for j in nums: + for j in robo.joint_nums: if j == 0: continue # antecedent index i = robo.geos[j].ant @@ -217,18 +431,36 @@ def inverse_dynamic_model(robo): # compute j^zeta_j : relative acceleration (6x1) # TODO: check joint flexibility model = _compute_relative_acceleration(model, robo, j) - # backward recursion - for j in reversed(nums): + # first backward recursion - initialisation step + for j in reversed(robo.joint_nums): + # 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: + if not robo.is_floating: continue + else: + # compute 0^\dot{V}_0 : base acceleration + model = _compute_base_acceleration(model, robo) # antecedent index i = robo.geos[j].ant - if j != 0: - pass - else: - pass + # 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.nums: + for j in robo.joint_nums: if j == 0: continue - pass + # 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 diff --git a/pysymoro/robotf.py b/pysymoro/robotf.py index de879db..cc52df6 100644 --- a/pysymoro/robotf.py +++ b/pysymoro/robotf.py @@ -89,7 +89,7 @@ def __init__( """Base velocity 6x1 column vector - a Screw.""" self.base_vel = Screw() """Base acceleration 6x1 column vector - a Screw.""" - self.base_acc = Screw() + self.base_accel = Screw() """Transformation matrix of base wrt a reference frame at time 0.""" self.base_tmat = eye(4) # call init methods @@ -464,8 +464,8 @@ def _init_maps(self): ('alpha', 'alpha'), ('d', 'd'), ('theta', 'theta'), ('r', 'r') ]) self._base_params_map = dict([ - ('V0', 'base_vel'), ('VP0', 'base_acc'), - ('W0', 'base_vel'), ('WP0', 'base_acc') + ('V0', 'base_vel'), ('VP0', 'base_accel'), + ('W0', 'base_vel'), ('WP0', 'base_accel') ]) self._misc_params_map = dict([ ('QP', 'qdots'), ('QDP', 'qddots'), ('GAM', 'torques'), From 14d16ef11ed2c5dc22240ff7fb60405014356d86 Mon Sep 17 00:00:00 2001 From: aravind Date: Fri, 16 May 2014 18:25:48 +0200 Subject: [PATCH 114/273] Refactor `__str__()` in `DynModel` class Refacto `__str__()` in `DynModel` to return all the attributes of the class. Since attributes can be dynamically added, the current version uses `dir()` to return all the attributes and then convert them to string. --- pysymoro/dynmodel.py | 25 ++++++++++++------------- 1 file changed, 12 insertions(+), 13 deletions(-) diff --git a/pysymoro/dynmodel.py b/pysymoro/dynmodel.py index 3060758..a3839e6 100644 --- a/pysymoro/dynmodel.py +++ b/pysymoro/dynmodel.py @@ -11,7 +11,7 @@ from pysymoro.screw import Screw from pysymoro.screw6 import Screw6 -from symoroutil.tools import skew +from symoroutils.tools import skew class DynModel(object): @@ -53,18 +53,17 @@ def __str__(self): # add header str_format = str_format + "DynModel:\n" str_format = str_format + "---------\n" - # add link velocity - str_format = str_format + "vels: \n" - str_format = str_format + self._str_items(self.vels) - # add gyroscopic acceleration - str_format = str_format + "gammas: \n" - str_format = str_format + self._str_items(self.gammas) - # add wrench - external + coriolis + centrifugal - str_format = str_format + "betas: \n" - str_format = str_format + self._str_items(self.betas) - # add relative acceleration - str_format = str_format + "zetas: \n" - str_format = str_format + self._str_items(self.zetas) + # 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 = str(items) + '\n' + str_format = str_format + str(attr) + ": \n" + str_format = str_format + attr_str return str_format def __repr__(self): From 5263ef9c3967f1dce7d796f16029a9cfcd7f3c58 Mon Sep 17 00:00:00 2001 From: aravind Date: Sat, 17 May 2014 10:24:26 +0200 Subject: [PATCH 115/273] Update UI for Visualisation window --- symoroviz/graphics.py | 28 ++++++++++++++-------------- 1 file changed, 14 insertions(+), 14 deletions(-) diff --git a/symoroviz/graphics.py b/symoroviz/graphics.py index 6ed87a3..84e1a2d 100644 --- a/symoroviz/graphics.py +++ b/symoroviz/graphics.py @@ -402,15 +402,15 @@ def init_ui(self): 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) + gridControl.Add(self.tButton, pos=(3, 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) + gridControl.Add(btnReset, pos=(5, 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) + gridControl.Add(btnRandom, pos=(6, 0), flag=wx.ALIGN_CENTER) self.spin_ctrls = {} gridJnts = wx.GridBagSizer(hgap=10, vgap=10) @@ -440,7 +440,7 @@ def init_ui(self): 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) + gridControl.Add(self.radioBox, pos=(7, 0), flag=wx.ALIGN_CENTER) choices = [] for jnt in self.canvas.jnt_objs: @@ -451,23 +451,23 @@ def init_ui(self): 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) + gridControl.Add(self.check_list, pos=(4, 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) + gridControl.Add(lbl_length, pos=(1, 0), flag=wx.ALIGN_CENTER) + gridControl.Add(self.jnt_slider, 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) - top_sizer.Add(ver_sizer, 0, wx.ALL, 10) - top_sizer.AddSpacer(10) top_sizer.Add(gridControl, 0, wx.ALL, 10) + top_sizer.AddSpacer(10) + top_sizer.Add(ver_sizer, 0, wx.ALL, 10) # button1 = wx.Button(self.panel, label="TEXT 1") # button2 = wx.Button(self.panel, label="TEXT 2") From 6b4772a6f9081316055ac1b846ea501075565b39 Mon Sep 17 00:00:00 2001 From: Bogdan Date: Wed, 21 May 2014 09:47:53 +0200 Subject: [PATCH 116/273] Fix loop solving issue Small fix in symbol manager --- symoroutils/symbolmgr.py | 14 ++++++++------ 1 file changed, 8 insertions(+), 6 deletions(-) diff --git a/symoroutils/symbolmgr.py b/symoroutils/symbolmgr.py index ad85574..e51fe75 100644 --- a/symoroutils/symbolmgr.py +++ b/symoroutils/symbolmgr.py @@ -84,7 +84,8 @@ def CS12_simp(self, sym, silent=False): return sym names, short_form = tools.trignometric_info(sym) names = list(names) - names.sort() + if short_form: + names.sort() sym2 = sym for n1, n2 in itertools.combinations(names, 2): if short_form: @@ -211,11 +212,12 @@ def replace(self, old_sym, name, index='', forced=False): 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: + 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] From 8d17d0e8108283e29118cb7fb430a9572429df0d Mon Sep 17 00:00:00 2001 From: Bogdan Date: Thu, 15 May 2014 12:55:28 +0200 Subject: [PATCH 117/273] Fix for Inertia matrix, matlab functions Now Inertia matrix is generated for more than 24 links, Element-wise initialization of result in matlab generated function --- pysymoro/dynamics.py | 21 ++++++++++++++++----- pysymoro/geometry.py | 4 ++-- symoroutils/genfunc.py | 32 ++++++++++++++++++++++++-------- symoroutils/symbolmgr.py | 25 ++++++++++++++++++------- 4 files changed, 60 insertions(+), 22 deletions(-) diff --git a/pysymoro/dynamics.py b/pysymoro/dynamics.py index 5994b5c..ffcb18c 100644 --- a/pysymoro/dynamics.py +++ b/pysymoro/dynamics.py @@ -20,7 +20,15 @@ from symoroutils.paramsinit import ParamsInit -chars = 'ABCDEFGHJKLMNPQRSTUVWXYZ' +chars = ('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') + inert_names = ('XXR', 'XYR', 'XZR', 'YYR', 'YZR', 'ZZR', 'MXR', 'MYR', 'MZR', 'MR') @@ -325,16 +333,19 @@ def compute_torque(robo, symo, j, Fjnt, Njnt, name=None): """Internal function. Computes actuation torques - projection of joint wrench on the joint axis """ - if name is None: - name = str(robo.GAM[j]) + '%s' 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) + if name is None: + name = str(robo.GAM[j]) + else: + name = name % j + symo.replace(tau_total, name, forced=True) def inertia_spatial(J, MS, M): - return Matrix([(M*sympy.eye(3)).row_join(tools.skew(MS).T), tools.skew(MS).row_join(J)]) + return Matrix([(M*sympy.eye(3)).row_join(tools.skew(MS).T), + tools.skew(MS).row_join(J)]) def compute_joint_torque_deriv(symo, param, arg, index): diff --git a/pysymoro/geometry.py b/pysymoro/geometry.py index b654dc0..78598d7 100644 --- a/pysymoro/geometry.py +++ b/pysymoro/geometry.py @@ -514,8 +514,8 @@ def direct_geometric(robo, frames, trig_subs): 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) + 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/symoroutils/genfunc.py b/symoroutils/genfunc.py index 67cf713..13ce1d5 100644 --- a/symoroutils/genfunc.py +++ b/symoroutils/genfunc.py @@ -9,7 +9,7 @@ def gen_fheader_matlab(symo, name, args, multival=False): func_head = [] - func_head.append('function RESULT=%s_func (' % name) + func_head.append('function RESULT=%s (' % name) func_head.append(convert_syms_matlab(args)) func_head.append(')\n') return func_head @@ -71,13 +71,18 @@ def gen_fbody_matlab(symo, name, to_return, args, ret_name=''): # set of defined symbols arg_syms = symo.extract_syms(args) # final symbols to be compute - res_syms = symo.extract_syms(to_return) - if isinstance(to_return, Matrix): - to_ret_str = convert_mat_matlab(to_return) + 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: - to_ret_str = convert_syms_matlab(to_return) + 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 - print arg_syms, res_syms order_list = symo.sift_syms(res_syms, arg_syms) # list of instructions in final function func_body = [] @@ -110,9 +115,20 @@ def gen_fbody_matlab(symo, name, to_return, args, ret_name=''): folded += 1 else: item = '%s%s=%s;\n' % (space * folded, s, symo.sydi[s]) - item.replace('**', '^') + item = item.replace('**', '^') func_body.append(item) - if multival: + 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) diff --git a/symoroutils/symbolmgr.py b/symoroutils/symbolmgr.py index 3c3ed79..ad85574 100644 --- a/symoroutils/symbolmgr.py +++ b/symoroutils/symbolmgr.py @@ -264,13 +264,17 @@ def mat_replace(self, M, name, index='', 2) >>> A = symo.mat_replace(B+C+..., 'A') # for the case when B+C+... is small enough """ - for i1 in xrange(M.shape[0] - skip): - for i2 in xrange(M.shape[1]): - if symmet and i1 > i2: + 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 + str(i1 + 1) + str(i2 + 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) @@ -289,10 +293,17 @@ def unfold(self, expr): expr: symbolic expression Unfolded expression """ - while self.sydi.keys() & expr.atoms(): + 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 @@ -430,7 +441,7 @@ def file_close(self): def gen_fheader(self, name, *args): fun_head = [] - fun_head.append('def %s_func(*args):\n' % name) + 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) @@ -594,4 +605,4 @@ def gen_func(self, name, to_return, args): computes symbols in to_return have been generated. """ exec self.gen_func_string(name, to_return, args) - return eval('%s_func' % name) + return eval('%s' % name) From 2c55537d09642a3d6510faa2c29a70ca0165daba Mon Sep 17 00:00:00 2001 From: Bogdan Date: Wed, 21 May 2014 09:47:53 +0200 Subject: [PATCH 118/273] Fix loop solving issue Small fix in symbol manager --- symoroutils/symbolmgr.py | 14 ++++++++------ 1 file changed, 8 insertions(+), 6 deletions(-) diff --git a/symoroutils/symbolmgr.py b/symoroutils/symbolmgr.py index ad85574..e51fe75 100644 --- a/symoroutils/symbolmgr.py +++ b/symoroutils/symbolmgr.py @@ -84,7 +84,8 @@ def CS12_simp(self, sym, silent=False): return sym names, short_form = tools.trignometric_info(sym) names = list(names) - names.sort() + if short_form: + names.sort() sym2 = sym for n1, n2 in itertools.combinations(names, 2): if short_form: @@ -211,11 +212,12 @@ def replace(self, old_sym, name, index='', forced=False): 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: + 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] From a3fcd279b66e2ad275c5dbd993b0cb59c29a081a Mon Sep 17 00:00:00 2001 From: aravind Date: Wed, 21 May 2014 14:37:55 +0200 Subject: [PATCH 119/273] Fix errors found using pylint --- pysymoro/dynmodel.py | 8 ++++++-- pysymoro/dynparams.py | 4 ++-- pysymoro/geoparams.py | 4 ++-- pysymoro/screw.py | 11 ----------- 4 files changed, 10 insertions(+), 17 deletions(-) diff --git a/pysymoro/dynmodel.py b/pysymoro/dynmodel.py index a3839e6..fa86563 100644 --- a/pysymoro/dynmodel.py +++ b/pysymoro/dynmodel.py @@ -137,9 +137,9 @@ def _compute_link_velocity(model, robo, j, i): if i == 0: model.vels[i] = robo.base_vel # local variables j_s_i = robo.geos[j].tmat.s_i_wrt_j - i_v_i = models.vels[i].val 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 @@ -443,6 +443,7 @@ def inverse_dynamic_model(robo): else: # compute 0^\dot{V}_0 : base acceleration model = _compute_base_acceleration(model, robo) + continue # antecedent index i = robo.geos[j].ant # compute i^I_i^c : composite spatial inertia matrix @@ -474,6 +475,9 @@ def direct_dynamic_model(robo): Returns: The direct dynamic model of the robot. """ - pass + # some book keeping variables + model = DynModel(robo.joint_nums) + # TODO: complete direct dynamic model + return model diff --git a/pysymoro/dynparams.py b/pysymoro/dynparams.py index 1e927b4..66f2516 100644 --- a/pysymoro/dynparams.py +++ b/pysymoro/dynparams.py @@ -104,7 +104,7 @@ 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.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), @@ -115,7 +115,7 @@ def __str__(self): def __repr__(self): repr_format = str(self).lstrip().rstrip() - repr_format = re.sub('\s+', ', ', repr_format) + repr_format = re.sub(r'\s+', ', ', repr_format) repr_format = '(' + repr_format + ')' return repr_format diff --git a/pysymoro/geoparams.py b/pysymoro/geoparams.py index caca755..6ce5d0a 100644 --- a/pysymoro/geoparams.py +++ b/pysymoro/geoparams.py @@ -62,7 +62,7 @@ def __str__(self): def __repr__(self): repr_format = str(self).lstrip().rstrip() - repr_format = re.sub('\s+', ', ', repr_format) + repr_format = re.sub(r'\s+', ', ', repr_format) repr_format = '(' + repr_format + ')' return repr_format @@ -122,7 +122,7 @@ def q(self): returned. """ if self.sigma == 2: - return None + return 0 return ((1 - self.sigma) * self.theta) + (self.sigma * self.r) diff --git a/pysymoro/screw.py b/pysymoro/screw.py index 8779062..2466605 100644 --- a/pysymoro/screw.py +++ b/pysymoro/screw.py @@ -47,17 +47,6 @@ def __repr__(self): repr_format = 'Screw({0})'.format(str(self)) return repr_format -# def __init__(self, value=zeros(6, 1)): -# """ -# Another Constructor. -# -# Args: -# value: A 6x1 Matrix set to 0 by default. -# """ -# if value.rows != 6 or value.cols != 1: -# raise ShapeError("Matrix size has to be 6x1.") -# self._val = value - @property def val(self): """ From 25528e42b9bae8fc2ef25e1091ba98f4c3aba46f Mon Sep 17 00:00:00 2001 From: aravind Date: Wed, 21 May 2014 17:40:28 +0200 Subject: [PATCH 120/273] Fix bugs in IDyM Fix computation of beta_0 term bug. Fix `TypeError` bug when summing Matrix with string. --- pysymoro/dynmodel.py | 12 ++++++++---- 1 file changed, 8 insertions(+), 4 deletions(-) diff --git a/pysymoro/dynmodel.py b/pysymoro/dynmodel.py index fa86563..c2cb82a 100644 --- a/pysymoro/dynmodel.py +++ b/pysymoro/dynmodel.py @@ -7,6 +7,7 @@ """ +from sympy import Matrix from sympy import sign from pysymoro.screw import Screw @@ -189,7 +190,7 @@ def _compute_gyroscopic_acceleration(model, robo, j, i): """ j_gamma_j = Screw() # local variables - j_rot_i = robo.geos[j].tmat.rot_inv + 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 @@ -364,9 +365,9 @@ def _compute_joint_torque(model, robo, j): j_f_j = model.wrenchs[j].val # actual computation wrench_term = j_f_j.transpose() * j_a_j - actuator_inertia_term = ia_j * qddot_j - coriolis_friction_term = f_cj * sign(qdot_j) - viscous_friction_term = f_vj * qdot_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 @@ -432,6 +433,9 @@ def inverse_dynamic_model(robo): 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 From 3c69f949a1c8722e8c01c652fb78d87f9a458df6 Mon Sep 17 00:00:00 2001 From: Bogdan Date: Thu, 22 May 2014 13:13:19 +0200 Subject: [PATCH 121/273] IGM fix IGM works for less than 6 DoF --- pysymoro/geometry.py | 9 +-- pysymoro/invgeom.py | 107 +++++++++++++++++++-------------- pysymoro/tests/test_invgeom.py | 100 ++++++++++++++++++++++++++++++ symoroui/definition.py | 2 +- 4 files changed, 167 insertions(+), 51 deletions(-) create mode 100644 pysymoro/tests/test_invgeom.py diff --git a/pysymoro/geometry.py b/pysymoro/geometry.py index 78598d7..c0eb7a2 100644 --- a/pysymoro/geometry.py +++ b/pysymoro/geometry.py @@ -6,6 +6,7 @@ """ from sympy import Matrix, zeros, eye, sin, cos +from copy import copy from symoroutils import symbolmgr from symoroutils import tools @@ -156,11 +157,11 @@ def __init__(self, symo=None, trig_subs=False, simplify=True): self.symo = symo self.trig_subs = trig_subs and symo is not None self.T_tmp = eye(4) - self.siplify = simplify + self.simplify = simplify def process(self, tr): if tr.type == 0: # rotation - if self.rot.axis == tr.axis and self.siplify: + if self.rot.axis == tr.axis and self.simplify: self.rot.val += tr.val self.rot.name += tr.name else: # translation @@ -168,7 +169,7 @@ def process(self, tr): if self.trig_subs: self.symo.trig_replace(self.rot_mat, self.rot.val, self.rot.name) - self.rot = tr + self.rot = copy(tr) elif tr.type == 1: self.trans += self.rot_mat * self.rot.rot() * tr.trans() if self.trig_subs: @@ -188,7 +189,7 @@ def process_left(self, tr): if self.trig_subs: self.symo.trig_replace(self.rot_mat, self.rot.val, self.rot.name) - self.rot = tr + self.rot = copy(tr) elif tr.type == 1: self.trans += tr.trans() diff --git a/pysymoro/invgeom.py b/pysymoro/invgeom.py index c6e6ba5..9f3d053 100644 --- a/pysymoro/invgeom.py +++ b/pysymoro/invgeom.py @@ -8,7 +8,7 @@ from heapq import heapify, heappop from sympy import var, sin, cos, eye, atan2, sqrt, pi -from sympy import Matrix, Symbol, Expr +from sympy import Matrix, Symbol, Expr, trigsimp from pysymoro.geometry import transform_list, to_matrix from symoroutils import symbolmgr @@ -25,7 +25,11 @@ (0, 2, 0): 3, (0, 2, 1): 4} -def _paul_solve(robo, symo, nTm, n, m, knowns=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) th_all = set() r_all = set() @@ -52,6 +56,7 @@ def _paul_solve(robo, symo, nTm, n, m, knowns=set()): repeat = False 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: @@ -63,9 +68,10 @@ def _paul_solve(robo, symo, nTm, n, m, knowns=set()): 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) - iTm + M_eq = tr.matrix() * to_matrix(tr_list, simplify=False) while True: - found = _look_for_eq(symo, M_eq, knowns, th_all, r_all) + 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 @@ -77,6 +83,14 @@ def _paul_solve(robo, symo, nTm, n, m, knowns=set()): return knowns +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 @@ -100,9 +114,9 @@ def _look_for_eq(symo, M_eq, knowns, th_all, r_all): 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): - continue eq = M_eq[e1, e2] + if not isinstance(eq, Expr) or eq.is_Atom: + continue 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] @@ -166,9 +180,9 @@ def _try_solve_0(symo, eq_sys, knowns): 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) knowns.add(r) res = True @@ -240,9 +254,9 @@ def _try_solve_2(symo, eq_sys, knowns): 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) + _solve_type_5(symo, X1, -Y1, -Z1, X2, -Y2, -Z2, th, r) knowns |= {th, r} return True return False @@ -306,8 +320,8 @@ def _try_solve_4(symo, eq_sys, knowns): 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] @@ -322,6 +336,7 @@ def _try_solve_4(symo, eq_sys, knowns): 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: @@ -329,7 +344,7 @@ def _try_solve_4(symo, eq_sys, knowns): if not all_ok: continue symo.write_line("# Solving type 8") - _solve_type_8(symo, X1, Y1, Z1, Z2, th1, th2) + _solve_type_8(symo, X1, Y1, -Z1, -Z2, th1, th2) knowns |= {th1, th2} res = True return res @@ -340,9 +355,9 @@ 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 == tools.ZERO and Y != tools.ZERO: C = symo.replace(Z/Y, 'C', th) @@ -371,12 +386,12 @@ 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) + 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 == tools.ZERO and Y1 == tools.ZERO: @@ -395,10 +410,10 @@ 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, (tools.ONE, - tools.ONE)) symo.add_to_dict(r, YPS*sqrt((Y1/X1)**2 + (Y2/X2)**2)) @@ -412,12 +427,12 @@ 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) @@ -435,12 +450,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) @@ -457,17 +472,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, (tools.ONE, - tools.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)) diff --git a/pysymoro/tests/test_invgeom.py b/pysymoro/tests/test_invgeom.py new file mode 100644 index 0000000..df3ce1e --- /dev/null +++ b/pysymoro/tests/test_invgeom.py @@ -0,0 +1,100 @@ +#!/usr/bin/env python +# -*- coding: utf-8 -*- + + +"""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 GeoParams class.""" + def setUp(self): + self.symo = symbolmgr.SymbolManager() + + def test_igm_2r(self): + print "######## test_igm ##########" + 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): + print "######## test_igm ##########" + 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): + 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) + + +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/symoroui/definition.py b/symoroui/definition.py index 2514612..9360abd 100644 --- a/symoroui/definition.py +++ b/symoroui/definition.py @@ -114,7 +114,7 @@ def get_values(self): 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.ch_is_mobile.Value, self.cmb_structure.Value), 'keep_geo': self.ch_keep_geo.Value, 'keep_dyn': self.ch_keep_dyn.Value, From 00332b7d24321e145664398ff5570df6e35c14cb Mon Sep 17 00:00:00 2001 From: Bogdan Date: Wed, 21 May 2014 09:47:53 +0200 Subject: [PATCH 122/273] Fix loop solving issue Small fix in symbol manager --- symoroutils/symbolmgr.py | 14 ++++++++------ 1 file changed, 8 insertions(+), 6 deletions(-) diff --git a/symoroutils/symbolmgr.py b/symoroutils/symbolmgr.py index fbc3cfa..e0de1f1 100644 --- a/symoroutils/symbolmgr.py +++ b/symoroutils/symbolmgr.py @@ -84,7 +84,8 @@ def CS12_simp(self, sym, silent=False): return sym names, short_form = tools.trignometric_info(sym) names = list(names) - names.sort() + if short_form: + names.sort() sym2 = sym for n1, n2 in itertools.combinations(names, 2): if short_form: @@ -211,11 +212,12 @@ def replace(self, old_sym, name, index='', forced=False): 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: + 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] From a3b864f48b7d91a92f317c8a6ed7319317f50094 Mon Sep 17 00:00:00 2001 From: Bogdan Date: Thu, 22 May 2014 13:13:19 +0200 Subject: [PATCH 123/273] IGM fix IGM works for less than 6 DoF Conflicts: pysymoro/geometry.py pysymoro/invgeom.py --- pysymoro/geometry.py | 453 ++++++++++++++++++--------------- pysymoro/invgeom.py | 269 +++++++++++--------- pysymoro/tests/test_invgeom.py | 100 ++++++++ symoroui/definition.py | 2 +- symoroutils/symbolmgr.py | 5 +- 5 files changed, 492 insertions(+), 337 deletions(-) create mode 100644 pysymoro/tests/test_invgeom.py diff --git a/pysymoro/geometry.py b/pysymoro/geometry.py index 9d3ae86..c0eb7a2 100644 --- a/pysymoro/geometry.py +++ b/pysymoro/geometry.py @@ -6,6 +6,7 @@ """ from sympy import Matrix, zeros, eye, sin, cos +from copy import copy from symoroutils import symbolmgr from symoroutils import tools @@ -99,36 +100,235 @@ 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] +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 - Frame index. - invert: bool, optional - Defines the transformation direction + 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 - 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 + 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: - 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 + 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) -#TODO: rewrite the description -def _transform_const_sep(robo, j, invert=False): +def _transform(robo, j, invert=False): """Transform matrix between frames j and ant[j] Parameters @@ -145,17 +345,15 @@ def _transform_const_sep(robo, j, invert=False): 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 + 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('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 + 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): @@ -188,159 +386,12 @@ def compute_screw_transform(robo, symo, j, antRj, antPj, jTant): 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: 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) - 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): +def _rot(axis=2, th=0): """Rotation matrix about axis Parameters ========== - axis: {'x', 'y', 'z'} + axis: {0, 1, 2} Rotation axis th: var Rotation angle @@ -349,11 +400,12 @@ def _rot(axis='z', th=0): ======= rot: Matrix 3x3 """ - if axis == 'x': + 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 == 'y': + elif axis == 1: return Matrix([[cos(th), 0, sin(th)], [0, 1, 0], [-sin(th), 0, cos(th)]]) @@ -363,12 +415,12 @@ def _rot(axis='z', th=0): [0, 0, 1]]) -def _trans_vect(axis='z', p=0): +def _trans_vec(axis=2, p=0): """Translation vector along axis Parameters ========== - axis: {'x', 'y', 'z'} + axis: {0, 1, 2} Translation axis p: var Translation distance @@ -377,31 +429,13 @@ def _trans_vect(axis='z', p=0): ======= v: Matrix 3x1 """ - axis_dict = {'x': 0, 'y': 1, 'z': 2} + assert axis in {0, 1, 2} v = zeros(3, 1) - v[axis_dict[axis]] = p + v[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): +def _rot_trans(axis=2, th=0, p=0): """Transformation matrix with rotation about and translation along axis @@ -418,7 +452,8 @@ def _rot_trans(axis='z', th=0, p=0): ======= rot_trans: Matrix 4x4 """ - return Matrix([_rot(axis, th).row_join(_trans_vect(axis, p)), + assert axis in {0, 1, 2} + return Matrix([_rot(axis, th).row_join(_trans_vec(axis, p)), [0, 0, 0, 1]]) @@ -480,10 +515,8 @@ def direct_geometric(robo, frames, trig_subs): 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) + 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/invgeom.py b/pysymoro/invgeom.py index b7e96c1..9f3d053 100644 --- a/pysymoro/invgeom.py +++ b/pysymoro/invgeom.py @@ -8,9 +8,9 @@ from heapq import heapify, heappop from sympy import var, sin, cos, eye, atan2, sqrt, pi -from sympy import Matrix, Symbol, Expr +from sympy import Matrix, Symbol, Expr, trigsimp -from pysymoro.geometry import dgm +from pysymoro.geometry import transform_list, to_matrix from symoroutils import symbolmgr from symoroutils import tools @@ -25,97 +25,138 @@ (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 knowns + + +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 known + return tr_list[:var_idx], tr_list[var_idx:] -def _look_for_eq(symo, M_eq, known, th_all, r_all): +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): +def loop_solve(robo, symo, know=None): #TODO: rewrite; Add parallelogram detection q_vec = 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]) @@ -133,29 +174,30 @@ def igm_Paul(robo, T_ref, n): #TODO: think about smarter way of matching -def _try_solve_0(symo, eq_sys, known): +def _try_solve_0(symo, eq_sys, knowns): res = False for eq, [r], th_names in eq_sys: 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 == tools.ZERO, Yi == tools.ZERO, Zi == tools.ZERO]) <= 1 + zero = tools.ZERO + i_ok &= sum([Xi == zero, Yi == zero, Zi == zero]) <= 1 if not i_ok: continue j_ok = False @@ -172,12 +214,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)): @@ -212,10 +254,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 @@ -224,7 +266,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)): @@ -267,19 +309,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): +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] @@ -294,6 +336,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: @@ -301,8 +344,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 @@ -312,9 +355,9 @@ 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 == tools.ZERO and Y != tools.ZERO: C = symo.replace(Z/Y, 'C', th) @@ -343,12 +386,12 @@ 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) + 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 == tools.ZERO and Y1 == tools.ZERO: @@ -367,10 +410,10 @@ 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, (tools.ONE, - tools.ONE)) symo.add_to_dict(r, YPS*sqrt((Y1/X1)**2 + (Y2/X2)**2)) @@ -384,12 +427,12 @@ 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) @@ -407,12 +450,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) @@ -420,11 +463,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): @@ -434,17 +472,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, (tools.ONE, - tools.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)) @@ -463,12 +501,6 @@ def _solve_square(symo, A, B, C, x): 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: @@ -486,11 +518,4 @@ def _get_coefs(eq, A1, A2, *xs): # 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/tests/test_invgeom.py b/pysymoro/tests/test_invgeom.py new file mode 100644 index 0000000..df3ce1e --- /dev/null +++ b/pysymoro/tests/test_invgeom.py @@ -0,0 +1,100 @@ +#!/usr/bin/env python +# -*- coding: utf-8 -*- + + +"""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 GeoParams class.""" + def setUp(self): + self.symo = symbolmgr.SymbolManager() + + def test_igm_2r(self): + print "######## test_igm ##########" + 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): + print "######## test_igm ##########" + 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): + 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) + + +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/symoroui/definition.py b/symoroui/definition.py index ed0f5de..95358bd 100644 --- a/symoroui/definition.py +++ b/symoroui/definition.py @@ -108,7 +108,7 @@ def get_values(self): 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.ch_is_mobile.Value, self.cmb_structure.Value), 'keep_geo': self.ch_keep_geo.Value, 'keep_dyn': self.ch_keep_dyn.Value, diff --git a/symoroutils/symbolmgr.py b/symoroutils/symbolmgr.py index e0de1f1..6903ee8 100644 --- a/symoroutils/symbolmgr.py +++ b/symoroutils/symbolmgr.py @@ -223,10 +223,7 @@ def replace(self, old_sym, name, index='', forced=False): 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 + return new_sym def mat_replace(self, M, name, index='', forced=False, skip=0, symmet=False): From 30fe9c922c2d291bf6f393d671a20dfc5a91f826 Mon Sep 17 00:00:00 2001 From: aravind Date: Thu, 22 May 2014 14:29:03 +0200 Subject: [PATCH 124/273] Fix test code Fix `pysymoro/tests/test_geoparams.py` to assert to `0` for a fixed joint `q` value. --- pysymoro/tests/test_geoparams.py | 2 +- pysymoro/tests/test_invgeom.py | 11 +++-------- 2 files changed, 4 insertions(+), 9 deletions(-) diff --git a/pysymoro/tests/test_geoparams.py b/pysymoro/tests/test_geoparams.py index e06d608..ff1b2a0 100644 --- a/pysymoro/tests/test_geoparams.py +++ b/pysymoro/tests/test_geoparams.py @@ -64,7 +64,7 @@ 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, None) + self.assertEqual(self.data_fixed.q, 0) def run_tests(): diff --git a/pysymoro/tests/test_invgeom.py b/pysymoro/tests/test_invgeom.py index df3ce1e..ff31135 100644 --- a/pysymoro/tests/test_invgeom.py +++ b/pysymoro/tests/test_invgeom.py @@ -2,29 +2,26 @@ # -*- coding: utf-8 -*- -"""Unit test module for GeoParams class.""" +"""Unit test module for invgeom module.""" import unittest - from sympy import var, Matrix from numpy import random, amax, matrix, eye, zeros - +from pysymoro import geometry from pysymoro import invgeom from symoroutils import samplerobots from symoroutils import symbolmgr -from pysymoro import geometry class TestIGM(unittest.TestCase): - """Unit test for GeoParams class.""" + """Unit test for invgeom module.""" def setUp(self): self.symo = symbolmgr.SymbolManager() def test_igm_2r(self): - print "######## test_igm ##########" robo = samplerobots.planar2r() nTm = Matrix(4, 4, 12 * [invgeom.EMPTY] + [0, 0, 0, 1]) nTm[0, 3], nTm[1, 3] = var('p1, p2') @@ -45,7 +42,6 @@ def test_igm_2r(self): self.assertLess(amax(matrix(f06(q))-Ttest), 1e-12) def test_igm_rx90(self): - print "######## test_igm ##########" robo = samplerobots.rx90() #robo.r[6] = var('R6') #robo.gamma[6] = var('G6') # invgeom.T_GENERAL @@ -66,7 +62,6 @@ def test_igm_rx90(self): 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, From f5adf5eb290b900f7411d5a5ca77e08ff7b94ead Mon Sep 17 00:00:00 2001 From: aravind Date: Thu, 22 May 2014 17:06:55 +0200 Subject: [PATCH 125/273] Fix gravity issue in acceleration computation Modify base acceleration computation to return just the gravity effect in the case of fixed base of robots. This makes sure the base acceleration is initialised for the final forward recursion. --- pysymoro/dynmodel.py | 20 ++++++++++++-------- pysymoro/robotf.py | 3 +++ 2 files changed, 15 insertions(+), 8 deletions(-) diff --git a/pysymoro/dynmodel.py b/pysymoro/dynmodel.py index c2cb82a..d1c82a3 100644 --- a/pysymoro/dynmodel.py +++ b/pysymoro/dynmodel.py @@ -378,7 +378,9 @@ def _compute_joint_torque(model, robo, j): def _compute_base_acceleration(model, robo): """ Compute the base acceleration for a robot with floating base without - and with taking gravity into account. + 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 @@ -394,8 +396,9 @@ def _compute_base_acceleration(model, robo): o_inertia_o_c = model.composite_inertias[0].val o_beta_o_c = model.composite_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 + if robo.is_floating: + # 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_no_gravity = o_vdot_o # compute base acceleration taking gravity into account @@ -404,6 +407,7 @@ def _compute_base_acceleration(model, robo): 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 @@ -443,11 +447,11 @@ def inverse_dynamic_model(robo): # second backward recursion - compute composite terms for j in reversed(robo.joint_nums): if j == 0: - if not robo.is_floating: continue - else: - # compute 0^\dot{V}_0 : base acceleration - model = _compute_base_acceleration(model, robo) - continue + # 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 diff --git a/pysymoro/robotf.py b/pysymoro/robotf.py index cc52df6..224d086 100644 --- a/pysymoro/robotf.py +++ b/pysymoro/robotf.py @@ -472,4 +472,7 @@ def _init_maps(self): ('eta', 'etas'), ('k', 'stiffness'), ('G', 'gravity'), ]) + # methods for backward-compatability with the old Robot (for fixed + # base) class. + From b01b5303931af7c5e929cdd1d1d73869e3fb4e55 Mon Sep 17 00:00:00 2001 From: aravind Date: Fri, 23 May 2014 11:25:33 +0200 Subject: [PATCH 126/273] Fix bug in UI Fix bug in UI during new robot creation. Add `is_wmr` attribute to `Robot` class. Add `is_floating` attribute to `Robot` class. Set `is_mobile` == `is_floating` in `Robot` class for backward compatibility. Connect `is_wmr` and `is_floating` with the main window and new robot definition UI. --- pysymoro/robot.py | 10 +++++++--- symoroui/definition.py | 33 ++++++++++++++++++++++----------- symoroui/layout.py | 7 +++++-- 3 files changed, 34 insertions(+), 16 deletions(-) diff --git a/pysymoro/robot.py b/pysymoro/robot.py index 3310eb5..2eb8ad8 100644 --- a/pysymoro/robot.py +++ b/pysymoro/robot.py @@ -33,15 +33,19 @@ class Robot(object): 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): + def __init__(self, name, NL=0, NJ=0, NF=0, is_floating=False, + structure=TREE, is_wmr=False): # member variables: """ name of the robot: string""" self.name = name """ directory name""" self.directory = filemgr.get_folder_path(name) """ whether the base frame is floating: bool""" - self.is_mobile = is_mobile + self.is_floating = is_floating + # backward compatability + self.is_mobile = self.is_floating + """ whether the robot is wheeled mobile robot""" + self.is_wmr = is_wmr """ number of links: int""" self.nl = NL """ number of joints: int""" diff --git a/symoroui/definition.py b/symoroui/definition.py index 9360abd..cf42988 100644 --- a/symoroui/definition.py +++ b/symoroui/definition.py @@ -18,13 +18,14 @@ class DialogDefinition(wx.Dialog): """Creates the dialog box to define a new robot.""" - def __init__(self, prefix, name, nl, nj, structure, is_mobile, parent=None): + def __init__(self, prefix, name, nl, nj, structure, is_floating, + is_wmr, 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.init_ui(name, nl, nj, is_floating, is_wmr, structure) self.SetTitle(prefix + ": New robot definition") - def init_ui(self, name, nl, nj, is_mobile, structure): + def init_ui(self, name, nl, nj, is_floating, is_wmr, structure): main_sizer = wx.BoxSizer(wx.VERTICAL) #title label_main = wx.StaticText(self, label="Robot definition") @@ -60,11 +61,11 @@ def init_ui(self, name, nl, nj, is_mobile, structure): self.OnTypeChanged(None) main_sizer.Add(grid, 0, wx.ALL | wx.ALIGN_CENTER_HORIZONTAL, 15) self.ch_is_floating = wx.CheckBox(self, label=' Is Floating Base') - self.ch_is_floating.Value = is_mobile + self.ch_is_floating.Value = is_floating self.ch_is_wmr = wx.CheckBox( self, label=' Is Wheeled Mobile Robot' ) - self.ch_is_wmr.Value = is_mobile + self.ch_is_wmr.Value = is_wmr 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') @@ -113,12 +114,22 @@ 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} + params = { + 'init_pars': ( + name, nl, nj, 2*nj - nl, + self.ch_is_floating.Value, self.cmb_structure.Value + ), + 'name': name, + 'num_links': nl, + 'num_joints': nj, + 'structure': self.cmb_structure.Value, + 'is_floating': self.ch_is_floating.Value, + 'is_wmr': self.ch_is_wmr.Value, + 'keep_geo': self.ch_keep_geo.Value, + 'keep_dyn': self.ch_keep_dyn.Value, + 'keep_base': self.ch_keep_base.Value + } + return params class DialogVisualisation(wx.Dialog): diff --git a/symoroui/layout.py b/symoroui/layout.py index ee899e9..3ec79b5 100644 --- a/symoroui/layout.py +++ b/symoroui/layout.py @@ -392,7 +392,8 @@ def feed_data(self): ('name', self.robo.name), ('NF', self.robo.nf), ('NL', self.robo.nl), ('NJ', self.robo.nj), ('type', self.robo.structure), - ('floating', self.robo.is_mobile), ('wmr', False), + ('floating', self.robo.is_floating), + ('wmr', self.robo.is_wmr), ('loops', self.robo.nj-self.robo.nl) ] for name, info in names: @@ -578,7 +579,8 @@ 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_mobile + self.robo.nj, self.robo.structure, + self.robo.is_floating, self.robo.is_wmr ) if dialog.ShowModal() == wx.ID_OK: result = dialog.get_values() @@ -611,6 +613,7 @@ def OnNew(self, event): new_robo.v0 = self.robo.v0 new_robo.vdot0 = self.robo.vdot0 new_robo.G = self.robo.G + new_robo.is_wmr = result['is_wmr'] self.robo = new_robo self.robo.directory = filemgr.get_folder_path(self.robo.name) self.feed_data() From 82c735cc9707a3bde0f0eef1898a7df6bb8def6d Mon Sep 17 00:00:00 2001 From: aravind Date: Sat, 24 May 2014 02:45:34 +0200 Subject: [PATCH 127/273] Add Direct Dynamic Model for floating base robots --- pysymoro/dynmodel.py | 342 ++++++++++++++++++++++++++++++++++++++++--- 1 file changed, 323 insertions(+), 19 deletions(-) diff --git a/pysymoro/dynmodel.py b/pysymoro/dynmodel.py index d1c82a3..a789d9a 100644 --- a/pysymoro/dynmodel.py +++ b/pysymoro/dynmodel.py @@ -21,7 +21,7 @@ class DynModel(object): Hold the various components obtained during the computation of dynamic models (inverse and direct). """ - def __init__(self, joints): + def __init__(self, joints, model_type='inverse'): """ Constructor period. @@ -29,33 +29,63 @@ def __init__(self, joints): 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 # link velocity self.vels = list(None for j in joints) # gyroscopic acceleration self.gammas = list(None for j in joints) - # wrench - external+coriolis+centrifugal - self.betas = list(None for j in joints) # relative acceleration self.zetas = list(None for j in joints) - # composite spatial inertia matrix - self.composite_inertias = list(None for j in joints) - # composite wrench - self.composite_betas = 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) - # joint torque - self.torques = 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:\n" - str_format = str_format + "---------\n" + 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('_')] + attrs = [ + attr for attr in dir(self) \ + if not attr.startswith('_') or not 'model_type' + ] # add each attribute for attr in attrs: items = getattr(self, attr) @@ -225,8 +255,11 @@ def _compute_relative_acceleration(model, robo, j): j_zeta_j = Screw() # local variables j_a_j = robo.geos[j].axisa - qddot_j = robo.qddots[j] 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 @@ -371,7 +404,225 @@ def _compute_joint_torque(model, robo, j): gamma_j = wrench_term + actuator_inertia_term + \ viscous_friction_term + coriolis_friction_term # store computed torque in model - model.torques[j] = gamma_j + 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 = 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 + 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_jinv() * \ + 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 + i_inertia_i = robo.dyns[i].spatial_inertia.val + j_s_i = robo.geos[j].tmat.s_j_wrt_i + 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.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 @@ -393,8 +644,12 @@ def _compute_base_acceleration(model, robo): gravity = Screw() # local variables gravity.lin = robo.gravity - o_inertia_o_c = model.composite_inertias[0].val - o_beta_o_c = model.composite_betas[0].val + 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 if robo.is_floating: # TODO: replace sympy's matrix inversion with custom function @@ -420,7 +675,7 @@ def inverse_dynamic_model(robo): The inverse dynamic model of the robot. """ # some book keeping variables - model = DynModel(robo.joint_nums) + model = DynModel(robo.joint_nums, 'inverse') # first forward recursion for j in robo.joint_nums: if j == 0: continue @@ -484,8 +739,57 @@ def direct_dynamic_model(robo): The direct dynamic model of the robot. """ # some book keeping variables - model = DynModel(robo.joint_nums) - # TODO: complete direct dynamic model + model = DynModel(robo.joint_nums, '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) + # backward recursion + for j in reversed(robo.joint_nums): + if j == 0: continue + if j == robo.num_joints: + # initialise star_inertia, star_beta + model.star_inertias[j] = robo.dyns[j].spatial_inertia + model.star_betas[j] = model.betas[j] + # 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 + 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 From 6b627ecc9da733413a4d21fb4c9a80d094e236d1 Mon Sep 17 00:00:00 2001 From: Bogdan Date: Wed, 28 May 2014 13:53:27 +0200 Subject: [PATCH 128/273] Jacobian fix fix in jacobian interface --- pysymoro/kinematics.py | 4 +++- symoroui/kinematics.py | 13 ++++++------- 2 files changed, 9 insertions(+), 8 deletions(-) diff --git a/pysymoro/kinematics.py b/pysymoro/kinematics.py index db9c5e5..b503d6d 100644 --- a/pysymoro/kinematics.py +++ b/pysymoro/kinematics.py @@ -6,7 +6,7 @@ """ -from sympy import Matrix, zeros +from sympy import Matrix, zeros, trigsimp from pysymoro.geometry import dgm, Transform from pysymoro.geometry import compute_rot_trans, Z_AXIS @@ -83,6 +83,7 @@ def _jac(robo, symo, n, i, j, chain=None, forced=False, trig_subs=False): M = [] if chain is None: chain = robo.chain(n) + print chain chain.reverse() # chain_ext = chain + [robo.ant[min(chain)]] # if not i in chain_ext: @@ -115,6 +116,7 @@ def _jac(robo, symo, n, i, j, chain=None, forced=False, trig_subs=False): jTn = dgm(robo, symo, j, n, fast_form=False, trig_subs=trig_subs) jPn = Transform.P(jTn) L = -tools.skew(iRj*jPn) + L = L.applyfunc(trigsimp) if forced: symo.mat_replace(Jac, 'J', '', forced) L = symo.mat_replace(L, 'L', '', forced) diff --git a/symoroui/kinematics.py b/symoroui/kinematics.py index ef8c8f2..eecb716 100644 --- a/symoroui/kinematics.py +++ b/symoroui/kinematics.py @@ -41,17 +41,16 @@ def init_ui(self): 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) From 481d9440fe37db6222eb7c9598b9ede2e155e1f7 Mon Sep 17 00:00:00 2001 From: aravind Date: Wed, 28 May 2014 16:06:05 +0200 Subject: [PATCH 129/273] Fix new robot creation bug in UI --- symoroui/definition.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/symoroui/definition.py b/symoroui/definition.py index 95358bd..ed0f5de 100644 --- a/symoroui/definition.py +++ b/symoroui/definition.py @@ -108,7 +108,7 @@ def get_values(self): 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.ch_is_mobile.Value, self.cmb_structure.Value), 'keep_geo': self.ch_keep_geo.Value, 'keep_dyn': self.ch_keep_dyn.Value, From 8736309f5d0cd1329cfdf795dd574fa2dd3a6712 Mon Sep 17 00:00:00 2001 From: aravind Date: Thu, 29 May 2014 02:55:49 +0200 Subject: [PATCH 130/273] Fix base acceleration computation --- pysymoro/dynmodel.py | 16 +++++++--------- 1 file changed, 7 insertions(+), 9 deletions(-) diff --git a/pysymoro/dynmodel.py b/pysymoro/dynmodel.py index a789d9a..4943d23 100644 --- a/pysymoro/dynmodel.py +++ b/pysymoro/dynmodel.py @@ -192,8 +192,6 @@ def _compute_link_acceleration(model, robo, j, i): An instance of DynModel that contains all the new values. """ j_vdot_j = Screw() - if i == 0 and not robo.is_floating: - model.accels[i] = robo.base_accel # local variables j_s_i = robo.geos[j].tmat.s_j_wrt_i i_vdot_i = model.accels[i].val @@ -644,14 +642,14 @@ def _compute_base_acceleration(model, robo): gravity = Screw() # local variables gravity.lin = robo.gravity - 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 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 From 092bd005e0932cbb489258853cc5d6d4ffdf8683 Mon Sep 17 00:00:00 2001 From: aravind Date: Sat, 31 May 2014 05:03:09 +0200 Subject: [PATCH 131/273] Fix direct dynamic model computation Fix direct dynamic model computation with the correct `star_inertia` and `star_beta` initialisation and usage. --- pysymoro/dynmodel.py | 55 ++++++++++++++++++++++++++++++++++++-------- 1 file changed, 46 insertions(+), 9 deletions(-) diff --git a/pysymoro/dynmodel.py b/pysymoro/dynmodel.py index 4943d23..111b017 100644 --- a/pysymoro/dynmodel.py +++ b/pysymoro/dynmodel.py @@ -84,7 +84,7 @@ def __str__(self): # get all the attributes currently in the class attrs = [ attr for attr in dir(self) \ - if not attr.startswith('_') or not 'model_type' + if not attr.startswith('_') ] # add each attribute for attr in attrs: @@ -92,7 +92,7 @@ def __str__(self): if hasattr(items, '__iter__'): attr_str = self._str_items(items) else: - attr_str = str(items) + '\n' + attr_str = '\t' + str(items) + '\n' str_format = str_format + str(attr) + ": \n" str_format = str_format + attr_str return str_format @@ -151,6 +151,38 @@ def _init_composite_beta(model, robo, 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. @@ -531,8 +563,8 @@ def _compute_star_inertia(model, robo, j, i): """ i_inertia_i_s = Screw6() # local variables - i_inertia_i = robo.dyns[i].spatial_inertia.val 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) @@ -558,7 +590,7 @@ def _compute_star_beta(model, robo, j, i): i_beta_i_s = Screw() # local variables j_s_i = robo.geos[j].tmat.s_j_wrt_i - i_beta_i = model.betas[i].val + 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) @@ -749,13 +781,18 @@ def direct_dynamic_model(robo): model = _compute_gyroscopic_acceleration(model, robo, j, i) # compute j^beta_j : external+coriolis+centrifugal wrench (6x1) model = _compute_beta_wrench(model, robo, j) - # backward recursion + # 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_star_inertia(model, robo, j) + # initialise j^beta_j^c : composite 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 - if j == robo.num_joints: - # initialise star_inertia, star_beta - model.star_inertias[j] = robo.dyns[j].spatial_inertia - model.star_betas[j] = model.betas[j] # antecedent index i = robo.geos[j].ant # compute H_j : joint inertia (scalar term) From 1ce3708e0991a1d704cbc18291d988ddd705d135 Mon Sep 17 00:00:00 2001 From: aravind Date: Mon, 2 Jun 2014 18:26:35 +0200 Subject: [PATCH 132/273] Minor modifications --- pysymoro/dynmodel.py | 12 ++++++------ pysymoro/robotf.py | 1 + 2 files changed, 7 insertions(+), 6 deletions(-) diff --git a/pysymoro/dynmodel.py b/pysymoro/dynmodel.py index 111b017..4d7f0da 100644 --- a/pysymoro/dynmodel.py +++ b/pysymoro/dynmodel.py @@ -454,12 +454,12 @@ def _compute_joint_inertia(model, robo, j): h_j = 0 # local variables j_a_j = robo.geos[j].axisa - ia_j = robo.dyns[j].ia + 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 + model.joint_inertias[j] = h_j[0, 0] return model @@ -482,7 +482,7 @@ def _compute_no_qddot_inertia(model, robo, j): 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_jinv() * \ + 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 @@ -786,9 +786,9 @@ def direct_dynamic_model(robo): if j == 0: # compute 0^beta_0 model = _compute_beta_wrench(model, robo, j) - # initialise j^I_j^c : composite spatial inertia matrix + # initialise j^I_j^* : star spatial inertia matrix model = _init_star_inertia(model, robo, j) - # initialise j^beta_j^c : composite wrench + # 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): @@ -803,7 +803,7 @@ def direct_dynamic_model(robo): 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 + # 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) diff --git a/pysymoro/robotf.py b/pysymoro/robotf.py index 224d086..92329f2 100644 --- a/pysymoro/robotf.py +++ b/pysymoro/robotf.py @@ -105,6 +105,7 @@ def __str__(self): 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 + ("\tWMR: %s\n" % str(self.is_wmr)) str_format = str_format + ("\tStructure: %s\n" % str(self.structure)) str_format = str_format + '\n' # add geometric params From 0220c4322f7df809450727d48c59d29524ddfa5a Mon Sep 17 00:00:00 2001 From: aravind Date: Wed, 4 Jun 2014 14:58:23 +0200 Subject: [PATCH 133/273] Add unit test for dynamic model computation --- pysymoro/tests/test_dynmodel.py | 246 ++++++++++++++++++++++++++++++++ 1 file changed, 246 insertions(+) create mode 100644 pysymoro/tests/test_dynmodel.py diff --git a/pysymoro/tests/test_dynmodel.py b/pysymoro/tests/test_dynmodel.py new file mode 100644 index 0000000..7b35c5b --- /dev/null +++ b/pysymoro/tests/test_dynmodel.py @@ -0,0 +1,246 @@ +#!/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) + print(robo) + return robo + + +def planar2r_numerical(): + # 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, False, tools.SIMPLE) + # 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 external forces + params = { + 1: { + 'fx_ext': 0, 'fy_ext': 0, 'fz_ext': 0, + 'mx_ext': 0, 'my_ext': 0, 'mz_ext': 0 + }, + 2: { + 'fx_ext': 0, 'fy_ext': 0, 'fz_ext': 0, + 'mx_ext': 0, 'my_ext': 0, 'mz_ext': 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': 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() + # 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() + # 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() + + From a64e1872d4edb9d332f0f853b7456eda49b5b7d9 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Ga=C3=ABl=20Ecorchard?= Date: Tue, 11 Mar 2014 12:43:24 +0100 Subject: [PATCH 134/273] Add backup files to gitignore --- .gitignore | 1 + 1 file changed, 1 insertion(+) diff --git a/.gitignore b/.gitignore index 8c9bb68..c75b45b 100644 --- a/.gitignore +++ b/.gitignore @@ -2,6 +2,7 @@ *.swp *.tmp *.bak +*~ # Byte-compiled / optimized / DLL files __pycache__/ From cb17bc430a2b7ed1179cea7a9997b5c03a5b3da8 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Ga=C3=ABl=20Ecorchard?= Date: Tue, 11 Mar 2014 16:27:15 +0100 Subject: [PATCH 135/273] Add a linux launch file to test without installing --- launch | 7 +++++++ 1 file changed, 7 insertions(+) create mode 100755 launch diff --git a/launch b/launch new file mode 100755 index 0000000..927459e --- /dev/null +++ b/launch @@ -0,0 +1,7 @@ +#!/bin/sh + +DIR="$(dirname $0)" +PYTHONPATH="$DIR" + +python "$DIR"/bin/symoro-bin.py + From fba2629e7c1a0dd79ac97ccf9b30d0e14c72ef33 Mon Sep 17 00:00:00 2001 From: Gael Ecorchard Date: Thu, 5 Jun 2014 22:39:09 +0200 Subject: [PATCH 136/273] Add a validator for numerical values --- bin/symoro-bin.py | 8 ++++---- symoroviz/graphics.py | 15 ++++++++------- 2 files changed, 12 insertions(+), 11 deletions(-) diff --git a/bin/symoro-bin.py b/bin/symoro-bin.py index b2d0584..7909b26 100644 --- a/bin/symoro-bin.py +++ b/bin/symoro-bin.py @@ -3,7 +3,7 @@ """ -This is the main executable script of SYMORO. +This is the main executable script of SYMORO. """ @@ -20,10 +20,10 @@ def main(): app = wx.App(redirect=False) style = wx.DEFAULT_FRAME_STYLE ^ wx.MAXIMIZE_BOX ^ wx.RESIZE_BORDER frame = layout.MainFrame( - parent=None, + parent=None, id=wx.ID_ANY, - title=ui_labels.MAIN_WIN['window_title'], - size=(-1, -1), + title=ui_labels.MAIN_WIN['window_title'], + size=(-1, -1), style=style ) frame.Show() diff --git a/symoroviz/graphics.py b/symoroviz/graphics.py index 38bd4c4..e393821 100644 --- a/symoroviz/graphics.py +++ b/symoroviz/graphics.py @@ -28,7 +28,6 @@ #TODO: Random button class myGLCanvas(GLCanvas): - def __init__(self, parent, robo, params, size=(600, 600)): super(myGLCanvas, self).__init__(parent, size=size) self.Bind(wx.EVT_PAINT, self.OnPaintAll) @@ -61,7 +60,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 @@ -74,11 +75,11 @@ 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 + #print self.length 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): @@ -427,8 +428,8 @@ def init_ui(self): 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) + # 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 From b1d9ca4709d2a703920152c7516ca6c416f54432 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Ga=C3=ABl=20Ecorchard?= Date: Wed, 19 Mar 2014 17:22:39 +0100 Subject: [PATCH 137/273] Add tooltips and clean up code --- pysymoro/geometry.py | 4 ++-- pysymoro/invgeom.py | 16 ++++++++-------- pysymoro/kinematics.py | 18 +++++++++--------- 3 files changed, 19 insertions(+), 19 deletions(-) diff --git a/pysymoro/geometry.py b/pysymoro/geometry.py index c0eb7a2..eb31dd3 100644 --- a/pysymoro/geometry.py +++ b/pysymoro/geometry.py @@ -486,7 +486,7 @@ def direct_geometric_fast(robo, i, j): """ symo = symbolmgr.SymbolManager() symo.file_open(robo, 'fgm') - symo.write_params_table(robo, 'Direct Geometrix model') + symo.write_params_table(robo, 'Direct Geometric model') dgm(robo, symo, i, j, fast_form=True, forced=True) symo.file_close() return symo @@ -512,7 +512,7 @@ def direct_geometric(robo, frames, trig_subs): """ symo = symbolmgr.SymbolManager() symo.file_open(robo, 'trm') - symo.write_params_table(robo, 'Direct Geometrix model') + 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) diff --git a/pysymoro/invgeom.py b/pysymoro/invgeom.py index 9f3d053..f3f34f3 100644 --- a/pysymoro/invgeom.py +++ b/pysymoro/invgeom.py @@ -20,7 +20,7 @@ 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} @@ -33,7 +33,7 @@ def _paul_solve(robo, symo, nTm, n, m, known_vars=None): chain = robo.loop_chain(m, n) th_all = set() r_all = set() - #create the set of all knowns symbols + # Create the set of all knowns symbols for i in chain: if i >= 0: if robo.sigma[i] == 0 and isinstance(robo.theta[i], Expr): @@ -140,8 +140,8 @@ def _look_for_eq(symo, M_eq, knowns, th_all, r_all): def loop_solve(robo, symo, know=None): - #TODO: rewrite; Add parallelogram detection - q_vec = q_vec = [robo.get_q(i) for i in xrange(robo.NF)] + # TODO: rewrite; Add parallelogram detection + q_vec = [robo.get_q(i) for i in xrange(robo.NF)] loops = [] if know is None: know = robo.q_active @@ -167,13 +167,13 @@ def igm_Paul(robo, T_ref, n): T_ref = Matrix(4, 4, T_ref) 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 +# 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: @@ -314,7 +314,7 @@ def _try_solve_3(symo, eq_sys, knowns): return False -#TODO: make it with itertool +# TODO: make it with itertool def _try_solve_4(symo, eq_sys, knowns): res = False for i in xrange(len(eq_sys)): @@ -515,7 +515,7 @@ def _get_coefs(eq, A1, A2, *xs): 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 = 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) return X, Y, Z, is_ok diff --git a/pysymoro/kinematics.py b/pysymoro/kinematics.py index db9c5e5..f091f4f 100644 --- a/pysymoro/kinematics.py +++ b/pysymoro/kinematics.py @@ -78,17 +78,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 = [] 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) @@ -286,9 +286,9 @@ def jdot_qdot(robo): def jacobian(robo, n, i, j): 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 From 1c70f7685496346395e198eabaf3c5be0d375b6b Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Ga=C3=ABl=20Ecorchard?= Date: Wed, 19 Mar 2014 18:42:02 +0100 Subject: [PATCH 138/273] Solve GL frame not being updated on Linux --- symoroviz/graphics.py | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) diff --git a/symoroviz/graphics.py b/symoroviz/graphics.py index e393821..650db7f 100644 --- a/symoroviz/graphics.py +++ b/symoroviz/graphics.py @@ -334,6 +334,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): @@ -370,8 +371,8 @@ def InitGL(self): class MainWindow(wx.Frame): def __init__(self, prefix, robo, params=None, parent=None, identifier=-1): - wx.Frame.__init__( - self, parent, identifier, prefix + ': Robot representation', + super(MainWindow, self).__init__( + parent, identifier, prefix + ': Robot representation', style=wx.DEFAULT_FRAME_STYLE | wx.FULL_REPAINT_ON_RESIZE ) self.robo = robo From a1e28c9cba8caf6acf1204c7f30dec921202b660 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Ga=C3=ABl=20Ecorchard?= Date: Tue, 13 May 2014 15:24:29 +0200 Subject: [PATCH 139/273] Fix launch and proper application exit --- launch | 3 +- symoroui/layout.py | 81 +++++++++++++++++++++++----------------------- 2 files changed, 42 insertions(+), 42 deletions(-) diff --git a/launch b/launch index 927459e..a78c2b3 100755 --- a/launch +++ b/launch @@ -1,7 +1,6 @@ #!/bin/sh DIR="$(dirname $0)" -PYTHONPATH="$DIR" -python "$DIR"/bin/symoro-bin.py +PYTHONPATH="$DIR":"$PYTHONPATH" python "$DIR"/bin/symoro-bin.py diff --git a/symoroui/layout.py b/symoroui/layout.py index 6bc3cef..85f4a88 100644 --- a/symoroui/layout.py +++ b/symoroui/layout.py @@ -4,7 +4,7 @@ """ This module creates the main window user interface and draws the -interface on the screen for the SYMORO package. +interface on the screen for the SYMORO package. """ @@ -58,7 +58,7 @@ def __init__(self, *args, **kwargs): self.statusbar.SetStatusWidths(widths=[-1, -1]) self.statusbar.SetStatusText(text="Ready", number=0) self.statusbar.SetStatusText( - text="Location of robot files is %s" + text="Location of robot files is %s" % filemgr.get_base_path(), number = 1 ) @@ -73,7 +73,7 @@ def params_in_grid(self, szr_grd, elements, rows, cols, width=70): field_id = int(elements[key].id) if control is 'cmb': ctrl = wx.ComboBox( - parent=self.panel, style=wx.CB_READONLY, + parent=self.panel, style=wx.CB_READONLY, size=(width, -1), name=name ) ctrl.Bind(wx.EVT_COMBOBOX, handler) @@ -83,7 +83,7 @@ def params_in_grid(self, szr_grd, elements, rows, cols, width=70): ) else: ctrl = wx.TextCtrl( - parent=self.panel, size=(width, -1), + parent=self.panel, size=(width, -1), name=name, id=field_id ) ctrl.Bind(wx.EVT_KILL_FOCUS, handler) @@ -95,11 +95,11 @@ def params_in_grid(self, szr_grd, elements, rows, cols, width=70): ), proportion=0, flag=wx.ALL | wx.ALIGN_RIGHT, border=5 ) szr_ele.Add( - ctrl, proportion=0, + ctrl, proportion=0, flag=wx.ALL | wx.ALIGN_LEFT, border=1 ) szr_grd.Add( - szr_ele, pos=(place[0], place[1]), + szr_ele, pos=(place[0], place[1]), flag=wx.ALL | wx.ALIGN_RIGHT, border=2 ) @@ -132,11 +132,11 @@ def create_ui(self): name=ui_labels.ROBOT_TYPE[key].name ) szr_grd_robot_type.Add( - wx.StaticText(self.panel, label=label), + 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), + self.widgets[name], pos=(idx, 1), flag=wx.LEFT | wx.RIGHT, border=10 ) szr_robot_type.Add( @@ -152,8 +152,8 @@ def create_ui(self): ) 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_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) @@ -171,7 +171,7 @@ def create_ui(self): idx = (j*4) + i name = 'Z'+str(idx) txt_z_element = wx.TextCtrl( - parent=self.panel, name=name, + parent=self.panel, name=name, id=idx, size=(60, -1) ) self.widgets[name] = txt_z_element @@ -179,7 +179,7 @@ def create_ui(self): wx.EVT_KILL_FOCUS, self.OnZParamChanged ) szr_grd_loc.Add( - txt_z_element, pos=(j + 1, i + 1), + 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)) @@ -191,11 +191,11 @@ def create_ui(self): lbl_row, pos=(i+1, 0), flag=wx.RIGHT, border=3 ) szr_grd_loc.Add( - lbl_col, pos=(0, i+1), + lbl_col, pos=(0, i+1), flag=wx.ALIGN_CENTER_HORIZONTAL, border=3 ) szr_grd_loc.Add( - lbl_last_row, pos=(4, i+1), + 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) @@ -208,7 +208,7 @@ def create_ui(self): ) szr_grd_geom = wx.GridBagSizer(0, 5) self.params_in_grid( - szr_grd_geom, elements=ui_labels.GEOM_PARAMS, + szr_grd_geom, elements=ui_labels.GEOM_PARAMS, rows=2, cols=5, width=70 ) szr_geom_params.Add(szr_grd_geom) @@ -225,7 +225,7 @@ def create_ui(self): name=ui_labels.DYN_PARAMS['link'].name ) cmb_link.Bind( - wx.EVT_COMBOBOX, + wx.EVT_COMBOBOX, getattr(self, ui_labels.DYN_PARAMS['link'].handler) ) self.widgets['link'] = cmb_link @@ -233,7 +233,7 @@ def create_ui(self): szr_link.Add( wx.StaticText( self.panel, label=ui_labels.DYN_PARAMS['link'].label - ), proportion=0, + ), proportion=0, flag=wx.ALL | wx.ALIGN_LEFT, border=5 ) szr_link.AddSpacer((4,4)) @@ -259,7 +259,7 @@ def create_ui(self): ) szr_grd_base_velacc = wx.GridBagSizer(0, 0) self.params_in_grid( - szr_grd_base_velacc, elements=ui_labels.BASE_VEL_ACC, + szr_grd_base_velacc, elements=ui_labels.BASE_VEL_ACC, rows=3, cols=4, width=60 ) szr_base_velacc.Add(szr_grd_base_velacc) @@ -273,10 +273,10 @@ def create_ui(self): ) szr_grd_joint_velacc = wx.GridBagSizer(5, 5) self.params_in_grid( - szr_grd_joint_velacc, elements=ui_labels.JOINT_VEL_ACC, - rows=3, cols=1, width=75, + szr_grd_joint_velacc, elements=ui_labels.JOINT_VEL_ACC, + rows=3, cols=1, width=75, ) - szr_joint_velacc.Add(szr_grd_joint_velacc, + 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) @@ -396,7 +396,7 @@ def feed_data(self): def create_menu(self): """Method to create the menu bar""" menu_bar = wx.MenuBar() - # menu item - file + # menu item - file file_menu = wx.Menu() m_new = wx.MenuItem( file_menu, wx.ID_NEW, ui_labels.FILE_MENU['m_new'] @@ -514,7 +514,7 @@ def create_menu(self): # menu item - identification iden_menu = wx.Menu() m_base_inertial_params = wx.MenuItem( - iden_menu, wx.ID_ANY, + iden_menu, wx.ID_ANY, ui_labels.IDEN_MENU['m_base_inertial_params'] ) self.Bind( @@ -527,13 +527,13 @@ def create_menu(self): self.Bind(wx.EVT_MENU, self.OnDynIdentifModel, m_dyn_iden_model) iden_menu.AppendItem(m_dyn_iden_model) m_energy_iden_model = wx.MenuItem( - iden_menu, wx.ID_ANY, + iden_menu, wx.ID_ANY, ui_labels.IDEN_MENU['m_energy_iden_model'] ) # TODO: uncomment the 3 lines below to add the event -# self.Bind( -# wx.EVT_MENU, self.OnEnergyIdentifModel, 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 @@ -549,7 +549,7 @@ def create_menu(self): def OnNew(self, event): dialog = ui_definition.DialogDefinition( - ui_labels.MAIN_WIN['prog_name'], + ui_labels.MAIN_WIN['prog_name'], self.robo.name, self.robo.nl, self.robo.nj, self.robo.structure, self.robo.is_mobile ) @@ -591,24 +591,24 @@ def OnNew(self, event): def message_error(self, message): wx.MessageDialog( - None, + None, message, - 'Error', + 'Error', wx.OK | wx.ICON_ERROR ).ShowModal() def message_warning(self, message): wx.MessageDialog( - None, + None, message, - 'Error', + 'Error', wx.OK | wx.ICON_WARNING ).ShowModal() def message_info(self, message): wx.MessageDialog( - None, - message, + None, + message, 'Information', wx.OK | wx.ICON_INFORMATION ).ShowModal() @@ -633,10 +633,10 @@ def OnOpen(self, event): if self.OnSave(None) == tools.FAIL: return dialog = wx.FileDialog( - self, - message="Choose PAR file", + self, + message="Choose PAR file", style=wx.OPEN, - wildcard='*.par', + wildcard='*.par', defaultFile='*.par' ) if dialog.ShowModal() == wx.ID_OK: @@ -694,7 +694,7 @@ def OnFastGeometricModel(self, event): def OnIgmPaul(self, event): dialog = ui_geometry.DialogPaul( - ui_labels.MAIN_WIN['prog_name'], + ui_labels.MAIN_WIN['prog_name'], self.robo.endeffectors, str(invgeom.EMPTY) ) @@ -776,12 +776,12 @@ def OnVisualisation(self, event): if dialog.ShowModal() == wx.ID_OK: self.par_dict = dialog.get_values() graphics.MainWindow( - ui_labels.MAIN_WIN['prog_name'], self.robo, + ui_labels.MAIN_WIN['prog_name'], self.robo, self.par_dict, self ) else: graphics.MainWindow( - ui_labels.MAIN_WIN['prog_name'], self.robo, + ui_labels.MAIN_WIN['prog_name'], self.robo, self.par_dict, self ) @@ -796,5 +796,6 @@ def OnClose(self, event): elif result == wx.CANCEL: return self.Destroy() + wx.GetApp().ExitMainLoop() From cd9f2185b27e4f7fe7d5de4e703fe605a114a6d1 Mon Sep 17 00:00:00 2001 From: aravind Date: Thu, 5 Jun 2014 23:07:32 +0200 Subject: [PATCH 140/273] Rename `launch` to `launch.sh` Rename `launch` to `launch.sh` in order to play nice with Windows and its need for file extensions. --- launch => launch.sh | 0 1 file changed, 0 insertions(+), 0 deletions(-) rename launch => launch.sh (100%) diff --git a/launch b/launch.sh similarity index 100% rename from launch rename to launch.sh From 5ec066a33ff9dc1d2ca3adddfaa43322414263a4 Mon Sep 17 00:00:00 2001 From: aravind Date: Fri, 6 Jun 2014 11:38:04 +0200 Subject: [PATCH 141/273] Fix base acceleration computation without gravity Fix base acceleration computation without gravity effect by using `copy` to copy the object instead of a simple assignment. This is cause simple assignment assigns the address of the object instead of the object's value. --- pysymoro/dynmodel.py | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/pysymoro/dynmodel.py b/pysymoro/dynmodel.py index 4d7f0da..613883b 100644 --- a/pysymoro/dynmodel.py +++ b/pysymoro/dynmodel.py @@ -7,6 +7,8 @@ """ +import copy + from sympy import Matrix from sympy import sign @@ -685,7 +687,7 @@ def _compute_base_acceleration(model, robo): # 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_no_gravity = o_vdot_o + model.base_accel_no_gravity = copy.copy(o_vdot_o) # compute base acceleration taking gravity into account o_vdot_o.val = o_vdot_o.val + gravity.val # store in model From 4f0a1449a476d6fc42ab469ecebb3bc3717e0029 Mon Sep 17 00:00:00 2001 From: aravind Date: Fri, 6 Jun 2014 14:08:38 +0200 Subject: [PATCH 142/273] Add `__str__()` method to `Screw6` class --- pysymoro/screw6.py | 13 +++++++++++++ 1 file changed, 13 insertions(+) diff --git a/pysymoro/screw6.py b/pysymoro/screw6.py index 16aad01..e91d937 100644 --- a/pysymoro/screw6.py +++ b/pysymoro/screw6.py @@ -60,6 +60,19 @@ def __init__(self, *args, **kwargs): 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): """ From 3a96f16f9ab71d273c265255d65e7c3090c3acf8 Mon Sep 17 00:00:00 2001 From: aravind Date: Fri, 6 Jun 2014 14:40:09 +0200 Subject: [PATCH 143/273] Add method to set dynamic params to zero Add `set_to_zero()` method in `DynParams` class to set all the dynamic parameter values to zero. Update unit test code accordingly. --- pysymoro/dynparams.py | 20 ++++++++++++++++++++ pysymoro/tests/test_dynparams.py | 18 ++++++++++++++++++ 2 files changed, 38 insertions(+) diff --git a/pysymoro/dynparams.py b/pysymoro/dynparams.py index 66f2516..9d11db5 100644 --- a/pysymoro/dynparams.py +++ b/pysymoro/dynparams.py @@ -137,6 +137,26 @@ def update_params(self, params): "%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.""" diff --git a/pysymoro/tests/test_dynparams.py b/pysymoro/tests/test_dynparams.py index d5ffd01..dca180c 100644 --- a/pysymoro/tests/test_dynparams.py +++ b/pysymoro/tests/test_dynparams.py @@ -126,6 +126,24 @@ def test_update_params(self): 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""" From 81f6f9d52a694bee71a55b8e0b6bd0d19a411a68 Mon Sep 17 00:00:00 2001 From: aravind Date: Fri, 6 Jun 2014 16:00:35 +0200 Subject: [PATCH 144/273] Add method to set dynamic params to zero Add method to set dynamic parameters to zero in `FloatingRobot` class. This method allows the links for which dynamic parameters are to be zero. --- pysymoro/robotf.py | 18 ++++++++++++++++++ 1 file changed, 18 insertions(+) diff --git a/pysymoro/robotf.py b/pysymoro/robotf.py index 92329f2..5899a21 100644 --- a/pysymoro/robotf.py +++ b/pysymoro/robotf.py @@ -269,6 +269,24 @@ def update_params(self, kind, params): 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 From a2327d2a13ff40dfc231fe52dc76b87df337ab78 Mon Sep 17 00:00:00 2001 From: aravind Date: Fri, 6 Jun 2014 16:05:29 +0200 Subject: [PATCH 145/273] Add floating robot test case to dynmodel unit test --- pysymoro/tests/test_dynmodel.py | 78 +++++++++++++++++++++++++++++++-- 1 file changed, 75 insertions(+), 3 deletions(-) diff --git a/pysymoro/tests/test_dynmodel.py b/pysymoro/tests/test_dynmodel.py index 7b35c5b..196925a 100644 --- a/pysymoro/tests/test_dynmodel.py +++ b/pysymoro/tests/test_dynmodel.py @@ -42,7 +42,7 @@ def planar2r(): return robo -def planar2r_numerical(): +def planar2r_numerical(is_floating=False): # numerical values (random in some case) L1 = 0.5 L2 = 0.4 @@ -60,7 +60,8 @@ def planar2r_numerical(): IA2 = 0.0 G3 = 9.81 # create robot - robo = Robot('planar2r', 2, 2, 3, False, tools.SIMPLE) + robo = Robot('planar2r', 2, 2, 3, is_floating, tools.SIMPLE) + robo.set_dyns_to_zero() # update geometric params params = { 1: {'sigma': 0, 'mu': 1}, @@ -181,7 +182,6 @@ def test_when_zero(self): ) # 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) @@ -221,6 +221,78 @@ def test_when_random(self): 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]) From b97deaee81f485395aea95493916df83b4b1462f Mon Sep 17 00:00:00 2001 From: aravind Date: Sat, 7 Jun 2014 01:39:45 +0200 Subject: [PATCH 146/273] Minor modifications --- pysymoro/tests/test_dynmodel.py | 15 ++------------- 1 file changed, 2 insertions(+), 13 deletions(-) diff --git a/pysymoro/tests/test_dynmodel.py b/pysymoro/tests/test_dynmodel.py index 196925a..0879b48 100644 --- a/pysymoro/tests/test_dynmodel.py +++ b/pysymoro/tests/test_dynmodel.py @@ -58,7 +58,7 @@ def planar2r_numerical(is_floating=False): FV2 = 0.18 IA1 = 0.0 IA2 = 0.0 - G3 = 9.81 + G3 = -9.81 # create robot robo = Robot('planar2r', 2, 2, 3, is_floating, tools.SIMPLE) robo.set_dyns_to_zero() @@ -80,18 +80,6 @@ def planar2r_numerical(is_floating=False): } } robo.update_params('dyns', params) - # update external forces - params = { - 1: { - 'fx_ext': 0, 'fy_ext': 0, 'fz_ext': 0, - 'mx_ext': 0, 'my_ext': 0, 'mz_ext': 0 - }, - 2: { - 'fx_ext': 0, 'fy_ext': 0, 'fz_ext': 0, - 'mx_ext': 0, 'my_ext': 0, 'mz_ext': 0 - } - } - robo.update_params('dyns', params) # update joint params params = {0: {'qdots': 0, 'qddots': 0, 'torques': 0}} robo.update_params('misc', params) @@ -182,6 +170,7 @@ def test_when_zero(self): ) # compute DDyM robo.compute_ddym() + print(robo.idym) # 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) From ebcb4de37e0944a29ec264fb28553ffe608c8a82 Mon Sep 17 00:00:00 2001 From: aravind Date: Tue, 10 Jun 2014 15:50:23 +0200 Subject: [PATCH 147/273] Add `is_symbolic` attribute Add `is_symbolic` attribute to `FloatingRobot` and `DynModel` class. --- pysymoro/dynmodel.py | 8 +++++--- pysymoro/robotf.py | 5 ++++- pysymoro/tests/test_dynmodel.py | 7 +++++-- 3 files changed, 14 insertions(+), 6 deletions(-) diff --git a/pysymoro/dynmodel.py b/pysymoro/dynmodel.py index 613883b..2444d8d 100644 --- a/pysymoro/dynmodel.py +++ b/pysymoro/dynmodel.py @@ -23,7 +23,7 @@ class DynModel(object): Hold the various components obtained during the computation of dynamic models (inverse and direct). """ - def __init__(self, joints, model_type='inverse'): + def __init__(self, joints, is_symbolic, model_type='inverse'): """ Constructor period. @@ -39,6 +39,8 @@ def __init__(self, joints, model_type='inverse'): """ # 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 @@ -707,7 +709,7 @@ def inverse_dynamic_model(robo): The inverse dynamic model of the robot. """ # some book keeping variables - model = DynModel(robo.joint_nums, 'inverse') + model = DynModel(robo.joint_nums, robo.is_symbolic, 'inverse') # first forward recursion for j in robo.joint_nums: if j == 0: continue @@ -771,7 +773,7 @@ def direct_dynamic_model(robo): The direct dynamic model of the robot. """ # some book keeping variables - model = DynModel(robo.joint_nums, 'direct') + model = DynModel(robo.joint_nums, robo.is_symbolic, 'direct') # first forward recursion for j in robo.joint_nums: if j == 0: continue diff --git a/pysymoro/robotf.py b/pysymoro/robotf.py index 5899a21..5c4fa4a 100644 --- a/pysymoro/robotf.py +++ b/pysymoro/robotf.py @@ -26,7 +26,8 @@ class FloatingRobot(object): """ def __init__( self, name, links=0, joints=0, frames=0, - is_floating=True, structure=tools.TREE, is_wmr=False + is_floating=True, structure=tools.TREE, is_wmr=False, + is_symbolic=True ): """ Constructor period. @@ -49,6 +50,8 @@ def __init__( self.structure = structure """To indicate if the robot is a wheeled mobile robot""" self.is_wmr = is_wmr + """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 diff --git a/pysymoro/tests/test_dynmodel.py b/pysymoro/tests/test_dynmodel.py index 0879b48..694d815 100644 --- a/pysymoro/tests/test_dynmodel.py +++ b/pysymoro/tests/test_dynmodel.py @@ -60,7 +60,10 @@ def planar2r_numerical(is_floating=False): IA2 = 0.0 G3 = -9.81 # create robot - robo = Robot('planar2r', 2, 2, 3, is_floating, tools.SIMPLE) + robo = Robot( + 'planar2r', 2, 2, 3, is_floating, + tools.SIMPLE, is_symbolic=False + ) robo.set_dyns_to_zero() # update geometric params params = { @@ -170,7 +173,6 @@ def test_when_zero(self): ) # compute DDyM robo.compute_ddym() - print(robo.idym) # 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) @@ -197,6 +199,7 @@ def test_when_random(self): 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]] From 7a2989ad0105fd87a6de25a94f45eaceda6b5aaf Mon Sep 17 00:00:00 2001 From: aravind Date: Wed, 11 Jun 2014 19:32:12 +0200 Subject: [PATCH 148/273] Add inverse dynamic model Add old-style inverse dynamic model. --- pysymoro/fldyn.py | 251 +++++++++++++++++++++++++++++++++++++++ symoroutils/symbolmgr.py | 2 + 2 files changed, 253 insertions(+) create mode 100644 pysymoro/fldyn.py diff --git a/pysymoro/fldyn.py b/pysymoro/fldyn.py new file mode 100644 index 0000000..e770e6d --- /dev/null +++ b/pysymoro/fldyn.py @@ -0,0 +1,251 @@ + +import sympy +from sympy import Matrix +from copy import copy, deepcopy + +from pysymoro.geometry import compute_screw_transform +from pysymoro.geometry import compute_rot_trans, Transform +from pysymoro.kinematics import compute_vel_acc +from pysymoro.kinematics import compute_omega +from symoroutils import symbolmgr +from symoroutils import tools +from symoroutils.paramsinit import ParamsInit + + +def inertia_spatial(J, MS, M): + """ + Compute spatial inertia matrix (internal function). + """ + return Matrix([ + (M * sympy.eye(3)).row_join(tools.skew(MS).T), + tools.skew(MS).row_join(J) + ]) + + +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). + + Notes + ===== + beta is the output parameter + """ + E1 = symo.mat_replace(robo.J[j]*w[j], 'JW', j) + E2 = symo.mat_replace(tools.skew(w[j])*E1, 'KW', j) + E3 = tools.skew(w[j])*robo.MS[j] + E4 = symo.mat_replace(tools.skew(w[j])*E3, 'SW', j) + E5 = -robo.Nex[j] - E2 + E6 = -robo.Fex[j] - E4 + beta[j] = Matrix([E6, E5]) + + +def compute_gamma(robo, symo, j, antRj, antPj, w, wi, gamma): + """ + Compute gyroscopic acceleration (internal function). + + Notes + ===== + gamma is the output parameter + """ + E1 = symo.mat_replace( + tools.skew(wi[j]) * Matrix([0, 0, robo.qdot[j]]), + 'WQ', j + ) + E2 = (1 - robo.sigma[j]) * E1 + E3 = 2 * robo.sigma[j] * E1 + E4 = tools.skew(w[robo.ant[j]]) * antPj[j] + E5 = tools.skew(w[robo.ant[j]]) * E4 + E6 = antRj[j].T * E5 + E7 = symo.mat_replace(E6 + E3, 'LW', j) + gamma[j] = Matrix([E7, E2]) + + +def compute_zeta(robo, symo, j, gamma, jaj, zeta): + """ + Compute relative acceleration (internal function). + + Note: + zeta is the output parameter + """ + expr = gamma[j] + (robo.qddot[j] * jaj[j]) + zeta[j] = symo.mat_replace(expr, 'ZETA', j) + + +def replace_composite_terms( + grandJ, beta, j, composite_inertia, composite_beta +): + """ + Replace composite inertia and beta (internal function). + + Note: + composite_inertia are composite_beta are the output parameters + """ + composite_inertia[j] = symo.mat_replace( + grandJ[j], 'MJE', j, symmet=True + ) + composite_beta[j] = symo.mat_replace(beta[j], 'VBE', j) + + +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 + replace_composite_terms( + composite_inertia, composite_beta, i, + composite_inertia, composite_beta + ) + + +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 * 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 compute_reaction_wrench( + robo, symo, j, grandVp, + composite_inertia, composite_beta, react_wrench +): + """ + Compute reaction wrench (internal function). + + Note: + react_wrench is the output parameter + """ + expr = composite_inertia[j] * grandVp[j] + expr = symo.mat_replace(expr, 'DY', j) + wrench = expr - composite_beta[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 compute_torque(robo, symo, j, jaj, react_wrench): + """ + Compute torque (internal function). + + Note: + torque is the output parameter. + """ + symbl_name = 'GAM' + str(j) + 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, symbl_name, forced=True) + + +def fl_inverse_dynamic_model(robo): + symo = symbolmgr.SymbolManager() + symo.file_open(robo, 'flidm') + title = 'Inverse Dynamic Model - NE' + symo.write_params_table(robo, title, inert=True, dynam=True) + # 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) + grandVp = ParamsInit.init_vec(robo, 6) + react_wrench = ParamsInit.init_vec(robo, 6) + torque = ParamsInit.init_scalar(robo) + # for flexible joints + #H_inv = ParamsInit.init_scalar(robo) + #juj = ParamsInit.init_vec(robo, 6) # Jj*aj / Hj + #Tau = ParamsInit.init_scalar(robo) + # base acceleration initialisation + # TODO: move to second forward recursion + grandVp[0] = Matrix([robo.vdot0 - robo.G, robo.w0]) + # 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) + # TODO: check joint flexibility + 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( + grandJ, beta, j, composite_inertia, composite_beta + ) + # second backward recursion - compute composite term + for j in reversed(xrange(0, robo.NL)): + if j == 0: + # TODO: compute base acceleration + continue + # compute i^I_i^c and i^beta_i^c + compute_composite_terms( + robo, symo, j, jTant, zeta, + composite_inertia, composite_beta + ) + # 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) + symo.file_close() + return symo + + diff --git a/symoroutils/symbolmgr.py b/symoroutils/symbolmgr.py index e51fe75..d2fe538 100644 --- a/symoroutils/symbolmgr.py +++ b/symoroutils/symbolmgr.py @@ -608,3 +608,5 @@ def gen_func(self, name, to_return, args): """ exec self.gen_func_string(name, to_return, args) return eval('%s' % name) + + From de2bb69a1f291ad7047be396cc91eebe2aebff9c Mon Sep 17 00:00:00 2001 From: aravind Date: Thu, 12 Jun 2014 11:41:11 +0200 Subject: [PATCH 149/273] Code cleanup --- pysymoro/fldyn.py | 40 +++++++++++++++++++++------------------- 1 file changed, 21 insertions(+), 19 deletions(-) diff --git a/pysymoro/fldyn.py b/pysymoro/fldyn.py index e770e6d..895721a 100644 --- a/pysymoro/fldyn.py +++ b/pysymoro/fldyn.py @@ -1,7 +1,6 @@ import sympy from sympy import Matrix -from copy import copy, deepcopy from pysymoro.geometry import compute_screw_transform from pysymoro.geometry import compute_rot_trans, Transform @@ -31,13 +30,16 @@ def compute_beta(robo, symo, j, w, beta): ===== beta is the output parameter """ - E1 = symo.mat_replace(robo.J[j]*w[j], 'JW', j) - E2 = symo.mat_replace(tools.skew(w[j])*E1, 'KW', j) - E3 = tools.skew(w[j])*robo.MS[j] - E4 = symo.mat_replace(tools.skew(w[j])*E3, 'SW', j) - E5 = -robo.Nex[j] - E2 - E6 = -robo.Fex[j] - E4 - beta[j] = Matrix([E6, E5]) + 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]) def compute_gamma(robo, symo, j, antRj, antPj, w, wi, gamma): @@ -48,17 +50,16 @@ def compute_gamma(robo, symo, j, antRj, antPj, w, wi, gamma): ===== gamma is the output parameter """ - E1 = symo.mat_replace( - tools.skew(wi[j]) * Matrix([0, 0, robo.qdot[j]]), - 'WQ', j - ) - E2 = (1 - robo.sigma[j]) * E1 - E3 = 2 * robo.sigma[j] * E1 - E4 = tools.skew(w[robo.ant[j]]) * antPj[j] - E5 = tools.skew(w[robo.ant[j]]) * E4 - E6 = antRj[j].T * E5 - E7 = symo.mat_replace(E6 + E3, 'LW', j) - gamma[j] = Matrix([E7, E2]) + 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[robo.ant[j]]) * antPj[j] + expr5 = tools.skew(w[robo.ant[j]]) * expr4 + expr6 = antRj[j].transpose() * expr5 + expr7 = expr6 + expr3 + expr7 = symo.mat_replace(expr7, 'LW', j) + gamma[j] = Matrix([expr7, expr2]) def compute_zeta(robo, symo, j, gamma, jaj, zeta): @@ -126,6 +127,7 @@ def compute_link_accel(robo, symo, j, jTant, zeta, grandVp): 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 compute_reaction_wrench( robo, symo, j, grandVp, composite_inertia, composite_beta, react_wrench From 6ab40808772e56e0248282b6afb462047a368ff7 Mon Sep 17 00:00:00 2001 From: aravind Date: Thu, 12 Jun 2014 18:20:56 +0200 Subject: [PATCH 150/273] Add base acceleration computation --- pysymoro/fldyn.py | 38 ++++++++++++++++++++++++++++---------- 1 file changed, 28 insertions(+), 10 deletions(-) diff --git a/pysymoro/fldyn.py b/pysymoro/fldyn.py index 895721a..4bbcc97 100644 --- a/pysymoro/fldyn.py +++ b/pysymoro/fldyn.py @@ -1,3 +1,5 @@ +# -*- coding: utf-8 -*- + import sympy from sympy import Matrix @@ -74,7 +76,7 @@ def compute_zeta(robo, symo, j, gamma, jaj, zeta): def replace_composite_terms( - grandJ, beta, j, composite_inertia, composite_beta + symo, grandJ, beta, j, composite_inertia, composite_beta ): """ Replace composite inertia and beta (internal function). @@ -100,7 +102,7 @@ def compute_composite_terms( """ i = robo.ant[j] expr1 = jTant[j].transpose() * composite_inertia[j] - expr1 = symo.mat_replace(expr1, 'GX' 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] @@ -110,7 +112,7 @@ def compute_composite_terms( composite_inertia[i] = composite_inertia[i] + expr2 composite_beta[i] = composite_beta[i] - expr4 + expr3 replace_composite_terms( - composite_inertia, composite_beta, i, + symo, composite_inertia, composite_beta, i, composite_inertia, composite_beta ) @@ -123,11 +125,28 @@ def compute_link_accel(robo, symo, j, jTant, zeta, grandVp): grandVp is the output parameter """ i = robo.ant[j] - grandVp[j] = (jTant * grandVp[i]) + zeta[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 compute_base_accel( + robo, symo, composite_inertia, composite_beta, grandVp +): + """ + Compute base acceleration (internal function). + + Note: + grandVp is the output parameter + """ + if robo.is_floating: + grandVp[0] = composite_inertia[0].inv() * composite_beta[0] + else: + grandVp[0] = Matrix([robo.vdot0 - robo.G, robo.w0]) + grandVp[0][:3, 0] = symo.mat_replace(grandVp[0][:3, 0], 'VP', 0) + grandVp[0][3:, 0] = symo.mat_replace(grandVp[0][3:, 0], 'WP', 0) + + def compute_reaction_wrench( robo, symo, j, grandVp, composite_inertia, composite_beta, react_wrench @@ -145,7 +164,7 @@ def compute_reaction_wrench( react_wrench[j][3:, 0] = symo.mat_replace(wrench[3:, 0], 'N', j) -def compute_torque(robo, symo, j, jaj, react_wrench): +def compute_torque(robo, symo, j, jaj, react_wrench, torque): """ Compute torque (internal function). @@ -189,9 +208,6 @@ def fl_inverse_dynamic_model(robo): #H_inv = ParamsInit.init_scalar(robo) #juj = ParamsInit.init_vec(robo, 6) # Jj*aj / Hj #Tau = ParamsInit.init_scalar(robo) - # base acceleration initialisation - # TODO: move to second forward recursion - grandVp[0] = Matrix([robo.vdot0 - robo.G, robo.w0]) # init transformation antRj, antPj = compute_rot_trans(robo, symo) # first forward recursion @@ -224,12 +240,14 @@ def fl_inverse_dynamic_model(robo): # compute 0^beta_0 compute_beta(robo, symo, j, w, beta) replace_composite_terms( - grandJ, beta, j, composite_inertia, composite_beta + 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: - # TODO: compute base acceleration + compute_base_accel( + robo, symo, composite_inertia, composite_beta, grandVp + ) continue # compute i^I_i^c and i^beta_i^c compute_composite_terms( From 6d8a0a16c0cee1db8cc62b4b1235c7836ab118b6 Mon Sep 17 00:00:00 2001 From: aravind Date: Wed, 18 Jun 2014 02:06:15 +0200 Subject: [PATCH 151/273] Fix computation (equation) mistakes Fix computation of base acceleration to correctly remove the gravity effect. Fix computation of link acceleration to use the correct transformation (screw) matrix. Fix computation of `composite_beta` by using the correct equation (this was a mistake in the paper). --- pysymoro/dynmodel.py | 10 +++++----- symoroutils/samplerobots.py | 8 ++++---- 2 files changed, 9 insertions(+), 9 deletions(-) diff --git a/pysymoro/dynmodel.py b/pysymoro/dynmodel.py index 2444d8d..28daf52 100644 --- a/pysymoro/dynmodel.py +++ b/pysymoro/dynmodel.py @@ -229,7 +229,7 @@ def _compute_link_acceleration(model, robo, j, i): """ j_vdot_j = Screw() # local variables - j_s_i = robo.geos[j].tmat.s_j_wrt_i + 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 @@ -379,7 +379,7 @@ def _compute_composite_beta(model, robo, j, i): 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) + \ + 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 @@ -689,9 +689,9 @@ def _compute_base_acceleration(model, robo): # 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_no_gravity = copy.copy(o_vdot_o) - # compute base acceleration taking gravity into account - o_vdot_o.val = o_vdot_o.val + gravity.val + 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 diff --git a/symoroutils/samplerobots.py b/symoroutils/samplerobots.py index 0077c53..8ebf7fd 100644 --- a/symoroutils/samplerobots.py +++ b/symoroutils/samplerobots.py @@ -61,7 +61,7 @@ def planar2r(): robo.b = [0, 0, 0, 0] robo.alpha = [0, 0, 0, 0] robo.d = [0, 0, var('L2'), var('L3')] - robo.theta = [0, var('th1'), var('th2'), 0] + robo.theta = [0, var('q1'), var('q2'), 0] robo.r = [0, 0, 0, 0] robo.num = range(0, 3) robo.Nex = [zeros(3, 1) for i in robo.num] @@ -80,9 +80,9 @@ def planar2r(): robo.wdot0 = zeros(3, 1) robo.v0 = zeros(3, 1) robo.vdot0 = zeros(3, 1) - robo.q = var('O, th1, th2') - robo.qdot = var('O, th1d, th2d') - robo.qddot = var('O, th1dd, th2dd') + robo.q = var('O, q1, q2') + robo.qdot = var('O, QP1, QP2') + robo.qddot = var('O, QDP1, QDP2') return robo From 3f16d5dd1bd4b3408c92e3faa1ca0e451be3ad38 Mon Sep 17 00:00:00 2001 From: aravind Date: Fri, 20 Jun 2014 14:17:11 +0200 Subject: [PATCH 152/273] Fix `composite_beta` computation Fix bug in `composite_beta` computation due to wrong sign usage. --- pysymoro/fldyn.py | 4 +++- symoroui/layout.py | 2 ++ 2 files changed, 5 insertions(+), 1 deletion(-) diff --git a/pysymoro/fldyn.py b/pysymoro/fldyn.py index 4bbcc97..46718f6 100644 --- a/pysymoro/fldyn.py +++ b/pysymoro/fldyn.py @@ -42,6 +42,7 @@ def compute_beta(robo, symo, j, w, beta): 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): @@ -62,6 +63,7 @@ def compute_gamma(robo, symo, j, antRj, antPj, w, wi, gamma): 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): @@ -110,7 +112,7 @@ def compute_composite_terms( 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 + composite_beta[i] = composite_beta[i] + expr4 - expr3 replace_composite_terms( symo, composite_inertia, composite_beta, i, composite_inertia, composite_beta diff --git a/symoroui/layout.py b/symoroui/layout.py index 3ec79b5..295c413 100644 --- a/symoroui/layout.py +++ b/symoroui/layout.py @@ -17,6 +17,7 @@ from pysymoro import geometry from pysymoro import kinematics from pysymoro import dynamics +from pysymoro import fldyn from pysymoro import invgeom from symoroutils import parfile from symoroutils import filemgr @@ -776,6 +777,7 @@ def OnJpqp(self, event): def OnInverseDynamic(self, event): dynamics.inverse_dynamic_NE(self.robo) + fldyn.fl_inverse_dynamic_model(self.robo) self.model_success('idm') def OnInertiaMatrix(self, event): From a70f5bab9fc3e28fa4f72de146daff86e94a1b65 Mon Sep 17 00:00:00 2001 From: aravind Date: Sat, 21 Jun 2014 08:46:38 +0200 Subject: [PATCH 153/273] Replace with efficient composite terms computation Replace computation of `composite_inertia` and `composite_beta` with efficient methods. For `composite_inertia` calculation, method in Appendix 8 of Khalil and Dombre is used. For `composite_beta` calculation, the multiplication order of matrices are chosen to have the least number of operations. --- pysymoro/fldyn.py | 68 ++++++++++++++++++++++++++++++++++++++++------- 1 file changed, 59 insertions(+), 9 deletions(-) diff --git a/pysymoro/fldyn.py b/pysymoro/fldyn.py index 46718f6..67d8619 100644 --- a/pysymoro/fldyn.py +++ b/pysymoro/fldyn.py @@ -13,13 +13,13 @@ from symoroutils.paramsinit import ParamsInit -def inertia_spatial(J, MS, M): +def inertia_spatial(inertia, ms_tensor, mass): """ Compute spatial inertia matrix (internal function). """ return Matrix([ - (M * sympy.eye(3)).row_join(tools.skew(MS).T), - tools.skew(MS).row_join(J) + (mass * sympy.eye(3)).row_join(tools.skew(ms_tensor).transpose()), + tools.skew(ms_tensor).row_join(inertia) ]) @@ -77,6 +77,48 @@ def compute_zeta(robo, symo, j, gamma, jaj, zeta): 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 +): + 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 ): @@ -113,10 +155,6 @@ def compute_composite_terms( expr4 = symo.mat_replace(expr4, 'SBE', j) composite_inertia[i] = composite_inertia[i] + expr2 composite_beta[i] = composite_beta[i] + expr4 - expr3 - replace_composite_terms( - symo, composite_inertia, composite_beta, i, - composite_inertia, composite_beta - ) def compute_link_accel(robo, symo, j, jTant, zeta, grandVp): @@ -203,6 +241,7 @@ def fl_inverse_dynamic_model(robo): 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) @@ -252,8 +291,19 @@ def fl_inverse_dynamic_model(robo): ) continue # compute i^I_i^c and i^beta_i^c - compute_composite_terms( - robo, symo, j, jTant, zeta, + #compute_composite_terms( + # robo, symo, j, jTant, zeta, + # composite_inertia, composite_beta + #) + 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 ) # second forward recursion From 59e7d95d7dbb86010f8f07aa707f3b1af80b542b Mon Sep 17 00:00:00 2001 From: Bogdan Date: Wed, 25 Jun 2014 22:46:35 +0200 Subject: [PATCH 154/273] small kinematics fix --- pysymoro/kinematics.py | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/pysymoro/kinematics.py b/pysymoro/kinematics.py index b503d6d..f1a0f6e 100644 --- a/pysymoro/kinematics.py +++ b/pysymoro/kinematics.py @@ -80,7 +80,7 @@ def _jac(robo, symo, n, i, j, chain=None, forced=False, trig_subs=False): """ # 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) print chain @@ -109,8 +109,8 @@ 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) From acb9c0e7821155781f7600f2be856167d5a501f6 Mon Sep 17 00:00:00 2001 From: aravind Date: Thu, 3 Jul 2014 15:40:05 +0200 Subject: [PATCH 155/273] Add `LICENCE` --- LICENCE | 22 ++++++++++++++++++++++ 1 file changed, 22 insertions(+) create mode 100644 LICENCE 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. From c5c2dea23124dac3cd552dd86e0398b0166f45bb Mon Sep 17 00:00:00 2001 From: aravind Date: Thu, 3 Jul 2014 16:10:28 +0200 Subject: [PATCH 156/273] Update README --- README.md | 34 ++++++++++++++++++++++++++-------- 1 file changed, 26 insertions(+), 8 deletions(-) diff --git a/README.md b/README.md index e3c38c1..35c9f79 100644 --- a/README.md +++ b/README.md @@ -1,27 +1,45 @@ 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. - -For more details on SYMORO, please see http://www.irccyn.ec-nantes.fr/spip.php?article601&lang=en +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. Requirements ------------ + python (>= 2.7,    3.* is not supported) -+ sympy (>= 0.7.3) ++ sympy (== 0.7.3) + numpy (>= 1.6.1) -+ wxPython (>= 2.8.12.1) ++ wxPython (>= 2.8.12) + OpenGL (>= 3.0.1b2) -Installation and Usage ----------------------- +Getting Started +--------------- + Run `python setup.py develop` the very first time so that python-egg-info is set up correctly and the relevant folders are included in the PATH. + To use the software, run `symoro-bin`. This script is present in the `bin/` folder. ++ 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/ +[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 + From c4e67f004ad8c45fa9bbc9847137b593acf2f78f Mon Sep 17 00:00:00 2001 From: aravind Date: Thu, 3 Jul 2014 17:06:25 +0200 Subject: [PATCH 157/273] Add LICENCE stub to all source code --- bin/symoro-bin.py | 4 ++++ pysymoro/dynamics.py | 6 +++++- pysymoro/geometry.py | 7 ++++++- pysymoro/invgeom.py | 7 ++++++- pysymoro/kinematics.py | 6 +++++- pysymoro/robot.py | 9 +++++---- pysymoro/tests/test.py | 5 +++++ pysymoro/tests/test_invgeom.py | 4 ++++ symoroui/definition.py | 5 ++++- symoroui/geometry.py | 5 ++++- symoroui/kinematics.py | 5 ++++- symoroui/labels.py | 5 ++++- symoroui/layout.py | 5 ++++- symoroutils/filemgr.py | 4 ++++ symoroutils/paramsinit.py | 4 ++++ symoroutils/parfile.py | 5 ++++- symoroutils/samplerobots.py | 4 ++++ symoroutils/symbolmgr.py | 4 ++++ symoroutils/tests/test_filemgr.py | 4 ++++ symoroutils/tests/test_parfile.py | 4 ++++ symoroutils/tests/test_symbolmgr.py | 4 ++++ symoroutils/tools.py | 4 ++++ symoroviz/graphics.py | 5 ++++- symoroviz/objects.py | 5 ++++- symoroviz/primitives.py | 5 ++++- 25 files changed, 108 insertions(+), 17 deletions(-) diff --git a/bin/symoro-bin.py b/bin/symoro-bin.py index 7909b26..2934322 100644 --- a/bin/symoro-bin.py +++ b/bin/symoro-bin.py @@ -2,6 +2,10 @@ # -*- 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. """ diff --git a/pysymoro/dynamics.py b/pysymoro/dynamics.py index 845cb66..0868f38 100644 --- a/pysymoro/dynamics.py +++ b/pysymoro/dynamics.py @@ -1,6 +1,10 @@ -#!/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 module of SYMORO package provides symbolic modeling of robot dynamics. diff --git a/pysymoro/geometry.py b/pysymoro/geometry.py index eb31dd3..50f668c 100644 --- a/pysymoro/geometry.py +++ b/pysymoro/geometry.py @@ -1,10 +1,15 @@ -#!/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 module of SYMORO package computes the geometric models. """ + from sympy import Matrix, zeros, eye, sin, cos from copy import copy diff --git a/pysymoro/invgeom.py b/pysymoro/invgeom.py index f3f34f3..a976ee7 100644 --- a/pysymoro/invgeom.py +++ b/pysymoro/invgeom.py @@ -1,11 +1,16 @@ -#!/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 module of SYMORO package provides symbolic solutions for inverse geompetric problem. """ + from heapq import heapify, heappop from sympy import var, sin, cos, eye, atan2, sqrt, pi from sympy import Matrix, Symbol, Expr, trigsimp diff --git a/pysymoro/kinematics.py b/pysymoro/kinematics.py index f091f4f..b244f5a 100644 --- a/pysymoro/kinematics.py +++ b/pysymoro/kinematics.py @@ -1,6 +1,10 @@ -#!/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 module of SYMORO package computes the kinematic models. """ diff --git a/pysymoro/robot.py b/pysymoro/robot.py index e0f4d52..8305ff3 100644 --- a/pysymoro/robot.py +++ b/pysymoro/robot.py @@ -1,12 +1,13 @@ # -*- 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. - -The core symbolic library is sympy. - -ECN - ARIA1 2013 """ diff --git a/pysymoro/tests/test.py b/pysymoro/tests/test.py index ea5c707..4e9f88d 100644 --- a/pysymoro/tests/test.py +++ b/pysymoro/tests/test.py @@ -1,6 +1,11 @@ #!/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 """ diff --git a/pysymoro/tests/test_invgeom.py b/pysymoro/tests/test_invgeom.py index df3ce1e..04b0066 100644 --- a/pysymoro/tests/test_invgeom.py +++ b/pysymoro/tests/test_invgeom.py @@ -2,6 +2,10 @@ # -*- 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.""" diff --git a/symoroui/definition.py b/symoroui/definition.py index ed0f5de..9361f33 100644 --- a/symoroui/definition.py +++ b/symoroui/definition.py @@ -1,7 +1,10 @@ -#!/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 module creates the dialog box to define a new robot and visualisation parameters. diff --git a/symoroui/geometry.py b/symoroui/geometry.py index 4a29403..58551e8 100644 --- a/symoroui/geometry.py +++ b/symoroui/geometry.py @@ -1,7 +1,10 @@ -#!/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 module creates the dialog boxes to specify parameters for geometric model calculations. diff --git a/symoroui/kinematics.py b/symoroui/kinematics.py index ef8c8f2..cda0771 100644 --- a/symoroui/kinematics.py +++ b/symoroui/kinematics.py @@ -1,7 +1,10 @@ -#!/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 module creates the dialog box for differrent kinematic model parameters. diff --git a/symoroui/labels.py b/symoroui/labels.py index 8797a68..b724e31 100644 --- a/symoroui/labels.py +++ b/symoroui/labels.py @@ -1,7 +1,10 @@ -#!/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 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 diff --git a/symoroui/layout.py b/symoroui/layout.py index 85f4a88..7841537 100644 --- a/symoroui/layout.py +++ b/symoroui/layout.py @@ -1,7 +1,10 @@ -#!/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 module creates the main window user interface and draws the interface on the screen for the SYMORO package. diff --git a/symoroutils/filemgr.py b/symoroutils/filemgr.py index 1f5f987..c89e3e4 100644 --- a/symoroutils/filemgr.py +++ b/symoroutils/filemgr.py @@ -1,6 +1,10 @@ # -*- 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.""" diff --git a/symoroutils/paramsinit.py b/symoroutils/paramsinit.py index 2a5a2f7..a0024a4 100644 --- a/symoroutils/paramsinit.py +++ b/symoroutils/paramsinit.py @@ -1,6 +1,10 @@ # -*- 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. diff --git a/symoroutils/parfile.py b/symoroutils/parfile.py index cd2abd7..54edba6 100644 --- a/symoroutils/parfile.py +++ b/symoroutils/parfile.py @@ -1,7 +1,10 @@ -#!/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 module performs writing and reading data into PAR file. PAR is a plain text file used to represent the different parameters of the robot. diff --git a/symoroutils/samplerobots.py b/symoroutils/samplerobots.py index 7529100..f640099 100644 --- a/symoroutils/samplerobots.py +++ b/symoroutils/samplerobots.py @@ -1,6 +1,10 @@ # -*- 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. diff --git a/symoroutils/symbolmgr.py b/symoroutils/symbolmgr.py index 6903ee8..47df110 100644 --- a/symoroutils/symbolmgr.py +++ b/symoroutils/symbolmgr.py @@ -1,6 +1,10 @@ # -*- 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 diff --git a/symoroutils/tests/test_filemgr.py b/symoroutils/tests/test_filemgr.py index 67327d3..1aff510 100644 --- a/symoroutils/tests/test_filemgr.py +++ b/symoroutils/tests/test_filemgr.py @@ -2,6 +2,10 @@ # -*- 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.""" diff --git a/symoroutils/tests/test_parfile.py b/symoroutils/tests/test_parfile.py index 744f969..d0dc25e 100644 --- a/symoroutils/tests/test_parfile.py +++ b/symoroutils/tests/test_parfile.py @@ -2,6 +2,10 @@ # -*- 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.""" diff --git a/symoroutils/tests/test_symbolmgr.py b/symoroutils/tests/test_symbolmgr.py index ab86513..d869d04 100644 --- a/symoroutils/tests/test_symbolmgr.py +++ b/symoroutils/tests/test_symbolmgr.py @@ -2,6 +2,10 @@ # -*- 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.""" diff --git a/symoroutils/tools.py b/symoroutils/tools.py index 85263ed..5de2327 100644 --- a/symoroutils/tools.py +++ b/symoroutils/tools.py @@ -1,6 +1,10 @@ # -*- 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. diff --git a/symoroviz/graphics.py b/symoroviz/graphics.py index 650db7f..2d0e127 100644 --- a/symoroviz/graphics.py +++ b/symoroviz/graphics.py @@ -1,7 +1,10 @@ -#!/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. + + from math import atan2 import wx diff --git a/symoroviz/objects.py b/symoroviz/objects.py index 2185691..817d3b4 100644 --- a/symoroviz/objects.py +++ b/symoroviz/objects.py @@ -1,7 +1,10 @@ -#!/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. + + import OpenGL.GL as gL from numpy import degrees, identity, array diff --git a/symoroviz/primitives.py b/symoroviz/primitives.py index abcbad6..58ec7a2 100644 --- a/symoroviz/primitives.py +++ b/symoroviz/primitives.py @@ -1,7 +1,10 @@ -#!/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. + + from itertools import product from numpy import sin, cos, pi From be92c9704ceedf14c4a4c54277a5c575bee5d99c Mon Sep 17 00:00:00 2001 From: aravind Date: Mon, 7 Jul 2014 12:05:00 +0200 Subject: [PATCH 158/273] Update README --- README.md | 7 ++----- 1 file changed, 2 insertions(+), 5 deletions(-) diff --git a/README.md b/README.md index 35c9f79..e7d8d23 100644 --- a/README.md +++ b/README.md @@ -19,11 +19,7 @@ Requirements Getting Started --------------- -+ Run `python setup.py develop` the very first time so that - python-egg-info is set up correctly and the relevant folders are - included in the PATH. -+ To use the software, run `symoro-bin`. This script is present in - the `bin/` folder. ++ For setting up SYMORO, see [Setup][lk:setup]. + If you have any queries, contact [Aravind][el:aravind]. @@ -38,6 +34,7 @@ See [Contributors][lk:contributors]. [lk:irccyn]: http://www.irccyn.ec-nantes.fr/ +[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 From ad9bdf37959a30829a5ad4d3ff9de5c62b50c0a0 Mon Sep 17 00:00:00 2001 From: aravind Date: Tue, 8 Jul 2014 11:59:03 +0200 Subject: [PATCH 159/273] Minor modifications --- pysymoro/kinematics.py | 1 - pysymoro/tests/test_dynmodel.py | 1 - symoroutils/symbolmgr.py | 2 +- symoroviz/graphics.py | 1 - 4 files changed, 1 insertion(+), 4 deletions(-) diff --git a/pysymoro/kinematics.py b/pysymoro/kinematics.py index f54d530..e7437a6 100644 --- a/pysymoro/kinematics.py +++ b/pysymoro/kinematics.py @@ -87,7 +87,6 @@ def _jac(robo, symo, n, i, j, chain=None, forced=False, trig_subs=False): J_col_list = [] if chain is None: chain = robo.chain(n) - print chain chain.reverse() # chain_ext = chain + [robo.ant[min(chain)]] # if not i in chain_ext: diff --git a/pysymoro/tests/test_dynmodel.py b/pysymoro/tests/test_dynmodel.py index 694d815..3302c46 100644 --- a/pysymoro/tests/test_dynmodel.py +++ b/pysymoro/tests/test_dynmodel.py @@ -38,7 +38,6 @@ def planar2r(): 2: {'gravity': var('G3')}, } robo.update_params('misc', params) - print(robo) return robo diff --git a/symoroutils/symbolmgr.py b/symoroutils/symbolmgr.py index bc5fe64..722d1d5 100644 --- a/symoroutils/symbolmgr.py +++ b/symoroutils/symbolmgr.py @@ -412,7 +412,7 @@ def write_line(self, line=''): Data to be written. If empty, it adds an empty line """ if self.file_out == 'disp': - print line + print(line) elif self.file_out is not None: self.file_out.write(str(line) + '\n') diff --git a/symoroviz/graphics.py b/symoroviz/graphics.py index da9d7ba..da23370 100644 --- a/symoroviz/graphics.py +++ b/symoroviz/graphics.py @@ -79,7 +79,6 @@ def assign_mono_scale(self): if minv == inf: minv = 1. self.length = 0.4 * minv - #print self.length for jnt in self.jnt_objs: if isinstance(jnt, PrismaticJoint): jnt.r = 3.5 * self.length From b5e63633d3dbae729287d0751018111e61c1addf Mon Sep 17 00:00:00 2001 From: aravind Date: Tue, 8 Jul 2014 16:30:28 +0200 Subject: [PATCH 160/273] Add translation of robot in visualisation --- symoroviz/graphics.py | 146 +++++++++++++++++++----------------------- 1 file changed, 67 insertions(+), 79 deletions(-) diff --git a/symoroviz/graphics.py b/symoroviz/graphics.py index da23370..cf7702a 100644 --- a/symoroviz/graphics.py +++ b/symoroviz/graphics.py @@ -24,15 +24,18 @@ from symoroutils import symbolmgr from symoroutils import tools -from objects import Frame, RevoluteJoint, FixedJoint, PrismaticJoint +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) @@ -49,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 = {} @@ -98,7 +101,6 @@ 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: @@ -112,8 +114,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): @@ -152,17 +156,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) @@ -217,8 +229,9 @@ def solve(self): def generate_loop_fcn(self): 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) + 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] @@ -242,7 +255,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() @@ -346,7 +360,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) @@ -359,40 +372,38 @@ 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): + 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.canvas = VizGlCanvas( + self, robo, self.params_dict, size=(600, 600) + ) self.p = wx.lib.scrolledpanel.ScrolledPanel(self, -1) self.p.SetMinSize((350,600)) 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): @@ -402,90 +413,78 @@ def init_ui(self): 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=(3, 0), flag=wx.ALIGN_CENTER) - btnReset = wx.Button(self.p, label="Reset All") btnReset.Bind(wx.EVT_BUTTON, self.OnResetJoints) gridControl.Add(btnReset, pos=(5, 0), flag=wx.ALIGN_CENTER) - btnRandom = wx.Button(self.p, label="Random") btnRandom.Bind(wx.EVT_BUTTON, self.OnFindRandom) gridControl.Add(btnRandom, pos=(6, 0), flag=wx.ALIGN_CENTER) - self.spin_ctrls = {} gridJnts = 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.) + 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.0 + ) 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 - + gridJnts.Add( + s, pos=(p_index, 1), flag=wx.ALIGN_CENTER_VERTICAL + ) + p_index = p_index + 1 if self.robo.structure == tools.CLOSED_LOOP: choise_list = ['Break Loops', 'Make Loops'] - self.radioBox = wx.RadioBox(self.p, choices=choise_list, - style=wx.RA_SPECIFY_ROWS) + 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=(7, 0), flag=wx.ALIGN_CENTER) - + gridControl.Add( + self.radioBox, pos=(7, 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=(4, 0), flag=wx.ALIGN_CENTER) - + gridControl.Add( + self.check_list, pos=(4, 0), flag=wx.ALIGN_CENTER + ) 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) gridControl.Add(lbl_length, pos=(1, 0), flag=wx.ALIGN_CENTER) gridControl.Add(self.jnt_slider, pos=(2, 0), flag=wx.ALIGN_CENTER) - - q_box = wx.StaticBoxSizer(wx.StaticBox(self.p, label='Joint variables')) + 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) - top_sizer.Add(gridControl, 0, wx.ALL, 10) top_sizer.AddSpacer(10) top_sizer.Add(ver_sizer, 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.SetSizer(top_sizer) def OnChangeRepresentation(self, evt): @@ -561,14 +560,3 @@ def OnSliderChanged(self, _): self.canvas.change_lengths(self.jnt_slider.Value/100.) -if __name__ == '__main__': - app = wx.PySimpleApp() - from pysymoro import robot - robo = samplerobots.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() From a53a961c5dc5ec176cda4bdea4f4c533bd4af22d Mon Sep 17 00:00:00 2001 From: aravind Date: Tue, 8 Jul 2014 16:47:21 +0200 Subject: [PATCH 161/273] Add help text in visualisation Add help text and `Default Position` button in Visualisation. --- symoroviz/graphics.py | 26 +++++++++++++++++++++++++- 1 file changed, 25 insertions(+), 1 deletion(-) diff --git a/symoroviz/graphics.py b/symoroviz/graphics.py index cf7702a..6f10660 100644 --- a/symoroviz/graphics.py +++ b/symoroviz/graphics.py @@ -396,7 +396,7 @@ def __init__( self, robo, self.params_dict, size=(600, 600) ) self.p = wx.lib.scrolledpanel.ScrolledPanel(self, -1) - self.p.SetMinSize((350,600)) + self.p.SetMinSize((350,650)) self.init_ui() self.p.SetupScrolling() self.update_spin_controls() @@ -423,6 +423,27 @@ def init_ui(self): btnRandom = wx.Button(self.p, label="Random") btnRandom.Bind(wx.EVT_BUTTON, self.OnFindRandom) gridControl.Add(btnRandom, pos=(6, 0), flag=wx.ALIGN_CENTER) + btnHome = wx.Button(self.p, label="Default Position") + btnHome.Bind(wx.EVT_BUTTON, self.OnHomePosition) + gridControl.Add(btnHome, pos=(7,0), flag=wx.ALIGN_CENTER) + move_help = """ + To Translate: + Left button + + move mouse + + To Rotate: + Right button + + move mouse + + To Zoom: + Left and Right + button + move + mouse + """ + gridControl.Add( + wx.StaticText(self.p, label=move_help), + pos=(8,0), flag=wx.ALIGN_LEFT + ) self.spin_ctrls = {} gridJnts = wx.GridBagSizer(hgap=10, vgap=10) p_index = 0 @@ -531,6 +552,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 """ From 8777b68e598c5fa6ff21acc474306895ae58337a Mon Sep 17 00:00:00 2001 From: aravind Date: Wed, 9 Jul 2014 16:12:17 +0200 Subject: [PATCH 162/273] Integrate IDyM for floating base with UI Integrate Inverse Dynamic Model computation for robots with floating base with the UI. Rename the function that performs the recursive Newton-Euler algorithm to `composite_newton_euler()`. Add `compute_idym()` and `compute_ddym()` methods to `Robot` class. --- pysymoro/fldyn.py | 8 +------- pysymoro/kinematics.py | 27 --------------------------- pysymoro/robot.py | 36 +++++++++++++++++++++++++++++++++--- symoroui/layout.py | 3 +-- 4 files changed, 35 insertions(+), 39 deletions(-) diff --git a/pysymoro/fldyn.py b/pysymoro/fldyn.py index 67d8619..9f4543c 100644 --- a/pysymoro/fldyn.py +++ b/pysymoro/fldyn.py @@ -221,11 +221,7 @@ def compute_torque(robo, symo, j, jaj, react_wrench, torque): torque[j] = symo.replace(tau_total, symbl_name, forced=True) -def fl_inverse_dynamic_model(robo): - symo = symbolmgr.SymbolManager() - symo.file_open(robo, 'flidm') - title = 'Inverse Dynamic Model - NE' - symo.write_params_table(robo, title, inert=True, dynam=True) +def composite_newton_euler(robo, symo): # antecedent angular velocity, projected into jth frame # j^omega_i wi = ParamsInit.init_vec(robo) @@ -317,7 +313,5 @@ def fl_inverse_dynamic_model(robo): ) # compute torque compute_torque(robo, symo, j, jaj, react_wrench, torque) - symo.file_close() - return symo diff --git a/pysymoro/kinematics.py b/pysymoro/kinematics.py index e7437a6..ab4a604 100644 --- a/pysymoro/kinematics.py +++ b/pysymoro/kinematics.py @@ -16,7 +16,6 @@ from pysymoro.geometry import compute_rot_trans, Z_AXIS from symoroutils import symbolmgr from symoroutils import tools -from symoroutils import samplerobots from symoroutils.paramsinit import ParamsInit @@ -340,29 +339,3 @@ def kinematic_constraints(robo): return symo -#symo = symbolmgr.SymbolManager() -#from symoro import Robot -#from symoroutils import symbolmgr -#kinematic_constraints(samplerobots.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(samplerobots.rx90(), symo, 2, 5, 5) -## -#def b(): -# symo = symbolmgr.SymbolManager() -# print _jac_inv(samplerobots.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/robot.py b/pysymoro/robot.py index da54500..d979973 100644 --- a/pysymoro/robot.py +++ b/pysymoro/robot.py @@ -20,15 +20,15 @@ from sympy import Symbol, Matrix, Expr, Integer from sympy import Mul, Add, factor, zeros, var, sympify, eye +from pysymoro import dynamics +from pysymoro import fldyn 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 -#TODO: write consistency check -#TODO: Ask about QP QDP file writing. Number of joints is different -#from number of links class Robot(object): """Container of the robot parametric description. Responsible for low-level geometric transformation @@ -206,6 +206,36 @@ def get_q(self, 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 Algorith' + symo.write_params_table(self, title, inert=True, dynam=True) + if 1 in self.eta: + # with flexible joints + pass + elif self.is_floating: + # with rigid joints and floating base + fldyn.composite_newton_euler(self, symo) + else: + # with rigid joints and fixed base + dynamics.Newton_Euler(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. Also choose the Newton-Euler + algorithm based on the robot type. + """ + pass + @property def q_vec(self): """Generates vector of joint variables diff --git a/symoroui/layout.py b/symoroui/layout.py index a59acf8..4e9dae3 100644 --- a/symoroui/layout.py +++ b/symoroui/layout.py @@ -778,8 +778,7 @@ def OnJpqp(self, event): self.model_success('jpqp') def OnInverseDynamic(self, event): - dynamics.inverse_dynamic_NE(self.robo) - fldyn.fl_inverse_dynamic_model(self.robo) + self.robo.compute_idym() self.model_success('idm') def OnInertiaMatrix(self, event): From 54326983ca5d651c377e82e59ebe2d79ade2ed16 Mon Sep 17 00:00:00 2001 From: aravind Date: Wed, 9 Jul 2014 16:20:38 +0200 Subject: [PATCH 163/273] Rename `Newton_Euler()` to `default_newton_euler()` Rename `Newton_Euler()` to `default_newton_euler()` Update all function calls accordingly. --- pysymoro/dynamics.py | 10 ++++++---- pysymoro/robot.py | 2 +- 2 files changed, 7 insertions(+), 5 deletions(-) diff --git a/pysymoro/dynamics.py b/pysymoro/dynamics.py index dc1b022..b9dac6f 100644 --- a/pysymoro/dynamics.py +++ b/pysymoro/dynamics.py @@ -37,7 +37,7 @@ 'ZZR', 'MXR', 'MYR', 'MZR', 'MR') -def Newton_Euler(robo, symo): +def default_newton_euler(robo, symo): """Internal function. Computes Inverse Dynamic Model using Newton-Euler formulation @@ -253,7 +253,9 @@ def inertia_matrix(robo): def inverse_dynamic_NE(robo): - """Computes Inverse Dynamic Model using + """ + OLD FUNCTION. NOT TO BE USED. + Computes Inverse Dynamic Model using Newton-Euler formulation Parameters @@ -270,7 +272,7 @@ def inverse_dynamic_NE(robo): 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) + default_newton_euler(robo, symo) symo.file_close() return symo @@ -295,7 +297,7 @@ def pseudo_force_NE(robo): 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) + default_newton_euler(robo_pseudo, symo) symo.file_close() return symo diff --git a/pysymoro/robot.py b/pysymoro/robot.py index d979973..32b9c41 100644 --- a/pysymoro/robot.py +++ b/pysymoro/robot.py @@ -224,7 +224,7 @@ def compute_idym(self): fldyn.composite_newton_euler(self, symo) else: # with rigid joints and fixed base - dynamics.Newton_Euler(self, symo) + dynamics.default_newton_euler(self, symo) symo.file_close() return symo From 2c124d506a1be2333564b5388b20bb9354cb9160 Mon Sep 17 00:00:00 2001 From: aravind Date: Wed, 9 Jul 2014 17:04:51 +0200 Subject: [PATCH 164/273] Add `compute_pseudotorques()` method to `Robot` class Add `compute_pseudotorques()` method to `Robot` class. Update calls from the UI accordingly. --- pysymoro/dynamics.py | 4 +++- pysymoro/robot.py | 30 ++++++++++++++++++++++++++---- symoroui/layout.py | 2 +- 3 files changed, 30 insertions(+), 6 deletions(-) diff --git a/pysymoro/dynamics.py b/pysymoro/dynamics.py index b9dac6f..c739e5d 100644 --- a/pysymoro/dynamics.py +++ b/pysymoro/dynamics.py @@ -278,7 +278,9 @@ def inverse_dynamic_NE(robo): def pseudo_force_NE(robo): - """Computes Coriolis, Centrifugal, Gravity, Friction and external + """ + OLD FUNCTION. NOT TO BE USED. + Computes Coriolis, Centrifugal, Gravity, Friction and external torques using Newton-Euler formulation Parameters diff --git a/pysymoro/robot.py b/pysymoro/robot.py index 32b9c41..695bc1e 100644 --- a/pysymoro/robot.py +++ b/pysymoro/robot.py @@ -13,7 +13,7 @@ import re import os -from copy import copy +import copy from itertools import combinations from sympy import sin, cos, sign, pi @@ -214,7 +214,7 @@ def compute_idym(self): """ symo = symbolmgr.SymbolManager() symo.file_open(self, 'idm') - title = 'Inverse dynamic model using Newton - Euler Algorith' + title = 'Inverse dynamic model using Newton - Euler Algorithm' symo.write_params_table(self, title, inert=True, dynam=True) if 1 in self.eta: # with flexible joints @@ -231,11 +231,33 @@ def compute_idym(self): def compute_ddym(self): """ Compute the Direct Dynamic Model of the robot using the - recursive Newton-Euler algorithm. Also choose the Newton-Euler - algorithm based on the robot type. + recursive Newton-Euler algorithm. """ pass + def compute_pseudotorques(self): + """ + Compute Coriolis, Centrifugal, Gravity, Friction and external + torques using Newton-Euler algortihm. + """ + pseudorobo = copy.deepcopy(self) + pseudorobo.qddot = zeros(pseudorobo.NL, 1) + symo = symbolmgr.SymbolManager() + symo.file_open(self, 'ccg') + title = 'Pseudo forces using Newton - Euler Algorithm' + symo.write_params_table(self, title, inert=True, dynam=True) + if 1 in self.eta: + # with flexible joints + pass + elif self.is_floating: + # with rigid joints and floating base + fldyn.composite_newton_euler(pseudorobo, symo) + else: + # with rigid joints and fixed base + dynamics.default_newton_euler(pseudorobo, symo) + symo.file_close() + return symo + @property def q_vec(self): """Generates vector of joint variables diff --git a/symoroui/layout.py b/symoroui/layout.py index 4e9dae3..7d22145 100644 --- a/symoroui/layout.py +++ b/symoroui/layout.py @@ -786,7 +786,7 @@ def OnInertiaMatrix(self, event): self.model_success('inm') def OnCentrCoriolGravTorq(self, event): - dynamics.pseudo_force_NE(self.robo) + self.robo.compute_pseudotorques() self.model_success('ccg') def OnDirectDynamicModel(self, event): From 8148564ba3f374308ddc1d9008de1d26b3de74e0 Mon Sep 17 00:00:00 2001 From: aravind Date: Thu, 10 Jul 2014 13:43:14 +0200 Subject: [PATCH 165/273] Start Direct Dynamic Modelling for floating base robots --- pysymoro/fldyn.py | 187 ++++++++++++++++++++++++++++++++++++++++++++-- 1 file changed, 180 insertions(+), 7 deletions(-) diff --git a/pysymoro/fldyn.py b/pysymoro/fldyn.py index 9f4543c..033bf9b 100644 --- a/pysymoro/fldyn.py +++ b/pysymoro/fldyn.py @@ -134,6 +134,21 @@ def replace_composite_terms( composite_beta[j] = symo.mat_replace(beta[j], 'VBE', j) +def replace_star_terms( + symo, grandJ, beta, j, star_inertia, star_beta +): + """ + Replace star inertia and beta (internal function). + + Note: + star_inertia are star_beta are the output parameters + """ + star_inertia[j] = symo.mat_replace( + grandJ[j], 'MJE', j, symmet=True + ) + star_beta[j] = symo.mat_replace(beta[j], 'VBE', j) + + def compute_composite_terms( robo, symo, j, jTant, zeta, composite_inertia, composite_beta @@ -157,6 +172,73 @@ def compute_composite_terms( composite_beta[i] = composite_beta[i] + expr4 - expr3 +def compute_hinv(robo, symo, j, jaj, star_inertia, inertia_jaj, h_inv): + """ + Note: + h_inv and inertia_jaj are the output parameters + """ + inertia_jaj[j] = star_inertia[j] * jaj[j] + inertia_jaj[j] = symo.mat_replace(inertia_jaj, 'JA', j) + h = jaj[j].dot(inertia_jaj[j]) + robo.IA[j] + h_inv[j] = 1 / h + h_inv[j] = symo.mat_replace(h_inv[j], 'JD', j) + + +def compute_tau(robo, symo, j, tau): + """ + Note: + tau is the output parameter + """ + if robo.sigma[j] == 2: + tau[j] = 0 + else: + joint_friction = robo.fric_s(j) + robo.fric_v(j) + tau[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, star_inertia, star_beta +): + """ + Note: + star_inertia and 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_inv[j] = 1 / (jaj[j].dot(inertia_jaj) + robo.IA[j]) + h_inv[j] = symo.mat_replace(h_inv[j], 'JD', j) + jah = ineria_jaj * h_inv[j] + jah = symo.mat_replace(jah, 'JU', j) + grandK = star_inertia[j] - (jah * inertia_jaj.transpose()) + grandK = symo.mat_replace(grandK, 'GK', j) + expr1 = grandK * gamma[j] + expr1 = symo.mat_replace(expr1, 'NG', j) + expr2 = expr1 + (jah * (tau[j] + jaj[j].dot(star_beta[j]))) + expr2 = symo.mat_replace(expr2, 'VS', j) + alpha = expr2 - star_beta[j] + alpha = symo.mat_replace(alpha, 'AP', j) + expr3 = jTant[j].transpose() * grandK + 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, gamma, tau, grandVp, + star_beta, star_inertia, qddot +): + """ + Note: + qddot is the output parameter + """ + pass + + def compute_link_accel(robo, symo, j, jTant, zeta, grandVp): """ Compute link acceleration (internal function). @@ -170,7 +252,7 @@ def compute_link_accel(robo, symo, j, jTant, zeta, grandVp): grandVp[j][3:, 0] = symo.mat_replace(grandVp[j][3:, 0], 'WP', j) -def compute_base_accel( +def compute_base_accel_composite( robo, symo, composite_inertia, composite_beta, grandVp ): """ @@ -241,10 +323,6 @@ def composite_newton_euler(robo, symo): grandVp = ParamsInit.init_vec(robo, 6) react_wrench = ParamsInit.init_vec(robo, 6) torque = ParamsInit.init_scalar(robo) - # for flexible joints - #H_inv = ParamsInit.init_scalar(robo) - #juj = ParamsInit.init_vec(robo, 6) # Jj*aj / Hj - #Tau = ParamsInit.init_scalar(robo) # init transformation antRj, antPj = compute_rot_trans(robo, symo) # first forward recursion @@ -267,7 +345,6 @@ def composite_newton_euler(robo, symo): # compute j^beta_j : external+coriolis+centrifugal wrench (6x1) compute_beta(robo, symo, j, w, beta) # compute j^zeta_j : relative acceleration (6x1) - # TODO: check joint flexibility compute_zeta(robo, symo, j, gamma, jaj, zeta) # first backward recursion - initialisation step for j in reversed(xrange(0, robo.NL)): @@ -282,7 +359,7 @@ def composite_newton_euler(robo, symo): # second backward recursion - compute composite term for j in reversed(xrange(0, robo.NL)): if j == 0: - compute_base_accel( + compute_base_accel_composite( robo, symo, composite_inertia, composite_beta, grandVp ) continue @@ -315,3 +392,99 @@ def composite_newton_euler(robo, symo): compute_torque(robo, symo, j, jaj, react_wrench, torque) +def fl_direct_dynamic_model(robo): + symo = symbolmgr.SymbolManager() + symo.file_open(robo, 'flddm') + title = 'Direct Dynamic Model - NE' + symo.write_params_table(robo, title, inert=True, dynam=True) + # 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) + alpha = ParamsInit.init_vec(robo, 6) + k_inertia = ParamsInit.init_vec(robo, 6) + h_inv = ParamsInit.init_scalar(robo) + tau = ParamsInit.init_scalar(robo) + star_inertia = ParamsInit.init_mat(robo, 6) + star_beta = ParamsInit.init_vec(robo, 6) + 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) + # 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_star_terms( + symo, grandJ, beta, j, star_inertia, star_beta + ) + # second backward recursion - compute star terms + for j in reversed(xrange(0, robo.NL)): + if j == 0: continue + compute_tau(robo, symo, j, tau) + compute_star_terms( + robo, symo, j, jaj, jTant, gamma, tau, + h_inv, star_inertia, star_beta + ) + replace_star_terms( + symo, star_inertia, star_beta, robo.ant[j], + star_inertia, star_beta + ) + # second forward recursion + for j in xrange(0, robo.NL): + if j == 0: + # compute 0^\dot{V}_0 : base acceleration + # for fixed base robots, the value returned is just the + # effect of gravity + compute_base_accel(robo, symo) + continue + # compute qddot_j : joint acceleration + compute_joint_accel( + robo, symo, j, jaj, jTant, h_inv, gamma, tau, grandVp, + star_beta, star_inertia + ) + # compute j^F_j : reaction wrench as a function of alpha + compute_reaction_wrench( + robo, symo, j, grandVp, + composite_inertia, composite_beta, react_wrench + ) + # compute j^zeta_j : relative acceleration (6x1) + compute_zeta(robo, symo, j, gamma, jaj, zeta) + # compute j^Vdot_j : link acceleration + compute_link_accel(robo, symo, j, jTant, zeta, grandVp) + symo.file_close() + return symo + + From 0c3ced465bb7eb9831fdce36dbb375de9584906f Mon Sep 17 00:00:00 2001 From: aravind Date: Thu, 10 Jul 2014 16:57:51 +0200 Subject: [PATCH 166/273] Complete Direct Dynamic Model for floating base robots --- pysymoro/fldyn.py | 103 +++++++++++++++++++++++++++++----------------- 1 file changed, 66 insertions(+), 37 deletions(-) diff --git a/pysymoro/fldyn.py b/pysymoro/fldyn.py index 033bf9b..02bf4ca 100644 --- a/pysymoro/fldyn.py +++ b/pysymoro/fldyn.py @@ -66,14 +66,16 @@ def compute_gamma(robo, symo, j, antRj, antPj, w, wi, gamma): gamma[j] = symo.mat_replace(gamma[j], 'GYACC', j) -def compute_zeta(robo, symo, j, gamma, jaj, zeta): +def compute_zeta(robo, symo, j, gamma, jaj, zeta, qddot=None): """ Compute relative acceleration (internal function). Note: zeta is the output parameter """ - expr = gamma[j] + (robo.qddot[j] * jaj[j]) + if qddot == None: + qddot = robo.qddot + expr = gamma[j] + (qddot[j] * jaj[j]) zeta[j] = symo.mat_replace(expr, 'ZETA', j) @@ -184,7 +186,7 @@ def compute_hinv(robo, symo, j, jaj, star_inertia, inertia_jaj, h_inv): h_inv[j] = symo.mat_replace(h_inv[j], 'JD', j) -def compute_tau(robo, symo, j, tau): +def compute_tau(robo, symo, j, jaj, star_beta, tau): """ Note: tau is the output parameter @@ -193,34 +195,34 @@ def compute_tau(robo, symo, j, tau): tau[j] = 0 else: joint_friction = robo.fric_s(j) + robo.fric_v(j) - tau[j] = robo.GAM[j] - joint_friction + 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, star_inertia, star_beta + h_inv, jah, star_inertia, star_beta ): """ Note: - star_inertia and star_beta are the output parameters + 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_inv[j] = 1 / (jaj[j].dot(inertia_jaj) + robo.IA[j]) h_inv[j] = symo.mat_replace(h_inv[j], 'JD', j) - jah = ineria_jaj * h_inv[j] - jah = symo.mat_replace(jah, 'JU', j) - grandK = star_inertia[j] - (jah * inertia_jaj.transpose()) - grandK = symo.mat_replace(grandK, 'GK', j) - expr1 = grandK * gamma[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) + expr1 = k_inertia * gamma[j] expr1 = symo.mat_replace(expr1, 'NG', j) - expr2 = expr1 + (jah * (tau[j] + jaj[j].dot(star_beta[j]))) + expr2 = expr1 + (jah[j] * tau[j]) expr2 = symo.mat_replace(expr2, 'VS', j) alpha = expr2 - star_beta[j] alpha = symo.mat_replace(alpha, 'AP', j) - expr3 = jTant[j].transpose() * grandK + 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) @@ -229,14 +231,25 @@ def compute_star_terms( def compute_joint_accel( - robo, symo, j, jaj, jTant, h_inv, gamma, tau, grandVp, - star_beta, star_inertia, qddot + 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 """ - pass + 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], str(robo.qddot[j]), forced=True) def compute_link_accel(robo, symo, j, jTant, zeta, grandVp): @@ -252,11 +265,27 @@ def compute_link_accel(robo, symo, j, jTant, zeta, grandVp): grandVp[j][3:, 0] = symo.mat_replace(grandVp[j][3:, 0], 'WP', j) +def compute_base_accel(robo, symo, star_inertia, star_beta, grandVp): + """ + Compute base acceleration (internal function). + + Note: + grandVp is the output parameter + """ + if robo.is_floating: + grandVp[0] = star_inertia[0].inv() * star_beta[0] + else: + grandVp[0] = Matrix([robo.vdot0 - robo.G, robo.w0]) + grandVp[0][:3, 0] = symo.mat_replace(grandVp[0][:3, 0], 'VP', 0) + grandVp[0][3:, 0] = symo.mat_replace(grandVp[0][3:, 0], 'WP', 0) + + def compute_base_accel_composite( robo, symo, composite_inertia, composite_beta, grandVp ): """ - Compute base acceleration (internal function). + Compute base acceleration when using composite inertia matrix + (internal function). Note: grandVp is the output parameter @@ -270,8 +299,7 @@ def compute_base_accel_composite( def compute_reaction_wrench( - robo, symo, j, grandVp, - composite_inertia, composite_beta, react_wrench + robo, symo, j, grandVp, inertia, beta_wrench, react_wrench ): """ Compute reaction wrench (internal function). @@ -279,9 +307,9 @@ def compute_reaction_wrench( Note: react_wrench is the output parameter """ - expr = composite_inertia[j] * grandVp[j] + expr = inertia[j] * grandVp[j] expr = symo.mat_replace(expr, 'DY', j) - wrench = expr - composite_beta[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) @@ -359,6 +387,7 @@ def composite_newton_euler(robo, symo): # second backward recursion - compute composite term for j in reversed(xrange(0, robo.NL)): if j == 0: + # compute 0^\dot{V}_0 : base acceleration compute_base_accel_composite( robo, symo, composite_inertia, composite_beta, grandVp ) @@ -392,7 +421,7 @@ def composite_newton_euler(robo, symo): compute_torque(robo, symo, j, jaj, react_wrench, torque) -def fl_direct_dynamic_model(robo): +def direct_dynamic_model(robo): symo = symbolmgr.SymbolManager() symo.file_open(robo, 'flddm') title = 'Direct Dynamic Model - NE' @@ -410,12 +439,12 @@ def fl_direct_dynamic_model(robo): gamma = ParamsInit.init_vec(robo, 6) beta = ParamsInit.init_vec(robo, 6) zeta = ParamsInit.init_vec(robo, 6) - alpha = ParamsInit.init_vec(robo, 6) - k_inertia = 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) @@ -453,10 +482,10 @@ def fl_direct_dynamic_model(robo): # second backward recursion - compute star terms for j in reversed(xrange(0, robo.NL)): if j == 0: continue - compute_tau(robo, symo, j, tau) + compute_tau(robo, symo, j, jaj, star_beta, tau) compute_star_terms( robo, symo, j, jaj, jTant, gamma, tau, - h_inv, star_inertia, star_beta + h_inv, jah, star_inertia, star_beta ) replace_star_terms( symo, star_inertia, star_beta, robo.ant[j], @@ -466,24 +495,24 @@ def fl_direct_dynamic_model(robo): for j in xrange(0, robo.NL): if j == 0: # compute 0^\dot{V}_0 : base acceleration - # for fixed base robots, the value returned is just the - # effect of gravity - compute_base_accel(robo, symo) + compute_base_accel( + robo, symo, star_inertia, star_beta, grandVp + ) continue # compute qddot_j : joint acceleration compute_joint_accel( - robo, symo, j, jaj, jTant, h_inv, gamma, tau, grandVp, - star_beta, star_inertia - ) - # compute j^F_j : reaction wrench as a function of alpha - compute_reaction_wrench( - robo, symo, j, grandVp, - composite_inertia, composite_beta, react_wrench + 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) + 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 + ) symo.file_close() return symo From 27fc40868ca079ea030dd34f9c0d2360db2f982d Mon Sep 17 00:00:00 2001 From: aravind Date: Thu, 10 Jul 2014 17:35:47 +0200 Subject: [PATCH 167/273] Fix symbol replace call with the correct function Call `replace()` instead of `mat_replace()` for `h_inv` since it is a scalar. --- pysymoro/fldyn.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/pysymoro/fldyn.py b/pysymoro/fldyn.py index 02bf4ca..1086f07 100644 --- a/pysymoro/fldyn.py +++ b/pysymoro/fldyn.py @@ -211,7 +211,7 @@ def compute_star_terms( inertia_jaj = star_inertia[j] * jaj[j] inertia_jaj = symo.mat_replace(inertia_jaj, 'JA', j) h_inv[j] = 1 / (jaj[j].dot(inertia_jaj) + robo.IA[j]) - h_inv[j] = symo.mat_replace(h_inv[j], 'JD', j) + 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()) From 51f45c0a4f057685cde82e4ed6b01eaf1ff1413d Mon Sep 17 00:00:00 2001 From: aravind Date: Sun, 13 Jul 2014 04:46:49 +0200 Subject: [PATCH 168/273] Integrate DDyM for floating base with UI Integrate Direct Dynamic Model computation for robots with floating base with the UI. Update `compute_ddym()` method in `Robot` class accordingly. --- pysymoro/dynamics.py | 8 +++++--- pysymoro/fldyn.py | 8 +------- pysymoro/robot.py | 15 +++++++++++++-- symoroui/layout.py | 2 +- 4 files changed, 20 insertions(+), 13 deletions(-) diff --git a/pysymoro/dynamics.py b/pysymoro/dynamics.py index c739e5d..1ba027c 100644 --- a/pysymoro/dynamics.py +++ b/pysymoro/dynamics.py @@ -138,7 +138,7 @@ def dynamic_identification_NE(robo): return symo -def compute_direct_dynamic_NE(symo, robo): +def compute_direct_dynamic_NE(robo, symo): # antecedent angular velocity, projected into jth frame wi = ParamsInit.init_vec(robo) w = ParamsInit.init_w(robo) @@ -181,7 +181,9 @@ def compute_direct_dynamic_NE(symo, robo): def direct_dynamic_NE(robo): - """Computes Direct Dynamic Model using + """ + OLD FUNCTION. NOT TO BE USED. + Computes Direct Dynamic Model using Newton-Euler formulation Parameters @@ -198,7 +200,7 @@ def direct_dynamic_NE(robo): symo.file_open(robo, 'ddm') title = 'Direct dynamic model using Newton - Euler Algorith' symo.write_params_table(robo, title, inert=True, dynam=True) - compute_direct_dynamic_NE(symo, robo) + compute_direct_dynamic_NE(robo, symo) symo.file_close() return symo diff --git a/pysymoro/fldyn.py b/pysymoro/fldyn.py index 1086f07..8948358 100644 --- a/pysymoro/fldyn.py +++ b/pysymoro/fldyn.py @@ -421,11 +421,7 @@ def composite_newton_euler(robo, symo): compute_torque(robo, symo, j, jaj, react_wrench, torque) -def direct_dynamic_model(robo): - symo = symbolmgr.SymbolManager() - symo.file_open(robo, 'flddm') - title = 'Direct Dynamic Model - NE' - symo.write_params_table(robo, title, inert=True, dynam=True) +def direct_dynamic_model(robo, symo): # antecedent angular velocity, projected into jth frame # j^omega_i wi = ParamsInit.init_vec(robo) @@ -513,7 +509,5 @@ def direct_dynamic_model(robo): robo, symo, j, grandVp, star_inertia, star_beta, react_wrench ) - symo.file_close() - return symo diff --git a/pysymoro/robot.py b/pysymoro/robot.py index 695bc1e..6365e2d 100644 --- a/pysymoro/robot.py +++ b/pysymoro/robot.py @@ -214,7 +214,7 @@ def compute_idym(self): """ symo = symbolmgr.SymbolManager() symo.file_open(self, 'idm') - title = 'Inverse dynamic model using Newton - Euler Algorithm' + title = "Inverse Dynamic Model using Newton-Euler Algorithm" symo.write_params_table(self, title, inert=True, dynam=True) if 1 in self.eta: # with flexible joints @@ -233,7 +233,18 @@ def compute_ddym(self): Compute the Direct Dynamic Model of the robot using the recursive Newton-Euler algorithm. """ - pass + symo = symbolmgr.SymbolManager() + symo.file_open(self, 'ddm') + title = "Direct Dynamic Model using Newton-Euler Algorithm" + symo.write_params_table(self, title, inert=True, dynam=True) + if self.is_floating: + # with rigid joints and floating base + fldyn.direct_dynamic_model(self, symo) + else: + # with rigid joints and fixed base + dynamics.compute_direct_dynamic_NE(self, symo) + symo.file_close() + return symo def compute_pseudotorques(self): """ diff --git a/symoroui/layout.py b/symoroui/layout.py index 7d22145..7160ef7 100644 --- a/symoroui/layout.py +++ b/symoroui/layout.py @@ -790,7 +790,7 @@ def OnCentrCoriolGravTorq(self, event): self.model_success('ccg') def OnDirectDynamicModel(self, event): - dynamics.direct_dynamic_NE(self.robo) + self.robo.compute_ddym() self.model_success('ddm') def OnBaseInertialParams(self, event): From 0e4b0a5f0b60945042cfc6c60fd8ce3bdbc9fbbd Mon Sep 17 00:00:00 2001 From: aravind Date: Thu, 17 Jul 2014 02:07:12 +0200 Subject: [PATCH 169/273] Fix tab order in UI fields --- symoroui/labels.py | 22 +++++++++++----------- symoroui/layout.py | 6 +++--- 2 files changed, 14 insertions(+), 14 deletions(-) diff --git a/symoroui/labels.py b/symoroui/labels.py index 5e17fef..1dafaa6 100644 --- a/symoroui/labels.py +++ b/symoroui/labels.py @@ -43,26 +43,26 @@ # joint velocity and acceleration params JOINT_PARAMS = OrderedDict([ ('joint', FieldEntry('Joint', 'joint', 'cmb', (0, 0), 'OnJointChanged', -1)), - ('qp', FieldEntry('QP', 'QP', 'txt', (2, 0), 'OnSpeedChanged', -1)), - ('qdp', FieldEntry('QDP', 'QDP', 'txt', (2, 1), 'OnSpeedChanged', -1)), - ('gam', FieldEntry('GAM', 'GAM', 'txt', (1, 1), 'OnSpeedChanged', -1)), + ('eta', FieldEntry('eta', 'eta', 'cmb', (0, 1), 'OnSpeedChanged', -1)), ('stiff', FieldEntry('k', 'k', 'txt', (1, 0), 'OnSpeedChanged', -1)), - ('eta', FieldEntry('eta', 'eta', 'cmb', (0, 1), '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([ - ('wx', FieldEntry('W0X', 'W0X', 'txt', (0, 1), 'OnBaseTwistChanged', 0)), - ('wy', FieldEntry('W0Y', 'W0Y', 'txt', (1, 1), 'OnBaseTwistChanged', 1)), - ('wz', FieldEntry('W0Z', 'W0Z', 'txt', (2, 1), 'OnBaseTwistChanged', 2)), - ('wpx', FieldEntry('WP0X', 'WP0X', 'txt', (0, 3), 'OnBaseTwistChanged', 0)), - ('wpy', FieldEntry('WP0Y', 'WP0Y', 'txt', (1, 3), 'OnBaseTwistChanged', 1)), - ('wpz', FieldEntry('WP0Z', 'WP0Z', 'txt', (2, 3), 'OnBaseTwistChanged', 2)), ('vx', FieldEntry('V0X', 'V0X', 'txt', (0, 0), 'OnBaseTwistChanged', 0)), ('vy', FieldEntry('V0Y', 'V0Y', 'txt', (1, 0), 'OnBaseTwistChanged', 1)), ('vz', FieldEntry('V0Z', 'V0Z', 'txt', (2, 0), 'OnBaseTwistChanged', 2)), + ('wx', FieldEntry('W0X', 'W0X', 'txt', (0, 1), 'OnBaseTwistChanged', 0)), + ('wy', FieldEntry('W0Y', 'W0Y', 'txt', (1, 1), 'OnBaseTwistChanged', 1)), + ('wz', FieldEntry('W0Z', 'W0Z', 'txt', (2, 1), 'OnBaseTwistChanged', 2)), ('vpx', FieldEntry('VP0X', 'VP0X', 'txt', (0, 2), 'OnBaseTwistChanged', 0)), ('vpy', FieldEntry('VP0Y', 'VP0Y', 'txt', (1, 2), 'OnBaseTwistChanged', 1)), - ('vpz', FieldEntry('VP0Z', 'VP0Z', 'txt', (2, 2), 'OnBaseTwistChanged', 2)) + ('vpz', FieldEntry('VP0Z', 'VP0Z', 'txt', (2, 2), 'OnBaseTwistChanged', 2)), + ('wpx', FieldEntry('WP0X', 'WP0X', 'txt', (0, 3), 'OnBaseTwistChanged', 0)), + ('wpy', FieldEntry('WP0Y', 'WP0Y', 'txt', (1, 3), 'OnBaseTwistChanged', 1)), + ('wpz', FieldEntry('WP0Z', 'WP0Z', 'txt', (2, 3), 'OnBaseTwistChanged', 2)) ]) # inertial params DYN_PARAMS_I = OrderedDict([ diff --git a/symoroui/layout.py b/symoroui/layout.py index 7160ef7..6049066 100644 --- a/symoroui/layout.py +++ b/symoroui/layout.py @@ -359,10 +359,10 @@ def update_dyn_params(self): def update_joint_params(self): pars = self._extract_param_names(ui_labels.JOINT_PARAMS) index = int(self.widgets['joint'].Value) - self.widgets[pars[-1]].SetValue( - str(self.robo.get_val(index, pars[-1])) + self.widgets[pars[0]].SetValue( + str(self.robo.get_val(index, pars[0])) ) - self.update_params(index, pars[:-1]) + self.update_params(index, pars[1:]) def update_base_twist_params(self): pars = dict( From 87d7f65f30f15150878cd12befaf736b65de5614 Mon Sep 17 00:00:00 2001 From: aravind Date: Thu, 17 Jul 2014 02:15:56 +0200 Subject: [PATCH 170/273] Update sample robot parameters --- symoroutils/samplerobots.py | 41 ++++++++++++++++++++++--------------- 1 file changed, 24 insertions(+), 17 deletions(-) diff --git a/symoroutils/samplerobots.py b/symoroutils/samplerobots.py index 3eae4b5..38956e7 100644 --- a/symoroutils/samplerobots.py +++ b/symoroutils/samplerobots.py @@ -57,16 +57,16 @@ def cart_pole(): def planar2r(): """Generate Robot instance of 2R Planar robot""" - robo = Robot('Planar2R', 2, 2, 3, False) + robo = Robot('Planar2R', 2, 2, 2, False) robo.structure = tools.SIMPLE - robo.sigma = [2, 0, 0, 2] - robo.mu = [0, 1, 1, 0] - robo.gamma = [0, 0, 0, 0] - robo.b = [0, 0, 0, 0] - robo.alpha = [0, 0, 0, 0] - robo.d = [0, 0, var('L2'), var('L3')] - robo.theta = [0, var('q1'), var('q2'), 0] - robo.r = [0, 0, 0, 0] + 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] @@ -76,9 +76,13 @@ def planar2r(): 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] - 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 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) @@ -113,7 +117,7 @@ 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, 0] + 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')) @@ -139,11 +143,14 @@ def rx90(): 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] + 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.num.append(0) return robo From 96c97d13a546cfb2cb10ca6273828185f4742b6c Mon Sep 17 00:00:00 2001 From: aravind Date: Thu, 17 Jul 2014 02:58:35 +0200 Subject: [PATCH 171/273] Fix definition of terms in PAR file Fix definition of base velocity and acceleration terms in PAR file. This was due to an error in the `Robot` class. --- pysymoro/robot.py | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/pysymoro/robot.py b/pysymoro/robot.py index 6365e2d..ed40aec 100644 --- a/pysymoro/robot.py +++ b/pysymoro/robot.py @@ -361,11 +361,11 @@ def WP0(self): @property def V0(self): - return self.w0 + return self.v0 @property def VP0(self): - return self.wdot0 + return self.vdot0 @property def QP(self): From c6617484c81520dc40914356c3a973d4b3a9270e Mon Sep 17 00:00:00 2001 From: aravind Date: Thu, 17 Jul 2014 03:07:47 +0200 Subject: [PATCH 172/273] Fix `OnSave()` issue at `OnClose()` --- symoroui/layout.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/symoroui/layout.py b/symoroui/layout.py index 6049066..54f64b7 100644 --- a/symoroui/layout.py +++ b/symoroui/layout.py @@ -824,7 +824,7 @@ def OnClose(self, event): 'Do you want to save changes?', 'Please confirm', wx.ICON_QUESTION | wx.YES_NO | wx.CANCEL, self) if result == wx.YES: - if self.OnSave(_) == tools.FAIL: + if self.OnSave(event) == tools.FAIL: return elif result == wx.CANCEL: return From 5ee459083730b26f2584007ed0f652d7ec08a8af Mon Sep 17 00:00:00 2001 From: aravind Date: Thu, 17 Jul 2014 03:50:28 +0200 Subject: [PATCH 173/273] Fix PAR file reading for floating base robots --- symoroutils/parfile.py | 2 ++ 1 file changed, 2 insertions(+) diff --git a/symoroutils/parfile.py b/symoroutils/parfile.py index 54edba6..1795850 100644 --- a/symoroutils/parfile.py +++ b/symoroutils/parfile.py @@ -50,6 +50,8 @@ def _extract_vals(robo, key, line): line = line.replace('}', '') if key in _ZERO_BASED: k = 0 + elif robo.is_mobile and key in _NL: + k = 0 else: k = 1 items = line.split(',') From 08ad03377d23b5fa9c882946e68b2356736388ed Mon Sep 17 00:00:00 2001 From: aravind Date: Thu, 17 Jul 2014 11:52:53 +0200 Subject: [PATCH 174/273] Modify function name --- pysymoro/fldyn.py | 2 +- pysymoro/robot.py | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/pysymoro/fldyn.py b/pysymoro/fldyn.py index 8948358..4b7b3da 100644 --- a/pysymoro/fldyn.py +++ b/pysymoro/fldyn.py @@ -421,7 +421,7 @@ def composite_newton_euler(robo, symo): compute_torque(robo, symo, j, jaj, react_wrench, torque) -def direct_dynamic_model(robo, symo): +def direct_dynamic_newton_euler(robo, symo): # antecedent angular velocity, projected into jth frame # j^omega_i wi = ParamsInit.init_vec(robo) diff --git a/pysymoro/robot.py b/pysymoro/robot.py index ed40aec..9a294ae 100644 --- a/pysymoro/robot.py +++ b/pysymoro/robot.py @@ -239,7 +239,7 @@ def compute_ddym(self): symo.write_params_table(self, title, inert=True, dynam=True) if self.is_floating: # with rigid joints and floating base - fldyn.direct_dynamic_model(self, symo) + fldyn.direct_dynamic_newton_euler(self, symo) else: # with rigid joints and fixed base dynamics.compute_direct_dynamic_NE(self, symo) From 71ecda351748a24c2516d70c3bb33269e8480dc2 Mon Sep 17 00:00:00 2001 From: aravind Date: Thu, 17 Jul 2014 14:28:40 +0200 Subject: [PATCH 175/273] Complete IDyM for robots with flexible joints Complete Inverse Dynamic Model for robots with flexible joints. --- pysymoro/fldyn.py | 171 ++++++++++++++++++++++++++++++++++++++++++---- 1 file changed, 159 insertions(+), 12 deletions(-) diff --git a/pysymoro/fldyn.py b/pysymoro/fldyn.py index 4b7b3da..49ea790 100644 --- a/pysymoro/fldyn.py +++ b/pysymoro/fldyn.py @@ -83,6 +83,13 @@ 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) @@ -174,19 +181,23 @@ def compute_composite_terms( composite_beta[i] = composite_beta[i] + expr4 - expr3 -def compute_hinv(robo, symo, j, jaj, star_inertia, inertia_jaj, h_inv): +def compute_hinv( + robo, symo, j, jaj, star_inertia, inertia_jaj, h_inv, flex=False +): """ Note: h_inv and inertia_jaj are the output parameters """ inertia_jaj[j] = star_inertia[j] * jaj[j] inertia_jaj[j] = symo.mat_replace(inertia_jaj, 'JA', j) - h = jaj[j].dot(inertia_jaj[j]) + robo.IA[j] + h = jaj[j].dot(inertia_jaj[j]) + if not flex: + h = h + robo.IA[j] h_inv[j] = 1 / h h_inv[j] = symo.mat_replace(h_inv[j], 'JD', j) -def compute_tau(robo, symo, j, jaj, star_beta, tau): +def compute_tau(robo, symo, j, jaj, star_beta, tau, flex=False): """ Note: tau is the output parameter @@ -194,14 +205,17 @@ def compute_tau(robo, symo, j, jaj, star_beta, tau): if robo.sigma[j] == 2: tau[j] = 0 else: - joint_friction = robo.fric_s(j) + robo.fric_v(j) + 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 + h_inv, jah, star_inertia, star_beta, flex=False ): """ Note: @@ -210,15 +224,24 @@ def compute_star_terms( i = robo.ant[j] inertia_jaj = star_inertia[j] * jaj[j] inertia_jaj = symo.mat_replace(inertia_jaj, 'JA', j) - h_inv[j] = 1 / (jaj[j].dot(inertia_jaj) + robo.IA[j]) - 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) + h = jaj[j].dot(inertia_jaj[j]) + 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) - expr2 = expr1 + (jah[j] * tau[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) @@ -511,3 +534,127 @@ def direct_dynamic_newton_euler(robo, symo): ) +def flexible_newton_euler(robo, symo): + # 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) + # 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) + if robo.eta[j]: + # 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)): + # skip base terms for non-floating robots + if robo.is_floating and 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) + else: + continue + replace_star_terms( + symo, grandJ, beta, j, star_inertia, star_beta + ) + # second backward recursion - compute star terms + for j in reversed(xrange(0, robo.NL)): + if j == 0: continue + # skip base terms for non-floating robots + if not robo.is_floating and robo.ant[j] == 0: 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 + ) + # second forward recursion + for j in xrange(0, robo.NL): + if robo.is_floating and j == 0: + # compute 0^\dot{V}_0 : base acceleration + compute_base_accel( + robo, symo, star_inertia, star_beta, grandVp + ) + continue + else: + continue + 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) + + From c958d3257f6d7bc580856cf489d737453337671f Mon Sep 17 00:00:00 2001 From: aravind Date: Fri, 18 Jul 2014 15:12:35 +0200 Subject: [PATCH 176/273] Update direct dynamic model Update direct dynamic model in `pysymoro/fldyn.py` to efficiently compute the direct dynamic model for fixed base robots along with floating base robots. Update function names and their calls accordingly. --- pysymoro/fldyn.py | 55 +++++++++++++++++++++++++---------------------- pysymoro/robot.py | 11 +++------- 2 files changed, 32 insertions(+), 34 deletions(-) diff --git a/pysymoro/fldyn.py b/pysymoro/fldyn.py index 49ea790..c70fbcb 100644 --- a/pysymoro/fldyn.py +++ b/pysymoro/fldyn.py @@ -182,19 +182,21 @@ def compute_composite_terms( def compute_hinv( - robo, symo, j, jaj, star_inertia, inertia_jaj, h_inv, flex=False + robo, symo, j, jaj, star_inertia, jah, h_inv, flex=False ): """ Note: - h_inv and inertia_jaj are the output parameters + h_inv and jah are the output parameters """ - inertia_jaj[j] = star_inertia[j] * jaj[j] - inertia_jaj[j] = symo.mat_replace(inertia_jaj, 'JA', j) - h = jaj[j].dot(inertia_jaj[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] h_inv[j] = 1 / h - h_inv[j] = symo.mat_replace(h_inv[j], 'JD', j) + 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): @@ -224,7 +226,7 @@ def compute_star_terms( 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[j]) + h = jaj[j].dot(inertia_jaj) if not flex: h = h + robo.IA[j] if not flex or robo.eta[j]: @@ -354,7 +356,7 @@ def compute_torque(robo, symo, j, jaj, react_wrench, torque): torque[j] = symo.replace(tau_total, symbl_name, forced=True) -def composite_newton_euler(robo, symo): +def composite_inverse_dynmodel(robo, symo): # antecedent angular velocity, projected into jth frame # j^omega_i wi = ParamsInit.init_vec(robo) @@ -444,7 +446,7 @@ def composite_newton_euler(robo, symo): compute_torque(robo, symo, j, jaj, react_wrench, torque) -def direct_dynamic_newton_euler(robo, symo): +def direct_dynmodel(robo, symo): # antecedent angular velocity, projected into jth frame # j^omega_i wi = ParamsInit.init_vec(robo) @@ -482,19 +484,20 @@ def direct_dynamic_newton_euler(robo, symo): 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) # first backward recursion - initialisation step for j in reversed(xrange(0, robo.NL)): - if j == 0: + # skip base terms for non-floating robots + if robo.is_floating and 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) + elif j == 0: + continue replace_star_terms( symo, grandJ, beta, j, star_inertia, star_beta ) @@ -502,14 +505,18 @@ def direct_dynamic_newton_euler(robo, symo): for j in reversed(xrange(0, 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 - ) - replace_star_terms( - symo, star_inertia, star_beta, robo.ant[j], - star_inertia, star_beta - ) + # skip base terms for non-floating robots + if not robo.is_floating and robo.ant[j] == 0: + compute_hinv(robo, symo, j, jaj, star_inertia, jah, h_inv) + else: + compute_star_terms( + robo, symo, j, jaj, jTant, gamma, tau, + h_inv, jah, star_inertia, star_beta + ) + replace_star_terms( + symo, star_inertia, star_beta, robo.ant[j], + star_inertia, star_beta + ) # second forward recursion for j in xrange(0, robo.NL): if j == 0: @@ -534,7 +541,7 @@ def direct_dynamic_newton_euler(robo, symo): ) -def flexible_newton_euler(robo, symo): +def flexible_inverse_dynmodel(robo, symo): # antecedent angular velocity, projected into jth frame # j^omega_i wi = ParamsInit.init_vec(robo) @@ -575,8 +582,6 @@ def flexible_newton_euler(robo, symo): 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) @@ -629,14 +634,12 @@ def flexible_newton_euler(robo, symo): ) # second forward recursion for j in xrange(0, robo.NL): - if robo.is_floating and j == 0: + if j == 0: # compute 0^\dot{V}_0 : base acceleration compute_base_accel( robo, symo, star_inertia, star_beta, grandVp ) continue - else: - continue if robo.eta[j]: # when flexible # compute qddot_j : joint acceleration diff --git a/pysymoro/robot.py b/pysymoro/robot.py index 9a294ae..611375a 100644 --- a/pysymoro/robot.py +++ b/pysymoro/robot.py @@ -218,10 +218,10 @@ def compute_idym(self): symo.write_params_table(self, title, inert=True, dynam=True) if 1 in self.eta: # with flexible joints - pass + fldyn.flexible_inverse_dynmodel(self, symo) elif self.is_floating: # with rigid joints and floating base - fldyn.composite_newton_euler(self, symo) + fldyn.composite_inverse_dynmodel(self, symo) else: # with rigid joints and fixed base dynamics.default_newton_euler(self, symo) @@ -237,12 +237,7 @@ def compute_ddym(self): symo.file_open(self, 'ddm') title = "Direct Dynamic Model using Newton-Euler Algorithm" symo.write_params_table(self, title, inert=True, dynam=True) - if self.is_floating: - # with rigid joints and floating base - fldyn.direct_dynamic_newton_euler(self, symo) - else: - # with rigid joints and fixed base - dynamics.compute_direct_dynamic_NE(self, symo) + fldyn.direct_dynmodel(self, symo) symo.file_close() return symo From 320183a1a23d26648674f6ef1ba1a9b613a3a777 Mon Sep 17 00:00:00 2001 From: aravind Date: Fri, 18 Jul 2014 15:35:23 +0200 Subject: [PATCH 177/273] Move base acceleration computation Move base acceleration computation outside the recursive loops. --- pysymoro/fldyn.py | 185 +++++++++++++++++++++++----------------------- 1 file changed, 94 insertions(+), 91 deletions(-) diff --git a/pysymoro/fldyn.py b/pysymoro/fldyn.py index c70fbcb..7d2800e 100644 --- a/pysymoro/fldyn.py +++ b/pysymoro/fldyn.py @@ -297,12 +297,17 @@ def compute_base_accel(robo, symo, star_inertia, star_beta, grandVp): Note: grandVp is the output parameter """ + forced = False + grandVp[0] = Matrix([robo.vdot0 - robo.G, robo.w0]) if robo.is_floating: grandVp[0] = star_inertia[0].inv() * star_beta[0] - else: - grandVp[0] = Matrix([robo.vdot0 - robo.G, robo.w0]) - grandVp[0][:3, 0] = symo.mat_replace(grandVp[0][:3, 0], 'VP', 0) - grandVp[0][3:, 0] = symo.mat_replace(grandVp[0][3:, 0], 'WP', 0) + forced = True + 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( @@ -315,12 +320,17 @@ def compute_base_accel_composite( Note: grandVp is the output parameter """ + forced = False + grandVp[0] = Matrix([robo.vdot0 - robo.G, robo.w0]) if robo.is_floating: grandVp[0] = composite_inertia[0].inv() * composite_beta[0] - else: - grandVp[0] = Matrix([robo.vdot0 - robo.G, robo.w0]) - grandVp[0][:3, 0] = symo.mat_replace(grandVp[0][:3, 0], 'VP', 0) - grandVp[0][3:, 0] = symo.mat_replace(grandVp[0][3:, 0], 'WP', 0) + forced = True + 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( @@ -411,17 +421,7 @@ def composite_inverse_dynmodel(robo, symo): ) # second backward recursion - compute composite term for j in reversed(xrange(0, robo.NL)): - if j == 0: - # compute 0^\dot{V}_0 : base acceleration - compute_base_accel_composite( - robo, symo, composite_inertia, composite_beta, grandVp - ) - continue - # compute i^I_i^c and i^beta_i^c - #compute_composite_terms( - # robo, symo, j, jTant, zeta, - # composite_inertia, composite_beta - #) + if j == 0: continue compute_composite_inertia( robo, symo, j, antRj, antPj, comp_inertia3, comp_ms, comp_mass, composite_inertia @@ -433,6 +433,11 @@ def composite_inverse_dynmodel(robo, symo): symo, composite_inertia, composite_beta, robo.ant[j], composite_inertia, composite_beta ) + # 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 @@ -446,7 +451,7 @@ def composite_inverse_dynmodel(robo, symo): compute_torque(robo, symo, j, jaj, react_wrench, torque) -def direct_dynmodel(robo, symo): +def flexible_inverse_dynmodel(robo, symo): # antecedent angular velocity, projected into jth frame # j^omega_i wi = ParamsInit.init_vec(robo) @@ -465,10 +470,13 @@ def direct_dynmodel(robo, symo): 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 @@ -488,6 +496,9 @@ def direct_dynmodel(robo, symo): 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 robo.eta[j]: + # 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)): # skip base terms for non-floating robots @@ -496,7 +507,7 @@ def direct_dynmodel(robo, symo): grandJ[j] = inertia_spatial(robo.J[j], robo.MS[j], robo.M[j]) # compute 0^beta_0 compute_beta(robo, symo, j, w, beta) - elif j == 0: + else: continue replace_star_terms( symo, grandJ, beta, j, star_inertia, star_beta @@ -504,34 +515,49 @@ def direct_dynmodel(robo, symo): # second backward recursion - compute star terms for j in reversed(xrange(0, robo.NL)): if j == 0: continue - compute_tau(robo, symo, j, jaj, star_beta, tau) # skip base terms for non-floating robots - if not robo.is_floating and robo.ant[j] == 0: - compute_hinv(robo, symo, j, jaj, star_inertia, jah, h_inv) + if not robo.is_floating and robo.ant[j] == 0: 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 - ) - replace_star_terms( - symo, star_inertia, star_beta, robo.ant[j], - star_inertia, star_beta + 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(0, robo.NL): - if j == 0: - # compute 0^\dot{V}_0 : base acceleration - compute_base_accel( - robo, symo, star_inertia, star_beta, grandVp + 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 ) - continue - # 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^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 @@ -539,9 +565,12 @@ def direct_dynmodel(robo, symo): 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 flexible_inverse_dynmodel(robo, symo): +def direct_dynmodel(robo, symo): # antecedent angular velocity, projected into jth frame # j^omega_i wi = ParamsInit.init_vec(robo) @@ -560,13 +589,10 @@ def flexible_inverse_dynmodel(robo, symo): 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 @@ -586,9 +612,6 @@ def flexible_inverse_dynmodel(robo, symo): 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 robo.eta[j]: - # 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)): # skip base terms for non-floating robots @@ -597,7 +620,7 @@ def flexible_inverse_dynmodel(robo, symo): grandJ[j] = inertia_spatial(robo.J[j], robo.MS[j], robo.M[j]) # compute 0^beta_0 compute_beta(robo, symo, j, w, beta) - else: + elif j == 0: continue replace_star_terms( symo, grandJ, beta, j, star_inertia, star_beta @@ -605,50 +628,33 @@ def flexible_inverse_dynmodel(robo, symo): # second backward recursion - compute star terms for j in reversed(xrange(0, robo.NL)): if j == 0: continue + compute_tau(robo, symo, j, jaj, star_beta, tau) # skip base terms for non-floating robots - if not robo.is_floating and robo.ant[j] == 0: 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 - ) + if not robo.is_floating and robo.ant[j] == 0: + compute_hinv(robo, symo, j, jaj, star_inertia, jah, h_inv) 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 - ) - # second forward recursion - for j in xrange(0, robo.NL): - if j == 0: - # compute 0^\dot{V}_0 : base acceleration - compute_base_accel( - robo, symo, star_inertia, star_beta, grandVp + h_inv, jah, star_inertia, star_beta ) - continue - 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 + replace_star_terms( + symo, star_inertia, star_beta, robo.ant[j], + star_inertia, star_beta ) - # compute j^zeta_j : relative acceleration (6x1) - compute_zeta(robo, symo, j, gamma, jaj, zeta, qddot) + # 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 @@ -656,8 +662,5 @@ def flexible_inverse_dynmodel(robo, symo): 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) From 0acc9b946b6580ae95a533caf54bc37f39d44562 Mon Sep 17 00:00:00 2001 From: aravind Date: Fri, 18 Jul 2014 15:40:06 +0200 Subject: [PATCH 178/273] Rename `pysymoro/fldyn.py` to `pysymoro/nealgos.py` Update calls accordingly. --- pysymoro/{fldyn.py => nealgos.py} | 0 pysymoro/robot.py | 10 +++++----- symoroui/layout.py | 1 - 3 files changed, 5 insertions(+), 6 deletions(-) rename pysymoro/{fldyn.py => nealgos.py} (100%) diff --git a/pysymoro/fldyn.py b/pysymoro/nealgos.py similarity index 100% rename from pysymoro/fldyn.py rename to pysymoro/nealgos.py diff --git a/pysymoro/robot.py b/pysymoro/robot.py index 611375a..6aad982 100644 --- a/pysymoro/robot.py +++ b/pysymoro/robot.py @@ -21,7 +21,7 @@ from sympy import Mul, Add, factor, zeros, var, sympify, eye from pysymoro import dynamics -from pysymoro import fldyn +from pysymoro import nealgos from symoroutils import filemgr from symoroutils import symbolmgr from symoroutils import tools @@ -218,10 +218,10 @@ def compute_idym(self): symo.write_params_table(self, title, inert=True, dynam=True) if 1 in self.eta: # with flexible joints - fldyn.flexible_inverse_dynmodel(self, symo) + nealgos.flexible_inverse_dynmodel(self, symo) elif self.is_floating: # with rigid joints and floating base - fldyn.composite_inverse_dynmodel(self, symo) + nealgos.composite_inverse_dynmodel(self, symo) else: # with rigid joints and fixed base dynamics.default_newton_euler(self, symo) @@ -237,7 +237,7 @@ def compute_ddym(self): symo.file_open(self, 'ddm') title = "Direct Dynamic Model using Newton-Euler Algorithm" symo.write_params_table(self, title, inert=True, dynam=True) - fldyn.direct_dynmodel(self, symo) + nealgos.direct_dynmodel(self, symo) symo.file_close() return symo @@ -257,7 +257,7 @@ def compute_pseudotorques(self): pass elif self.is_floating: # with rigid joints and floating base - fldyn.composite_newton_euler(pseudorobo, symo) + nealgos.composite_inverse_dynmodel(pseudorobo, symo) else: # with rigid joints and fixed base dynamics.default_newton_euler(pseudorobo, symo) diff --git a/symoroui/layout.py b/symoroui/layout.py index 54f64b7..f750e9a 100644 --- a/symoroui/layout.py +++ b/symoroui/layout.py @@ -20,7 +20,6 @@ from pysymoro import geometry from pysymoro import kinematics from pysymoro import dynamics -from pysymoro import fldyn from pysymoro import invgeom from symoroutils import parfile from symoroutils import filemgr From 694710f1d9e345359091fde044c06d20725203f8 Mon Sep 17 00:00:00 2001 From: aravind Date: Sat, 19 Jul 2014 10:15:19 +0200 Subject: [PATCH 179/273] Update naming of intermediate variables Update naming of intermediate variables to be consistent throughout all the different algorithms. Naming format : [r][c]j where, r = row number, c = column number, j = link/joint number --- pysymoro/dynamics.py | 59 ++++++++++++++++++++++++++------------------ pysymoro/nealgos.py | 5 ++-- 2 files changed, 37 insertions(+), 27 deletions(-) diff --git a/pysymoro/dynamics.py b/pysymoro/dynamics.py index 1ba027c..1338ebd 100644 --- a/pysymoro/dynamics.py +++ b/pysymoro/dynamics.py @@ -305,8 +305,14 @@ def pseudo_force_NE(robo): symo.file_close() return symo -#TODO naming stuff -def compute_wrench(robo, symo, j, w, wdot, U, vdot, F, N, name='%s'): + +def get_symbol(symbol, name): + if name is None: + return symbol + else: + return symbol + name + +def compute_wrench(robo, symo, j, w, wdot, U, vdot, F, N, name=None): """Internal function. Computes total wrench (torques and forces) of the link j @@ -315,14 +321,17 @@ def compute_wrench(robo, symo, j, w, wdot, U, vdot, F, N, name='%s'): F, N are the output parameters """ F[j] = robo.M[j]*vdot[j] + U[j]*robo.MS[j] - symo.mat_replace(F[j], ('F' + name) % j) - Psi = symo.mat_replace(robo.J[j]*w[j], ('PSI' + name) % j) - N[j] = robo.J[j]*wdot[j] + tools.skew(w[j])*Psi - symo.mat_replace(N[j], ('No' + name) % j) - -#TODO refabrish the naming system -def compute_joint_wrench(robo, symo, j, antRj, antPj, vdot, - Fjnt, Njnt, F, N, Fex, Nex, name='%s'): + F[j] = symo.mat_replace(F[j], get_symbol('F', name), j) + Psi = robo.J[j] * w[j] + Psi = symo.mat_replace(Psi, get_symbol('PSI', name), j) + N[j] = (robo.J[j] * wdot[j]) + (tools.skew(w[j]) * Psi) + N[j] = symo.mat_replace(N[j], get_symbol('No', name), j) + + +def compute_joint_wrench( + robo, symo, j, antRj, antPj, vdot, + Fjnt, Njnt, F, N, Fex, Nex, name=None +): """Internal function. Computes wrench (torques and forces) of the joint j @@ -330,10 +339,11 @@ def compute_joint_wrench(robo, symo, j, antRj, antPj, vdot, ===== Fjnt, Njnt, Fex, Nex are the output parameters """ - Fjnt[j] = symo.mat_replace(F[j] + Fex[j], ('E' + name) % j) - Njnt[j] = N[j] + Nex[j] + tools.skew(robo.MS[j])*vdot[j] - symo.mat_replace(Njnt[j], ('N' + name) % j) - f_ant = symo.mat_replace(antRj[j]*Fjnt[j], ('FDI' + name) % j) + Fjnt[j] = F[j] + Fex[j] + Fjnt[j] = symo.mat_replace(Fjnt[j], get_symbol('E', name), j) + Njnt[j] = N[j] + Nex[j] + (tools.skew(robo.MS[j]) * vdot[j]) + Njnt[j] = symo.mat_replace(Njnt[j], get_symbol('N', name), j) + f_ant = symo.mat_replace(antRj[j]*Fjnt[j], get_symbol('FDI', name), j) if robo.ant[j] != - 1: Fex[robo.ant[j]] += f_ant Nex[robo.ant[j]] += antRj[j]*Njnt[j] + tools.skew(antPj[j])*f_ant @@ -403,15 +413,16 @@ def compute_link_acc(robo, symo, j, antRj, antPj, link_acc, w, wi): ===== link_acc is the output parameter """ - E1 = symo.mat_replace(tools.skew(wi[j])*Matrix([0, 0, robo.qdot[j]]), - 'WQ', j) - E2 = (1 - robo.sigma[j])*E1 - E3 = 2*robo.sigma[j]*E1 - E4 = tools.skew(w[robo.ant[j]])*antPj[j] - E5 = tools.skew(w[robo.ant[j]])*E4 - E6 = antRj[j].T*E5 - E7 = symo.mat_replace(E6 + E3, 'LW', j) - link_acc[j] = Matrix([E7, E2]) + 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[robo.ant[j]]) * antPj[j] + expr5 = tools.skew(w[robo.ant[j]]) * expr4 + expr6 = antRj[j].transpose() * expr5 + expr7 = expr6 + expr3 + expr7 = symo.mat_replace(expr7, 'LW', j) + link_acc[j] = Matrix([expr7, expr2]) def replace_beta_J_star(robo, symo, j, grandJ, beta_star): @@ -477,7 +488,7 @@ def compute_acceleration(robo, symo, j, jTant, grandVp, qddot = 0 else: qddot = H_inv[j]*Tau[j] - E1 - qddot = symo.replace(qddot, str(robo.qddot[j]), forced=True) + 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) diff --git a/pysymoro/nealgos.py b/pysymoro/nealgos.py index 7d2800e..760e373 100644 --- a/pysymoro/nealgos.py +++ b/pysymoro/nealgos.py @@ -274,7 +274,7 @@ def compute_joint_accel( qddot[j] = 0 else: qddot[j] = (h_inv[j] * tau[j]) - expr2 - qddot[j] = symo.replace(qddot[j], str(robo.qddot[j]), forced=True) + qddot[j] = symo.replace(qddot[j], 'QDP', j, forced=True) def compute_link_accel(robo, symo, j, jTant, zeta, grandVp): @@ -356,14 +356,13 @@ def compute_torque(robo, symo, j, jaj, react_wrench, torque): Note: torque is the output parameter. """ - symbl_name = 'GAM' + str(j) 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, symbl_name, forced=True) + torque[j] = symo.replace(tau_total, 'GAM', j, forced=True) def composite_inverse_dynmodel(robo, symo): From 963184da4c817826ad9a5eb626030378378551c0 Mon Sep 17 00:00:00 2001 From: aravind Date: Mon, 21 Jul 2014 14:14:32 +0200 Subject: [PATCH 180/273] Modify composite/star terms for link 0 Force to replace variables in the case of link 0 for composite/star terms. --- pysymoro/nealgos.py | 14 ++++++++++---- 1 file changed, 10 insertions(+), 4 deletions(-) diff --git a/pysymoro/nealgos.py b/pysymoro/nealgos.py index 760e373..b2e4569 100644 --- a/pysymoro/nealgos.py +++ b/pysymoro/nealgos.py @@ -137,10 +137,14 @@ def replace_composite_terms( Note: composite_inertia are composite_beta are the output parameters """ + forced = False + if j == 0: forced = True composite_inertia[j] = symo.mat_replace( - grandJ[j], 'MJE', j, symmet=True + grandJ[j], 'MJE', j, symmet=True, forced=forced + ) + composite_beta[j] = symo.mat_replace( + beta[j], 'VBE', j, forced=forced ) - composite_beta[j] = symo.mat_replace(beta[j], 'VBE', j) def replace_star_terms( @@ -152,10 +156,12 @@ def replace_star_terms( Note: star_inertia are star_beta are the output parameters """ + forced = False + if j == 0: forced = True star_inertia[j] = symo.mat_replace( - grandJ[j], 'MJE', j, symmet=True + grandJ[j], 'MJE', j, symmet=True, forced=forced ) - star_beta[j] = symo.mat_replace(beta[j], 'VBE', j) + star_beta[j] = symo.mat_replace(beta[j], 'VBE', j, forced=forced) def compute_composite_terms( From 1416537caa41139b83c0439640fdab3ea4a1c9c2 Mon Sep 17 00:00:00 2001 From: aravind Date: Mon, 21 Jul 2014 17:04:29 +0200 Subject: [PATCH 181/273] Revert DDyM --- pysymoro/nealgos.py | 54 +++++++++++++++++++++++----------------- pysymoro/robot.py | 7 +++++- symoroutils/symbolmgr.py | 9 +++++++ 3 files changed, 46 insertions(+), 24 deletions(-) diff --git a/pysymoro/nealgos.py b/pysymoro/nealgos.py index b2e4569..a0647c4 100644 --- a/pysymoro/nealgos.py +++ b/pysymoro/nealgos.py @@ -129,7 +129,8 @@ def compute_composite_beta( def replace_composite_terms( - symo, grandJ, beta, j, composite_inertia, composite_beta + symo, grandJ, beta, j, composite_inertia, + composite_beta, replace=False ): """ Replace composite inertia and beta (internal function). @@ -138,17 +139,20 @@ def replace_composite_terms( composite_inertia are composite_beta are the output parameters """ forced = False - if j == 0: forced = True + if replace and j == 0: forced = False composite_inertia[j] = symo.mat_replace( grandJ[j], 'MJE', j, symmet=True, forced=forced ) + if replace and j == 0: + symo.write_equation('MJE220', 'MJE110') + symo.write_equation('MJE330', 'MJE110') composite_beta[j] = symo.mat_replace( beta[j], 'VBE', j, forced=forced ) def replace_star_terms( - symo, grandJ, beta, j, star_inertia, star_beta + symo, grandJ, beta, j, star_inertia, star_beta, replace=False ): """ Replace star inertia and beta (internal function). @@ -157,7 +161,7 @@ def replace_star_terms( star_inertia are star_beta are the output parameters """ forced = False - if j == 0: forced = True + if replace and j == 0: forced = False star_inertia[j] = symo.mat_replace( grandJ[j], 'MJE', j, symmet=True, forced=forced ) @@ -306,8 +310,13 @@ def compute_base_accel(robo, symo, star_inertia, star_beta, grandVp): forced = False grandVp[0] = Matrix([robo.vdot0 - robo.G, robo.w0]) if robo.is_floating: - grandVp[0] = star_inertia[0].inv() * star_beta[0] forced = True + inv_base_star_inertia = star_inertia[0].inv() + inv_base_star_inertia = symo.mat_replace( + inv_base_star_inertia, 'InvMJE', 0, + symmet=True, forced=forced + ) + grandVp[0] = inv_base_star_inertia * star_beta[0] grandVp[0][:3, 0] = symo.mat_replace( grandVp[0][:3, 0], 'VP', 0, forced=forced ) @@ -329,8 +338,14 @@ def compute_base_accel_composite( forced = False grandVp[0] = Matrix([robo.vdot0 - robo.G, robo.w0]) if robo.is_floating: - grandVp[0] = composite_inertia[0].inv() * composite_beta[0] forced = True + symo.flushout() + inv_base_comp_inertia = composite_inertia[0].inv() + inv_base_comp_inertia = symo.mat_replace( + inv_base_comp_inertia, 'InvMJE', 0, + symmet=True, forced=forced + ) + grandVp[0] = inv_base_comp_inertia * composite_beta[0] grandVp[0][:3, 0] = symo.mat_replace( grandVp[0][:3, 0], 'VP', 0, forced=forced ) @@ -436,7 +451,7 @@ def composite_inverse_dynmodel(robo, symo): ) replace_composite_terms( symo, composite_inertia, composite_beta, robo.ant[j], - composite_inertia, composite_beta + composite_inertia, composite_beta, replace=True ) # compute base acceleration : this returns the correct value for # fixed base and floating base robots @@ -619,14 +634,11 @@ def direct_dynmodel(robo, symo): compute_beta(robo, symo, j, w, beta) # first backward recursion - initialisation step for j in reversed(xrange(0, robo.NL)): - # skip base terms for non-floating robots - if robo.is_floating and j == 0: + 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) - elif j == 0: - continue replace_star_terms( symo, grandJ, beta, j, star_inertia, star_beta ) @@ -634,18 +646,14 @@ def direct_dynmodel(robo, symo): for j in reversed(xrange(0, robo.NL)): if j == 0: continue compute_tau(robo, symo, j, jaj, star_beta, tau) - # skip base terms for non-floating robots - if not robo.is_floating and robo.ant[j] == 0: - compute_hinv(robo, symo, j, jaj, star_inertia, jah, h_inv) - else: - compute_star_terms( - robo, symo, j, jaj, jTant, gamma, tau, - h_inv, jah, star_inertia, star_beta - ) - replace_star_terms( - symo, star_inertia, star_beta, robo.ant[j], - star_inertia, star_beta - ) + compute_star_terms( + robo, symo, j, jaj, jTant, gamma, tau, + h_inv, jah, star_inertia, star_beta + ) + 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( diff --git a/pysymoro/robot.py b/pysymoro/robot.py index 6aad982..7d183af 100644 --- a/pysymoro/robot.py +++ b/pysymoro/robot.py @@ -237,7 +237,12 @@ def compute_ddym(self): symo.file_open(self, 'ddm') title = "Direct Dynamic Model using Newton-Euler Algorithm" symo.write_params_table(self, title, inert=True, dynam=True) - nealgos.direct_dynmodel(self, symo) + if self.is_floating: + # with floating base + nealgos.direct_dynmodel(self, symo) + else: + # with fixed base + dynamics.compute_direct_dynamic_NE(self, symo) symo.file_close() return symo diff --git a/symoroutils/symbolmgr.py b/symoroutils/symbolmgr.py index 722d1d5..099b8e5 100644 --- a/symoroutils/symbolmgr.py +++ b/symoroutils/symbolmgr.py @@ -8,6 +8,7 @@ """This module contains the Symbol Manager tools.""" import itertools +import os from sympy import sin, cos from sympy import Symbol, Matrix, Expr @@ -416,6 +417,14 @@ def write_line(self, 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 From 19452124d8028447f79acb24cc64bca444c796cc Mon Sep 17 00:00:00 2001 From: aravind Date: Mon, 21 Jul 2014 18:16:01 +0200 Subject: [PATCH 182/273] Fix display of correct antecedent value in UI --- symoroui/layout.py | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/symoroui/layout.py b/symoroui/layout.py index f750e9a..44df0f8 100644 --- a/symoroui/layout.py +++ b/symoroui/layout.py @@ -346,9 +346,9 @@ def update_params(self, index, pars): def update_geo_params(self): pars = self._extract_param_names(ui_labels.GEOM_PARAMS) index = int(self.widgets['frame'].Value) - for par in pars[1:4]: + for par in pars[0:3]: self.widgets[par].SetValue(str(self.robo.get_val(index, par))) - self.update_params(index, pars[4:]) + self.update_params(index, pars[3:]) def update_dyn_params(self): pars = self._extract_param_names(ui_labels.DYN_PARAMS) From fab54308a87230425ab0a6d00fe2375b058d3846 Mon Sep 17 00:00:00 2001 From: aravind Date: Mon, 21 Jul 2014 19:43:03 +0200 Subject: [PATCH 183/273] Modify default values for floating base robots Modify default values for floating base robots on creation of robot. The parameters effected are base velocity, base acceleration, gravity vector, Z transformation matrix. --- pysymoro/robot.py | 62 +++++++++++++++++++++++++++++++++++++++++++--- symoroui/layout.py | 2 ++ 2 files changed, 60 insertions(+), 4 deletions(-) diff --git a/pysymoro/robot.py b/pysymoro/robot.py index 7d183af..ca1b944 100644 --- a/pysymoro/robot.py +++ b/pysymoro/robot.py @@ -111,16 +111,70 @@ def __init__(self, name, NL=0, NJ=0, NF=0, is_floating=False, 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('G3')]) + 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] - # member methods: + 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 _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 + except IndexError: + # just ignore exception + pass + + 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 = var('th{0}'.format(j)) + elif self.sigma[j] == 1: + self.r = var('r{0}'.format(j)) + elif self.sigma[j] == 2: + self.theta = 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: + self.G = Matrix([var('GX'), var('GY'), var('GZ')]) + self.v0 = Matrix([var('VX0'), var('VY0'), var('VZ0')]) + self.w0 = Matrix([var('WX0'), var('WY0'), var('WZ0')]) + self.vdot0 = Matrix([var('VPX0'), var('VPY0'), var('VPZ0')]) + self.wdot0 = Matrix([var('WPX0'), var('WPY0'), var('WPZ0')]) + # 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)) + 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) diff --git a/symoroui/layout.py b/symoroui/layout.py index 44df0f8..6cff4af 100644 --- a/symoroui/layout.py +++ b/symoroui/layout.py @@ -587,6 +587,7 @@ def OnNew(self, event): if dialog.ShowModal() == wx.ID_OK: result = dialog.get_values() new_robo = Robot(*result['init_pars']) + 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] @@ -615,6 +616,7 @@ def OnNew(self, event): new_robo.v0 = self.robo.v0 new_robo.vdot0 = self.robo.vdot0 new_robo.G = self.robo.G + new_robo.set_defaults(joint=True) new_robo.is_wmr = result['is_wmr'] self.robo = new_robo self.robo.directory = filemgr.get_folder_path(self.robo.name) From d794c9d65ae65bb3089ad5670482b8ad8905db1d Mon Sep 17 00:00:00 2001 From: aravind Date: Mon, 21 Jul 2014 20:03:27 +0200 Subject: [PATCH 184/273] Modify UI in new robot creation During new robot creation, modify to make base parameters checkbox state depend on floating base checkbox. --- symoroui/definition.py | 133 ++++++++++++++++++++++++++--------------- 1 file changed, 85 insertions(+), 48 deletions(-) diff --git a/symoroui/definition.py b/symoroui/definition.py index 36ca6d2..044ea15 100644 --- a/symoroui/definition.py +++ b/symoroui/definition.py @@ -23,37 +23,52 @@ class DialogDefinition(wx.Dialog): """Creates the dialog box to define a new robot.""" def __init__(self, prefix, name, nl, nj, structure, is_floating, is_wmr, parent=None): - super(DialogDefinition, self).__init__(parent, style=wx.SYSTEM_MENU | - wx.CAPTION | wx.CLOSE_BOX | wx.CLIP_CHILDREN) + 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_wmr, structure) self.SetTitle(prefix + ": New robot definition") def init_ui(self, name, nl, nj, is_floating, is_wmr, 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 + # title + main_sizer.Add( + wx.StaticText(self, label="Robot definition"), 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) + 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) + ) + grid.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) 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( + 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 + ) 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) + 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=[tools.SIMPLE, tools.TREE, tools.CLOSED_LOOP], @@ -63,28 +78,47 @@ def init_ui(self, name, nl, nj, is_floating, is_wmr, structure): 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_floating = wx.CheckBox(self, label=' Is Floating Base') - self.ch_is_floating.Value = is_floating - self.ch_is_wmr = wx.CheckBox( + self.chk_is_floating = wx.CheckBox( + self, label=' Is Floating Base' + ) + self.chk_is_floating.Value = is_floating + self.chk_is_floating.Bind(wx.EVT_CHECKBOX, self.OnFloating) + self.chk_is_wmr = wx.CheckBox( self, label=' Is Wheeled Mobile Robot' ) - self.ch_is_wmr.Value = is_wmr - 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_floating, 0, - wx.LEFT | wx.RIGHT | wx.ALIGN_LEFT, 15) - main_sizer.Add(self.ch_is_wmr, 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) + self.chk_is_wmr.Value = is_wmr + 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_base = wx.CheckBox( + self, label=' Keep base parameters' + ) + self.chk_keep_base.Value = True + main_sizer.Add( + self.chk_is_floating, 0, + wx.LEFT | wx.RIGHT | wx.ALIGN_LEFT, 15 + ) + main_sizer.Add( + self.chk_is_wmr, 0, + wx.LEFT | wx.RIGHT | wx.ALIGN_LEFT, 15 + ) + main_sizer.Add( + self.chk_keep_geo, 0, + wx.TOP | wx.LEFT | wx.ALIGN_LEFT, 15 + ) + main_sizer.Add( + self.chk_keep_dyn, 0, + wx.TOP | wx.LEFT | wx.ALIGN_LEFT, 15 + ) + main_sizer.Add( + self.chk_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) @@ -108,6 +142,9 @@ def OnTypeChanged(self, _): self.spin_joints.Enable(False) self.OnSpinNL(None) + def OnFloating(self, _): + self.chk_keep_base.Value = not self.chk_is_floating.Value + def OnSpinNL(self, _): self.spin_joints.SetRange(int(self.spin_links.Value), 100) if self.cmb_structure.GetSelection() != 2: @@ -120,17 +157,17 @@ def get_values(self): params = { 'init_pars': ( name, nl, nj, 2*nj - nl, - self.ch_is_floating.Value, self.cmb_structure.Value + self.chk_is_floating.Value, self.cmb_structure.Value ), 'name': name, 'num_links': nl, 'num_joints': nj, 'structure': self.cmb_structure.Value, - 'is_floating': self.ch_is_floating.Value, - 'is_wmr': self.ch_is_wmr.Value, - 'keep_geo': self.ch_keep_geo.Value, - 'keep_dyn': self.ch_keep_dyn.Value, - 'keep_base': self.ch_keep_base.Value + 'is_floating': self.chk_is_floating.Value, + 'is_wmr': self.chk_is_wmr.Value, + 'keep_geo': self.chk_keep_geo.Value, + 'keep_dyn': self.chk_keep_dyn.Value, + 'keep_base': self.chk_keep_base.Value } return params From 67efe7536208068026eb94915ffa35fb52772f58 Mon Sep 17 00:00:00 2001 From: aravind Date: Tue, 22 Jul 2014 12:34:42 +0200 Subject: [PATCH 185/273] Minor modifications --- symoroui/definition.py | 107 ++++++++++++++++++++++------------------- 1 file changed, 57 insertions(+), 50 deletions(-) diff --git a/symoroui/definition.py b/symoroui/definition.py index 044ea15..bebb69a 100644 --- a/symoroui/definition.py +++ b/symoroui/definition.py @@ -31,23 +31,23 @@ def __init__(self, prefix, name, nl, nj, structure, is_floating, self.SetTitle(prefix + ": New robot definition") def init_ui(self, name, nl, nj, is_floating, is_wmr, structure): - main_sizer = wx.BoxSizer(wx.VERTICAL) + szr_topmost = wx.BoxSizer(wx.VERTICAL) # title - main_sizer.Add( + szr_topmost.Add( wx.StaticText(self, label="Robot definition"), 0, wx.ALL | wx.ALIGN_CENTER_HORIZONTAL, 25 ) # grid - grid = wx.GridBagSizer(15, 15) - grid.Add( + 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 ) - grid.Add( + szr_grd.Add( wx.TextCtrl(self, size=(92, -1), name='name', value=name), pos=(0, 1) ) - grid.Add( + szr_grd.Add( wx.StaticText(self, label='Number of moving links:'), pos=(1, 0), flag=wx.BOTTOM | wx.TOP | wx.ALIGN_LEFT, border=2 @@ -56,16 +56,16 @@ def init_ui(self, name, nl, nj, is_floating, is_wmr, structure): 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)) - grid.Add( + 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 ) - grid.Add(self.spin_joints, pos=(2, 1)) - grid.Add( + 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 ) @@ -74,10 +74,12 @@ def init_ui(self, name, nl, nj, is_floating, is_wmr, structure): choices=[tools.SIMPLE, tools.TREE, tools.CLOSED_LOOP], value=structure ) - grid.Add(self.cmb_structure, pos=(3, 1)) + szr_grd.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) + szr_topmost.Add( + szr_grd, 0, wx.ALL | wx.ALIGN_CENTER_HORIZONTAL, 15 + ) self.chk_is_floating = wx.CheckBox( self, label=' Is Floating Base' ) @@ -99,35 +101,35 @@ def init_ui(self, name, nl, nj, is_floating, is_wmr, structure): self, label=' Keep base parameters' ) self.chk_keep_base.Value = True - main_sizer.Add( + szr_topmost.Add( self.chk_is_floating, 0, wx.LEFT | wx.RIGHT | wx.ALIGN_LEFT, 15 ) - main_sizer.Add( + szr_topmost.Add( self.chk_is_wmr, 0, wx.LEFT | wx.RIGHT | wx.ALIGN_LEFT, 15 ) - main_sizer.Add( + szr_topmost.Add( self.chk_keep_geo, 0, wx.TOP | wx.LEFT | wx.ALIGN_LEFT, 15 ) - main_sizer.Add( + szr_topmost.Add( self.chk_keep_dyn, 0, wx.TOP | wx.LEFT | wx.ALIGN_LEFT, 15 ) - main_sizer.Add( + szr_topmost.Add( self.chk_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) + 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) @@ -199,37 +201,42 @@ def construct_sym(self): self.syms.add(at) def init_ui(self): - p = scrolled.ScrolledPanel(self, -1) - vbox = wx.BoxSizer(wx.VERTICAL) + 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. - 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) + 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 - 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) + 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) From 6a68276d128d0dc81391eeffb5f8d5adff2e1729 Mon Sep 17 00:00:00 2001 From: aravind Date: Tue, 22 Jul 2014 13:10:29 +0200 Subject: [PATCH 186/273] Minor modification Modify to update joint parameters based on `sigma` value. --- symoroui/layout.py | 1 + 1 file changed, 1 insertion(+) diff --git a/symoroui/layout.py b/symoroui/layout.py index 6cff4af..8650e3e 100644 --- a/symoroui/layout.py +++ b/symoroui/layout.py @@ -349,6 +349,7 @@ def update_geo_params(self): for par in pars[0:3]: self.widgets[par].SetValue(str(self.robo.get_val(index, par))) self.update_params(index, pars[3:]) + self.robo.set_defaults(joint=True) def update_dyn_params(self): pars = self._extract_param_names(ui_labels.DYN_PARAMS) From 25b036529ea96e8ae9f8afe11455d844e1e12605 Mon Sep 17 00:00:00 2001 From: aravind Date: Tue, 22 Jul 2014 13:16:53 +0200 Subject: [PATCH 187/273] Minor modifications --- pysymoro/robot.py | 96 +++++++++++++++++++++++------------------------ 1 file changed, 48 insertions(+), 48 deletions(-) diff --git a/pysymoro/robot.py b/pysymoro/robot.py index ca1b944..5e90b78 100644 --- a/pysymoro/robot.py +++ b/pysymoro/robot.py @@ -59,8 +59,8 @@ def __init__(self, name, NL=0, NJ=0, NF=0, is_floating=False, self.sigma = [0 for i in xrange(NF + 1)] """ index of antecedent joint: list of int""" self.ant = range(-1, self.NF - 1) - """motorization, if 1, then the joint im motorized""" - self.mu = [0 for i in xrange(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""" @@ -128,52 +128,6 @@ def set_defaults(self, joint=False, geom=False, base=False): if base: self._set_base_defaults() - 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 - except IndexError: - # just ignore exception - pass - - 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 = var('th{0}'.format(j)) - elif self.sigma[j] == 1: - self.r = var('r{0}'.format(j)) - elif self.sigma[j] == 2: - self.theta = 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: - self.G = Matrix([var('GX'), var('GY'), var('GZ')]) - self.v0 = Matrix([var('VX0'), var('VY0'), var('VZ0')]) - self.w0 = Matrix([var('WX0'), var('WY0'), var('WZ0')]) - self.vdot0 = Matrix([var('VPX0'), var('VPY0'), var('VPZ0')]) - self.wdot0 = Matrix([var('WPX0'), var('WPY0'), var('WPZ0')]) - # 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)) - def put_val(self, j, name, val): try: if isinstance(val, str) or isinstance(val, unicode): @@ -646,4 +600,50 @@ def get_param_vec(self, head, j): 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 + except IndexError: + # just ignore exception + pass + + 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: + self.G = Matrix([var('GX'), var('GY'), var('GZ')]) + self.v0 = Matrix([var('VX0'), var('VY0'), var('VZ0')]) + self.w0 = Matrix([var('WX0'), var('WY0'), var('WZ0')]) + self.vdot0 = Matrix([var('VPX0'), var('VPY0'), var('VPZ0')]) + self.wdot0 = Matrix([var('WPX0'), var('WPY0'), var('WPZ0')]) + # 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)) + From 60c170b3c50a646f7bab2cc444937bc02d767418 Mon Sep 17 00:00:00 2001 From: aravind Date: Tue, 22 Jul 2014 13:24:25 +0200 Subject: [PATCH 188/273] Update README --- README.md | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/README.md b/README.md index e7d8d23..7d4d869 100644 --- a/README.md +++ b/README.md @@ -7,6 +7,9 @@ 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 ------------ @@ -34,6 +37,7 @@ 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 From 0af463fd084b60dc6d1d590afd9c5970fef00930 Mon Sep 17 00:00:00 2001 From: aravind Date: Tue, 22 Jul 2014 18:07:32 +0200 Subject: [PATCH 189/273] Modify to have consistent usage Modify to have consistent usage of `is_floating` and `is_mobile`. Remove reference to `is_wmr` and replace with `is_mobile`. Update all UI portions. Fix regex pattern to match correctly to `is_floating` and `is_mobile` terms when reading from PAR files. --- pysymoro/robot.py | 12 ++++++------ pysymoro/robotf.py | 6 +++--- symoroui/definition.py | 20 +++++++++++--------- symoroui/labels.py | 2 +- symoroui/layout.py | 12 +++++++----- symoroutils/parfile.py | 40 ++++++++++++++++++++++++++-------------- symoroutils/symbolmgr.py | 2 +- 7 files changed, 55 insertions(+), 39 deletions(-) diff --git a/pysymoro/robot.py b/pysymoro/robot.py index 5e90b78..6f00844 100644 --- a/pysymoro/robot.py +++ b/pysymoro/robot.py @@ -34,8 +34,10 @@ class Robot(object): 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_wmr=False): + def __init__( + self, name, NL=0, NJ=0, NF=0, is_floating=False, + structure=TREE, is_mobile=False + ): # member variables: """ name of the robot: string""" self.name = name @@ -43,10 +45,8 @@ def __init__(self, name, NL=0, NJ=0, NF=0, is_floating=False, self.directory = filemgr.get_folder_path(name) """ whether the base frame is floating: bool""" self.is_floating = is_floating - # backward compatability - self.is_mobile = self.is_floating - """ whether the robot is wheeled mobile robot""" - self.is_wmr = is_wmr + """ whether the robot is a mobile robot""" + self.is_mobile = is_mobile """ number of links: int""" self.nl = NL """ number of joints: int""" diff --git a/pysymoro/robotf.py b/pysymoro/robotf.py index 5c4fa4a..8de8e19 100644 --- a/pysymoro/robotf.py +++ b/pysymoro/robotf.py @@ -26,7 +26,7 @@ class FloatingRobot(object): """ def __init__( self, name, links=0, joints=0, frames=0, - is_floating=True, structure=tools.TREE, is_wmr=False, + is_floating=True, structure=tools.TREE, is_mobile=False, is_symbolic=True ): """ @@ -49,7 +49,7 @@ def __init__( """Type of the robot structure - simple, tree, closed-loop""" self.structure = structure """To indicate if the robot is a wheeled mobile robot""" - self.is_wmr = is_wmr + 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 @@ -108,7 +108,7 @@ def __str__(self): 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 + ("\tWMR: %s\n" % str(self.is_wmr)) + str_format = str_format + ("\tWMR: %s\n" % str(self.is_mobile)) str_format = str_format + ("\tStructure: %s\n" % str(self.structure)) str_format = str_format + '\n' # add geometric params diff --git a/symoroui/definition.py b/symoroui/definition.py index bebb69a..29f7c81 100644 --- a/symoroui/definition.py +++ b/symoroui/definition.py @@ -21,16 +21,18 @@ class DialogDefinition(wx.Dialog): """Creates the dialog box to define a new robot.""" - def __init__(self, prefix, name, nl, nj, structure, is_floating, - is_wmr, parent=None): + 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_wmr, structure) + 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_wmr, structure): + def init_ui(self, name, nl, nj, is_floating, is_mobile, structure): szr_topmost = wx.BoxSizer(wx.VERTICAL) # title szr_topmost.Add( @@ -85,10 +87,10 @@ def init_ui(self, name, nl, nj, is_floating, is_wmr, structure): ) self.chk_is_floating.Value = is_floating self.chk_is_floating.Bind(wx.EVT_CHECKBOX, self.OnFloating) - self.chk_is_wmr = wx.CheckBox( - self, label=' Is Wheeled Mobile Robot' + self.chk_is_mobile = wx.CheckBox( + self, label=' Is Mobile Robot' ) - self.chk_is_wmr.Value = is_wmr + self.chk_is_mobile.Value = is_mobile self.chk_keep_geo = wx.CheckBox( self, label=' Keep geometric parameters' ) @@ -106,7 +108,7 @@ def init_ui(self, name, nl, nj, is_floating, is_wmr, structure): wx.LEFT | wx.RIGHT | wx.ALIGN_LEFT, 15 ) szr_topmost.Add( - self.chk_is_wmr, 0, + self.chk_is_mobile, 0, wx.LEFT | wx.RIGHT | wx.ALIGN_LEFT, 15 ) szr_topmost.Add( @@ -166,7 +168,7 @@ def get_values(self): 'num_joints': nj, 'structure': self.cmb_structure.Value, 'is_floating': self.chk_is_floating.Value, - 'is_wmr': self.chk_is_wmr.Value, + 'is_mobile': self.chk_is_mobile.Value, 'keep_geo': self.chk_keep_geo.Value, 'keep_dyn': self.chk_keep_dyn.Value, 'keep_base': self.chk_keep_base.Value diff --git a/symoroui/labels.py b/symoroui/labels.py index 1dafaa6..446aa2a 100644 --- a/symoroui/labels.py +++ b/symoroui/labels.py @@ -130,7 +130,7 @@ ('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_wmr', FieldEntry('Is Wheeled Mobile Robot:', 'wmr', 'lbl', (6, 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)) ]) diff --git a/symoroui/layout.py b/symoroui/layout.py index 8650e3e..83ebe1e 100644 --- a/symoroui/layout.py +++ b/symoroui/layout.py @@ -396,17 +396,19 @@ def feed_data(self): ('NL', self.robo.nl), ('NJ', self.robo.nj), ('type', self.robo.structure), ('floating', self.robo.is_floating), - ('wmr', self.robo.is_wmr), + ('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(int(not self.robo.is_mobile), - self.robo.NL) + 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']), @@ -583,7 +585,7 @@ def OnNew(self, event): 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_wmr + self.robo.is_floating, self.robo.is_mobile ) if dialog.ShowModal() == wx.ID_OK: result = dialog.get_values() @@ -618,7 +620,7 @@ def OnNew(self, event): new_robo.vdot0 = self.robo.vdot0 new_robo.G = self.robo.G new_robo.set_defaults(joint=True) - new_robo.is_wmr = result['is_wmr'] + new_robo.is_mobile = result['is_mobile'] self.robo = new_robo self.robo.directory = filemgr.get_folder_path(self.robo.name) self.feed_data() diff --git a/symoroutils/parfile.py b/symoroutils/parfile.py index 1795850..d31390a 100644 --- a/symoroutils/parfile.py +++ b/symoroutils/parfile.py @@ -50,7 +50,7 @@ def _extract_vals(robo, key, line): line = line.replace('}', '') if key in _ZERO_BASED: k = 0 - elif robo.is_mobile and key in _NL: + elif (robo.is_floating or robo.is_mobile) and key in _NL: k = 0 else: k = 1 @@ -71,23 +71,24 @@ def _extract_vals(robo, key, line): def _write_par_list(robo, f, key, N0, N): - f.write('%s = {%s' % (key, robo.get_val(N0, key))) + f.write('{0} = {{{1}'.format(key, robo.get_val(N0, key))) for i in xrange(N0 + 1, N): - f.write(',%s' % robo.get_val(i, key)) + f.write(',{0}'.format(robo.get_val(i, key))) f.write('}\n') def writepar(robo): fname = filemgr.make_file_path(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' % tools.TYPES.index(robo.structure)) - f.write('is_mobile = %s\n' % int(robo.is_mobile)) + 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)) f.write('\n(* Geometric parameters *)\n') - if robo.is_mobile: + if robo.is_floating or robo.is_mobile: N0 = 0 else: N0 = 1 @@ -118,6 +119,7 @@ def readpar(robo_name, file_path): #initialize the Robot instance f.seek(0) d = {} + is_floating = False is_mobile = False for line in f.readlines(): for s in ('NJ', 'NL', 'Type'): @@ -125,16 +127,26 @@ def readpar(robo_name, file_path): if match: d[s] = int(match.group(1)) continue - match = re.match(r'^is_mobile.*=([\d\s]*)(\(\*.*)?', line) + # 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'] - robo = robot.Robot(robo_name, d['NL'], d['NJ'], NF, - is_mobile, tools.TYPES[d['Type']]) + robo = robot.Robot( + name=robo_name, + NL=d['NL'], NJ=d['NJ'], NF=NF, + is_floating=is_floating, + structure=tools.TYPES[d['Type']], + is_mobile=is_mobile + ) robo.directory = os.path.dirname(file_path) - #fitting the data + # fitting the data acc_line = '' key = '' f.seek(0) diff --git a/symoroutils/symbolmgr.py b/symoroutils/symbolmgr.py index 099b8e5..a422966 100644 --- a/symoroutils/symbolmgr.py +++ b/symoroutils/symbolmgr.py @@ -355,7 +355,7 @@ def write_params_table(self, robo, title='', geom=True, inert=False, self.write_param('Geometric parameters', robo.get_geom_head(), robo, range(1, robo.NF)) if inert: - if robo.is_mobile: + if robo.is_floating or robo.is_mobile: start_frame = 0 else: start_frame = 1 From 25772f04a75214d319b6a0a1dd19a7ff8cdd5c92 Mon Sep 17 00:00:00 2001 From: aravind Date: Tue, 22 Jul 2014 18:12:00 +0200 Subject: [PATCH 190/273] Minor modification --- pysymoro/robotf.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/pysymoro/robotf.py b/pysymoro/robotf.py index 8de8e19..1c217fa 100644 --- a/pysymoro/robotf.py +++ b/pysymoro/robotf.py @@ -108,7 +108,7 @@ def __str__(self): 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 + ("\tWMR: %s\n" % str(self.is_mobile)) + 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 From 2a1b8516167499aa8bb44d5c4daeaca754f6038c Mon Sep 17 00:00:00 2001 From: aravind Date: Wed, 23 Jul 2014 12:51:14 +0200 Subject: [PATCH 191/273] Re-organise code in `pysymoro/dynamics.py` --- pysymoro/dynamics.py | 350 ++++++++++++++++++++++--------------------- 1 file changed, 178 insertions(+), 172 deletions(-) diff --git a/pysymoro/dynamics.py b/pysymoro/dynamics.py index 1338ebd..db601cd 100644 --- a/pysymoro/dynamics.py +++ b/pysymoro/dynamics.py @@ -63,81 +63,14 @@ def default_newton_euler(robo, symo): 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) + 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 = ParamsInit.init_vec(robo) - Njnt = ParamsInit.init_vec(robo) - # init file output, writing the robot description - symo = symbolmgr.SymbolManager() - 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 = 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 = '%s' + 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_wrench(robo_tmp, symo, k, w, wdot, U, vdot, F, N, name) - # init external forces - Fex = ParamsInit.init_vec(robo) - Nex = ParamsInit.init_vec(robo) - for j in reversed(xrange(1, k + 1)): - compute_joint_wrench(robo_tmp, symo, j, antRj, antPj, - vdot, Fjnt, Njnt, F, N, Fex, Nex, name) - for j in xrange(1, k + 1): - compute_torque(robo_tmp, symo, j, Fjnt, Njnt, 'DG' + name) - # 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 compute_direct_dynamic_NE(robo, symo): # antecedent angular velocity, projected into jth frame wi = ParamsInit.init_vec(robo) @@ -180,31 +113,6 @@ def compute_direct_dynamic_NE(robo, symo): return robo.qddot -def direct_dynamic_NE(robo): - """ - OLD FUNCTION. NOT TO BE USED. - 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 - """ - symo = symbolmgr.SymbolManager() - symo.file_open(robo, 'ddm') - title = 'Direct dynamic model using Newton - Euler Algorith' - symo.write_params_table(robo, title, inert=True, dynam=True) - compute_direct_dynamic_NE(robo, symo) - symo.file_close() - return symo - - def compute_inertia_matrix(symo, robo, forced=False): Jplus, MSplus, Mplus = ParamsInit.init_jplus(robo) AJE1 = ParamsInit.init_vec(robo) @@ -254,57 +162,6 @@ def inertia_matrix(robo): return symo -def inverse_dynamic_NE(robo): - """ - OLD FUNCTION. NOT TO BE USED. - 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 = symbolmgr.SymbolManager() - symo.file_open(robo, 'idm') - title = 'Inverse dynamic model using Newton - Euler Algorith' - symo.write_params_table(robo, title, inert=True, dynam=True) - default_newton_euler(robo, symo) - symo.file_close() - return symo - - -def pseudo_force_NE(robo): - """ - OLD FUNCTION. NOT TO BE USED. - 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 = symbolmgr.SymbolManager() - symo.file_open(robo, 'ccg') - title = 'Pseudo forces using Newton - Euler Algorith' - symo.write_params_table(robo, title, inert=True, dynam=True) - default_newton_euler(robo_pseudo, symo) - symo.file_close() - return symo - def get_symbol(symbol, name): if name is None: @@ -312,6 +169,7 @@ def get_symbol(symbol, name): else: return symbol + name + def compute_wrench(robo, symo, j, w, wdot, U, vdot, F, N, name=None): """Internal function. Computes total wrench (torques and forces) of the link j @@ -363,31 +221,6 @@ def compute_torque(robo, symo, j, Fjnt, Njnt, name=None): symo.replace(tau_total, name, forced=True) -def inertia_spatial(J, MS, M): - return Matrix([(M*sympy.eye(3)).row_join(tools.skew(MS).T), - tools.skew(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 : 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 compute_beta(robo, symo, j, w, beta_star): """Internal function. Computes link's wrench when the joint accelerations are zero @@ -575,6 +408,179 @@ def compute_A_triangle(robo, symo, j, k, ka, antRj, antPj, f, n, A, AJE1): A[ka-1, j-1] = A[j-1, ka-1] +def inertia_spatial(J, MS, M): + return Matrix([ + (M*sympy.eye(3)).row_join(tools.skew(MS).T), + tools.skew(MS).row_join(J) + ]) + + +def inverse_dynamic_NE(robo): + """ + OLD FUNCTION. NOT TO BE USED. + 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 = symbolmgr.SymbolManager() + symo.file_open(robo, 'idm') + title = 'Inverse dynamic model using Newton - Euler Algorith' + symo.write_params_table(robo, title, inert=True, dynam=True) + default_newton_euler(robo, symo) + symo.file_close() + return symo + + +def direct_dynamic_NE(robo): + """ + OLD FUNCTION. NOT TO BE USED. + 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 + """ + symo = symbolmgr.SymbolManager() + symo.file_open(robo, 'ddm') + title = 'Direct dynamic model using Newton - Euler Algorith' + symo.write_params_table(robo, title, inert=True, dynam=True) + compute_direct_dynamic_NE(robo, symo) + symo.file_close() + return symo + + +def pseudo_force_NE(robo): + """ + OLD FUNCTION. NOT TO BE USED. + 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 = symbolmgr.SymbolManager() + symo.file_open(robo, 'ccg') + title = 'Pseudo forces using Newton - Euler Algorith' + symo.write_params_table(robo, title, inert=True, dynam=True) + default_newton_euler(robo_pseudo, symo) + symo.file_close() + return symo + + +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 = ParamsInit.init_vec(robo) + Njnt = ParamsInit.init_vec(robo) + # init file output, writing the robot description + symo = symbolmgr.SymbolManager() + 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 = 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 = '%s' + 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_wrench(robo_tmp, symo, k, w, wdot, U, vdot, F, N, name) + # init external forces + Fex = ParamsInit.init_vec(robo) + Nex = ParamsInit.init_vec(robo) + for j in reversed(xrange(1, k + 1)): + compute_joint_wrench(robo_tmp, symo, j, antRj, antPj, + vdot, Fjnt, Njnt, F, N, Fex, Nex, name) + for j in xrange(1, k + 1): + compute_torque(robo_tmp, symo, j, Fjnt, Njnt, 'DG' + name) + # 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 compute_joint_torque_deriv(symo, param, arg, index): + """Internal function. Computes joint reactive torques + in case 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) + + # TODO:Finish base parameters computation def base_paremeters(robo_orig): """Computes grouped inertia parameters. New parametrization From 17bdd586f041f670735f48f91679d8dfc5c35198 Mon Sep 17 00:00:00 2001 From: aravind Date: Wed, 23 Jul 2014 12:55:09 +0200 Subject: [PATCH 192/273] Minor modification --- pysymoro/dynamics.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/pysymoro/dynamics.py b/pysymoro/dynamics.py index db601cd..1eb4576 100644 --- a/pysymoro/dynamics.py +++ b/pysymoro/dynamics.py @@ -202,7 +202,7 @@ def compute_joint_wrench( Njnt[j] = N[j] + Nex[j] + (tools.skew(robo.MS[j]) * vdot[j]) Njnt[j] = symo.mat_replace(Njnt[j], get_symbol('N', name), j) f_ant = symo.mat_replace(antRj[j]*Fjnt[j], get_symbol('FDI', name), j) - if robo.ant[j] != - 1: + if robo.ant[j] != -1: Fex[robo.ant[j]] += f_ant Nex[robo.ant[j]] += antRj[j]*Njnt[j] + tools.skew(antPj[j])*f_ant From 5d009e695ce3ed337051649827a4673acee7fe26 Mon Sep 17 00:00:00 2001 From: aravind Date: Wed, 23 Jul 2014 13:50:58 +0200 Subject: [PATCH 193/273] Add IDyM computation for mobile robots Add inverse dynamic model computation for mobile robots. Move inverse dynamic model computation for fixed base robots to `pysymoro/nealgos.py` and its dependency functions. Add docstring to various Newton-Euler functions. Update `direct_dynmodel()` function name to `floating_direct_dynmodel()` in `pysymoro/nealgos.py` --- pysymoro/dynamics.py | 1 - pysymoro/nealgos.py | 186 ++++++++++++++++++++++++++++++++++++++----- 2 files changed, 167 insertions(+), 20 deletions(-) diff --git a/pysymoro/dynamics.py b/pysymoro/dynamics.py index 1eb4576..a575021 100644 --- a/pysymoro/dynamics.py +++ b/pysymoro/dynamics.py @@ -162,7 +162,6 @@ def inertia_matrix(robo): return symo - def get_symbol(symbol, name): if name is None: return symbol diff --git a/pysymoro/nealgos.py b/pysymoro/nealgos.py index a0647c4..ceef4ef 100644 --- a/pysymoro/nealgos.py +++ b/pysymoro/nealgos.py @@ -1,11 +1,13 @@ # -*- 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, Transform +from pysymoro.geometry import compute_rot_trans from pysymoro.kinematics import compute_vel_acc from pysymoro.kinematics import compute_omega from symoroutils import symbolmgr @@ -23,14 +25,85 @@ def inertia_spatial(inertia, ms_tensor, mass): ]) +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, Fjnt, Njnt, F, N, 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). - Notes - ===== - beta is the output parameter + Note: + beta is the output parameter """ expr1 = robo.J[j] * w[j] expr1 = symo.mat_replace(expr1, 'JW', j) @@ -49,9 +122,8 @@ def compute_gamma(robo, symo, j, antRj, antPj, w, wi, gamma): """ Compute gyroscopic acceleration (internal function). - Notes - ===== - gamma is the output parameter + Note: + gamma is the output parameter """ expr1 = tools.skew(wi[j]) * Matrix([0, 0, robo.qdot[j]]) expr1 = symo.mat_replace(expr1, 'WQ', j) @@ -370,23 +442,83 @@ def compute_reaction_wrench( react_wrench[j][3:, 0] = symo.mat_replace(wrench[3:, 0], 'N', j) -def compute_torque(robo, symo, j, jaj, react_wrench, torque): +def fixed_inverse_dynmodel(robo, symo): """ - Compute torque (internal function). + Compute the Inverse Dynamic Model using Newton-Euler algorithm for + tree structure robots with fixed base. - Note: - torque is the output parameter. + Parameters: + robo: Robot - instance of robot description container + symo: symbolmgr.SymbolManager - instance of symbolic manager """ - 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) + # 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, + Fjnt, Njnt, F, N, 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, + Fjnt, Njnt, F, N, 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) @@ -472,6 +604,14 @@ def composite_inverse_dynmodel(robo, symo): 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) @@ -590,7 +730,15 @@ def flexible_inverse_dynmodel(robo, symo): compute_torque(robo, symo, j, jaj, react_wrench, torque) -def direct_dynmodel(robo, symo): +def floating_direct_dynmodel(robo, symo): + """ + Compute the Direct Dynamic Model using Newton-Euler algorithm for + robots with 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) From 76a949f3ea3c46620365a0a7b25465f9b55518ca Mon Sep 17 00:00:00 2001 From: aravind Date: Wed, 23 Jul 2014 13:54:51 +0200 Subject: [PATCH 194/273] Update `Robot` class methods Update `Robot` class methods to call the correct dynamic model functions from `nealgos` module. --- pysymoro/robot.py | 7 +++++-- 1 file changed, 5 insertions(+), 2 deletions(-) diff --git a/pysymoro/robot.py b/pysymoro/robot.py index 6f00844..ef4d372 100644 --- a/pysymoro/robot.py +++ b/pysymoro/robot.py @@ -230,9 +230,12 @@ def compute_idym(self): elif self.is_floating: # with rigid joints and floating base nealgos.composite_inverse_dynmodel(self, symo) + elif self.is_mobile: + # mobile robot with rigid joints - known base acceleration + nealgos.mobile_inverse_dynmodel(self, symo) else: # with rigid joints and fixed base - dynamics.default_newton_euler(self, symo) + nealgos.fixed_inverse_dynmodel(self, symo) symo.file_close() return symo @@ -247,7 +250,7 @@ def compute_ddym(self): symo.write_params_table(self, title, inert=True, dynam=True) if self.is_floating: # with floating base - nealgos.direct_dynmodel(self, symo) + nealgos.floating_direct_dynmodel(self, symo) else: # with fixed base dynamics.compute_direct_dynamic_NE(self, symo) From a60e19535fe040adbb77dbbce40b631b840f7fd0 Mon Sep 17 00:00:00 2001 From: aravind Date: Wed, 23 Jul 2014 14:06:20 +0200 Subject: [PATCH 195/273] Update UI and set correct base params for mobile robots --- pysymoro/robot.py | 2 +- symoroui/definition.py | 10 +++++++--- 2 files changed, 8 insertions(+), 4 deletions(-) diff --git a/pysymoro/robot.py b/pysymoro/robot.py index ef4d372..819b0e1 100644 --- a/pysymoro/robot.py +++ b/pysymoro/robot.py @@ -636,7 +636,7 @@ 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: + if self.is_floating or self.is_mobile: self.G = Matrix([var('GX'), var('GY'), var('GZ')]) self.v0 = Matrix([var('VX0'), var('VY0'), var('VZ0')]) self.w0 = Matrix([var('WX0'), var('WY0'), var('WZ0')]) diff --git a/symoroui/definition.py b/symoroui/definition.py index 29f7c81..1793c46 100644 --- a/symoroui/definition.py +++ b/symoroui/definition.py @@ -86,11 +86,12 @@ def init_ui(self, name, nl, nj, is_floating, is_mobile, structure): self, label=' Is Floating Base' ) self.chk_is_floating.Value = is_floating - self.chk_is_floating.Bind(wx.EVT_CHECKBOX, self.OnFloating) + self.chk_is_floating.Bind(wx.EVT_CHECKBOX, self.OnBaseParamsNo) self.chk_is_mobile = wx.CheckBox( self, label=' Is Mobile Robot' ) self.chk_is_mobile.Value = is_mobile + self.chk_is_mobile.Bind(wx.EVT_CHECKBOX, self.OnBaseParamsNo) self.chk_keep_geo = wx.CheckBox( self, label=' Keep geometric parameters' ) @@ -146,8 +147,11 @@ def OnTypeChanged(self, _): self.spin_joints.Enable(False) self.OnSpinNL(None) - def OnFloating(self, _): - self.chk_keep_base.Value = not self.chk_is_floating.Value + def OnBaseParamsNo(self, _): + value = True + if self.chk_is_floating.Value or self.chk_is_mobile.Value: + value = False + self.chk_keep_base.Value = value def OnSpinNL(self, _): self.spin_joints.SetRange(int(self.spin_links.Value), 100) From 90b24090824d7ccf3b65317274fd4f88fcf3f059 Mon Sep 17 00:00:00 2001 From: aravind Date: Wed, 23 Jul 2014 14:14:05 +0200 Subject: [PATCH 196/273] Fix UI bugs on new mobile robot creation --- symoroui/definition.py | 5 +---- symoroui/layout.py | 11 +++++++++-- 2 files changed, 10 insertions(+), 6 deletions(-) diff --git a/symoroui/definition.py b/symoroui/definition.py index 1793c46..095d839 100644 --- a/symoroui/definition.py +++ b/symoroui/definition.py @@ -163,13 +163,10 @@ def get_values(self): nl = int(self.spin_links.Value) nj = int(self.spin_joints.Value) params = { - 'init_pars': ( - name, nl, nj, 2*nj - nl, - self.chk_is_floating.Value, self.cmb_structure.Value - ), 'name': name, 'num_links': nl, 'num_joints': nj, + 'num_frames': (2 * nj) -nl, 'structure': self.cmb_structure.Value, 'is_floating': self.chk_is_floating.Value, 'is_mobile': self.chk_is_mobile.Value, diff --git a/symoroui/layout.py b/symoroui/layout.py index 83ebe1e..4478c37 100644 --- a/symoroui/layout.py +++ b/symoroui/layout.py @@ -589,7 +589,15 @@ def OnNew(self, event): ) if dialog.ShowModal() == wx.ID_OK: result = dialog.get_values() - new_robo = Robot(*result['init_pars']) + 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) @@ -620,7 +628,6 @@ def OnNew(self, event): new_robo.vdot0 = self.robo.vdot0 new_robo.G = self.robo.G new_robo.set_defaults(joint=True) - new_robo.is_mobile = result['is_mobile'] self.robo = new_robo self.robo.directory = filemgr.get_folder_path(self.robo.name) self.feed_data() From 2faf43101a5028c4208a352a8c3669c6b9ac64c3 Mon Sep 17 00:00:00 2001 From: aravind Date: Wed, 23 Jul 2014 16:21:05 +0200 Subject: [PATCH 197/273] Fix inertia matrix computation Fix inertia matrix computation so that A11, A12 and A22 matrices are computed correctly. Specifically this fix ensures all columns of A12 matrix are computed. Update symbols to indicate A11 matrix by `Jcomp` in the output file. --- pysymoro/dynamics.py | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/pysymoro/dynamics.py b/pysymoro/dynamics.py index a575021..4cee9f2 100644 --- a/pysymoro/dynamics.py +++ b/pysymoro/dynamics.py @@ -136,7 +136,7 @@ def compute_inertia_matrix(symo, robo, forced=False): antRj, antPj, f, n, A, AJE1) symo.mat_replace(A, 'A', forced=forced, symmet=True) J_base = inertia_spatial(Jplus[0], MSplus[0], Mplus[0]) - symo.mat_replace(J_base, 'JP', 0, forced=forced, symmet=True) + symo.mat_replace(J_base, 'Jcomp', 0, forced=forced, symmet=True) return A @@ -395,8 +395,8 @@ def compute_A_triangle(robo, symo, j, k, ka, antRj, antPj, f, n, A, AJE1): else: n[ka] = antRj[k]*n[k] + tools.skew(antPj[k])*antRj[k]*f[k] if ka == 0: - symo.mat_replace(f[ka], 'AV0') - symo.mat_replace(n[ka], 'AW0') + symo.mat_replace(f[ka], 'AV0', j, forced=True) + symo.mat_replace(n[ka], 'AW0', j, forced=True) else: symo.mat_replace(f[ka], 'E' + chars[j], ka) symo.mat_replace(n[ka], 'N' + chars[j], ka) From cfc39bd1f8da837e8e459c1a5adaab8fd3d2d843 Mon Sep 17 00:00:00 2001 From: aravind Date: Wed, 23 Jul 2014 16:25:17 +0200 Subject: [PATCH 198/273] Add `pysymoro/inertia.py` file --- pysymoro/inertia.py | 0 1 file changed, 0 insertions(+), 0 deletions(-) create mode 100644 pysymoro/inertia.py diff --git a/pysymoro/inertia.py b/pysymoro/inertia.py new file mode 100644 index 0000000..e69de29 From 28aaf2427f5e5a70bdd559ec7a77c67219347fc4 Mon Sep 17 00:00:00 2001 From: aravind Date: Wed, 23 Jul 2014 18:26:34 +0200 Subject: [PATCH 199/273] Refactor code - inertia matrix computation Move all inertia matrix computation functions from `pysymoro/dynamics.py` to `pysymoro/inertia.py`. --- pysymoro/dynamics.py | 158 +++++++----------------------------- pysymoro/inertia.py | 189 +++++++++++++++++++++++++++++++++++++++++++ pysymoro/nealgos.py | 1 - 3 files changed, 216 insertions(+), 132 deletions(-) diff --git a/pysymoro/dynamics.py b/pysymoro/dynamics.py index 4cee9f2..3fa2c15 100644 --- a/pysymoro/dynamics.py +++ b/pysymoro/dynamics.py @@ -11,28 +11,21 @@ """ +from copy import copy, deepcopy + import sympy from sympy import Matrix -from copy import copy, deepcopy from pysymoro.geometry import compute_screw_transform from pysymoro.geometry import compute_rot_trans, Transform from pysymoro.kinematics import compute_vel_acc from pysymoro.kinematics import compute_omega +from pysymoro import inertia from symoroutils import symbolmgr from symoroutils import tools from symoroutils.paramsinit import ParamsInit -chars = ('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') - inert_names = ('XXR', 'XYR', 'XZR', 'YYR', 'YZR', 'ZZR', 'MXR', 'MYR', 'MZR', 'MR') @@ -113,55 +106,6 @@ def compute_direct_dynamic_NE(robo, symo): return robo.qddot -def compute_inertia_matrix(symo, robo, forced=False): - Jplus, MSplus, Mplus = ParamsInit.init_jplus(robo) - AJE1 = ParamsInit.init_vec(robo) - f = ParamsInit.init_vec(robo, ext=1) - n = ParamsInit.init_vec(robo, ext=1) - A = sympy.zeros(robo.nl, robo.nl) - # init transformation - antRj, antPj = compute_rot_trans(robo, symo) - for j in reversed(xrange(robo.NL)): - replace_Jplus(robo, symo, j, Jplus, MSplus, Mplus) - if j != 0: - 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 != 0: - 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=forced, symmet=True) - J_base = inertia_spatial(Jplus[0], MSplus[0], Mplus[0]) - symo.mat_replace(J_base, 'Jcomp', 0, forced=forced, symmet=True) - return A - - -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 - """ - symo = symbolmgr.SymbolManager() - symo.file_open(robo, 'inm') - title = 'Inertia Matrix using composite links' - symo.write_params_table(robo, title, inert=True, dynam=True) - compute_inertia_matrix(symo, robo, forced=True) - symo.file_close() - return symo - - def get_symbol(symbol, name): if name is None: return symbol @@ -335,78 +279,6 @@ def compute_coupled_forces(robo, symo, j, grandVp, grandJ, beta_star): 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 = tools.skew(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*tools.skew(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-1, j-1] = 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-1, j-1] = 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] + tools.skew(antPj[k])*antRj[k]*f[k] - else: - n[ka] = antRj[k]*n[k] + tools.skew(antPj[k])*antRj[k]*f[k] - if ka == 0: - symo.mat_replace(f[ka], 'AV0', j, forced=True) - symo.mat_replace(n[ka], 'AW0', j, forced=True) - 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-1, ka-1] = n[ka][2] - elif robo.sigma[ka] == 1: - A[j-1, ka-1] = f[ka][2] - A[ka-1, j-1] = A[j-1, ka-1] - - def inertia_spatial(J, MS, M): return Matrix([ (M*sympy.eye(3)).row_join(tools.skew(MS).T), @@ -439,6 +311,30 @@ def inverse_dynamic_NE(robo): return symo +def inertia_matrix(robo): + """ + OLD FUNCTION. NOT TO BE USED. + 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 + """ + symo = symbolmgr.SymbolManager() + symo.file_open(robo, 'inm') + title = 'Inertia Matrix using composite links' + symo.write_params_table(robo, title, inert=True, dynam=True) + inertia.compute_inertia_matrix(robo, symo) + symo.file_close() + return symo + + def direct_dynamic_NE(robo): """ OLD FUNCTION. NOT TO BE USED. diff --git a/pysymoro/inertia.py b/pysymoro/inertia.py index e69de29..5a1c55d 100644 --- a/pysymoro/inertia.py +++ b/pysymoro/inertia.py @@ -0,0 +1,189 @@ +# -*- 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): + """ + 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 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] + symo.mat_replace(forces[j], 'E' + CHARSYMS[j], 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_a22 +): + """ + Compute elements below and above diagonal of the inertia matrix + (internal function). + + Note: + forces, moments, 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: + symo.mat_replace(forces[ka], 'AV0', j, forced=True) + 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] since A11 = A12 = 0. + """ + pass + + +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] + """ + 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_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_a22 + ) + symo.mat_replace(inertia_a22, 'A', forced=True, symmet=True) + inertia_a11 = inertia_spatial( + comp_inertia3[0], comp_ms[0], comp_mass[0] + ) + symo.mat_replace(inertia_a11, 'Jcomp', 0, forced=True, symmet=True) + return inertia_a22 + + diff --git a/pysymoro/nealgos.py b/pysymoro/nealgos.py index ceef4ef..0cbc495 100644 --- a/pysymoro/nealgos.py +++ b/pysymoro/nealgos.py @@ -10,7 +10,6 @@ from pysymoro.geometry import compute_rot_trans from pysymoro.kinematics import compute_vel_acc from pysymoro.kinematics import compute_omega -from symoroutils import symbolmgr from symoroutils import tools from symoroutils.paramsinit import ParamsInit From c59b381876d842326893e9e1a2731c311023bb7d Mon Sep 17 00:00:00 2001 From: aravind Date: Wed, 23 Jul 2014 19:18:21 +0200 Subject: [PATCH 200/273] Update UI call for inertia matrix Update UI menu call for inertia matrix. Add `compute_inertiamatrix()` method to `Robot` class. --- pysymoro/robot.py | 14 ++++++++++++++ symoroui/layout.py | 2 +- 2 files changed, 15 insertions(+), 1 deletion(-) diff --git a/pysymoro/robot.py b/pysymoro/robot.py index 819b0e1..bd94c3b 100644 --- a/pysymoro/robot.py +++ b/pysymoro/robot.py @@ -21,6 +21,7 @@ from sympy import Mul, Add, factor, zeros, var, sympify, eye from pysymoro import dynamics +from pysymoro import inertia from pysymoro import nealgos from symoroutils import filemgr from symoroutils import symbolmgr @@ -239,6 +240,19 @@ def compute_idym(self): 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 = "Direct Dynamic Model using Newton-Euler Algorithm" + symo.write_params_table(self, title, inert=True, dynam=True) + inertia.floating_inertia_matrix(self, symo) + symo.file_close() + return symo + def compute_ddym(self): """ Compute the Direct Dynamic Model of the robot using the diff --git a/symoroui/layout.py b/symoroui/layout.py index 4478c37..49ed87d 100644 --- a/symoroui/layout.py +++ b/symoroui/layout.py @@ -793,7 +793,7 @@ def OnInverseDynamic(self, event): self.model_success('idm') def OnInertiaMatrix(self, event): - dynamics.inertia_matrix(self.robo) + self.robo.compute_inertiamatrix() self.model_success('inm') def OnCentrCoriolGravTorq(self, event): From dd3ff03803fb53f197cb799fd0464dddfc615bbf Mon Sep 17 00:00:00 2001 From: aravind Date: Wed, 23 Jul 2014 19:40:44 +0200 Subject: [PATCH 201/273] Modify to return the entire inertia matrix Modify to return the entire inertia matrix of size [(6+n) x (6+n)]. --- pysymoro/inertia.py | 44 +++++++++++++++++++++++++++++++------------- 1 file changed, 31 insertions(+), 13 deletions(-) diff --git a/pysymoro/inertia.py b/pysymoro/inertia.py index 5a1c55d..157298b 100644 --- a/pysymoro/inertia.py +++ b/pysymoro/inertia.py @@ -32,7 +32,7 @@ def inertia_spatial(inertia, ms_tensor, mass): """ - Compute spatial inertia matrix (internal function). + Setup spatial inertia matrix (internal function). """ return Matrix([ (mass * sympy.eye(3)).row_join(tools.skew(ms_tensor).transpose()), @@ -90,7 +90,7 @@ def compute_diagonal_elements( Compute diagonal elements of the inertia matrix (internal function). Note: - forces, moments, inertia_a22 are the output parameters + 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]) @@ -100,20 +100,21 @@ def compute_diagonal_elements( 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] - symo.mat_replace(forces[j], 'E' + CHARSYMS[j], j) - symo.mat_replace(moments[j], 'N' + CHARSYMS[j], 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_a22 + 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_a22 are the output parameters + forces, moments, inertia_a12, inertia_a22 are the output + parameters """ forces[ka] = antRj[k] * forces[k] if k == j and robo.sigma[j] == 0: @@ -123,8 +124,12 @@ def compute_triangle_elements( moments[ka] = (antRj[k] * moments[k]) + \ (tools.skew(antPj[k]) * antRj[k] * forces[k]) if ka == 0: - symo.mat_replace(forces[ka], 'AV0', j, forced=True) - symo.mat_replace(moments[ka], 'AW0', j, forced=True) + 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) @@ -154,6 +159,7 @@ def floating_inertia_matrix(robo, symo): 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) @@ -176,14 +182,26 @@ def floating_inertia_matrix(robo, symo): k = ka ka = robo.ant[ka] compute_triangle_elements( - robo, symo, j, k, ka, antRj, antPj, - aje1, forces, moments, inertia_a22 + 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] ) - symo.mat_replace(inertia_a11, 'Jcomp', 0, forced=True, symmet=True) - return inertia_a22 + 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 completer inertia matrix + inertia = Matrix([ + inertia_a11.row_join(a12mat), + a12mat.transpose().row_join(inertia_a22) + ]) + return inertia From 7575672b8af940f7e407a06f14e63369767de899 Mon Sep 17 00:00:00 2001 From: aravind Date: Thu, 24 Jul 2014 13:24:59 +0200 Subject: [PATCH 202/273] Update to compute inertia matrix for fixed base robots Add function `fixed_inertia_matrix` in `pysymoro/inertia.py` to compute the inertia matrix for fixed base robots. Update `Robot` class to call the correct function accordingly. --- pysymoro/inertia.py | 39 ++++++++++++++++++++++++++++++++++++--- pysymoro/robot.py | 20 +++++++++++++------- 2 files changed, 49 insertions(+), 10 deletions(-) diff --git a/pysymoro/inertia.py b/pysymoro/inertia.py index 157298b..529a7ee 100644 --- a/pysymoro/inertia.py +++ b/pysymoro/inertia.py @@ -144,9 +144,41 @@ 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] since A11 = A12 = 0. + A = [A11, A12; A12.transpose(), A22]. """ - pass + # 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 != 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): @@ -155,6 +187,7 @@ def floating_inertia_matrix(robo, symo): 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) @@ -197,7 +230,7 @@ def floating_inertia_matrix(robo, symo): for j in xrange(1, robo.NL): a12mat[:, j] = inertia_a12[j] a12mat = a12mat[:, 1:] - # setup the completer inertia matrix + # setup the complete inertia matrix inertia = Matrix([ inertia_a11.row_join(a12mat), a12mat.transpose().row_join(inertia_a22) diff --git a/pysymoro/robot.py b/pysymoro/robot.py index bd94c3b..cef8794 100644 --- a/pysymoro/robot.py +++ b/pysymoro/robot.py @@ -247,9 +247,12 @@ def compute_inertiamatrix(self): """ symo = symbolmgr.SymbolManager() symo.file_open(self, 'inm') - title = "Direct Dynamic Model using Newton-Euler Algorithm" + title = "Inertia matrix using Composite links algorithm" symo.write_params_table(self, title, inert=True, dynam=True) - inertia.floating_inertia_matrix(self, symo) + if self.is_floating or self.is_mobile: + inertia.floating_inertia_matrix(self, symo) + else: + inertia.fixed_inertia_matrix(self, symo) symo.file_close() return symo @@ -280,17 +283,20 @@ def compute_pseudotorques(self): pseudorobo.qddot = zeros(pseudorobo.NL, 1) symo = symbolmgr.SymbolManager() symo.file_open(self, 'ccg') - title = 'Pseudo forces using Newton - Euler Algorithm' + title = 'Pseudo forces using Newton-Euler Algorithm' symo.write_params_table(self, title, inert=True, dynam=True) - if 1 in self.eta: + if 1 in pseudorobo.eta: # with flexible joints - pass - elif self.is_floating: + nealgos.flexible_inverse_dynmodel(pseudorobo, symo) + elif pseudorobo.is_floating: # with rigid joints and floating base nealgos.composite_inverse_dynmodel(pseudorobo, symo) + elif pseudorobo.is_mobile: + # mobile robot with rigid joints - known base acceleration + nealgos.mobile_inverse_dynmodel(pseudorobo, symo) else: # with rigid joints and fixed base - dynamics.default_newton_euler(pseudorobo, symo) + nealgos.fixed_inverse_dynmodel(pseudorobo, symo) symo.file_close() return symo From 84d5da90f95218e2b5e40e0278da87251076d8bd Mon Sep 17 00:00:00 2001 From: aravind Date: Thu, 24 Jul 2014 14:59:11 +0200 Subject: [PATCH 203/273] Fix UI bugs on `sigma` change --- pysymoro/robot.py | 10 ++++++++++ symoroui/layout.py | 3 ++- symoroutils/samplerobots.py | 26 +++++++++++++++----------- 3 files changed, 27 insertions(+), 12 deletions(-) diff --git a/pysymoro/robot.py b/pysymoro/robot.py index cef8794..1a9a406 100644 --- a/pysymoro/robot.py +++ b/pysymoro/robot.py @@ -634,6 +634,10 @@ def _set_joint_defaults(self): 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 @@ -645,11 +649,17 @@ def _set_geom_defaults(self): """ for j in xrange(1, self.NF): if self.sigma[j] == 0: + self.mu[j] = 1 self.theta[j] = var('th{0}'.format(j)) + self.r[j] = 0 elif self.sigma[j] == 1: + self.mu[j] = 1 + self.theta[j] = 0 self.r[j] = var('r{0}'.format(j)) elif self.sigma[j] == 2: self.mu[j] = 0 + self.theta[j] = 0 + self.r[j] = 0 def _set_base_defaults(self): """ diff --git a/symoroui/layout.py b/symoroui/layout.py index 49ed87d..4676b89 100644 --- a/symoroui/layout.py +++ b/symoroui/layout.py @@ -344,12 +344,13 @@ def update_params(self, index, pars): 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.robo.set_defaults(joint=True) + self.update_joint_params() def update_dyn_params(self): pars = self._extract_param_names(ui_labels.DYN_PARAMS) diff --git a/symoroutils/samplerobots.py b/symoroutils/samplerobots.py index 38956e7..9c7f3c6 100644 --- a/symoroutils/samplerobots.py +++ b/symoroutils/samplerobots.py @@ -26,8 +26,8 @@ def cart_pole(): 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.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 @@ -41,17 +41,21 @@ def cart_pole(): 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 = [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 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 = var('O, R1, Th2') - robo.qdot = var('O, R1d, Th2d') - robo.qddot = var('O, R1dd, Th2dd') + robo.q = [0, var('r1'), var('th2')] + robo.qdot = [0, var('r1d'), var('th2d')] + robo.qddot = [0, var('r1dd'), var('th2dd')] return robo @@ -88,9 +92,9 @@ def planar2r(): robo.wdot0 = zeros(3, 1) robo.v0 = zeros(3, 1) robo.vdot0 = zeros(3, 1) - robo.q = var('O, q1, q2') - robo.qdot = var('O, QP1, QP2') - robo.qddot = var('O, QDP1, QDP2') + robo.q = [0, var('q1'), var('q2')] + robo.qdot = [0, var('QP1'), var('QP2')] + robo.qddot = [0, var('QDP1'), var('QDP2')] return robo From 38f61b01582c7faf167b2ee1f7d024544ad711ea Mon Sep 17 00:00:00 2001 From: aravind Date: Thu, 24 Jul 2014 16:27:19 +0200 Subject: [PATCH 204/273] Modify to enable/disable menu items based on robot type --- symoroui/labels.py | 14 ++++++------- symoroui/layout.py | 50 +++++++++++++++++++++++++++++++++++++++------- 2 files changed, 50 insertions(+), 14 deletions(-) diff --git a/symoroui/labels.py b/symoroui/labels.py index 446aa2a..6a0b3fd 100644 --- a/symoroui/labels.py +++ b/symoroui/labels.py @@ -136,7 +136,7 @@ # menu bar # TODO: add event handlers to dict entries as well -MAIN_MENU = dict( +MAIN_MENU = OrderedDict( file_menu="&File", geom_menu="&Geometric", kin_menu="&Kinematic", @@ -145,19 +145,19 @@ optim_menu="&Optimiser", viz_menu="&Visualisation" ) -VIZ_MENU = dict(m_viz="Visualisation") -IDEN_MENU = dict( +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 = dict( +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 = dict( +KIN_MENU = OrderedDict( m_jac_matrix="Jacobian matrix", m_determinant="Determinant of a Jacobian", m_kin_constraint="Kinematic constraint equation of loops", @@ -165,13 +165,13 @@ m_acc="Accelerations", m_jpqp="Jpqp" ) -GEOM_MENU = dict( +GEOM_MENU = OrderedDict( m_trans_matrix="Transformation matrix", m_fast_dgm="Fast Geometric model", m_igm_paul="IGM - Paul method", m_geom_constraint="Geometric constraint equation of loops" ) -FILE_MENU = dict( +FILE_MENU = OrderedDict( m_new="&New", m_open="&Open", m_save="&Save", diff --git a/symoroui/layout.py b/symoroui/layout.py index 4676b89..3e12e3a 100644 --- a/symoroui/layout.py +++ b/symoroui/layout.py @@ -424,11 +424,47 @@ def feed_data(self): 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): - """Method to create the menu bar""" + """Create the menu bar""" menu_bar = wx.MenuBar() # menu item - file file_menu = wx.Menu() @@ -500,12 +536,6 @@ def create_menu(self): ) self.Bind(wx.EVT_MENU, self.OnDeterminant, m_determinant) kin_menu.AppendItem(m_determinant) - #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) m_vel = wx.MenuItem( kin_menu, wx.ID_ANY, ui_labels.KIN_MENU['m_vel'] ) @@ -521,6 +551,12 @@ def create_menu(self): ) 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() From ed9ec4ab4d66dee5c2666f4b9a1014f121fce5ad Mon Sep 17 00:00:00 2001 From: aravind Date: Thu, 24 Jul 2014 21:36:42 +0200 Subject: [PATCH 205/273] Rename file Rename `pysymoro/tests/test_dynmodel.py` to `pysymoro/tests/check_dynmodel.py`. --- pysymoro/tests/{test_dynmodel.py => check_dynmodel.py} | 0 1 file changed, 0 insertions(+), 0 deletions(-) rename pysymoro/tests/{test_dynmodel.py => check_dynmodel.py} (100%) diff --git a/pysymoro/tests/test_dynmodel.py b/pysymoro/tests/check_dynmodel.py similarity index 100% rename from pysymoro/tests/test_dynmodel.py rename to pysymoro/tests/check_dynmodel.py From e65732e12a68f8c8e815470d21f60356150a5c2b Mon Sep 17 00:00:00 2001 From: aravind Date: Fri, 25 Jul 2014 03:11:15 +0200 Subject: [PATCH 206/273] Modify new robot creation UI Modify new robot creation UI such that the base type is mutually exclusive. --- symoroui/definition.py | 37 ++++++++++++++++--------------------- 1 file changed, 16 insertions(+), 21 deletions(-) diff --git a/symoroui/definition.py b/symoroui/definition.py index 095d839..cc18b9c 100644 --- a/symoroui/definition.py +++ b/symoroui/definition.py @@ -82,16 +82,13 @@ def init_ui(self, name, nl, nj, is_floating, is_mobile, structure): szr_topmost.Add( szr_grd, 0, wx.ALL | wx.ALIGN_CENTER_HORIZONTAL, 15 ) - self.chk_is_floating = wx.CheckBox( - self, label=' Is Floating Base' + # 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.chk_is_floating.Value = is_floating - self.chk_is_floating.Bind(wx.EVT_CHECKBOX, self.OnBaseParamsNo) - self.chk_is_mobile = wx.CheckBox( - self, label=' Is Mobile Robot' - ) - self.chk_is_mobile.Value = is_mobile - self.chk_is_mobile.Bind(wx.EVT_CHECKBOX, self.OnBaseParamsNo) + self.rbx_base_type.Bind(wx.EVT_RADIOBOX, self.OnBaseType) self.chk_keep_geo = wx.CheckBox( self, label=' Keep geometric parameters' ) @@ -105,12 +102,8 @@ def init_ui(self, name, nl, nj, is_floating, is_mobile, structure): ) self.chk_keep_base.Value = True szr_topmost.Add( - self.chk_is_floating, 0, - wx.LEFT | wx.RIGHT | wx.ALIGN_LEFT, 15 - ) - szr_topmost.Add( - self.chk_is_mobile, 0, - wx.LEFT | wx.RIGHT | wx.ALIGN_LEFT, 15 + self.rbx_base_type, 0, + wx.LEFT | wx.RIGHT | wx.ALIGN_CENTER, 15 ) szr_topmost.Add( self.chk_keep_geo, 0, @@ -147,10 +140,9 @@ def OnTypeChanged(self, _): self.spin_joints.Enable(False) self.OnSpinNL(None) - def OnBaseParamsNo(self, _): - value = True - if self.chk_is_floating.Value or self.chk_is_mobile.Value: - value = False + 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, _): @@ -162,14 +154,17 @@ 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': self.chk_is_floating.Value, - 'is_mobile': self.chk_is_mobile.Value, + 'is_floating': is_floating, + 'is_mobile': is_mobile, 'keep_geo': self.chk_keep_geo.Value, 'keep_dyn': self.chk_keep_dyn.Value, 'keep_base': self.chk_keep_base.Value From d6bfa68795ce13b197c76bb5853ead6cf5f42210 Mon Sep 17 00:00:00 2001 From: aravind Date: Sun, 27 Jul 2014 21:58:02 +0200 Subject: [PATCH 207/273] Re-organise code --- pysymoro/dynamics.py | 265 +++++++++++++++++++++++-------------------- pysymoro/nealgos.py | 11 +- 2 files changed, 151 insertions(+), 125 deletions(-) diff --git a/pysymoro/dynamics.py b/pysymoro/dynamics.py index 3fa2c15..7820d3c 100644 --- a/pysymoro/dynamics.py +++ b/pysymoro/dynamics.py @@ -30,38 +30,93 @@ 'ZZR', 'MXR', 'MYR', 'MZR', 'MR') -def default_newton_euler(robo, symo): - """Internal function. Computes Inverse Dynamic Model using +def dynamic_identification_NE(robo): + """Computes Dynamic Identification Model using Newton-Euler formulation Parameters ========== robo : Robot Instance of robot description container - symo : symbolmgr.SymbolManager - Instance of symbolic manager + + Returns + ======= + symo.sydi : dictionary + Dictionary with the information of all the sybstitution """ - # init external forces - Fex = copy(robo.Fex) - Nex = copy(robo.Nex) + + # init forces vectors + Fjnt = ParamsInit.init_vec(robo) + Njnt = ParamsInit.init_vec(robo) + # init file output, writing the robot description + symo = symbolmgr.SymbolManager() + 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) - # init forces vectors - F = ParamsInit.init_vec(robo) - N = ParamsInit.init_vec(robo) - Fjnt = ParamsInit.init_vec(robo) - Njnt = ParamsInit.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) + # 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 = 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 = '%s' + 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_wrench(robo_tmp, symo, k, w, wdot, U, vdot, F, N, name) + # init external forces + Fex = ParamsInit.init_vec(robo) + Nex = ParamsInit.init_vec(robo) + for j in reversed(xrange(1, k + 1)): + compute_joint_wrench(robo_tmp, symo, j, antRj, antPj, + vdot, Fjnt, Njnt, F, N, Fex, Nex, name) + for j in xrange(1, k + 1): + compute_torque(robo_tmp, symo, j, Fjnt, Njnt, 'DG' + name) + # 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 compute_joint_torque_deriv(symo, param, arg, index): + """Internal function. Computes joint reactive torques + in case 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 compute_direct_dynamic_NE(robo, symo): @@ -114,7 +169,9 @@ def get_symbol(symbol, name): def compute_wrench(robo, symo, j, w, wdot, U, vdot, F, N, name=None): - """Internal function. Computes total wrench (torques and forces) + """ + AVAILABLE: nealgos - compute_dynamic_wrench() + Internal function. Computes total wrench (torques and forces) of the link j Notes @@ -133,7 +190,9 @@ def compute_joint_wrench( robo, symo, j, antRj, antPj, vdot, Fjnt, Njnt, F, N, Fex, Nex, name=None ): - """Internal function. Computes wrench (torques and forces) + """ + AVAILABLE: nealgos - compute_joint_wrench() + Internal function. Computes wrench (torques and forces) of the joint j Notes @@ -151,7 +210,9 @@ def compute_joint_wrench( def compute_torque(robo, symo, j, Fjnt, Njnt, name=None): - """Internal function. Computes actuation torques - projection of + """ + AVAILABLE: nealgos - compute_joint_torque() + Internal function. Computes actuation torques - projection of joint wrench on the joint axis """ if robo.sigma[j] != 2: @@ -165,7 +226,9 @@ def compute_torque(robo, symo, j, Fjnt, Njnt, name=None): def compute_beta(robo, symo, j, w, beta_star): - """Internal function. Computes link's wrench when + """ + AVAILABLE: nealgos - compute_beta(). + Internal function. Computes link's wrench when the joint accelerations are zero Notes @@ -182,7 +245,9 @@ def compute_beta(robo, symo, j, w, beta_star): def compute_link_acc(robo, symo, j, antRj, antPj, link_acc, w, wi): - """Internal function. Computes link's accelerations when + """ + AVAILABLE: nealgos - compute_gamma() + Internal function. Computes link's accelerations when the joint accelerations are zero Notes @@ -202,7 +267,9 @@ def compute_link_acc(robo, symo, j, antRj, antPj, link_acc, w, wi): def replace_beta_J_star(robo, symo, j, grandJ, beta_star): - """Internal function. Makes symbol substitution in beta_star + """ + AVIALABLE: nealgos - replace_star_terms() + Internal function. Makes symbol substitution in beta_star and grandJ """ grandJ[j] = symo.mat_replace(grandJ[j], 'MJE', j, symmet=True) @@ -210,7 +277,9 @@ def replace_beta_J_star(robo, symo, j, grandJ, beta_star): def compute_Tau(robo, symo, j, grandJ, beta_star, jaj, juj, H_inv, Tau): - """Internal function. Computes intermediat dynamic variables + """ + SIMILAR: nealgos - compute_tau(), compute_star_terms() + Internal function. Computes intermediat dynamic variables Notes ===== @@ -231,7 +300,9 @@ def compute_Tau(robo, symo, j, grandJ, beta_star, jaj, juj, H_inv, Tau): def compute_beta_J_star(robo, symo, j, grandJ, jaj, juj, Tau, beta_star, jTant, link_acc): - """Internal function. Computes intermediat dynamic variables + """ + SIMILAR: nealgos - compute_star_terms() + Internal function. Computes intermediat dynamic variables Notes ===== @@ -251,7 +322,9 @@ def compute_beta_J_star(robo, symo, j, grandJ, jaj, juj, Tau, def compute_acceleration(robo, symo, j, jTant, grandVp, juj, H_inv, jaj, Tau, link_acc): - """Internal function. Computes joint accelerations and links' twists + """ + SIMILAR: nealgos - compute_joint_accel(), compute_link_accel() + Internal function. Computes joint accelerations and links' twists Notes ===== @@ -271,7 +344,9 @@ def compute_acceleration(robo, symo, j, jTant, grandVp, def compute_coupled_forces(robo, symo, j, grandVp, grandJ, beta_star): - """Internal function. + """ + AVIALBLE: nealgos - compute_reaction_wrench() + Internal function. """ E3 = symo.mat_replace(grandJ[j]*grandVp[j], 'DY', j) couplforce = E3 - beta_star[j] @@ -280,12 +355,51 @@ def compute_coupled_forces(robo, symo, j, grandVp, grandJ, beta_star): def inertia_spatial(J, MS, M): + """ + AVAILABLE: nealgos - inertia_spatial() + """ return Matrix([ (M*sympy.eye(3)).row_join(tools.skew(MS).T), tools.skew(MS).row_join(J) ]) +def default_newton_euler(robo, symo): + """ + AVAILABLE: nealgos - fixed_inverse_dynmodel() + Internal function. Computes Inverse Dynamic Model using + Newton-Euler formulation + + 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) + 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 inverse_dynamic_NE(robo): """ OLD FUNCTION. NOT TO BE USED. @@ -387,95 +501,6 @@ def pseudo_force_NE(robo): return symo -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 = ParamsInit.init_vec(robo) - Njnt = ParamsInit.init_vec(robo) - # init file output, writing the robot description - symo = symbolmgr.SymbolManager() - 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 = 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 = '%s' + 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_wrench(robo_tmp, symo, k, w, wdot, U, vdot, F, N, name) - # init external forces - Fex = ParamsInit.init_vec(robo) - Nex = ParamsInit.init_vec(robo) - for j in reversed(xrange(1, k + 1)): - compute_joint_wrench(robo_tmp, symo, j, antRj, antPj, - vdot, Fjnt, Njnt, F, N, Fex, Nex, name) - for j in xrange(1, k + 1): - compute_torque(robo_tmp, symo, j, Fjnt, Njnt, 'DG' + name) - # 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 compute_joint_torque_deriv(symo, param, arg, index): - """Internal function. Computes joint reactive torques - in case 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) - - # TODO:Finish base parameters computation def base_paremeters(robo_orig): """Computes grouped inertia parameters. New parametrization diff --git a/pysymoro/nealgos.py b/pysymoro/nealgos.py index 0cbc495..8de2826 100644 --- a/pysymoro/nealgos.py +++ b/pysymoro/nealgos.py @@ -73,7 +73,7 @@ def compute_dynamic_wrench(robo, symo, j, w, wdot, U, vdot, F, N): def compute_joint_wrench( - robo, symo, j, antRj, antPj, vdot, Fjnt, Njnt, F, N, Fex, Nex + robo, symo, j, antRj, antPj, vdot, F, N, Fjnt, Njnt, Fex, Nex ): """ Compute reaction wrench (for default Newton-Euler) of joint j @@ -124,12 +124,13 @@ def compute_gamma(robo, symo, j, antRj, antPj, w, wi, gamma): 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[robo.ant[j]]) * antPj[j] - expr5 = tools.skew(w[robo.ant[j]]) * expr4 + 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) @@ -469,7 +470,7 @@ def fixed_inverse_dynmodel(robo, symo): for j in reversed(xrange(1, robo.NL)): compute_joint_wrench( robo, symo, j, antRj, antPj, vdot, - Fjnt, Njnt, F, N, Fex, Nex + F, N, Fjnt, Njnt, Fex, Nex ) for j in xrange(1, robo.NL): compute_joint_torque(robo, symo, j, Fjnt, Njnt, torque) @@ -503,7 +504,7 @@ def mobile_inverse_dynmodel(robo, symo): for j in reversed(xrange(0, robo.NL)): compute_joint_wrench( robo, symo, j, antRj, antPj, vdot, - Fjnt, Njnt, F, N, Fex, Nex + F, N, Fjnt, Njnt, Fex, Nex ) for j in xrange(1, robo.NL): compute_joint_torque(robo, symo, j, Fjnt, Njnt, torque) From 6fdc176cb84ca7323fc0d7bc333ae9e0c62e7408 Mon Sep 17 00:00:00 2001 From: aravind Date: Sun, 27 Jul 2014 22:44:05 +0200 Subject: [PATCH 208/273] Remove unnecessary code --- pysymoro/dynamics.py | 174 ++++++------------------------------------- 1 file changed, 21 insertions(+), 153 deletions(-) diff --git a/pysymoro/dynamics.py b/pysymoro/dynamics.py index 7820d3c..d61e387 100644 --- a/pysymoro/dynamics.py +++ b/pysymoro/dynamics.py @@ -81,19 +81,24 @@ def dynamic_identification_NE(robo): Fex = ParamsInit.init_vec(robo) Nex = ParamsInit.init_vec(robo) for j in reversed(xrange(1, k + 1)): - compute_joint_wrench(robo_tmp, symo, j, antRj, antPj, - vdot, Fjnt, Njnt, F, N, Fex, Nex, name) + compute_joint_wrench( + robo_tmp, symo, j, antRj, antPj, vdot, + Fjnt, Njnt, F, N, Fex, Nex, name + ) for j in xrange(1, k + 1): compute_torque(robo_tmp, symo, j, Fjnt, Njnt, 'DG' + name) # 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) + 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 @@ -161,11 +166,11 @@ def compute_direct_dynamic_NE(robo, symo): return robo.qddot -def get_symbol(symbol, name): +def get_symbol(symbol, name, idx): if name is None: return symbol else: - return symbol + name + return symbol + name.format(idx) def compute_wrench(robo, symo, j, w, wdot, U, vdot, F, N, name=None): @@ -179,11 +184,11 @@ def compute_wrench(robo, symo, j, w, wdot, U, vdot, F, N, name=None): 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], get_symbol('F', name), j) + F[j] = symo.mat_replace(F[j], get_symbol('F', name, j)) Psi = robo.J[j] * w[j] - Psi = symo.mat_replace(Psi, get_symbol('PSI', name), j) + Psi = symo.mat_replace(Psi, get_symbol('PSI', name, j)) N[j] = (robo.J[j] * wdot[j]) + (tools.skew(w[j]) * Psi) - N[j] = symo.mat_replace(N[j], get_symbol('No', name), j) + N[j] = symo.mat_replace(N[j], get_symbol('No', name, j)) def compute_joint_wrench( @@ -200,10 +205,10 @@ def compute_joint_wrench( Fjnt, Njnt, Fex, Nex are the output parameters """ Fjnt[j] = F[j] + Fex[j] - Fjnt[j] = symo.mat_replace(Fjnt[j], get_symbol('E', name), j) + Fjnt[j] = symo.mat_replace(Fjnt[j], get_symbol('E', name, j)) Njnt[j] = N[j] + Nex[j] + (tools.skew(robo.MS[j]) * vdot[j]) - Njnt[j] = symo.mat_replace(Njnt[j], get_symbol('N', name), j) - f_ant = symo.mat_replace(antRj[j]*Fjnt[j], get_symbol('FDI', name), j) + Njnt[j] = symo.mat_replace(Njnt[j], get_symbol('N', name, j)) + f_ant = symo.mat_replace(antRj[j]*Fjnt[j], get_symbol('FDI', name, j)) if robo.ant[j] != -1: Fex[robo.ant[j]] += f_ant Nex[robo.ant[j]] += antRj[j]*Njnt[j] + tools.skew(antPj[j])*f_ant @@ -364,143 +369,6 @@ def inertia_spatial(J, MS, M): ]) -def default_newton_euler(robo, symo): - """ - AVAILABLE: nealgos - fixed_inverse_dynmodel() - Internal function. Computes Inverse Dynamic Model using - Newton-Euler formulation - - 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) - 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 inverse_dynamic_NE(robo): - """ - OLD FUNCTION. NOT TO BE USED. - 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 = symbolmgr.SymbolManager() - symo.file_open(robo, 'idm') - title = 'Inverse dynamic model using Newton - Euler Algorith' - symo.write_params_table(robo, title, inert=True, dynam=True) - default_newton_euler(robo, symo) - symo.file_close() - return symo - - -def inertia_matrix(robo): - """ - OLD FUNCTION. NOT TO BE USED. - 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 - """ - symo = symbolmgr.SymbolManager() - symo.file_open(robo, 'inm') - title = 'Inertia Matrix using composite links' - symo.write_params_table(robo, title, inert=True, dynam=True) - inertia.compute_inertia_matrix(robo, symo) - symo.file_close() - return symo - - -def direct_dynamic_NE(robo): - """ - OLD FUNCTION. NOT TO BE USED. - 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 - """ - symo = symbolmgr.SymbolManager() - symo.file_open(robo, 'ddm') - title = 'Direct dynamic model using Newton - Euler Algorith' - symo.write_params_table(robo, title, inert=True, dynam=True) - compute_direct_dynamic_NE(robo, symo) - symo.file_close() - return symo - - -def pseudo_force_NE(robo): - """ - OLD FUNCTION. NOT TO BE USED. - 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 = symbolmgr.SymbolManager() - symo.file_open(robo, 'ccg') - title = 'Pseudo forces using Newton - Euler Algorith' - symo.write_params_table(robo, title, inert=True, dynam=True) - default_newton_euler(robo_pseudo, symo) - symo.file_close() - return symo - - # TODO:Finish base parameters computation def base_paremeters(robo_orig): """Computes grouped inertia parameters. New parametrization From 5265751f0fad5abbac7536b218625c02d9c66981 Mon Sep 17 00:00:00 2001 From: aravind Date: Sun, 27 Jul 2014 23:07:16 +0200 Subject: [PATCH 209/273] Fix symbols naming issue in dynamic identification model --- pysymoro/dynamics.py | 76 ++++++++++++++++++++------------------------ 1 file changed, 35 insertions(+), 41 deletions(-) diff --git a/pysymoro/dynamics.py b/pysymoro/dynamics.py index d61e387..2c72cf6 100644 --- a/pysymoro/dynamics.py +++ b/pysymoro/dynamics.py @@ -70,23 +70,25 @@ def dynamic_identification_NE(robo): if param_vec[i] == tools.ZERO: continue # change link names according to current non-zero parameter - name = '%s' + str(param_vec[i]) + name = '{0}' + 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_wrench(robo_tmp, symo, k, w, wdot, U, vdot, F, N, name) + compute_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_joint_wrench( - robo_tmp, symo, j, antRj, antPj, vdot, - Fjnt, Njnt, F, N, Fex, Nex, name + robo_tmp, symo, name, j, antRj, antPj, + vdot, F, N, Fjnt, Njnt, Fex, Nex ) for j in xrange(1, k + 1): - compute_torque(robo_tmp, symo, j, Fjnt, Njnt, 'DG' + name) + compute_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 @@ -167,23 +169,17 @@ def compute_direct_dynamic_NE(robo, symo): def get_symbol(symbol, name, idx): - if name is None: - return symbol - else: - return symbol + name.format(idx) + return symbol + name.format(idx) -def compute_wrench(robo, symo, j, w, wdot, U, vdot, F, N, name=None): +def compute_wrench(robo, symo, name, j, w, wdot, U, vdot, F, N): """ - AVAILABLE: nealgos - compute_dynamic_wrench() - Internal function. Computes total wrench (torques and forces) - of the link j + Compute total wrench of link j (internal function). - Notes - ===== - F, N are the output parameters + Note: + F, N are the output parameters """ - F[j] = robo.M[j]*vdot[j] + U[j]*robo.MS[j] + F[j] = (robo.M[j] * vdot[j]) + (U[j] * robo.MS[j]) F[j] = symo.mat_replace(F[j], get_symbol('F', name, j)) Psi = robo.J[j] * w[j] Psi = symo.mat_replace(Psi, get_symbol('PSI', name, j)) @@ -192,42 +188,40 @@ def compute_wrench(robo, symo, j, w, wdot, U, vdot, F, N, name=None): def compute_joint_wrench( - robo, symo, j, antRj, antPj, vdot, - Fjnt, Njnt, F, N, Fex, Nex, name=None + robo, symo, name, j, antRj, antPj, vdot, F, N, Fjnt, Njnt, Fex, Nex ): """ - AVAILABLE: nealgos - compute_joint_wrench() - Internal function. Computes wrench (torques and forces) - of the joint j + Compute reaction wrench (for default Newton-Euler) of joint j + (internal function). - Notes - ===== - Fjnt, Njnt, Fex, Nex are the output parameters + Note: + Fjnt, Njnt, Fex, Nex are the output parameters """ + i = robo.ant[j] Fjnt[j] = F[j] + Fex[j] Fjnt[j] = symo.mat_replace(Fjnt[j], get_symbol('E', name, j)) Njnt[j] = N[j] + Nex[j] + (tools.skew(robo.MS[j]) * vdot[j]) Njnt[j] = symo.mat_replace(Njnt[j], get_symbol('N', name, j)) - f_ant = symo.mat_replace(antRj[j]*Fjnt[j], get_symbol('FDI', name, j)) - if robo.ant[j] != -1: - Fex[robo.ant[j]] += f_ant - Nex[robo.ant[j]] += antRj[j]*Njnt[j] + tools.skew(antPj[j])*f_ant + f_ant = antRj[j] * Fjnt[j] + f_ant = symo.mat_replace(f_ant, get_symbol('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_torque(robo, symo, j, Fjnt, Njnt, name=None): +def compute_torque(robo, symo, name, j, Fjnt, Njnt): """ - AVAILABLE: nealgos - compute_joint_torque() - Internal function. Computes actuation torques - projection of - joint wrench on the joint axis + Compute actuator torques - projection of joint wrench on the joint + axis (internal function). """ - 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) - if name is None: - name = str(robo.GAM[j]) - else: - name = name % j - symo.replace(tau_total, name, forced=True) + 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_beta(robo, symo, j, w, beta_star): From 2da827943fe83c55ed8d22ce34cc6603250334b9 Mon Sep 17 00:00:00 2001 From: aravind Date: Sun, 27 Jul 2014 23:08:29 +0200 Subject: [PATCH 210/273] Add new file - `pysymoro/dyniden.py` --- pysymoro/dyniden.py | 0 1 file changed, 0 insertions(+), 0 deletions(-) create mode 100644 pysymoro/dyniden.py diff --git a/pysymoro/dyniden.py b/pysymoro/dyniden.py new file mode 100644 index 0000000..e69de29 From f8c3ed4beeae408e33aa8e514488ff16d332019c Mon Sep 17 00:00:00 2001 From: aravind Date: Sun, 27 Jul 2014 23:28:18 +0200 Subject: [PATCH 211/273] Add `compute_dynidenmodel()` method to `Robot` class --- pysymoro/robot.py | 15 ++++++++++++++- 1 file changed, 14 insertions(+), 1 deletion(-) diff --git a/pysymoro/robot.py b/pysymoro/robot.py index 1a9a406..f9e37a3 100644 --- a/pysymoro/robot.py +++ b/pysymoro/robot.py @@ -21,6 +21,7 @@ from sympy import Mul, Add, factor, zeros, var, sympify, eye from pysymoro import dynamics +from pysymoro import dyniden from pysymoro import inertia from pysymoro import nealgos from symoroutils import filemgr @@ -283,7 +284,7 @@ def compute_pseudotorques(self): pseudorobo.qddot = zeros(pseudorobo.NL, 1) symo = symbolmgr.SymbolManager() symo.file_open(self, 'ccg') - title = 'Pseudo forces using Newton-Euler Algorithm' + title = "Pseudo forces using Newton-Euler Algorithm" symo.write_params_table(self, title, inert=True, dynam=True) if 1 in pseudorobo.eta: # with flexible joints @@ -300,6 +301,18 @@ def compute_pseudotorques(self): symo.file_close() return symo + def compute_dynidenmodel(self): + """ + Compute the Dynamic Identification model of the robot. + """ + symo = symbolmgr.SymbolManager() + symo.file_open(robo, 'dim') + title = "Dynamic Identification Model (Newton-Euler method)" + symo.write_params_table(robo, 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 From a1eca9e4839f8b55cd1ba1b2496ec4986e2f241b Mon Sep 17 00:00:00 2001 From: aravind Date: Mon, 28 Jul 2014 00:56:16 +0200 Subject: [PATCH 212/273] Re-factor dynamic identification model code Move dynamic identification model code to `pysymoro/dyniden.py`. --- pysymoro/dynamics.py | 157 +----------------------------------------- pysymoro/dyniden.py | 158 +++++++++++++++++++++++++++++++++++++++++++ 2 files changed, 160 insertions(+), 155 deletions(-) diff --git a/pysymoro/dynamics.py b/pysymoro/dynamics.py index 2c72cf6..fd72d8f 100644 --- a/pysymoro/dynamics.py +++ b/pysymoro/dynamics.py @@ -11,7 +11,7 @@ """ -from copy import copy, deepcopy +from copy import copy import sympy from sympy import Matrix @@ -20,7 +20,6 @@ from pysymoro.geometry import compute_rot_trans, Transform from pysymoro.kinematics import compute_vel_acc from pysymoro.kinematics import compute_omega -from pysymoro import inertia from symoroutils import symbolmgr from symoroutils import tools from symoroutils.paramsinit import ParamsInit @@ -30,102 +29,6 @@ 'ZZR', 'MXR', 'MYR', 'MZR', 'MR') -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 = ParamsInit.init_vec(robo) - Njnt = ParamsInit.init_vec(robo) - # init file output, writing the robot description - symo = symbolmgr.SymbolManager() - 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 = 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 = '{0}' + 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_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_joint_wrench( - robo_tmp, symo, name, j, antRj, antPj, - vdot, F, N, Fjnt, Njnt, Fex, Nex - ) - for j in xrange(1, k + 1): - compute_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 - 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 compute_joint_torque_deriv(symo, param, arg, index): - """Internal function. Computes joint reactive torques - in case 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 compute_direct_dynamic_NE(robo, symo): # antecedent angular velocity, projected into jth frame wi = ParamsInit.init_vec(robo) @@ -168,62 +71,6 @@ def compute_direct_dynamic_NE(robo, symo): return robo.qddot -def get_symbol(symbol, name, idx): - return symbol + name.format(idx) - - -def compute_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] = symo.mat_replace(F[j], get_symbol('F', name, j)) - Psi = robo.J[j] * w[j] - Psi = symo.mat_replace(Psi, get_symbol('PSI', name, j)) - N[j] = (robo.J[j] * wdot[j]) + (tools.skew(w[j]) * Psi) - N[j] = symo.mat_replace(N[j], get_symbol('No', name, j)) - - -def compute_joint_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] = symo.mat_replace(Fjnt[j], get_symbol('E', name, j)) - Njnt[j] = N[j] + Nex[j] + (tools.skew(robo.MS[j]) * vdot[j]) - Njnt[j] = symo.mat_replace(Njnt[j], get_symbol('N', name, j)) - f_ant = antRj[j] * Fjnt[j] - f_ant = symo.mat_replace(f_ant, get_symbol('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_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_beta(robo, symo, j, w, beta_star): """ AVAILABLE: nealgos - compute_beta(). @@ -402,7 +249,7 @@ def base_paremeters(robo_orig): pass elif robo.sigma[j] == 2: # fixed joint, group everuthing - compute_lambda(robo, symo, j, antRj, antPj) + compute_lambda(robo, symo, j, antRj, antPj, lam) group_param_fix(robo, symo, j, lam) pass symo.write_line('*=*') diff --git a/pysymoro/dyniden.py b/pysymoro/dyniden.py index e69de29..bf124d5 100644 --- a/pysymoro/dyniden.py +++ b/pysymoro/dyniden.py @@ -0,0 +1,158 @@ +# -*- 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 _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 get_symbol(symbol, name, idx): + return symbol + name.format(idx) + + +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] = symo.mat_replace(F[j], get_symbol('F', name, j)) + Psi = robo.J[j] * w[j] + Psi = symo.mat_replace(Psi, get_symbol('PSI', name, j)) + N[j] = (robo.J[j] * wdot[j]) + (tools.skew(w[j]) * Psi) + N[j] = symo.mat_replace(N[j], get_symbol('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] = symo.mat_replace(Fjnt[j], get_symbol('E', name, j)) + Njnt[j] = N[j] + Nex[j] + (tools.skew(robo.MS[j]) * vdot[j]) + Njnt[j] = symo.mat_replace(Njnt[j], get_symbol('N', name, j)) + f_ant = antRj[j] * Fjnt[j] + f_ant = symo.mat_replace(f_ant, get_symbol('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_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)) + + +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) + # 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) + for k in xrange(1, 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 = '{0}' + 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 + ) + 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 + _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 + + From cdffda2279ba1b4e2a9200115b855e12e9d2cc2e Mon Sep 17 00:00:00 2001 From: aravind Date: Mon, 28 Jul 2014 01:02:10 +0200 Subject: [PATCH 213/273] Update to UI calls Update UI to call the correct dynamic identification model function. --- pysymoro/dyniden.py | 38 ++++++++++++++++++-------------------- symoroui/layout.py | 2 +- 2 files changed, 19 insertions(+), 21 deletions(-) diff --git a/pysymoro/dyniden.py b/pysymoro/dyniden.py index bf124d5..95e2466 100644 --- a/pysymoro/dyniden.py +++ b/pysymoro/dyniden.py @@ -21,26 +21,6 @@ from symoroutils import tools - - -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 get_symbol(symbol, name, idx): return symbol + name.format(idx) @@ -97,6 +77,24 @@ def _compute_joint_torque(robo, symo, name, j, Fjnt, Njnt): symo.replace(tau_total, get_symbol('DG', name, j)) +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 diff --git a/symoroui/layout.py b/symoroui/layout.py index 3e12e3a..dd48927 100644 --- a/symoroui/layout.py +++ b/symoroui/layout.py @@ -846,7 +846,7 @@ def OnBaseInertialParams(self, event): self.model_success('regp') def OnDynIdentifModel(self, event): - dynamics.dynamic_identification_NE(self.robo) + self.robo.compute_dynidenmodel() self.model_success('dim') def OnVisualisation(self, event): From 3fab0b40f6c24f2b8891984eaff63906671da081 Mon Sep 17 00:00:00 2001 From: aravind Date: Mon, 28 Jul 2014 01:05:46 +0200 Subject: [PATCH 214/273] Fix object names --- pysymoro/robot.py | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/pysymoro/robot.py b/pysymoro/robot.py index f9e37a3..8378a6d 100644 --- a/pysymoro/robot.py +++ b/pysymoro/robot.py @@ -306,9 +306,9 @@ def compute_dynidenmodel(self): Compute the Dynamic Identification model of the robot. """ symo = symbolmgr.SymbolManager() - symo.file_open(robo, 'dim') + symo.file_open(self, 'dim') title = "Dynamic Identification Model (Newton-Euler method)" - symo.write_params_table(robo, title, inert=True, dynam=True) + symo.write_params_table(self, title, inert=True, dynam=True) dyniden.dynamic_identification_model(self, symo) symo.file_close() return symo From dec57df1753d0f84ae1b93f901f16260d546dfe8 Mon Sep 17 00:00:00 2001 From: aravind Date: Mon, 28 Jul 2014 02:08:25 +0200 Subject: [PATCH 215/273] Update to include base in computation Update to include base in computation of the dynamic identification model. Also add support for mobile and floating base robots. --- pysymoro/dyniden.py | 32 ++++++++++++++++++++++++++++++-- 1 file changed, 30 insertions(+), 2 deletions(-) diff --git a/pysymoro/dyniden.py b/pysymoro/dyniden.py index 95e2466..5715dcb 100644 --- a/pysymoro/dyniden.py +++ b/pysymoro/dyniden.py @@ -63,6 +63,27 @@ def _compute_reaction_wrench( (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] = symo.mat_replace( + Fjnt[j], get_symbol('DE', name, j), forced=True + ) + Njnt[j] = N[j] + Nex[j] + (tools.skew(robo.MS[j]) * vdot[j]) + Njnt[j] = symo.mat_replace( + Njnt[j], get_symbol('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 @@ -74,7 +95,7 @@ def _compute_joint_torque(robo, symo, name, j, Fjnt, Njnt): 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)) + symo.replace(tau_total, get_symbol('DG', name, j), forced=True) def _compute_joint_torque_deriv(symo, param, arg, index): @@ -112,7 +133,9 @@ def dynamic_identification_model(robo, symo): 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): + # start link number + start_link = 0 if robo.is_floating or robo.is_mobile else 1 + 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) @@ -137,6 +160,11 @@ def dynamic_identification_model(robo, symo): 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 From 5e66a065cd939bc9f84dbf24235554cae5276b79 Mon Sep 17 00:00:00 2001 From: aravind Date: Mon, 28 Jul 2014 02:12:42 +0200 Subject: [PATCH 216/273] Minor modification --- pysymoro/dyniden.py | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/pysymoro/dyniden.py b/pysymoro/dyniden.py index 5715dcb..ee2d6e1 100644 --- a/pysymoro/dyniden.py +++ b/pysymoro/dyniden.py @@ -134,7 +134,8 @@ def dynamic_identification_model(robo, symo): robo_tmp.FV = sympy.zeros(robo.NL, 1) robo_tmp.FS = sympy.zeros(robo.NL, 1) # start link number - start_link = 0 if robo.is_floating or robo.is_mobile else 1 + is_fixed = False if robo.is_floating or is_mobile else True + start_link = 1 if is_fixed else 0 for k in xrange(start_link, robo.NL): param_vec = robo.get_inert_param(k) F = ParamsInit.init_vec(robo) From 4fa6e6734ae5aa312545783c58c784b60f5a9501 Mon Sep 17 00:00:00 2001 From: aravind Date: Mon, 28 Jul 2014 14:08:31 +0200 Subject: [PATCH 217/273] Add computation of `U` for link 0 --- pysymoro/dyniden.py | 11 ++++++++++- pysymoro/nealgos.py | 11 ++++++++++- 2 files changed, 20 insertions(+), 2 deletions(-) diff --git a/pysymoro/dyniden.py b/pysymoro/dyniden.py index ee2d6e1..725b9b2 100644 --- a/pysymoro/dyniden.py +++ b/pysymoro/dyniden.py @@ -128,13 +128,22 @@ def dynamic_identification_model(robo, symo): antRj, antPj = compute_rot_trans(robo, symo) # init velocities and accelerations w, wdot, vdot, U = compute_vel_acc(robo, symo, antRj, antPj) + dv0 = ParamsInit.product_combinations(robo.w0) + symo.mat_replace(dv0, 'DV', 0) + hatw_hatw = sympy.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[0] = hatw_hatw + tools.skew(robo.wdot0) + symo.mat_replace(U[0], 'U', 0) # 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 is_mobile else True + is_fixed = False if robo.is_floating or robo.is_mobile else True start_link = 1 if is_fixed else 0 for k in xrange(start_link, robo.NL): param_vec = robo.get_inert_param(k) diff --git a/pysymoro/nealgos.py b/pysymoro/nealgos.py index 8de2826..abdd6b1 100644 --- a/pysymoro/nealgos.py +++ b/pysymoro/nealgos.py @@ -211,7 +211,7 @@ def replace_composite_terms( composite_inertia are composite_beta are the output parameters """ forced = False - if replace and j == 0: forced = False + if replace and j == 0: forced = True composite_inertia[j] = symo.mat_replace( grandJ[j], 'MJE', j, symmet=True, forced=forced ) @@ -492,6 +492,15 @@ def mobile_inverse_dynmodel(robo, symo): antRj, antPj = compute_rot_trans(robo, symo) # init velocities and accelerations w, wdot, vdot, U = compute_vel_acc(robo, symo, antRj, antPj) + dv0 = ParamsInit.product_combinations(robo.w0) + symo.mat_replace(dv0, 'DV', 0) + hatw_hatw = sympy.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[0] = hatw_hatw + tools.skew(robo.wdot0) + symo.mat_replace(U[0], 'U', 0) # init forces vectors F = ParamsInit.init_vec(robo) N = ParamsInit.init_vec(robo) From efda46060488dddcbb0c28ef03c5f21027f6476e Mon Sep 17 00:00:00 2001 From: aravind Date: Mon, 28 Jul 2014 15:01:46 +0200 Subject: [PATCH 218/273] Fix naming of symbols Fix naming of symbols in dynamic identification model to be consistent. --- pysymoro/dyniden.py | 36 ++++++++++++++++++++++-------------- 1 file changed, 22 insertions(+), 14 deletions(-) diff --git a/pysymoro/dyniden.py b/pysymoro/dyniden.py index 725b9b2..ceb83cb 100644 --- a/pysymoro/dyniden.py +++ b/pysymoro/dyniden.py @@ -21,8 +21,16 @@ from symoroutils import tools -def get_symbol(symbol, name, idx): - return symbol + name.format(idx) +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): @@ -33,11 +41,11 @@ def _compute_dynamic_wrench(robo, symo, name, j, w, wdot, U, vdot, F, N): 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], get_symbol('F', name, j)) + F[j] = vec_replace_wrapper(symo, F[j], 'F', name, j) Psi = robo.J[j] * w[j] - Psi = symo.mat_replace(Psi, get_symbol('PSI', name, j)) + Psi = vec_replace_wrapper(symo, Psi, 'PSI', name, j) N[j] = (robo.J[j] * wdot[j]) + (tools.skew(w[j]) * Psi) - N[j] = symo.mat_replace(N[j], get_symbol('No', name, j)) + N[j] = vec_replace_wrapper(symo, N[j], 'No', name, j) def _compute_reaction_wrench( @@ -52,11 +60,11 @@ def _compute_reaction_wrench( """ i = robo.ant[j] Fjnt[j] = F[j] + Fex[j] - Fjnt[j] = symo.mat_replace(Fjnt[j], get_symbol('E', name, 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] = symo.mat_replace(Njnt[j], get_symbol('N', name, j)) + Njnt[j] = vec_replace_wrapper(symo, Njnt[j], 'N', name, j) f_ant = antRj[j] * Fjnt[j] - f_ant = symo.mat_replace(f_ant, get_symbol('FDI', name, 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] + \ @@ -75,12 +83,12 @@ def _compute_base_reaction_wrench( """ j = 0 Fjnt[j] = F[j] + Fex[j] - Fjnt[j] = symo.mat_replace( - Fjnt[j], get_symbol('DE', name, j), forced=True + 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] = symo.mat_replace( - Njnt[j], get_symbol('DN', name, j), forced=True + Njnt[j] = vec_replace_wrapper( + symo, Njnt[j], 'DN', name, j, forced=True ) @@ -144,7 +152,7 @@ def dynamic_identification_model(robo, symo): 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 = 1 if is_fixed else 0 + start_link = 0 for k in xrange(start_link, robo.NL): param_vec = robo.get_inert_param(k) F = ParamsInit.init_vec(robo) @@ -153,7 +161,7 @@ def dynamic_identification_model(robo, symo): if param_vec[i] == tools.ZERO: continue # change link names according to current non-zero parameter - name = '{0}' + str(param_vec[i]) + name = '{index}{element}' + str(param_vec[i]) # set the parameter to 1 mask = sympy.zeros(10, 1) mask[i] = 1 From ac24a16cc33b6a004e8a35e7461b5599453a2842 Mon Sep 17 00:00:00 2001 From: angelos1990 Date: Mon, 28 Jul 2014 15:49:18 +0200 Subject: [PATCH 219/273] Add Piper method files --- pysymoro/invdata.py | 1147 +++++++++++++++++++++++++++++++++ pysymoro/pieper.py | 384 +++++++++++ pysymoro/tests/test-pieper.py | 47 ++ 3 files changed, 1578 insertions(+) create mode 100644 pysymoro/invdata.py create mode 100644 pysymoro/pieper.py create mode 100644 pysymoro/tests/test-pieper.py diff --git a/pysymoro/invdata.py b/pysymoro/invdata.py new file mode 100644 index 0000000..cfb6bb2 --- /dev/null +++ b/pysymoro/invdata.py @@ -0,0 +1,1147 @@ +#!/usr/bin/env python +# -*- coding: utf-8 -*- + +""" +This module of SYMORO package provides symbolic +solutions for inverse geometric problem using Pieper Method. +------------------------------------------------------------- +Master M1 EMARO-ARIA: Group project +Students: PLATIS Angelos & LUGO Jesus +Supervisor: KHALIL Wisama +""" + +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/pieper.py b/pysymoro/pieper.py new file mode 100644 index 0000000..9ec7b03 --- /dev/null +++ b/pysymoro/pieper.py @@ -0,0 +1,384 @@ +#!/usr/bin/env python +# -*- coding: utf-8 -*- + +""" +This module of SYMORO package provides symbolic +solutions for inverse geometric problem using Pieper Method. +------------------------------------------------------------- +Master M1 EMARO-ARIA: Group project +Students: PLATIS Angelos & LUGO Jesus +Supervisor: KHALIL Wisama +""" + +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, solve_orientation, solve_orientation_prismatic, 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: + =========== + """ + 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("\r\n\r\n# This robot cannot be solved by PIEPER METHOD. The robot has less than 6-DOF. Try Paul Method. \r\n\r\n") + 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("\r\n\r\n# This robot cannot be solved by PIEPER METHOD. Try Paul method. \r\n\r\n") + elif m == 5: + bool_fail = 1 + symo.write_line("\r\n\r\n# This robot cannot be solved by PIEPER METHOD. Try Paul method. \r\n\r\n") + elif m == 5: + bool_fail = 1 + symo.write_line("\r\n\r\n# This robot cannot be solved by PIEPER METHOD. Try Paul method. \r\n\r\n") + bools = [bool_fail, bool_prism, bool_spherical] + + return pieper_joints, bools + +def _look_for_case_tree(robo,symo): + + 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} of the robot cannot be solved by PIEPER METHOD. This branch is redundant. \r\n\r\n".format(i+1)) + bool_fail[i] = 1 + elif len(globals()["bran"+str(i)]) < 6: + symo.write_line("\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".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} of this robot cannot be solved by PIEPER METHOD, because it is redundant. (More than three prismatic joints) \r\n\r\n".format(i+1)) + 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(" \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".format(i+1)) + elif m == 5: + bool_fail[i] = 1 + symo.write_line("\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".format(i+1)) + elif m == 5: + bool_fail[i] = 1 + symo.write_line("\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".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, 'igm_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): # pieper_joints will be a vector containing the position of the joints in the parameters table [pieper_joints = (q1,q2,q3)] + """ + Function that takes the already identified pieper_joints and returns the other X_joints. + + Parameters: + =========== + """ + 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 = robo.d[4] ## P36_1 = d4 + P36_2 = -sin(robo.alpha[4])*robo.r[4] ## P36_2 = -r4*Sa4 + P36_3 = cos(robo.alpha[4])*robo.r[4] ## P36_3 = r4*Ca4 + [px,py,pz] = invdata.T_GENERAL[:3,3] + g = Matrix([px,py,pz,1]) + + fc = Matrix([P36_1, cos(robo.alpha[3])*P36_2, sin(robo.alpha[3])*P36_2, 0]) ## fc = [d4; -Ca3*Sa4*r4; -Sa3*Sa4*r4] + fs = Matrix([-P36_2, cos(robo.alpha[3])*P36_1, sin(robo.alpha[3])*P36_1, 0]) ## fs = [Sa4*r4; Ca3*d4; Sa3*d4] + fr = Matrix([0, -sin(robo.alpha[3]), cos(robo.alpha[3]), 0]) ## fr = [0; -Sa3; Ca3] + f0 = Matrix([robo.d[3], -sin(robo.alpha[3])*P36_3, cos(robo.alpha[3])*P36_3, 1]) ## f0 = [d3; -Sa3*Ca4*r4; Ca3*Ca4*r4] + + # 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 = 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 = A0*[d5-d6; r5*(Ca5*Sa6-Ca6*Sa5); 0] + fc = Matrix([fc[0], fc[1], fc[2], 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 = A0*[r5*(Ca5*Sa6-Ca6*Sa5); d6-d5; 0] + fs = Matrix([fs[0], fs[1], fs[2], 0]) + fr = dot(A0, [0, 0, -1]) ## fr = A0*[0; 0; -1] + fr = Matrix([fr[0], fr[1], fr[2], 0]) + f0 = sum(dot(A0, [0, 0, robo.r[5]*cos(robo.alpha[5]-robo.alpha[6])]), P0) ## f0 = A0*[0; 0; r5*C(a5-a6)] + 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 = dot(A0, [-robo.r[5]*sin(robo.alpha[6]), robo.d[6], 0]) ## fs = A0*[-Sa6*r5; d6; 0] + fs = Matrix([fs[0], fs[1], fs[2], 0]) + fr = -dot(A0, [0, 0, 1]) ## fr = -A0*[0; 0; 1] + fr = Matrix([fr[0], fr[1], fr[2], 0]) + f0 = sum(dot(A0, [0, 0, -robo.r[5]*cos(robo.alpha[6])]), P0) ## f0 = A0*[0; 0; -Ca6*r5] + 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 = -robo.d[4] ## P63_1 = -d4 + P63_2 = -sin(robo.alpha[4])*robo.r[3] ## P63_2 = -r3*Sa4 + P63_3 = -cos(robo.alpha[4])*robo.r[3] ## P63_3 = -r3*Ca4 + + 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 = Matrix([P63_1, cos(robo.alpha[5])*P63_2, -sin(robo.alpha[5])*P63_2, 0]) ## fc = [-d4; -Ca5*Sa4*r3; Sa5*Sa4*r3] + fs = Matrix([-P63_2, cos(robo.alpha[5])*P63_1, -sin(robo.alpha[5])*P63_1, 0]) ## fs = [Sa4*r3; -Ca5*d4; Sa5*d4] + fr = Matrix([0, sin(robo.alpha[5]), cos(robo.alpha[5]), 0]) ## fr = [0; Sa5; Ca5] + f0 = Matrix([-robo.d[5], sin(robo.alpha[5])*P63_3, cos(robo.alpha[5])*P63_3, 1]) ## f0 = [-d5; -Sa5*Ca4*r3; -Ca5*Ca4*r3] + + # 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/tests/test-pieper.py b/pysymoro/tests/test-pieper.py new file mode 100644 index 0000000..976c3bb --- /dev/null +++ b/pysymoro/tests/test-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() From 3716ecfab04233163cd5075e272e0d3638b0090c Mon Sep 17 00:00:00 2001 From: aravind Date: Mon, 28 Jul 2014 15:50:31 +0200 Subject: [PATCH 220/273] Rename file --- pysymoro/tests/{test-pieper.py => check_pieper.py} | 0 1 file changed, 0 insertions(+), 0 deletions(-) rename pysymoro/tests/{test-pieper.py => check_pieper.py} (100%) diff --git a/pysymoro/tests/test-pieper.py b/pysymoro/tests/check_pieper.py similarity index 100% rename from pysymoro/tests/test-pieper.py rename to pysymoro/tests/check_pieper.py From a3402b188f8cb63f68f3f4a22453893db511ee16 Mon Sep 17 00:00:00 2001 From: angelos1990 Date: Mon, 28 Jul 2014 16:09:10 +0200 Subject: [PATCH 221/273] Add dialog window for Pieper method --- symoroui/geometry.py | 85 ++++++++++++++++++++++++++++++++++++++++++++ 1 file changed, 85 insertions(+) diff --git a/symoroui/geometry.py b/symoroui/geometry.py index 58551e8..0df2ed4 100644 --- a/symoroui/geometry.py +++ b/symoroui/geometry.py @@ -236,3 +236,88 @@ def get_values(self): 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, _): + 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) + + From a739d9b1ce0779598d00eee6a37a9e9637da7bad Mon Sep 17 00:00:00 2001 From: angelos1990 Date: Mon, 28 Jul 2014 16:16:47 +0200 Subject: [PATCH 222/273] Update UI labels for Pieper method --- symoroui/labels.py | 1 + 1 file changed, 1 insertion(+) diff --git a/symoroui/labels.py b/symoroui/labels.py index 6a0b3fd..a78348e 100644 --- a/symoroui/labels.py +++ b/symoroui/labels.py @@ -169,6 +169,7 @@ 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( From 8cfe8d3a6ca5a4163335ae64303cb6d3059a9166 Mon Sep 17 00:00:00 2001 From: angelos1990 Date: Mon, 28 Jul 2014 16:24:16 +0200 Subject: [PATCH 223/273] Update UI menu to include Pieper method --- symoroui/labels.py | 2 +- symoroui/layout.py | 20 +++++++++++++++++++- 2 files changed, 20 insertions(+), 2 deletions(-) diff --git a/symoroui/labels.py b/symoroui/labels.py index a78348e..aa61466 100644 --- a/symoroui/labels.py +++ b/symoroui/labels.py @@ -169,7 +169,7 @@ m_trans_matrix="Transformation matrix", m_fast_dgm="Fast Geometric model", m_igm_paul="IGM - Paul method", - m_igm_pieper="IGM - Pieper method" + m_igm_pieper="IGM - Pieper method", m_geom_constraint="Geometric constraint equation of loops" ) FILE_MENU = OrderedDict( diff --git a/symoroui/layout.py b/symoroui/layout.py index dd48927..dc827a5 100644 --- a/symoroui/layout.py +++ b/symoroui/layout.py @@ -21,6 +21,7 @@ from pysymoro import kinematics from pysymoro import dynamics from pysymoro import invgeom +from pysymoro import pieper from symoroutils import parfile from symoroutils import filemgr from symoroutils import samplerobots @@ -518,6 +519,11 @@ def create_menu(self): ) 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'] ) @@ -782,9 +788,21 @@ def OnIgmPaul(self, event): if dialog.ShowModal() == wx.ID_OK: lst_T, n = dialog.get_values() invgeom.igm_Paul(self.robo, lst_T, n) - self.model_success('igm') + self.model_success('igm_paul') 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() + pieper.igm_pieper(self.robo, lst_T, n) + self.model_success('igm_pieper') + dialog.Destroy() + def OnConstraintGeoEq(self, event): pass From b5a87584ad0a00f4b8c7d0907dbb6193336a038c Mon Sep 17 00:00:00 2001 From: aravind Date: Mon, 28 Jul 2014 16:28:35 +0200 Subject: [PATCH 224/273] Add licence stub to Pieper method files --- pysymoro/invdata.py | 103 ++++++++++++++++++++++---------------------- pysymoro/pieper.py | 19 ++++---- 2 files changed, 62 insertions(+), 60 deletions(-) diff --git a/pysymoro/invdata.py b/pysymoro/invdata.py index cfb6bb2..ee0bb4f 100644 --- a/pysymoro/invdata.py +++ b/pysymoro/invdata.py @@ -1,15 +1,16 @@ -#!/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 module of SYMORO package provides symbolic solutions for inverse geometric problem using Pieper Method. -------------------------------------------------------------- -Master M1 EMARO-ARIA: Group project -Students: PLATIS Angelos & LUGO Jesus -Supervisor: KHALIL Wisama """ + from sympy import var, sin, cos, eye, atan2, atan, sqrt, pi from sympy import Matrix, Symbol, Expr, trigsimp, zeros from numpy import dot, array @@ -20,9 +21,9 @@ EMPTY = var("EMPTY") -T_GENERAL = Matrix([var('sx,nx,ax,px'), +T_GENERAL = Matrix([var('sx,nx,ax,px'), var('sy,ny,ay,py'), - var('sz,nz,az,pz'), + var('sz,nz,az,pz'), [0, 0, 0, 1]]) @@ -92,8 +93,8 @@ def sin_alphaj_eq_0(robo, symo, X_joint, tc, ts, tr, t0, G, offset): 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 + + return def dj_eq_0(robo, symo, X_joint, tc, ts, tr, t0, G, offset): """ @@ -112,7 +113,7 @@ def dj_eq_0(robo, symo, X_joint, tc, ts, tr, t0, G, offset): [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] @@ -132,7 +133,7 @@ def dj_eq_0(robo, symo, X_joint, tc, ts, tr, t0, G, offset): 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) + 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) @@ -163,7 +164,7 @@ def dj_eq_0(robo, symo, X_joint, tc, ts, tr, t0, G, offset): coef = [el1,el2,el3,el4,el5,el6] _equation_solve(symo,coef,eq_type,qi,offseti) - return + return def dj_and_sin_alpha_dif_0(robo, symo, X_joint, tc, ts, tr, t0, G, offset): """ @@ -245,7 +246,7 @@ def dj_and_sin_alpha_dif_0(robo, symo, X_joint, tc, ts, tr, t0, G, offset): 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: @@ -276,7 +277,7 @@ def dj_and_sin_alpha_dif_0(robo, symo, X_joint, tc, ts, tr, t0, G, offset): coef = [el1,el2,el3,el4,el5,el6] _equation_solve(symo,coef,eq_type,qi,offseti) - return + return def cos_alpha_equal_0(robo, symo, X_joints, tc, ts, tr, t0, G, offset): """ @@ -295,26 +296,26 @@ def cos_alpha_equal_0(robo, symo, X_joints, tc, ts, tr, t0, G, offset): [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 + + # Solve qk if robo.sigma[k] == 0: - # Type 3 in rk + # 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] + 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] @@ -343,7 +344,7 @@ def cos_alpha_equal_0(robo, symo, X_joints, tc, ts, tr, t0, G, offset): coef = [el1,el2] _equation_solve(symo,coef,eq_type,qj,offsetj) - return + return def cos_alpha_dif_0(robo, symo, X_joints, tc, ts, tr, t0, G, offset): """ @@ -362,12 +363,12 @@ def cos_alpha_dif_0(robo, symo, X_joints, tc, ts, tr, t0, G, offset): [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 + eq_type = 6 t1 = tr*robo.r[k] + t0 t1 = symo.replace(trigsimp(t1), 't1', qk) @@ -378,9 +379,9 @@ def cos_alpha_dif_0(robo, symo, X_joints, tc, ts, tr, t0, G, offset): 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] + qk = robo.r[k] # eq 1.54 - eq_type = 2 + eq_type = 2 t2 = tc*cos(robo.theta[k]) + ts*sin(robo.theta[k]) + t0 t2 = symo.replace(trigsimp(t2), 't2', qk) @@ -416,7 +417,7 @@ def cos_alpha_dif_0(robo, symo, X_joints, tc, ts, tr, t0, G, offset): coef = [el1,el2] _equation_solve(symo,coef,eq_type,qj,offsetj) - return + return def cos_alpha_equal_zero(robo, symo, X_joints, tc, ts, tr, t0, G, offset): """ @@ -435,8 +436,8 @@ def cos_alpha_equal_zero(robo, symo, X_joints, tc, ts, tr, t0, G, offset): [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 + + # Solve qk if robo.sigma[k] == 0: qk = robo.theta[k] eq_type = 3 @@ -464,7 +465,7 @@ def cos_alpha_equal_zero(robo, symo, X_joints, tc, ts, tr, t0, G, offset): F = tr*qk + t2 F = symo.replace(trigsimp(F), 'F', qk) - # Solve qj + # Solve qj qj = robo.theta[j] eq_type = 3 el1 = F[x] @@ -481,7 +482,7 @@ def cos_alpha_equal_zero(robo, symo, X_joints, tc, ts, tr, t0, G, offset): coef = [el1,el2] _equation_solve(symo,coef,eq_type,qi,offseti) - return + return def cos_alpha_dif_zero(robo, symo, X_joints, tc, ts, tr, t0, G, offset): """ @@ -503,7 +504,7 @@ def cos_alpha_dif_zero(robo, symo, X_joints, tc, ts, tr, t0, G, offset): # Solve qk if robo.sigma[k] == 0: - eq_type = 6 + eq_type = 6 qk = robo.theta[k] t1 = tr*robo.r[k] + t0 t1 = symo.replace(trigsimp(t1), 't1', qk) @@ -550,7 +551,7 @@ def cos_alpha_dif_zero(robo, symo, X_joints, tc, ts, tr, t0, G, offset): 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] + coef = [el1,el2,el3] _equation_solve(symo,coef,eq_type,qk,offsetk) if robo.sigma[k] == 0: @@ -559,7 +560,7 @@ def cos_alpha_dif_zero(robo, symo, X_joints, tc, ts, tr, t0, G, offset): F = tr*qk + t2 F = symo.replace(trigsimp(F), 'F', qk) - # Solve qj + # Solve qj qj = robo.theta[j] eq_type = 4 el1 = F[x] @@ -579,7 +580,7 @@ def cos_alpha_dif_zero(robo, symo, X_joints, tc, ts, tr, t0, G, offset): coef = [el1,el2] _equation_solve(symo,coef,eq_type,qi,offseti) - return + return def ij_prismatic(robo, symo, X_joint, tc, ts, tr, t0, G, offset): """ @@ -610,7 +611,7 @@ def ij_prismatic(robo, symo, X_joint, tc, ts, tr, t0, G, offset): el3 = t1[x] - G[x] + robo.d[j] coef = [el1,el2,el3] else: - qk = robo.r[k] + 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) @@ -626,7 +627,7 @@ def ij_prismatic(robo, symo, X_joint, tc, ts, tr, t0, G, offset): F = tr*qk + t2 F = symo.replace(trigsimp(F), 'F', qk) - # Solve qj + # Solve qj qj = robo.r[j] eq_type = 1 el1 = -sin(robo.alpha[j]) @@ -634,7 +635,7 @@ def ij_prismatic(robo, symo, X_joint, tc, ts, tr, t0, G, offset): coef = [el1,el2] _equation_solve(symo,coef,eq_type,qj,offsetj) - # Solve qi + # Solve qi qi = robo.r[i] eq_type = 1 el1 = 1 @@ -642,11 +643,11 @@ def ij_prismatic(robo, symo, X_joint, tc, ts, tr, t0, G, offset): coef = [el1,el2] _equation_solve(symo,coef,eq_type,qi,offseti) - return + return def _equation_solve(symo, coef, eq_type, unknown, offset): """ - Function that solves the possible type of equations that we need to solve. + 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: @@ -654,7 +655,7 @@ def _equation_solve(symo, coef, eq_type, unknown, offset): 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 + 4) unknown: The unknown(s) of this equation system """ symo.write_line("\r\n\r\n") @@ -699,7 +700,7 @@ def _equation_solve(symo, coef, eq_type, unknown, offset): 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 @@ -743,7 +744,7 @@ def _equation_solve(symo, coef, eq_type, unknown, offset): 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 @@ -806,7 +807,7 @@ def _equation_solve(symo, coef, eq_type, unknown, offset): 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) + 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("#===============") @@ -861,7 +862,7 @@ def _equation_solve(symo, coef, eq_type, unknown, offset): 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 @@ -908,9 +909,9 @@ def _equation_solve(symo, coef, eq_type, unknown, offset): 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) + 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 @@ -944,7 +945,7 @@ def solve_position(robo, symo, com_key, X_joints, fc, fs, fr, f0, g): 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 @@ -989,7 +990,7 @@ def solve_position(robo, symo, com_key, X_joints, fc, fs, fr, f0, g): return -def solve_orientation(robo, symo, pieper_joints): +def solve_orientation(robo, symo, pieper_joints): """ Function that solves the orientation equation for the four spherical cases. @@ -1003,12 +1004,12 @@ def solve_orientation(robo, symo, pieper_joints): 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') @@ -1114,7 +1115,7 @@ def solve_position_prismatic(robo, symo, pieper_joints): 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 diff --git a/pysymoro/pieper.py b/pysymoro/pieper.py index 9ec7b03..af81216 100644 --- a/pysymoro/pieper.py +++ b/pysymoro/pieper.py @@ -1,15 +1,16 @@ -#!/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 module of SYMORO package provides symbolic solutions for inverse geometric problem using Pieper Method. -------------------------------------------------------------- -Master M1 EMARO-ARIA: Group project -Students: PLATIS Angelos & LUGO Jesus -Supervisor: KHALIL Wisama """ + from sympy import var, sin, cos, eye, atan2, sqrt, pi from sympy import Matrix, Symbol, Expr, trigsimp, zeros, ones from numpy import array, dot @@ -132,7 +133,7 @@ def _look_for_case_simple(robo, symo): bool_fail = 1 symo.write_line("\r\n\r\n# This robot cannot be solved by PIEPER METHOD. Try Paul method. \r\n\r\n") bools = [bool_fail, bool_prism, bool_spherical] - + return pieper_joints, bools def _look_for_case_tree(robo,symo): @@ -151,7 +152,7 @@ def _look_for_case_tree(robo,symo): [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 @@ -221,7 +222,7 @@ def _look_for_case_tree(robo,symo): 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): @@ -299,7 +300,7 @@ def XRRRXX(robo, symo, com_key, X_joints, pieper_joints): fr = Matrix([fr[0], fr[1], fr[2], 0]) f0 = sum(dot(A0, [0, 0, robo.r[5]*cos(robo.alpha[5]-robo.alpha[6])]), P0) ## f0 = A0*[0; 0; r5*C(a5-a6)] + 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 From a6ed89af9ca679d2baf4e314754551daf5e8ec3e Mon Sep 17 00:00:00 2001 From: aravind Date: Mon, 28 Jul 2014 17:25:19 +0200 Subject: [PATCH 225/273] Code cleanup --- pysymoro/pieper.py | 229 ++++++++++++++++++++++++++++++--------------- 1 file changed, 156 insertions(+), 73 deletions(-) diff --git a/pysymoro/pieper.py b/pysymoro/pieper.py index af81216..3a80081 100644 --- a/pysymoro/pieper.py +++ b/pysymoro/pieper.py @@ -17,14 +17,20 @@ from symoroutils import symbolmgr, tools from pysymoro import invdata -from pysymoro.invdata import solve_position, solve_orientation, solve_orientation_prismatic, solve_position_prismatic +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 +} -""" 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): """ @@ -42,7 +48,6 @@ def _pieper_solve(robo, symo): [com_key, X_joints] = _X_joints(robo,symo,pieper_joints) else: return - if bool_spherical == 1: m = pieper_joints[1] if m == 2: @@ -55,7 +60,6 @@ def _pieper_solve(robo, symo): 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 @@ -88,6 +92,7 @@ def _pieper_solve(robo, symo): break return + def _look_for_case_simple(robo, symo): """ Function that locates if a serial robot can be solved by PIEPER METHOD. @@ -95,49 +100,73 @@ def _look_for_case_simple(robo, symo): 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") + 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("\r\n\r\n# This robot cannot be solved by PIEPER METHOD. The robot has less than 6-DOF. Try Paul Method. \r\n\r\n") + 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") + 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") + 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") + 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("\r\n\r\n# This robot cannot be solved by PIEPER METHOD. Try Paul method. \r\n\r\n") + symo.write_line(try_paul_str) elif m == 5: bool_fail = 1 - symo.write_line("\r\n\r\n# This robot cannot be solved by PIEPER METHOD. Try Paul method. \r\n\r\n") + symo.write_line(try_paul_str) elif m == 5: bool_fail = 1 - symo.write_line("\r\n\r\n# This robot cannot be solved by PIEPER METHOD. Try Paul method. \r\n\r\n") + symo.write_line(try_paul_str) bools = [bool_fail, bool_prism, bool_spherical] - return pieper_joints, bools -def _look_for_case_tree(robo,symo): +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 = [] @@ -152,7 +181,6 @@ def _look_for_case_tree(robo,symo): [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 @@ -163,19 +191,26 @@ def _look_for_case_tree(robo,symo): 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} of the robot cannot be solved by PIEPER METHOD. This branch is redundant. \r\n\r\n".format(i+1)) + 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("\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".format(i+1)) + 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} of this robot cannot be solved by PIEPER METHOD, because it is redundant. (More than three prismatic joints) \r\n\r\n".format(i+1)) + 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 @@ -190,15 +225,29 @@ def _look_for_case_tree(robo,symo): 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)) + 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): + 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]] + 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)): @@ -206,25 +255,32 @@ def _look_for_case_tree(robo,symo): 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)) + 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(" \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".format(i+1)) + symo.write_line(try_paul_str.format(i+1)) elif m == 5: bool_fail[i] = 1 - symo.write_line("\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".format(i+1)) + symo.write_line(try_paul_str.format(i+1)) elif m == 5: bool_fail[i] = 1 - symo.write_line("\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".format(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, 'igm_pieper') @@ -233,13 +289,16 @@ def igm_pieper(robo, T_ref, n): symo.file_close() return symo -def _X_joints(robo, symo, pieper_joints): # pieper_joints will be a vector containing the position of the joints in the parameters table [pieper_joints = (q1,q2,q3)] + +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)): @@ -252,6 +311,7 @@ def _X_joints(robo, symo, pieper_joints): # pieper_joints will be a vector conta 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. @@ -261,24 +321,29 @@ def XXXRRR(robo, symo, com_key, X_joints, pieper_joints): 1) X_joints: Joints that are out of spherical wrist 2) pieper_joints: Joints that the spherical wrist is composed of. """ - P36_1 = robo.d[4] ## P36_1 = d4 - P36_2 = -sin(robo.alpha[4])*robo.r[4] ## P36_2 = -r4*Sa4 - P36_3 = cos(robo.alpha[4])*robo.r[4] ## P36_3 = r4*Ca4 + ## 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 = Matrix([P36_1, cos(robo.alpha[3])*P36_2, sin(robo.alpha[3])*P36_2, 0]) ## fc = [d4; -Ca3*Sa4*r4; -Sa3*Sa4*r4] - fs = Matrix([-P36_2, cos(robo.alpha[3])*P36_1, sin(robo.alpha[3])*P36_1, 0]) ## fs = [Sa4*r4; Ca3*d4; Sa3*d4] - fr = Matrix([0, -sin(robo.alpha[3]), cos(robo.alpha[3]), 0]) ## fr = [0; -Sa3; Ca3] - f0 = Matrix([robo.d[3], -sin(robo.alpha[3])*P36_3, cos(robo.alpha[3])*P36_3, 1]) ## f0 = [d3; -Sa3*Ca4*r4; Ca3*Ca4*r4] - + ## 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. @@ -291,23 +356,34 @@ def XRRRXX(robo, symo, com_key, X_joints, pieper_joints): 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 = 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 = A0*[d5-d6; r5*(Ca5*Sa6-Ca6*Sa5); 0] + ## 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 = 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 = A0*[r5*(Ca5*Sa6-Ca6*Sa5); d6-d5; 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 = dot(A0, [0, 0, -1]) ## fr = A0*[0; 0; -1] + ## fr = A0*[0; 0; -1] + fr = dot(A0, [0, 0, -1]) fr = Matrix([fr[0], fr[1], fr[2], 0]) - f0 = sum(dot(A0, [0, 0, robo.r[5]*cos(robo.alpha[5]-robo.alpha[6])]), P0) ## f0 = A0*[0; 0; r5*C(a5-a6)] + P0 + ## 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. @@ -320,24 +396,26 @@ def XXRRRX(robo, symo, com_key, X_joints, pieper_joints): 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 = 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 = dot(A0, [-robo.r[5]*sin(robo.alpha[6]), robo.d[6], 0]) ## fs = A0*[-Sa6*r5; d6; 0] + ## 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 = -dot(A0, [0, 0, 1]) ## fr = -A0*[0; 0; 1] + ## fr = -A0*[0; 0; 1] + fr = -dot(A0, [0, 0, 1]) fr = Matrix([fr[0], fr[1], fr[2], 0]) - f0 = sum(dot(A0, [0, 0, -robo.r[5]*cos(robo.alpha[6])]), P0) ## f0 = A0*[0; 0; -Ca6*r5] + P0 + ## 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. @@ -347,27 +425,31 @@ def RRRXXX(robo, symo, com_key, X_joints, pieper_joints): 1) X_joints: Joints that are out of spherical wrist 2) pieper_joints: Joints that the spherical wrist is composed of. """ - P63_1 = -robo.d[4] ## P63_1 = -d4 - P63_2 = -sin(robo.alpha[4])*robo.r[3] ## P63_2 = -r3*Sa4 - P63_3 = -cos(robo.alpha[4])*robo.r[3] ## P63_3 = -r3*Ca4 - + ## 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 = Matrix([P63_1, cos(robo.alpha[5])*P63_2, -sin(robo.alpha[5])*P63_2, 0]) ## fc = [-d4; -Ca5*Sa4*r3; Sa5*Sa4*r3] - fs = Matrix([-P63_2, cos(robo.alpha[5])*P63_1, -sin(robo.alpha[5])*P63_1, 0]) ## fs = [Sa4*r3; -Ca5*d4; Sa5*d4] - fr = Matrix([0, sin(robo.alpha[5]), cos(robo.alpha[5]), 0]) ## fr = [0; Sa5; Ca5] - f0 = Matrix([-robo.d[5], sin(robo.alpha[5])*P63_3, cos(robo.alpha[5])*P63_3, 1]) ## f0 = [-d5; -Sa5*Ca4*r3; -Ca5*Ca4*r3] - + ## 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. @@ -381,5 +463,6 @@ def Prismatic(robo, symo, X_joints, pieper_joints): solve_orientation_prismatic(robo, symo, X_joints) # Then Position Equations solve_position_prismatic(robo, symo, pieper_joints) - return + + From 798ff5d9800ef14d788bb2621fbfcdb8645d6a33 Mon Sep 17 00:00:00 2001 From: aravind Date: Tue, 29 Jul 2014 00:34:42 +0200 Subject: [PATCH 226/273] Modify matrix inversion to be performed numerically Add functions in `pysymoro/nealgos.py` to write equations in the output file that will compute the inverse of the inertia matrix (6x6) numerically. --- pysymoro/nealgos.py | 69 +++++++++++++++++++++++++++++++++++++++------ 1 file changed, 61 insertions(+), 8 deletions(-) diff --git a/pysymoro/nealgos.py b/pysymoro/nealgos.py index abdd6b1..d3771f2 100644 --- a/pysymoro/nealgos.py +++ b/pysymoro/nealgos.py @@ -211,13 +211,10 @@ def replace_composite_terms( composite_inertia are composite_beta are the output parameters """ forced = False - if replace and j == 0: forced = True + if replace and j == 0: forced = False composite_inertia[j] = symo.mat_replace( grandJ[j], 'MJE', j, symmet=True, forced=forced ) - if replace and j == 0: - symo.write_equation('MJE220', 'MJE110') - symo.write_equation('MJE330', 'MJE110') composite_beta[j] = symo.mat_replace( beta[j], 'VBE', j, forced=forced ) @@ -372,6 +369,58 @@ def compute_link_accel(robo, symo, j, jTant, zeta, grandVp): grandVp[j][3:, 0] = symo.mat_replace(grandVp[j][3:, 0], 'WP', j) +def write_numerical_inverse(symo, inertia): + """ + 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_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 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): + """ + 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 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). @@ -412,11 +461,15 @@ def compute_base_accel_composite( if robo.is_floating: forced = True symo.flushout() - inv_base_comp_inertia = composite_inertia[0].inv() - inv_base_comp_inertia = symo.mat_replace( - inv_base_comp_inertia, 'InvMJE', 0, - symmet=True, forced=forced + write_numerical_inverse(symo, composite_inertia[0]) + inv_base_comp_inertia = get_numerical_inverse_out( + composite_inertia[0] ) + #inv_base_comp_inertia = composite_inertia[0].inv() + #inv_base_comp_inertia = symo.mat_replace( + # inv_base_comp_inertia, 'InvMJE', 0, + # symmet=True, forced=forced + #) grandVp[0] = inv_base_comp_inertia * composite_beta[0] grandVp[0][:3, 0] = symo.mat_replace( grandVp[0][:3, 0], 'VP', 0, forced=forced From 164cb87b1f379b31f2f5aee9e879640b591c0bd0 Mon Sep 17 00:00:00 2001 From: aravind Date: Tue, 29 Jul 2014 00:46:11 +0200 Subject: [PATCH 227/273] Modify base acceleration computation Modify base acceleration computation for floating base robots to use the numerical method output. --- pysymoro/nealgos.py | 25 ++++++++++--------------- 1 file changed, 10 insertions(+), 15 deletions(-) diff --git a/pysymoro/nealgos.py b/pysymoro/nealgos.py index d3771f2..6801d06 100644 --- a/pysymoro/nealgos.py +++ b/pysymoro/nealgos.py @@ -369,7 +369,7 @@ def compute_link_accel(robo, symo, j, jTant, zeta, grandVp): grandVp[j][3:, 0] = symo.mat_replace(grandVp[j][3:, 0], 'WP', j) -def write_numerical_inverse(symo, inertia): +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. @@ -395,7 +395,7 @@ def write_numerical_inverse(symo, inertia): symo.write_line("# matrix to be compatible with future computation") for i in xrange(inertia.rows): for j in xrange(inertia.cols): - if i < j: + if symmet and i < j: continue symo.write_equation( 'InvMJE{row}{col}0'.format(row=i+1, col=j+1), @@ -405,14 +405,14 @@ def write_numerical_inverse(symo, inertia): symo.write_line("# NUMERICAL INVERSION OF INERTIA MATRIX - END") -def get_numerical_inverse_out(inertia): +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 i < j: + if symmet and i < j: inv_inertia[i, j] = inv_inertia[j, i] continue inv_inertia[i, j] = sympy.var( @@ -432,10 +432,10 @@ def compute_base_accel(robo, symo, star_inertia, star_beta, grandVp): grandVp[0] = Matrix([robo.vdot0 - robo.G, robo.w0]) if robo.is_floating: forced = True - inv_base_star_inertia = star_inertia[0].inv() - inv_base_star_inertia = symo.mat_replace( - inv_base_star_inertia, 'InvMJE', 0, - symmet=True, forced=forced + symo.flushout() + write_numerical_inverse(symo, star_inertia[0], symmet=False) + inv_base_star_inertia = get_numerical_inverse_out( + star_inertia[0], symmet=False ) grandVp[0] = inv_base_star_inertia * star_beta[0] grandVp[0][:3, 0] = symo.mat_replace( @@ -461,15 +461,10 @@ def compute_base_accel_composite( if robo.is_floating: forced = True symo.flushout() - write_numerical_inverse(symo, composite_inertia[0]) + write_numerical_inverse(symo, composite_inertia[0], symmet=True) inv_base_comp_inertia = get_numerical_inverse_out( - composite_inertia[0] + composite_inertia[0], symmet=True ) - #inv_base_comp_inertia = composite_inertia[0].inv() - #inv_base_comp_inertia = symo.mat_replace( - # inv_base_comp_inertia, 'InvMJE', 0, - # symmet=True, forced=forced - #) grandVp[0] = inv_base_comp_inertia * composite_beta[0] grandVp[0][:3, 0] = symo.mat_replace( grandVp[0][:3, 0], 'VP', 0, forced=forced From 3609b34910c0ca89c6904c6743a275c5510b6e25 Mon Sep 17 00:00:00 2001 From: aravind Date: Tue, 29 Jul 2014 02:56:04 +0200 Subject: [PATCH 228/273] Exclude `IA`,`FV`,`FS` parameters for link 0 --- pysymoro/dyniden.py | 2 ++ 1 file changed, 2 insertions(+) diff --git a/pysymoro/dyniden.py b/pysymoro/dyniden.py index ceb83cb..35fa0a2 100644 --- a/pysymoro/dyniden.py +++ b/pysymoro/dyniden.py @@ -188,6 +188,8 @@ def dynamic_identification_model(robo, symo): # 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 ) From c6318a7ee5a2de5cab4ac225257164031ceff684 Mon Sep 17 00:00:00 2001 From: aravind Date: Tue, 29 Jul 2014 13:18:12 +0200 Subject: [PATCH 229/273] Fix Jacobian determinant UI --- symoroui/kinematics.py | 20 ++++++++++---------- 1 file changed, 10 insertions(+), 10 deletions(-) diff --git a/symoroui/kinematics.py b/symoroui/kinematics.py index 5678ad7..8b5c878 100644 --- a/symoroui/kinematics.py +++ b/symoroui/kinematics.py @@ -112,22 +112,22 @@ def init_ui(self): 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), From fd7c130663da9191c3ce5c4eaa6cbc4e88b0eb4f Mon Sep 17 00:00:00 2001 From: aravind Date: Tue, 29 Jul 2014 14:39:07 +0200 Subject: [PATCH 230/273] Modify symbol creation Modify symbols in dynamic identification model so that none of the output variables appear in the right hand side of the equations. --- pysymoro/dyniden.py | 3 +++ 1 file changed, 3 insertions(+) diff --git a/pysymoro/dyniden.py b/pysymoro/dyniden.py index 35fa0a2..957edf5 100644 --- a/pysymoro/dyniden.py +++ b/pysymoro/dyniden.py @@ -136,6 +136,9 @@ def dynamic_identification_model(robo, symo): antRj, antPj = compute_rot_trans(robo, symo) # init velocities and accelerations w, wdot, vdot, U = compute_vel_acc(robo, symo, antRj, antPj) + w[0] = symo.mat_replace(w[0], 'W', 0) + wdot[0] = symo.mat_replace(wdot[0], 'WP', 0, forced=True) + vdot[0] = symo.mat_replace(vdot[0], 'VP', 0, forced=True) dv0 = ParamsInit.product_combinations(robo.w0) symo.mat_replace(dv0, 'DV', 0) hatw_hatw = sympy.Matrix([ From 1b9ab9eb5f5c1ce9f43134d71f64c4710304abb4 Mon Sep 17 00:00:00 2001 From: aravind Date: Tue, 29 Jul 2014 15:29:17 +0200 Subject: [PATCH 231/273] Modify to specify the robot type in output files --- pysymoro/nealgos.py | 4 ++-- pysymoro/robot.py | 38 ++++++++++++++++++++++++++++++-------- 2 files changed, 32 insertions(+), 10 deletions(-) diff --git a/pysymoro/nealgos.py b/pysymoro/nealgos.py index 6801d06..2484089 100644 --- a/pysymoro/nealgos.py +++ b/pysymoro/nealgos.py @@ -433,9 +433,9 @@ def compute_base_accel(robo, symo, star_inertia, star_beta, grandVp): if robo.is_floating: forced = True symo.flushout() - write_numerical_inverse(symo, star_inertia[0], symmet=False) + write_numerical_inverse(symo, star_inertia[0], symmet=True) inv_base_star_inertia = get_numerical_inverse_out( - star_inertia[0], symmet=False + star_inertia[0], symmet=True ) grandVp[0] = inv_base_star_inertia * star_beta[0] grandVp[0][:3, 0] = symo.mat_replace( diff --git a/pysymoro/robot.py b/pysymoro/robot.py index 8378a6d..a31d05f 100644 --- a/pysymoro/robot.py +++ b/pysymoro/robot.py @@ -224,19 +224,26 @@ def compute_idym(self): """ symo = symbolmgr.SymbolManager() symo.file_open(self, 'idm') - title = "Inverse Dynamic Model using Newton-Euler Algorithm" - symo.write_params_table(self, title, inert=True, dynam=True) + 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 @@ -248,11 +255,16 @@ def compute_inertiamatrix(self): """ symo = symbolmgr.SymbolManager() symo.file_open(self, 'inm') - title = "Inertia matrix using Composite links algorithm" - symo.write_params_table(self, title, inert=True, dynam=True) + 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 @@ -264,13 +276,16 @@ def compute_ddym(self): """ symo = symbolmgr.SymbolManager() symo.file_open(self, 'ddm') - title = "Direct Dynamic Model using Newton-Euler Algorithm" - symo.write_params_table(self, title, inert=True, dynam=True) + title = "Direct Dynamic Model using Newton-Euler Algorithm\n" if self.is_floating: # with floating base + title = title + "Robot with floating base\n" + symo.write_params_table(self, title, inert=True, dynam=True) nealgos.floating_direct_dynmodel(self, symo) else: # with fixed base + title = title + "Robot with fixed base\n" + symo.write_params_table(self, title, inert=True, dynam=True) dynamics.compute_direct_dynamic_NE(self, symo) symo.file_close() return symo @@ -284,19 +299,26 @@ def compute_pseudotorques(self): pseudorobo.qddot = zeros(pseudorobo.NL, 1) symo = symbolmgr.SymbolManager() symo.file_open(self, 'ccg') - title = "Pseudo forces using Newton-Euler Algorithm" - symo.write_params_table(self, title, inert=True, dynam=True) + title = "Pseudo forces using Newton-Euler Algorithm\n" if 1 in pseudorobo.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(pseudorobo, symo) elif pseudorobo.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(pseudorobo, symo) elif pseudorobo.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(pseudorobo, 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(pseudorobo, symo) symo.file_close() return symo From 0be832a8b610f245f0c0a8cb9842e265fb22ff19 Mon Sep 17 00:00:00 2001 From: aravind Date: Tue, 29 Jul 2014 15:34:31 +0200 Subject: [PATCH 232/273] Fix joint variable in UI --- pysymoro/robot.py | 6 ------ 1 file changed, 6 deletions(-) diff --git a/pysymoro/robot.py b/pysymoro/robot.py index a31d05f..b0d4683 100644 --- a/pysymoro/robot.py +++ b/pysymoro/robot.py @@ -684,17 +684,11 @@ def _set_geom_defaults(self): """ for j in xrange(1, self.NF): if self.sigma[j] == 0: - self.mu[j] = 1 self.theta[j] = var('th{0}'.format(j)) - self.r[j] = 0 elif self.sigma[j] == 1: - self.mu[j] = 1 - self.theta[j] = 0 self.r[j] = var('r{0}'.format(j)) elif self.sigma[j] == 2: self.mu[j] = 0 - self.theta[j] = 0 - self.r[j] = 0 def _set_base_defaults(self): """ From bac9303f8fd18dbd6c6946409daf2a6c66235ea9 Mon Sep 17 00:00:00 2001 From: aravind Date: Tue, 29 Jul 2014 16:34:00 +0200 Subject: [PATCH 233/273] Modify to read/write `eta` and `k` in PAR file --- symoroutils/parfile.py | 65 ++++++++++++++++++++++++------------------ 1 file changed, 38 insertions(+), 27 deletions(-) diff --git a/symoroutils/parfile.py b/symoroutils/parfile.py index d31390a..c28cb0c 100644 --- a/symoroutils/parfile.py +++ b/symoroutils/parfile.py @@ -19,30 +19,37 @@ 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', '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'] +_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', 'Sigma': 'sigma', 'B': 'b', 'R': 'r', - 'Alpha': 'alpha', 'Mu': 'mu', 'Theta': 'theta'} +_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): @@ -80,6 +87,7 @@ def _write_par_list(robo, f, key, N0, N): def writepar(robo): fname = filemgr.make_file_path(robo) 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)) @@ -87,24 +95,27 @@ def writepar(robo): 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') - if robo.is_floating or robo.is_mobile: - N0 = 0 - else: - N0 = 1 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) - f.write('\n(* Speed and acceleration of the base *)\n') + # 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') From ae6b8fd9a7b60303244bc427600c36cc1078b825 Mon Sep 17 00:00:00 2001 From: aravind Date: Tue, 29 Jul 2014 17:04:50 +0200 Subject: [PATCH 234/273] Fix IDyM for robots with flexible joints Modify so that all the required terms including that of link 0 are computed. --- pysymoro/nealgos.py | 10 +++------- 1 file changed, 3 insertions(+), 7 deletions(-) diff --git a/pysymoro/nealgos.py b/pysymoro/nealgos.py index 2484089..76d4a1c 100644 --- a/pysymoro/nealgos.py +++ b/pysymoro/nealgos.py @@ -713,27 +713,23 @@ def flexible_inverse_dynmodel(robo, symo): 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 robo.eta[j]: + if not robo.eta[j]: + # when rigid # 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)): - # skip base terms for non-floating robots - if robo.is_floating and j == 0: + 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) - else: - continue replace_star_terms( symo, grandJ, beta, j, star_inertia, star_beta ) # second backward recursion - compute star terms for j in reversed(xrange(0, robo.NL)): if j == 0: continue - # skip base terms for non-floating robots - if not robo.is_floating and robo.ant[j] == 0: continue # set composite flag to false when flexible if robo.eta[j]: use_composite = False if use_composite: From b9bd47ac6f640471bca61cc4252d0724e7928b3e Mon Sep 17 00:00:00 2001 From: aravind Date: Tue, 29 Jul 2014 17:14:50 +0200 Subject: [PATCH 235/273] Update new robot creation Update new robot creation to automatically preserve the joint parameters. --- symoroui/definition.py | 9 +++++++++ symoroui/layout.py | 7 +++++++ 2 files changed, 16 insertions(+) diff --git a/symoroui/definition.py b/symoroui/definition.py index cc18b9c..ff9f3d9 100644 --- a/symoroui/definition.py +++ b/symoroui/definition.py @@ -97,6 +97,10 @@ def init_ui(self, name, nl, nj, is_floating, is_mobile, structure): 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' ) @@ -113,6 +117,10 @@ def init_ui(self, name, nl, nj, is_floating, is_mobile, structure): 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 @@ -167,6 +175,7 @@ def get_values(self): '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 diff --git a/symoroui/layout.py b/symoroui/layout.py index dc827a5..9c0bd4d 100644 --- a/symoroui/layout.py +++ b/symoroui/layout.py @@ -663,6 +663,13 @@ def OnNew(self, event): 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 From 977e44f27500804a231078403e04bf5d58cec2e9 Mon Sep 17 00:00:00 2001 From: aravind Date: Tue, 29 Jul 2014 17:18:59 +0200 Subject: [PATCH 236/273] Add more comments to base acceleration output --- pysymoro/nealgos.py | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/pysymoro/nealgos.py b/pysymoro/nealgos.py index 76d4a1c..88480de 100644 --- a/pysymoro/nealgos.py +++ b/pysymoro/nealgos.py @@ -389,6 +389,10 @@ def write_numerical_inverse(symo, inertia, symmet=False): ) # 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 \ BETA0") symo.write_equation('numInvMJE0', 'numpy.linalg.inv(numMJE0)') # assign elements of the inverted matrix symo.write_line("# assign each element of the inverted (symmetric)") From 6f034ad6fe2a4349b5fc65af73c406b99ccd88ab Mon Sep 17 00:00:00 2001 From: aravind Date: Wed, 30 Jul 2014 12:23:42 +0200 Subject: [PATCH 237/273] Force convert base acceleration result to a symbol Force convert base acceleration result to a symbol in dynamic models. --- pysymoro/nealgos.py | 7 +++++-- 1 file changed, 5 insertions(+), 2 deletions(-) diff --git a/pysymoro/nealgos.py b/pysymoro/nealgos.py index 88480de..fe8951f 100644 --- a/pysymoro/nealgos.py +++ b/pysymoro/nealgos.py @@ -432,7 +432,7 @@ def compute_base_accel(robo, symo, star_inertia, star_beta, grandVp): Note: grandVp is the output parameter """ - forced = False + forced = True grandVp[0] = Matrix([robo.vdot0 - robo.G, robo.w0]) if robo.is_floating: forced = True @@ -460,7 +460,7 @@ def compute_base_accel_composite( Note: grandVp is the output parameter """ - forced = False + forced = True grandVp[0] = Matrix([robo.vdot0 - robo.G, robo.w0]) if robo.is_floating: forced = True @@ -544,6 +544,9 @@ def mobile_inverse_dynmodel(robo, symo): antRj, antPj = compute_rot_trans(robo, symo) # init velocities and accelerations w, wdot, vdot, U = compute_vel_acc(robo, symo, antRj, antPj) + w[0] = symo.mat_replace(w[0], 'W', 0) + wdot[0] = symo.mat_replace(wdot[0], 'WP', 0, forced=True) + vdot[0] = symo.mat_replace(vdot[0], 'VP', 0, forced=True) dv0 = ParamsInit.product_combinations(robo.w0) symo.mat_replace(dv0, 'DV', 0) hatw_hatw = sympy.Matrix([ From 1ce6785bdb440c9f4f5ebcf14491be758fbbea56 Mon Sep 17 00:00:00 2001 From: aravind Date: Wed, 30 Jul 2014 14:26:55 +0200 Subject: [PATCH 238/273] Update `k` value based on `eta` value change --- pysymoro/robot.py | 4 ++++ symoroui/layout.py | 3 +++ 2 files changed, 7 insertions(+) diff --git a/pysymoro/robot.py b/pysymoro/robot.py index b0d4683..ee8b740 100644 --- a/pysymoro/robot.py +++ b/pysymoro/robot.py @@ -676,6 +676,10 @@ def _set_joint_defaults(self): 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): """ diff --git a/symoroui/layout.py b/symoroui/layout.py index 9c0bd4d..02f7d9c 100644 --- a/symoroui/layout.py +++ b/symoroui/layout.py @@ -317,6 +317,8 @@ def OnDynParamChanged(self, event): 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) @@ -359,6 +361,7 @@ def update_dyn_params(self): 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( From e696ddb043f9c4d629c98aa4a275a9a18142d14a Mon Sep 17 00:00:00 2001 From: aravind Date: Wed, 30 Jul 2014 14:48:13 +0200 Subject: [PATCH 239/273] Include `eta`,`k` to be written in the output file --- pysymoro/robot.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/pysymoro/robot.py b/pysymoro/robot.py index ee8b740..cf576ef 100644 --- a/pysymoro/robot.py +++ b/pysymoro/robot.py @@ -612,7 +612,7 @@ def get_ext_dynam_head(self): get_ext_dynam_head: list of strings """ return ['j', 'FX', 'FY', 'FZ', 'CX', 'CY', 'CZ', - 'FS', 'FV', 'QP', 'QDP', 'GAM'] + 'FS', 'FV', 'QP', 'QDP', 'GAM', 'eta', 'k'] def get_dynam_head(self): """Returns header for inertia parameters. From c0748d23ed11506a135c253d06489d28bd62bfde Mon Sep 17 00:00:00 2001 From: aravind Date: Thu, 31 Jul 2014 15:02:31 +0200 Subject: [PATCH 240/273] Fix flexible IDyM --- pysymoro/nealgos.py | 16 +++++++++------- 1 file changed, 9 insertions(+), 7 deletions(-) diff --git a/pysymoro/nealgos.py b/pysymoro/nealgos.py index fe8951f..ba87272 100644 --- a/pysymoro/nealgos.py +++ b/pysymoro/nealgos.py @@ -392,7 +392,7 @@ def write_numerical_inverse(symo, inertia, symmet=False): symo.write_line( "# In Matlab this can be performed without matrix inverse" ) - symo.write_line("# VP0 = numMJE0 \ BETA0") + 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)") @@ -432,7 +432,7 @@ def compute_base_accel(robo, symo, star_inertia, star_beta, grandVp): Note: grandVp is the output parameter """ - forced = True + forced = False grandVp[0] = Matrix([robo.vdot0 - robo.G, robo.w0]) if robo.is_floating: forced = True @@ -460,7 +460,7 @@ def compute_base_accel_composite( Note: grandVp is the output parameter """ - forced = True + forced = False grandVp[0] = Matrix([robo.vdot0 - robo.G, robo.w0]) if robo.is_floating: forced = True @@ -724,9 +724,11 @@ def flexible_inverse_dynmodel(robo, symo): # 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(0, robo.NL)): - if j == 0: + 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 @@ -735,8 +737,8 @@ def flexible_inverse_dynmodel(robo, symo): symo, grandJ, beta, j, star_inertia, star_beta ) # second backward recursion - compute star terms - for j in reversed(xrange(0, robo.NL)): - if j == 0: continue + 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: From 8811db119b0b5d11a27a2bf12f8a81adcd84aad4 Mon Sep 17 00:00:00 2001 From: aravind Date: Thu, 31 Jul 2014 15:06:57 +0200 Subject: [PATCH 241/273] Fix inertia: stop computing for link 0 in fixed base case --- pysymoro/inertia.py | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/pysymoro/inertia.py b/pysymoro/inertia.py index 529a7ee..1ee0d0f 100644 --- a/pysymoro/inertia.py +++ b/pysymoro/inertia.py @@ -155,11 +155,11 @@ def fixed_inertia_matrix(robo, symo): 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)): + for j in reversed(xrange(1, robo.NL)): replace_composite_terms( symo, j, comp_inertia3, comp_ms, comp_mass ) - if j != 0: + if j != 1: compute_composite_inertia( robo, symo, j, antRj, antPj, aje1, comp_inertia3, comp_ms, comp_mass From 744fb0efc906439085df1cba9eae00db1f8660dc Mon Sep 17 00:00:00 2001 From: aravind Date: Thu, 31 Jul 2014 15:18:35 +0200 Subject: [PATCH 242/273] Modify UI labels and default values for new robot --- pysymoro/robot.py | 8 ++++---- symoroui/labels.py | 24 ++++++++++++------------ 2 files changed, 16 insertions(+), 16 deletions(-) diff --git a/pysymoro/robot.py b/pysymoro/robot.py index cf576ef..c890210 100644 --- a/pysymoro/robot.py +++ b/pysymoro/robot.py @@ -701,10 +701,10 @@ def _set_base_defaults(self): """ if self.is_floating or self.is_mobile: self.G = Matrix([var('GX'), var('GY'), var('GZ')]) - self.v0 = Matrix([var('VX0'), var('VY0'), var('VZ0')]) - self.w0 = Matrix([var('WX0'), var('WY0'), var('WZ0')]) - self.vdot0 = Matrix([var('VPX0'), var('VPY0'), var('VPZ0')]) - self.wdot0 = Matrix([var('WPX0'), var('WPY0'), var('WPZ0')]) + 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): diff --git a/symoroui/labels.py b/symoroui/labels.py index aa61466..66dcdbf 100644 --- a/symoroui/labels.py +++ b/symoroui/labels.py @@ -51,18 +51,18 @@ ]) # base velocity and acceleration params BASE_VEL_ACC = OrderedDict([ - ('vx', FieldEntry('V0X', 'V0X', 'txt', (0, 0), 'OnBaseTwistChanged', 0)), - ('vy', FieldEntry('V0Y', 'V0Y', 'txt', (1, 0), 'OnBaseTwistChanged', 1)), - ('vz', FieldEntry('V0Z', 'V0Z', 'txt', (2, 0), 'OnBaseTwistChanged', 2)), - ('wx', FieldEntry('W0X', 'W0X', 'txt', (0, 1), 'OnBaseTwistChanged', 0)), - ('wy', FieldEntry('W0Y', 'W0Y', 'txt', (1, 1), 'OnBaseTwistChanged', 1)), - ('wz', FieldEntry('W0Z', 'W0Z', 'txt', (2, 1), 'OnBaseTwistChanged', 2)), - ('vpx', FieldEntry('VP0X', 'VP0X', 'txt', (0, 2), 'OnBaseTwistChanged', 0)), - ('vpy', FieldEntry('VP0Y', 'VP0Y', 'txt', (1, 2), 'OnBaseTwistChanged', 1)), - ('vpz', FieldEntry('VP0Z', 'VP0Z', 'txt', (2, 2), 'OnBaseTwistChanged', 2)), - ('wpx', FieldEntry('WP0X', 'WP0X', 'txt', (0, 3), 'OnBaseTwistChanged', 0)), - ('wpy', FieldEntry('WP0Y', 'WP0Y', 'txt', (1, 3), 'OnBaseTwistChanged', 1)), - ('wpz', FieldEntry('WP0Z', 'WP0Z', 'txt', (2, 3), 'OnBaseTwistChanged', 2)) + ('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([ From 043470f908364bfc6379da609a1f94adbc5caaa9 Mon Sep 17 00:00:00 2001 From: aravind Date: Thu, 31 Jul 2014 15:46:20 +0200 Subject: [PATCH 243/273] Update velocity acceleration computation Update velocity acceleration computation to include link 0. --- pysymoro/kinematics.py | 48 ++++++++++++++++++++++++++++++------------ pysymoro/nealgos.py | 12 ----------- 2 files changed, 35 insertions(+), 25 deletions(-) diff --git a/pysymoro/kinematics.py b/pysymoro/kinematics.py index ab4a604..a87f62e 100644 --- a/pysymoro/kinematics.py +++ b/pysymoro/kinematics.py @@ -46,7 +46,9 @@ def _v_j(robo, j, antPj, jRant, v, w, qdj, forced=False): return v[j] -def _v_dot_j(robo, symo, j, jRant, antPj, w, wi, wdot, U, vdot, qdj, qddj): +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]], @@ -206,7 +208,9 @@ def _kinematic_loop_constraints(robo, symo, proj=None): 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 @@ -219,19 +223,37 @@ def compute_vel_acc(robo, symo, antRj, antPj, forced=False, gravity=True): #init velocities and accelerations 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 = ParamsInit.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) + 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, forced=True) + vdot[j] = symo.mat_replace(vdot[j], 'VP', j, forced=True) + 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 diff --git a/pysymoro/nealgos.py b/pysymoro/nealgos.py index ba87272..da211e6 100644 --- a/pysymoro/nealgos.py +++ b/pysymoro/nealgos.py @@ -544,18 +544,6 @@ def mobile_inverse_dynmodel(robo, symo): antRj, antPj = compute_rot_trans(robo, symo) # init velocities and accelerations w, wdot, vdot, U = compute_vel_acc(robo, symo, antRj, antPj) - w[0] = symo.mat_replace(w[0], 'W', 0) - wdot[0] = symo.mat_replace(wdot[0], 'WP', 0, forced=True) - vdot[0] = symo.mat_replace(vdot[0], 'VP', 0, forced=True) - dv0 = ParamsInit.product_combinations(robo.w0) - symo.mat_replace(dv0, 'DV', 0) - hatw_hatw = sympy.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[0] = hatw_hatw + tools.skew(robo.wdot0) - symo.mat_replace(U[0], 'U', 0) # init forces vectors F = ParamsInit.init_vec(robo) N = ParamsInit.init_vec(robo) From c0af6e64ff6383216aa6e32111ce0d0b1f4ea60c Mon Sep 17 00:00:00 2001 From: aravind Date: Thu, 31 Jul 2014 15:54:24 +0200 Subject: [PATCH 244/273] Update Dynamic Identification Model vel acc computation --- pysymoro/dyniden.py | 16 +++------------- 1 file changed, 3 insertions(+), 13 deletions(-) diff --git a/pysymoro/dyniden.py b/pysymoro/dyniden.py index 957edf5..d60abcb 100644 --- a/pysymoro/dyniden.py +++ b/pysymoro/dyniden.py @@ -135,19 +135,9 @@ def dynamic_identification_model(robo, symo): # init transformation antRj, antPj = compute_rot_trans(robo, symo) # init velocities and accelerations - w, wdot, vdot, U = compute_vel_acc(robo, symo, antRj, antPj) - w[0] = symo.mat_replace(w[0], 'W', 0) - wdot[0] = symo.mat_replace(wdot[0], 'WP', 0, forced=True) - vdot[0] = symo.mat_replace(vdot[0], 'VP', 0, forced=True) - dv0 = ParamsInit.product_combinations(robo.w0) - symo.mat_replace(dv0, 'DV', 0) - hatw_hatw = sympy.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[0] = hatw_hatw + tools.skew(robo.wdot0) - symo.mat_replace(U[0], 'U', 0) + 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) From a735fb38f5f5b8d708cf1eb783616ab3ebe242e9 Mon Sep 17 00:00:00 2001 From: aravind Date: Thu, 31 Jul 2014 16:26:11 +0200 Subject: [PATCH 245/273] Remove forced convention of symbols for base acceleration --- pysymoro/kinematics.py | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/pysymoro/kinematics.py b/pysymoro/kinematics.py index a87f62e..16bd6e1 100644 --- a/pysymoro/kinematics.py +++ b/pysymoro/kinematics.py @@ -232,8 +232,8 @@ def compute_vel_acc( 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, forced=True) - vdot[j] = symo.mat_replace(vdot[j], 'VP', j, forced=True) + 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([ From cb1050b88f30f57733d8860a80eda5be3cb3e9ea Mon Sep 17 00:00:00 2001 From: aravind Date: Sun, 3 Aug 2014 04:32:57 +0200 Subject: [PATCH 246/273] Add `symoroutils/configfile.py` --- symoroutils/configfile.py | 111 ++++++++++++++++++++++++++++++++++++++ 1 file changed, 111 insertions(+) create mode 100644 symoroutils/configfile.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) + + From c46c54d0a55c99e06fb93e450e1761458062e4b4 Mon Sep 17 00:00:00 2001 From: aravind Date: Sun, 3 Aug 2014 05:38:01 +0200 Subject: [PATCH 247/273] Rename `make_file_path()` to `get_file_path()` Rename `make_file_path()` to `get_file_path()` in `symoroutils/filemgr.py`. Update all calls. --- symoroutils/filemgr.py | 12 ++++++------ symoroutils/parfile.py | 2 +- symoroutils/symbolmgr.py | 2 +- symoroutils/tests/test_filemgr.py | 6 +++--- 4 files changed, 11 insertions(+), 11 deletions(-) diff --git a/symoroutils/filemgr.py b/symoroutils/filemgr.py index c89e3e4..9de7d15 100644 --- a/symoroutils/filemgr.py +++ b/symoroutils/filemgr.py @@ -76,13 +76,13 @@ def get_folder_path(robot_name): return folder_path -def make_file_path(robot, ext=None): +def get_file_path(robo, ext=None): """ Create the file path with the appropriate extension appended to the file name using an underscore. Args: - robot: An instance of the `Robot` class. + robo: An instance of the `Robot` class. ext: The extension (string) that is to be appended to the file name with an underscore. @@ -90,11 +90,11 @@ def make_file_path(robot, ext=None): The file path (string) created. """ if ext is None: - fname = '%s.par' % get_clean_name(robot.name) + fname = '{0}.par'.format(get_clean_name(robo.name)) else: - fname = '%s_%s.txt' % (get_clean_name(robot.name), ext) - file_path = os.path.join(robot.directory, fname) - make_folders(robot.directory) + 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/parfile.py b/symoroutils/parfile.py index c28cb0c..329a9a7 100644 --- a/symoroutils/parfile.py +++ b/symoroutils/parfile.py @@ -85,7 +85,7 @@ def _write_par_list(robo, f, key, N0, N): def writepar(robo): - fname = filemgr.make_file_path(robo) + fname = filemgr.get_file_path(robo) with open(fname, 'w') as f: # robot description f.write('(* Robotname = \'{0}\' *)\n'.format(robo.name)) diff --git a/symoroutils/symbolmgr.py b/symoroutils/symbolmgr.py index a422966..06e5f53 100644 --- a/symoroutils/symbolmgr.py +++ b/symoroutils/symbolmgr.py @@ -436,7 +436,7 @@ def file_open(self, robo, ext): ext: string provides the file name extention """ - fname = filemgr.make_file_path(robo, ext) + fname = filemgr.get_file_path(robo, ext) self.file_out = open(fname, 'w') def file_close(self): diff --git a/symoroutils/tests/test_filemgr.py b/symoroutils/tests/test_filemgr.py index 1aff510..7a971c3 100644 --- a/symoroutils/tests/test_filemgr.py +++ b/symoroutils/tests/test_filemgr.py @@ -51,21 +51,21 @@ def test_get_folder_path(self): os.path.join(self.base_path, rob2_clean) ) - def test_make_file_path(self): + 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.make_file_path(self.tmp_robot) + 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.make_file_path(self.tmp_robot, 'trm') + trm_format = filemgr.get_file_path(self.tmp_robot, 'trm') self.assertEqual(trm_format, trm_checker) def tearDown(self): From f819632ba85362800660cd06244f9a4694892076 Mon Sep 17 00:00:00 2001 From: aravind Date: Sun, 3 Aug 2014 05:43:58 +0200 Subject: [PATCH 248/273] Modify to store PAR file path in `Robot` class --- pysymoro/robot.py | 2 ++ 1 file changed, 2 insertions(+) diff --git a/pysymoro/robot.py b/pysymoro/robot.py index cf576ef..4961fe8 100644 --- a/pysymoro/robot.py +++ b/pysymoro/robot.py @@ -45,6 +45,8 @@ def __init__( self.name = name """ directory name""" self.directory = filemgr.get_folder_path(name) + """ PAR file path""" + self.par_file_path = filemgr.get_file_path(self) """ whether the base frame is floating: bool""" self.is_floating = is_floating """ whether the robot is a mobile robot""" From 1733e86b7168ff80adf72c66214e5cb7684d1318 Mon Sep 17 00:00:00 2001 From: aravind Date: Sun, 3 Aug 2014 05:48:54 +0200 Subject: [PATCH 249/273] Modify file path variable in `writepar()` Modify file path variable in `writepar()` to read from the `Robot` instance. --- symoroutils/parfile.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/symoroutils/parfile.py b/symoroutils/parfile.py index 329a9a7..297f7b3 100644 --- a/symoroutils/parfile.py +++ b/symoroutils/parfile.py @@ -85,7 +85,7 @@ def _write_par_list(robo, f, key, N0, N): def writepar(robo): - fname = filemgr.get_file_path(robo) + fname = robo.par_file_path with open(fname, 'w') as f: # robot description f.write('(* Robotname = \'{0}\' *)\n'.format(robo.name)) From 39c9190193d934470ffa7571081e13e213e926a0 Mon Sep 17 00:00:00 2001 From: aravind Date: Sun, 3 Aug 2014 05:58:08 +0200 Subject: [PATCH 250/273] Modify to store last used robot Modify `symoroui/layout.py` to store the last used robot in the settings file. --- symoroui/layout.py | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/symoroui/layout.py b/symoroui/layout.py index 02f7d9c..74aef95 100644 --- a/symoroui/layout.py +++ b/symoroui/layout.py @@ -22,8 +22,9 @@ from pysymoro import dynamics from pysymoro import invgeom from pysymoro import pieper -from symoroutils import parfile +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 @@ -904,6 +905,7 @@ def OnClose(self, event): return elif result == wx.CANCEL: return + configfile.set_last_robot(self.robo.par_file_path) self.Destroy() wx.GetApp().ExitMainLoop() From a47dba209e0f8e5db5abd57863c9b5a0269456d3 Mon Sep 17 00:00:00 2001 From: aravind Date: Sun, 3 Aug 2014 06:07:02 +0200 Subject: [PATCH 251/273] Modify default robot to RX90 --- symoroui/layout.py | 4 ++-- symoroutils/samplerobots.py | 2 +- 2 files changed, 3 insertions(+), 3 deletions(-) diff --git a/symoroui/layout.py b/symoroui/layout.py index 74aef95..ec16bda 100644 --- a/symoroui/layout.py +++ b/symoroui/layout.py @@ -44,8 +44,6 @@ def __init__(self, *args, **kwargs): self.statusbar = self.CreateStatusBar() # create menu bar self.create_menu() - # set default robot - self.robo = samplerobots.planar2r() # object to store different ui elements and their keys self.widgets = {} self.widget_keys = {} @@ -57,6 +55,8 @@ def __init__(self, *args, **kwargs): self.create_ui() self.panel.SetSizerAndFit(self.szr_topmost) self.Fit() + # load robot + self.robo = samplerobots.rx90() # update fields with data self.feed_data() # configure status bar diff --git a/symoroutils/samplerobots.py b/symoroutils/samplerobots.py index 9c7f3c6..b93388c 100644 --- a/symoroutils/samplerobots.py +++ b/symoroutils/samplerobots.py @@ -152,7 +152,7 @@ def rx90(): ("XZ{0}, YZ{0}, ZZ{0}") robo.J = [ Matrix(3, 3, var(inertia_matrix_terms.format(i))) \ - for i in robo.num + for i in num ] robo.G = Matrix([0, 0, var('G3')]) return robo From 9781eb7623d01d6f6f55ac5fa0501e54024a75d6 Mon Sep 17 00:00:00 2001 From: aravind Date: Sun, 3 Aug 2014 06:28:10 +0200 Subject: [PATCH 252/273] Modify to load the last used robot at program start --- symoroui/layout.py | 38 ++++++++++++++++++++++++++++++++++---- 1 file changed, 34 insertions(+), 4 deletions(-) diff --git a/symoroui/layout.py b/symoroui/layout.py index ec16bda..bea20a7 100644 --- a/symoroui/layout.py +++ b/symoroui/layout.py @@ -56,7 +56,7 @@ def __init__(self, *args, **kwargs): self.panel.SetSizerAndFit(self.szr_topmost) self.Fit() # load robot - self.robo = samplerobots.rx90() + self.robo = self.load_robot() # update fields with data self.feed_data() # configure status bar @@ -64,9 +64,39 @@ def __init__(self, *args, **kwargs): self.statusbar.SetStatusWidths(widths=[-1, -1]) self.statusbar.SetStatusText(text="Ready", number=0) self.statusbar.SetStatusText( - text="Location of robot files is %s" - % filemgr.get_base_path(), number = 1 - ) + 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.""" From bd9b71fe394a00e35c206bec96a15b850bfdca20 Mon Sep 17 00:00:00 2001 From: aravind Date: Sun, 3 Aug 2014 06:37:00 +0200 Subject: [PATCH 253/273] Modify default folder on `Open` command --- symoroui/layout.py | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/symoroui/layout.py b/symoroui/layout.py index bea20a7..d63ba8d 100644 --- a/symoroui/layout.py +++ b/symoroui/layout.py @@ -763,9 +763,9 @@ def OnOpen(self, event): dialog = wx.FileDialog( self, message="Choose PAR file", - style=wx.OPEN, + style=wx.FD_OPEN, wildcard='*.par', - defaultFile='*.par' + defaultDir=filemgr.get_base_path() ) if dialog.ShowModal() == wx.ID_OK: new_robo, flag = parfile.readpar( From 2361d957c954a0bf4ea0d17aaf41e88223d4c0e5 Mon Sep 17 00:00:00 2001 From: aravind Date: Sun, 3 Aug 2014 06:39:11 +0200 Subject: [PATCH 254/273] Minor modification --- symoroui/layout.py | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/symoroui/layout.py b/symoroui/layout.py index d63ba8d..55d7d35 100644 --- a/symoroui/layout.py +++ b/symoroui/layout.py @@ -787,7 +787,9 @@ def OnSave(self, event): def OnSaveAs(self, event): dialog = wx.FileDialog( - self, message="Save PAR file", + self, + message="Save PAR file", + style=wx.FD_SAVE, defaultFile=self.robo.name+'.par', defaultDir=self.robo.directory, wildcard='*.par' From bf7bc86b3ab61e57d4f2729cd29a33505da79ab6 Mon Sep 17 00:00:00 2001 From: aravind Date: Sun, 3 Aug 2014 07:21:35 +0200 Subject: [PATCH 255/273] Code cleanup --- symoroviz/objects.py | 169 +++++++++++++++++++++++-------------------- 1 file changed, 89 insertions(+), 80 deletions(-) diff --git a/symoroviz/objects.py b/symoroviz/objects.py index 817d3b4..7a30bb5 100644 --- a/symoroviz/objects.py +++ b/symoroviz/objects.py @@ -5,7 +5,7 @@ # https://github.com/symoro/symoro/blob/master/LICENCE for the licence. -import OpenGL.GL as gL +import OpenGL.GL as gl from numpy import degrees, identity, array @@ -21,19 +21,21 @@ def __init__(self, index=0, T=identity(4), show_frame=True): def draw_frame(self): if self.show_frame: - gL.glPushMatrix() - gL.glColor3f(1, 0, 0) + gl.glPushMatrix() + gl.glColor3f(1, 0, 0) self.draw_arrow() - gL.glRotatef(90, 0, 1, 0) - gL.glColor3f(0, 1, 0) + gl.glRotatef(90, 0, 1, 0) + gl.glColor3f(0, 1, 0) self.draw_arrow() - gL.glPopMatrix() + 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) + 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 @@ -42,19 +44,19 @@ def add_child(self, child): self.children.append(child) def draw_frames(self): - gL.glPushMatrix() - gL.glMultMatrixf(self.T) + gl.glPushMatrix() + gl.glMultMatrixf(self.T) self.draw_frame() for child in self.children: child.draw_frames() - gL.glPopMatrix() + gl.glPopMatrix() def draw(self): - gL.glPushMatrix() - gL.glMultMatrixf(self.T) + gl.glPushMatrix() + gl.glMultMatrixf(self.T) for child in self.children: child.draw() - gL.glPopMatrix() + gl.glPopMatrix() def __str__(self): return '{0} Children: {1}'.format(self.index, self.children) @@ -65,7 +67,6 @@ def set_length(self, 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 @@ -78,58 +79,64 @@ def __init__(self, index, theta=0., r=0., alpha=0., d=0., gamma=0., b=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() + 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) + 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() + gl.glPopMatrix() def draw(self): - gL.glPushMatrix() + gl.glPushMatrix() if self.b: self.draw_rod(self.b) - gL.glTranslatef(0, 0, self.b) - gL.glRotatef(degrees(self.gamma), 0, 0, 1) + 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) + 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) + 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) + gl.glTranslatef(0, 0, self.r) + gl.glRotatef(degrees(self.theta), 0, 0, 1) if self.shift: - gL.glPushMatrix() + gl.glPushMatrix() shift = self.shift*self.length self.draw_rod(shift) - gL.glTranslatef(0, 0, shift) + gl.glTranslatef(0, 0, shift) self.draw_joint() - gL.glPopMatrix() + gl.glPopMatrix() else: self.draw_joint() for child in self.children: child.draw() - gL.glPopMatrix() + gl.glPopMatrix() def set_length(self, new_length): if not self.init_length: @@ -141,17 +148,18 @@ def set_length(self, 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) + 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): @@ -168,16 +176,15 @@ def set_length(self, 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) + 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): @@ -188,51 +195,53 @@ def q(self, r): self.r = r def set_length(self, new_length): - self.box_vertices, self.box_normals = Primitives.box_array(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() + gl.glPushMatrix() if self.b: self.draw_rod(self.b) - gL.glTranslatef(0, 0, self.b) - gL.glRotatef(degrees(self.gamma), 0, 0, 1) + 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) + 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) + gl.glPopMatrix() + gl.glTranslatef(self.d, 0, 0) + gl.glRotatef(degrees(self.alpha), 1, 0, 0) if self.shift: - gL.glPushMatrix() + gl.glPushMatrix() shift = self.shift*self.length self.draw_rod(shift) - gL.glTranslatef(0, 0, shift) + gl.glTranslatef(0, 0, shift) self.draw_joint() - gL.glPopMatrix() + 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) + gl.glTranslatef(0, 0, self.r) + gl.glRotatef(degrees(self.theta), 0, 0, 1) for child in self.children: child.draw() - gL.glPopMatrix() + 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) + 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 = \ From 5f8ff0307a8932c84c9f487389b1531a0dc02a90 Mon Sep 17 00:00:00 2001 From: aravind Date: Sun, 3 Aug 2014 08:57:31 +0200 Subject: [PATCH 256/273] Add initial code to draw base and end-effectors --- symoroviz/graphics.py | 6 ++++ symoroviz/objects.py | 67 +++++++++++++++++++++++++++++++++++++++++-- 2 files changed, 70 insertions(+), 3 deletions(-) diff --git a/symoroviz/graphics.py b/symoroviz/graphics.py index 6f10660..6fd4320 100644 --- a/symoroviz/graphics.py +++ b/symoroviz/graphics.py @@ -28,6 +28,8 @@ from symoroviz.objects import RevoluteJoint from symoroviz.objects import FixedJoint from symoroviz.objects import PrismaticJoint +from symoroviz.objects import BaseObject +from symoroviz.objects import EndEffector #TODO: Fullscreen camera rotation bug #TODO: X-, Z-axis @@ -89,6 +91,10 @@ def assign_mono_scale(self): def add_items_to_frame(self, frame, index, jnt_hier): children = jnt_hier[index] + if not children: + frame.add_child(EndEffector(index)) + if index == 0: + frame.add_child(BaseObject(index)) for child_i in children: params = [child_i] for par in ['theta', 'r', 'alpha', 'd', 'gamma', 'b']: diff --git a/symoroviz/objects.py b/symoroviz/objects.py index 7a30bb5..8ad329f 100644 --- a/symoroviz/objects.py +++ b/symoroviz/objects.py @@ -19,6 +19,12 @@ def __init__(self, index=0, T=identity(4), show_frame=True): 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() @@ -58,9 +64,6 @@ def draw(self): 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) @@ -152,6 +155,14 @@ 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) @@ -180,6 +191,14 @@ 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) @@ -234,6 +253,14 @@ 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) @@ -249,3 +276,37 @@ def set_length(self, new_length): super(FixedJoint, self).set_length(new_length) +class BaseObject(object): + def __init__(self, index): + self.index = index + + def __str__(self): + return '(Base {0})'.format(self.index) + + def __repr__(self): + return self.__str__() + + def draw(self): + pass + + def draw_frames(self): + pass + + +class EndEffector(JointObject): + def __init__(self, *args): + super(EndEffector, self).__init__(*args) + + def __str__(self): + return '(End {0})'.format(self.index) + + def __repr__(self): + return self.__str__() + + def draw(self): + pass + + def draw_frames(self): + pass + + From 05add4bd50f6961afd3d54c6eb10637f225560c7 Mon Sep 17 00:00:00 2001 From: aravind Date: Mon, 4 Aug 2014 02:01:56 +0200 Subject: [PATCH 257/273] Minor modifications --- symoroui/labels.py | 2 +- symoroviz/graphics.py | 6 ++---- symoroviz/objects.py | 6 +++--- 3 files changed, 6 insertions(+), 8 deletions(-) diff --git a/symoroui/labels.py b/symoroui/labels.py index 66dcdbf..cb5780c 100644 --- a/symoroui/labels.py +++ b/symoroui/labels.py @@ -145,7 +145,7 @@ optim_menu="&Optimiser", viz_menu="&Visualisation" ) -VIZ_MENU = OrderedDict(m_viz="Visualisation") +VIZ_MENU = OrderedDict(m_viz="&Visualisation") IDEN_MENU = OrderedDict( m_base_inertial_params="Base Inertial parameters", m_dyn_iden_model="Dynamic Identification Model", diff --git a/symoroviz/graphics.py b/symoroviz/graphics.py index 6fd4320..7ebe58c 100644 --- a/symoroviz/graphics.py +++ b/symoroviz/graphics.py @@ -28,7 +28,7 @@ from symoroviz.objects import RevoluteJoint from symoroviz.objects import FixedJoint from symoroviz.objects import PrismaticJoint -from symoroviz.objects import BaseObject +from symoroviz.objects import BaseLink from symoroviz.objects import EndEffector #TODO: Fullscreen camera rotation bug @@ -94,7 +94,7 @@ def add_items_to_frame(self, frame, index, jnt_hier): if not children: frame.add_child(EndEffector(index)) if index == 0: - frame.add_child(BaseObject(index)) + frame.add_child(BaseLink(index)) for child_i in children: params = [child_i] for par in ['theta', 'r', 'alpha', 'd', 'gamma', 'b']: @@ -229,7 +229,6 @@ 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): @@ -348,7 +347,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() diff --git a/symoroviz/objects.py b/symoroviz/objects.py index 8ad329f..774401e 100644 --- a/symoroviz/objects.py +++ b/symoroviz/objects.py @@ -276,9 +276,9 @@ def set_length(self, new_length): super(FixedJoint, self).set_length(new_length) -class BaseObject(object): - def __init__(self, index): - self.index = index +class BaseLink(JointObject): + def __init__(self, *args): + super(BaseLink, self).__init__(*args) def __str__(self): return '(Base {0})'.format(self.index) From 515d64edc18cc7888a801916bc24773c4202d5cf Mon Sep 17 00:00:00 2001 From: aravind Date: Fri, 8 Aug 2014 11:11:22 +0200 Subject: [PATCH 258/273] Modify to indicate base and end-effector Modify to indicate connection with the base link by a black sphere and end-effector (terminal links) by a blue sphere. Remove not needed code. --- symoroviz/graphics.py | 35 +++++++++++++++++---- symoroviz/objects.py | 67 ++++++++++++++++++++--------------------- symoroviz/primitives.py | 1 - 3 files changed, 62 insertions(+), 41 deletions(-) diff --git a/symoroviz/graphics.py b/symoroviz/graphics.py index 7ebe58c..292338c 100644 --- a/symoroviz/graphics.py +++ b/symoroviz/graphics.py @@ -28,8 +28,6 @@ from symoroviz.objects import RevoluteJoint from symoroviz.objects import FixedJoint from symoroviz.objects import PrismaticJoint -from symoroviz.objects import BaseLink -from symoroviz.objects import EndEffector #TODO: Fullscreen camera rotation bug #TODO: X-, Z-axis @@ -91,10 +89,6 @@ def assign_mono_scale(self): def add_items_to_frame(self, frame, index, jnt_hier): children = jnt_hier[index] - if not children: - frame.add_child(EndEffector(index)) - if index == 0: - frame.add_child(BaseLink(index)) for child_i in children: params = [child_i] for par in ['theta', 'r', 'alpha', 'd', 'gamma', 'b']: @@ -113,6 +107,10 @@ def add_items_to_frame(self, frame, index, jnt_hier): 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) @@ -337,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) diff --git a/symoroviz/objects.py b/symoroviz/objects.py index 774401e..a8edb33 100644 --- a/symoroviz/objects.py +++ b/symoroviz/objects.py @@ -80,6 +80,8 @@ def __init__(self, index, theta=0., r=0., alpha=0., d=0., gamma=0., b=0.): 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() @@ -134,19 +136,46 @@ def draw(self): 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) @@ -237,9 +266,13 @@ def draw(self): 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) @@ -276,37 +309,3 @@ def set_length(self, new_length): super(FixedJoint, self).set_length(new_length) -class BaseLink(JointObject): - def __init__(self, *args): - super(BaseLink, self).__init__(*args) - - def __str__(self): - return '(Base {0})'.format(self.index) - - def __repr__(self): - return self.__str__() - - def draw(self): - pass - - def draw_frames(self): - pass - - -class EndEffector(JointObject): - def __init__(self, *args): - super(EndEffector, self).__init__(*args) - - def __str__(self): - return '(End {0})'.format(self.index) - - def __repr__(self): - return self.__str__() - - def draw(self): - pass - - def draw_frames(self): - pass - - diff --git a/symoroviz/primitives.py b/symoroviz/primitives.py index 58ec7a2..c7bda5f 100644 --- a/symoroviz/primitives.py +++ b/symoroviz/primitives.py @@ -101,7 +101,6 @@ def create_box_array(length=3., width=1.): class Primitives: - @classmethod def box_array(cls, length): """ Returns: From f65904f42e5acd941d46b526cfeddbbfb3c7369c Mon Sep 17 00:00:00 2001 From: aravind Date: Fri, 8 Aug 2014 15:17:35 +0200 Subject: [PATCH 259/273] Fix overlapping issue in visualisation UI Fix overlapping issue in visualisation UI. Re-factor and cleanup code. --- symoroviz/graphics.py | 163 ++++++++++++++++++++++++------------------ 1 file changed, 93 insertions(+), 70 deletions(-) diff --git a/symoroviz/graphics.py b/symoroviz/graphics.py index 292338c..294a678 100644 --- a/symoroviz/graphics.py +++ b/symoroviz/graphics.py @@ -434,25 +434,71 @@ def __init__( 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=(3, 0), flag=wx.ALIGN_CENTER) - btnReset = wx.Button(self.p, label="Reset All") - btnReset.Bind(wx.EVT_BUTTON, self.OnResetJoints) - gridControl.Add(btnReset, pos=(5, 0), flag=wx.ALIGN_CENTER) - btnRandom = wx.Button(self.p, label="Random") - btnRandom.Bind(wx.EVT_BUTTON, self.OnFindRandom) - gridControl.Add(btnRandom, pos=(6, 0), flag=wx.ALIGN_CENTER) - btnHome = wx.Button(self.p, label="Default Position") - btnHome.Bind(wx.EVT_BUTTON, self.OnHomePosition) - gridControl.Add(btnHome, pos=(7,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 + @@ -467,12 +513,13 @@ def init_ui(self): button + move mouse """ - gridControl.Add( + grd_szr_control.Add( wx.StaticText(self.p, label=move_help), - pos=(8,0), flag=wx.ALIGN_LEFT + 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 @@ -481,59 +528,35 @@ def init_ui(self): label = 'th' else: label = 'r' - gridJnts.Add( + grd_szr_joints.Add( wx.StaticText(self.p, label=label+str(jnt.index)), pos=(p_index, 0), flag=wx.ALIGN_CENTER_VERTICAL ) - s = FS.FloatSpin( + fsn_qvar = FS.FloatSpin( self.p, size=(70, -1), id=jnt.index, increment=0.05, min_val=-10., max_val=10.0 ) - s.Bind(FS.EVT_FLOATSPIN, self.OnSetJointVar) - s.SetDigits(2) + fsn_qvar.Bind(FS.EVT_FLOATSPIN, self.OnSetJointVar) + fsn_qvar.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 + # 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 - if self.robo.structure == tools.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=(7, 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=(4, 0), flag=wx.ALIGN_CENTER - ) - 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) - gridControl.Add(lbl_length, pos=(1, 0), flag=wx.ALIGN_CENTER) - gridControl.Add(self.jnt_slider, pos=(2, 0), flag=wx.ALIGN_CENTER) - q_box = wx.StaticBoxSizer( + # add all components to sizer + szr_qbox = 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) - top_sizer.Add(gridControl, 0, wx.ALL, 10) - top_sizer.AddSpacer(10) - top_sizer.Add(ver_sizer, 0, wx.ALL, 10) - self.p.SetSizer(top_sizer) + 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()) @@ -545,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(): @@ -558,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() @@ -599,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): @@ -608,6 +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.) + self.canvas.change_lengths(self.slr_joint_size.Value/100.) From 91928189a26093453bd01be873d91a6b0bf843a5 Mon Sep 17 00:00:00 2001 From: aravind Date: Fri, 8 Aug 2014 15:24:50 +0200 Subject: [PATCH 260/273] Minor modifications --- symoroviz/objects.py | 2 ++ 1 file changed, 2 insertions(+) diff --git a/symoroviz/objects.py b/symoroviz/objects.py index a8edb33..646aec5 100644 --- a/symoroviz/objects.py +++ b/symoroviz/objects.py @@ -28,8 +28,10 @@ def __repr__(self): 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() From f7c4141c97f86f9651b037a3800daa37e6e65443 Mon Sep 17 00:00:00 2001 From: aravind Date: Mon, 11 Aug 2014 00:55:26 +0200 Subject: [PATCH 261/273] Modify direct dynamic model computation Refactor in order to unify the DDyM computation for floating base and fixed base robots into the same function `floating_direct_dynmodel()` in `nealgos`. --- pysymoro/nealgos.py | 19 +++++++++++++------ 1 file changed, 13 insertions(+), 6 deletions(-) diff --git a/pysymoro/nealgos.py b/pysymoro/nealgos.py index da211e6..5e3066a 100644 --- a/pysymoro/nealgos.py +++ b/pysymoro/nealgos.py @@ -625,7 +625,8 @@ def composite_inverse_dynmodel(robo, symo): ) # second backward recursion - compute composite term for j in reversed(xrange(0, robo.NL)): - if j == 0: continue + if j == 0: + continue compute_composite_inertia( robo, symo, j, antRj, antPj, comp_inertia3, comp_ms, comp_mass, composite_inertia @@ -726,7 +727,8 @@ def flexible_inverse_dynmodel(robo, symo): ) # second backward recursion - compute star terms for j in reversed(xrange(first_link, robo.NL)): - if j == first_link: continue + if j == first_link: + continue # set composite flag to false when flexible if robo.eta[j]: use_composite = False if use_composite: @@ -830,9 +832,11 @@ def floating_direct_dynmodel(robo, symo): 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(0, robo.NL)): - if j == 0: + 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 @@ -841,13 +845,16 @@ def floating_direct_dynmodel(robo, symo): symo, grandJ, beta, j, star_inertia, star_beta ) # second backward recursion - compute star terms - for j in reversed(xrange(0, robo.NL)): - if j == 0: continue + 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 From 0f410bd237cd315cd78ad4db23bd9ba9c32c37f9 Mon Sep 17 00:00:00 2001 From: aravind Date: Mon, 11 Aug 2014 00:57:52 +0200 Subject: [PATCH 262/273] Rename function Rename `floating_direct_dynmodel()` to `direct_dynmodel()` in `nealgos`. --- pysymoro/nealgos.py | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/pysymoro/nealgos.py b/pysymoro/nealgos.py index 5e3066a..60084d4 100644 --- a/pysymoro/nealgos.py +++ b/pysymoro/nealgos.py @@ -782,10 +782,10 @@ def flexible_inverse_dynmodel(robo, symo): compute_torque(robo, symo, j, jaj, react_wrench, torque) -def floating_direct_dynmodel(robo, symo): +def direct_dynmodel(robo, symo): """ Compute the Direct Dynamic Model using Newton-Euler algorithm for - robots with floating base. + robots with floating and fixed base. Parameters: robo: Robot - instance of robot description container From e37011e773f6d118c6cdf2c2a735d728bed91a8c Mon Sep 17 00:00:00 2001 From: aravind Date: Mon, 11 Aug 2014 00:59:05 +0200 Subject: [PATCH 263/273] Update function calls in `Robot` class --- pysymoro/robot.py | 6 ++---- 1 file changed, 2 insertions(+), 4 deletions(-) diff --git a/pysymoro/robot.py b/pysymoro/robot.py index cd917f8..59fe4f5 100644 --- a/pysymoro/robot.py +++ b/pysymoro/robot.py @@ -282,13 +282,11 @@ def compute_ddym(self): if self.is_floating: # with floating base title = title + "Robot with floating base\n" - symo.write_params_table(self, title, inert=True, dynam=True) - nealgos.floating_direct_dynmodel(self, symo) else: # with fixed base title = title + "Robot with fixed base\n" - symo.write_params_table(self, title, inert=True, dynam=True) - dynamics.compute_direct_dynamic_NE(self, symo) + symo.write_params_table(self, title, inert=True, dynam=True) + nealgos.direct_dynmodel(self, symo) symo.file_close() return symo From 2e53e79327766a672c638fadf99c32cfbcc0cc7a Mon Sep 17 00:00:00 2001 From: aravind Date: Mon, 11 Aug 2014 01:06:46 +0200 Subject: [PATCH 264/273] Remove not needed code in `pysymoro/dynamics.py` --- pysymoro/dynamics.py | 181 ------------------------------------------- 1 file changed, 181 deletions(-) diff --git a/pysymoro/dynamics.py b/pysymoro/dynamics.py index fd72d8f..3a3f3a7 100644 --- a/pysymoro/dynamics.py +++ b/pysymoro/dynamics.py @@ -29,187 +29,6 @@ 'ZZR', 'MXR', 'MYR', 'MZR', 'MR') -def compute_direct_dynamic_NE(robo, symo): - # antecedent angular velocity, projected into jth frame - wi = ParamsInit.init_vec(robo) - w = ParamsInit.init_w(robo) - jaj = ParamsInit.init_vec(robo, 6) - # Twist transform list of Matrices 6x6 - jTant = ParamsInit.init_mat(robo, 6) - beta_star = ParamsInit.init_vec(robo, 6) - grandJ = ParamsInit.init_mat(robo, 6) - link_acc = ParamsInit.init_vec(robo, 6) - H_inv = ParamsInit.init_scalar(robo) - juj = ParamsInit.init_vec(robo, 6) # Jj*aj / Hj - Tau = ParamsInit.init_scalar(robo) - grandVp = ParamsInit.init_vec(robo, 6) - grandVp[0] = Matrix([robo.vdot0 - robo.G, robo.w0]) - # 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] != 0: - 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) - return robo.qddot - - -def compute_beta(robo, symo, j, w, beta_star): - """ - AVAILABLE: nealgos - compute_beta(). - 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(tools.skew(w[j])*E1, 'KW', j) - E3 = tools.skew(w[j])*robo.MS[j] - E4 = symo.mat_replace(tools.skew(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): - """ - AVAILABLE: nealgos - compute_gamma() - Internal function. Computes link's accelerations when - the joint accelerations are zero - - Notes - ===== - link_acc is the output parameter - """ - 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[robo.ant[j]]) * antPj[j] - expr5 = tools.skew(w[robo.ant[j]]) * expr4 - expr6 = antRj[j].transpose() * expr5 - expr7 = expr6 + expr3 - expr7 = symo.mat_replace(expr7, 'LW', j) - link_acc[j] = Matrix([expr7, expr2]) - - -def replace_beta_J_star(robo, symo, j, grandJ, beta_star): - """ - AVIALABLE: nealgos - replace_star_terms() - 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): - """ - SIMILAR: nealgos - compute_tau(), compute_star_terms() - 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): - """ - SIMILAR: nealgos - compute_star_terms() - 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): - """ - SIMILAR: nealgos - compute_joint_accel(), compute_link_accel() - 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): - """ - AVIALBLE: nealgos - compute_reaction_wrench() - 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 inertia_spatial(J, MS, M): - """ - AVAILABLE: nealgos - inertia_spatial() - """ - return Matrix([ - (M*sympy.eye(3)).row_join(tools.skew(MS).T), - tools.skew(MS).row_join(J) - ]) - - # TODO:Finish base parameters computation def base_paremeters(robo_orig): """Computes grouped inertia parameters. New parametrization From 29d21fc1954eeac9299bf423caf6bfcbce19b0c2 Mon Sep 17 00:00:00 2001 From: aravind Date: Mon, 11 Aug 2014 01:09:56 +0200 Subject: [PATCH 265/273] Remove unused code in `pysymoro/tests/oldtest.py` --- pysymoro/tests/oldtest.py | 44 --------------------------------------- 1 file changed, 44 deletions(-) diff --git a/pysymoro/tests/oldtest.py b/pysymoro/tests/oldtest.py index 8adb63d..c74f777 100644 --- a/pysymoro/tests/oldtest.py +++ b/pysymoro/tests/oldtest.py @@ -23,7 +23,6 @@ from pysymoro.geometry import Transform as trns from pysymoro import kinematics from pysymoro import invgeom -from pysymoro import dynamics from symoroutils import filemgr from symoroutils import parfile from symoroutils import samplerobots @@ -212,29 +211,6 @@ def test_jac2(self): self.assertLess(amax(j66 - X*j63), 1e-12) -class testDynamics(unittest.TestCase): - def test_dynamics(self): - robo = samplerobots.cart_pole() - - 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__': # suite = unittest.TestSuite() # suite.addTest(testMisc('test_robo_misc')) @@ -246,25 +222,5 @@ def test_dynamics(self): # suite.addTest(testKinematics('test_jac2')) # unittest.TextTestRunner(verbosity=2).run(suite) # unittest.main() - robo = samplerobots.planar2r() - - 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) - From 037f0e77a94ee7278a893cd2329c3408d4c0b0d6 Mon Sep 17 00:00:00 2001 From: aravind Date: Mon, 11 Aug 2014 01:17:15 +0200 Subject: [PATCH 266/273] Rename `pysymoro/dynamics.py` to `pysymoro/baseparams.py` Rename `pysymoro/dynamics.py` to `pysymoro/baseparams.py`. Update module import calls accordingly. Rename function name in `pysymoro/baseparams.py` and update accordingly. --- pysymoro/{dynamics.py => baseparams.py} | 13 +++++-------- pysymoro/robot.py | 1 - symoroui/layout.py | 4 ++-- 3 files changed, 7 insertions(+), 11 deletions(-) rename pysymoro/{dynamics.py => baseparams.py} (96%) diff --git a/pysymoro/dynamics.py b/pysymoro/baseparams.py similarity index 96% rename from pysymoro/dynamics.py rename to pysymoro/baseparams.py index 3a3f3a7..14cb0ae 100644 --- a/pysymoro/dynamics.py +++ b/pysymoro/baseparams.py @@ -6,8 +6,8 @@ """ -This module of SYMORO package provides symbolic -modeling of robot dynamics. +This module of SYMORO package contains function to compute the base +inertial parameters. """ @@ -16,13 +16,10 @@ import sympy from sympy import Matrix -from pysymoro.geometry import compute_screw_transform -from pysymoro.geometry import compute_rot_trans, Transform -from pysymoro.kinematics import compute_vel_acc -from pysymoro.kinematics import compute_omega +from pysymoro.geometry import compute_rot_trans +from pysymoro.geometry import Transform from symoroutils import symbolmgr from symoroutils import tools -from symoroutils.paramsinit import ParamsInit inert_names = ('XXR', 'XYR', 'XZR', 'YYR', 'YZR', @@ -30,7 +27,7 @@ # TODO:Finish base parameters computation -def base_paremeters(robo_orig): +def base_inertial_parameters(robo_orig): """Computes grouped inertia parameters. New parametrization contains less parameters but generates the same dynamic model diff --git a/pysymoro/robot.py b/pysymoro/robot.py index 59fe4f5..93826da 100644 --- a/pysymoro/robot.py +++ b/pysymoro/robot.py @@ -20,7 +20,6 @@ from sympy import Symbol, Matrix, Expr, Integer from sympy import Mul, Add, factor, zeros, var, sympify, eye -from pysymoro import dynamics from pysymoro import dyniden from pysymoro import inertia from pysymoro import nealgos diff --git a/symoroui/layout.py b/symoroui/layout.py index 55d7d35..df11b11 100644 --- a/symoroui/layout.py +++ b/symoroui/layout.py @@ -19,8 +19,8 @@ from pysymoro.robot import Robot from pysymoro import geometry from pysymoro import kinematics -from pysymoro import dynamics from pysymoro import invgeom +from pysymoro import baseparams from pysymoro import pieper from symoroutils import configfile from symoroutils import filemgr @@ -903,7 +903,7 @@ def OnDirectDynamicModel(self, event): self.model_success('ddm') def OnBaseInertialParams(self, event): - dynamics.base_paremeters(self.robo) + baseparams.base_inertial_parameters(self.robo) self.model_success('regp') def OnDynIdentifModel(self, event): From adcb94132318f23831f9a03b9bcd125816d36011 Mon Sep 17 00:00:00 2001 From: aravind Date: Mon, 11 Aug 2014 17:16:11 +0200 Subject: [PATCH 267/273] Add `prompt_file_save()` method Add `prompt_file_save()` method to `MainFrame` class so that user can rename output files after computation of various models. --- symoroui/layout.py | 50 ++++++++++++++++++++++++++++++++++++++++------ 1 file changed, 44 insertions(+), 6 deletions(-) diff --git a/symoroui/layout.py b/symoroui/layout.py index df11b11..01de627 100644 --- a/symoroui/layout.py +++ b/symoroui/layout.py @@ -12,6 +12,8 @@ import os +import re +import shutil from collections import OrderedDict import wx @@ -747,6 +749,41 @@ def model_success(self, model_name): ) self.message_info(msg) + def prompt_file_save(self, model_symo): + """ + Prompt a file dialog box and save the file. + """ + # 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="Provide a new file name", + 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) + def OnOpen(self, event): if self.changed: dialog_res = wx.MessageBox( @@ -789,7 +826,7 @@ def OnSaveAs(self, event): dialog = wx.FileDialog( self, message="Save PAR file", - style=wx.FD_SAVE, + style=wx.FD_SAVE | wx.FD_OVERWRITE_PROMPT, defaultFile=self.robo.name+'.par', defaultDir=self.robo.directory, wildcard='*.par' @@ -887,27 +924,28 @@ def OnJpqp(self, event): self.model_success('jpqp') def OnInverseDynamic(self, event): - self.robo.compute_idym() + model_symo = self.robo.compute_idym() self.model_success('idm') def OnInertiaMatrix(self, event): - self.robo.compute_inertiamatrix() + model_symo = self.robo.compute_inertiamatrix() self.model_success('inm') def OnCentrCoriolGravTorq(self, event): - self.robo.compute_pseudotorques() + model_symo = self.robo.compute_pseudotorques() self.model_success('ccg') def OnDirectDynamicModel(self, event): - self.robo.compute_ddym() + model_symo = self.robo.compute_ddym() self.model_success('ddm') + self.prompt_file_save(model_symo) def OnBaseInertialParams(self, event): baseparams.base_inertial_parameters(self.robo) self.model_success('regp') def OnDynIdentifModel(self, event): - self.robo.compute_dynidenmodel() + model_symo = self.robo.compute_dynidenmodel() self.model_success('dim') def OnVisualisation(self, event): From cedc75b010f3a39ae605794338d3b1a9f57a0dc9 Mon Sep 17 00:00:00 2001 From: aravind Date: Mon, 11 Aug 2014 17:53:43 +0200 Subject: [PATCH 268/273] Modify in order to prompt to rename output file Modify in order to prompt to rename output file for all models in `Dynamic` and `Identification` menus. Update other functions accordingly to return the required parameters. --- pysymoro/baseparams.py | 2 +- symoroui/layout.py | 46 ++++++++++++++++++++++++++++-------------- 2 files changed, 32 insertions(+), 16 deletions(-) diff --git a/pysymoro/baseparams.py b/pysymoro/baseparams.py index 14cb0ae..30c99f5 100644 --- a/pysymoro/baseparams.py +++ b/pysymoro/baseparams.py @@ -73,7 +73,7 @@ def base_inertial_parameters(robo_orig): title = robo.name + ' grouped inertia parameters' symo.write_params_table(robo, title, inert=True, equations=False) symo.file_close() - return robo, symo.sydi + return symo, robo def vec_mut_J(v, u): diff --git a/symoroui/layout.py b/symoroui/layout.py index 01de627..57ae9d4 100644 --- a/symoroui/layout.py +++ b/symoroui/layout.py @@ -743,15 +743,20 @@ def message_info(self, message): wx.OK | wx.ICON_INFORMATION ).ShowModal() - def model_success(self, model_name): - msg = 'The model has been saved in %s_%s.txt' % ( - os.path.join(self.robo.directory, self.robo.name), model_name - ) + 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 @@ -763,7 +768,7 @@ def prompt_file_save(self, model_symo): # create file dialog dialog = wx.FileDialog( self, - message="Provide a new file name", + message="Rename output file", style=wx.FD_SAVE | wx.FD_OVERWRITE_PROMPT, defaultFile=old_fname, defaultDir=self.robo.directory @@ -783,6 +788,9 @@ def prompt_file_save(self, model_symo): 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 OnOpen(self, event): if self.changed: @@ -920,33 +928,41 @@ def OnAccelerations(self, event): self.model_success('aclr') def OnJpqp(self, event): - kinematics.jdot_qdot(self.robo) - self.model_success('jpqp') + 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() - self.model_success('idm') + 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() - self.model_success('inm') + 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() - self.model_success('ccg') + 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() - self.model_success('ddm') - self.prompt_file_save(model_symo) + out_file_path = self.prompt_file_save(model_symo) + self.model_success(out_file_path) def OnBaseInertialParams(self, event): - baseparams.base_inertial_parameters(self.robo) - self.model_success('regp') + model_symo, base_robo = baseparams.base_inertial_parameters( + self.robo + ) + out_file_path = self.prompt_file_save(model_symo) + self.model_success(out_file_path) def OnDynIdentifModel(self, event): model_symo = self.robo.compute_dynidenmodel() - self.model_success('dim') + out_file_path = self.prompt_file_save(model_symo) + self.model_success(out_file_path) def OnVisualisation(self, event): dialog = ui_definition.DialogVisualisation( From 1f1337dbda307c0919ff3b1bb7d3fac8d9487501 Mon Sep 17 00:00:00 2001 From: aravind Date: Mon, 11 Aug 2014 18:27:07 +0200 Subject: [PATCH 269/273] Modify in order to prompt rename of output file Modify in order to prompt rename of output file for models in `Geometric` and `Kinematic` menus. Update other functions accordingly to return the required parameters. --- pysymoro/geometry.py | 2 + pysymoro/invgeom.py | 2 +- pysymoro/kinematics.py | 8 +- pysymoro/pieper.py | 2 +- symoroui/layout.py | 187 ++++++++++++++++++++++------------------- 5 files changed, 108 insertions(+), 93 deletions(-) diff --git a/pysymoro/geometry.py b/pysymoro/geometry.py index 50f668c..e9bd791 100644 --- a/pysymoro/geometry.py +++ b/pysymoro/geometry.py @@ -525,3 +525,5 @@ def direct_geometric(robo, frames, trig_subs): symo.write_line() symo.file_close() return symo + + diff --git a/pysymoro/invgeom.py b/pysymoro/invgeom.py index b42a9b5..1a3598d 100644 --- a/pysymoro/invgeom.py +++ b/pysymoro/invgeom.py @@ -167,7 +167,7 @@ def loop_solve(robo, symo, know=None): 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 = symbolmgr.SymbolManager() diff --git a/pysymoro/kinematics.py b/pysymoro/kinematics.py index 16bd6e1..9c7402d 100644 --- a/pysymoro/kinematics.py +++ b/pysymoro/kinematics.py @@ -201,10 +201,6 @@ 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 @@ -285,8 +281,10 @@ def accelerations(robo): return symo -#very simial to comute_vel_acc def jdot_qdot(robo): + """ + Similar to compute_vel_acc. + """ symo = symbolmgr.SymbolManager(None) symo.file_open(robo, 'jpqp') symo.write_params_table(robo, 'JdotQdot') diff --git a/pysymoro/pieper.py b/pysymoro/pieper.py index 3a80081..5b081e4 100644 --- a/pysymoro/pieper.py +++ b/pysymoro/pieper.py @@ -283,7 +283,7 @@ def _look_for_case_tree(robo,symo): def igm_pieper(robo, T_ref, n): symo = symbolmgr.SymbolManager() - symo.file_open(robo, 'igm_pieper') + symo.file_open(robo, 'pieper') symo.write_params_table(robo, 'Inverse Geometrix Model for frame %s' % n) _pieper_solve(robo, symo) symo.file_close() diff --git a/symoroui/layout.py b/symoroui/layout.py index 57ae9d4..7795d62 100644 --- a/symoroui/layout.py +++ b/symoroui/layout.py @@ -638,15 +638,16 @@ def create_menu(self): ) self.Bind(wx.EVT_MENU, self.OnDynIdentifModel, m_dyn_iden_model) iden_menu.AppendItem(m_dyn_iden_model) - m_energy_iden_model = wx.MenuItem( - iden_menu, wx.ID_ANY, - ui_labels.IDEN_MENU['m_energy_iden_model'] - ) - # TODO: uncomment the 3 lines below to add the event + # 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) + #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() @@ -659,66 +660,6 @@ def create_menu(self): # set menu bar self.SetMenuBar(menu_bar) - 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 message_error(self, message): wx.MessageDialog( None, @@ -792,6 +733,66 @@ def prompt_file_save(self, model_symo): 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( @@ -853,8 +854,11 @@ def OnTransformationMatrix(self, event): ) if dialog.ShowModal() == wx.ID_OK: frames, trig_subs = dialog.GetValues() - geometry.direct_geometric(self.robo, frames, trig_subs) - self.model_success('trm') + 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): @@ -863,8 +867,9 @@ def OnFastGeometricModel(self, event): ) if dialog.ShowModal() == wx.ID_OK: i, j = dialog.GetValues() - geometry.direct_geometric_fast(self.robo, i, j) - self.model_success('fgm') + 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): @@ -875,8 +880,9 @@ def OnIgmPaul(self, event): ) if dialog.ShowModal() == wx.ID_OK: lst_T, n = dialog.get_values() - invgeom.igm_Paul(self.robo, lst_T, n) - self.model_success('igm_paul') + 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): @@ -887,9 +893,10 @@ def OnIgmPieper(self, event): ) if dialog.ShowModal() == wx.ID_OK: lst_T, n = dialog.get_values() - pieper.igm_pieper(self.robo, lst_T, n) - self.model_success('igm_pieper') - dialog.Destroy() + 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 @@ -900,8 +907,9 @@ def OnJacobianMatrix(self, event): ) if dialog.ShowModal() == wx.ID_OK: n, i, j = dialog.get_values() - kinematics.jacobian(self.robo, n, i, j) - self.model_success('jac') + 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): @@ -909,23 +917,30 @@ def OnDeterminant(self, event): ui_labels.MAIN_WIN['prog_name'], self.robo ) if dialog.ShowModal() == wx.ID_OK: - kinematics.jacobian_determinant(self.robo, *dialog.get_values()) - self.model_success('det') + 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): - if kinematics.kinematic_constraints(self.robo) == tools.FAIL: - self.message_warning('There are no loops') + model_symo = kinematics.kinematic_constraints(self.robo) + if model_symo == tools.FAIL: + self.message_warning("There are no loops") else: - self.model_success('ckel') + out_file_path = self.prompt_file_save(model_symo) + self.model_success(out_file_path) def OnVelocities(self, event): - kinematics.velocities(self.robo) - self.model_success('vlct') + 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): - kinematics.accelerations(self.robo) - self.model_success('aclr') + 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) From f130711a10e4766694fc91c949dc243c79762e75 Mon Sep 17 00:00:00 2001 From: aravind Date: Mon, 11 Aug 2014 19:18:31 +0200 Subject: [PATCH 270/273] Re-factor base inertial parameters computation --- pysymoro/baseparams.py | 15 +------------- pysymoro/robot.py | 44 +++++++++++++++++++++++++++++++++--------- symoroui/layout.py | 5 +---- 3 files changed, 37 insertions(+), 27 deletions(-) diff --git a/pysymoro/baseparams.py b/pysymoro/baseparams.py index 30c99f5..4c18073 100644 --- a/pysymoro/baseparams.py +++ b/pysymoro/baseparams.py @@ -11,8 +11,6 @@ """ -from copy import copy - import sympy from sympy import Matrix @@ -27,7 +25,7 @@ # TODO:Finish base parameters computation -def base_inertial_parameters(robo_orig): +def base_inertial_parameters(robo, symo): """Computes grouped inertia parameters. New parametrization contains less parameters but generates the same dynamic model @@ -41,12 +39,7 @@ def base_inertial_parameters(robo_orig): symo.sydi : dictionary Dictionary with the information of all the sybstitution """ - robo = copy(robo_orig) lam = [0 for i in xrange(robo.NL)] - symo = symbolmgr.SymbolManager() - 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)): @@ -67,13 +60,7 @@ def base_inertial_parameters(robo_orig): # fixed joint, group everuthing compute_lambda(robo, symo, j, antRj, antPj, lam) 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 symo, robo def vec_mut_J(v, u): diff --git a/pysymoro/robot.py b/pysymoro/robot.py index 93826da..9d75ee6 100644 --- a/pysymoro/robot.py +++ b/pysymoro/robot.py @@ -20,6 +20,7 @@ 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 @@ -120,6 +121,9 @@ def __init__( """ k - joint stiffness""" self.k = [0 for j in numj] + def set_par_file_path(self, path): + self.par_file_path = path + def set_defaults(self, joint=False, geom=False, base=False): # joint params if joint: @@ -294,34 +298,56 @@ def compute_pseudotorques(self): Compute Coriolis, Centrifugal, Gravity, Friction and external torques using Newton-Euler algortihm. """ - pseudorobo = copy.deepcopy(self) - pseudorobo.qddot = zeros(pseudorobo.NL, 1) + 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 pseudorobo.eta: + 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(pseudorobo, symo) - elif pseudorobo.is_floating: + 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(pseudorobo, symo) - elif pseudorobo.is_mobile: + 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(pseudorobo, symo) + 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(pseudorobo, symo) + 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() + # save a new robot with base params + return symo, base_robo + def compute_dynidenmodel(self): """ Compute the Dynamic Identification model of the robot. diff --git a/symoroui/layout.py b/symoroui/layout.py index 7795d62..92ce88e 100644 --- a/symoroui/layout.py +++ b/symoroui/layout.py @@ -22,7 +22,6 @@ from pysymoro import geometry from pysymoro import kinematics from pysymoro import invgeom -from pysymoro import baseparams from pysymoro import pieper from symoroutils import configfile from symoroutils import filemgr @@ -968,9 +967,7 @@ def OnDirectDynamicModel(self, event): self.model_success(out_file_path) def OnBaseInertialParams(self, event): - model_symo, base_robo = baseparams.base_inertial_parameters( - self.robo - ) + model_symo, base_robo = self.robo.compute_baseparams() out_file_path = self.prompt_file_save(model_symo) self.model_success(out_file_path) From 98a543ceaaf5371a9c8ab4c26a6923b505bfd8ae Mon Sep 17 00:00:00 2001 From: aravind Date: Mon, 11 Aug 2014 20:35:50 +0200 Subject: [PATCH 271/273] Modify to save the new robot with base parameters --- pysymoro/robot.py | 12 +++++++++--- symoroui/layout.py | 48 +++++++++++++++++++++++++++++----------------- 2 files changed, 39 insertions(+), 21 deletions(-) diff --git a/pysymoro/robot.py b/pysymoro/robot.py index 9d75ee6..4fc5e8c 100644 --- a/pysymoro/robot.py +++ b/pysymoro/robot.py @@ -121,8 +121,11 @@ def __init__( """ k - joint stiffness""" self.k = [0 for j in numj] - def set_par_file_path(self, path): - self.par_file_path = path + def set_par_file_path(self): + self.par_file_path = filemgr.get_file_path(self) + + def set_directory(self): + self.directory = filemgr.get_folder_path(self.name) def set_defaults(self, joint=False, geom=False, base=False): # joint params @@ -345,7 +348,10 @@ def compute_baseparams(self): base_robo, title, inert=True, equations=False ) symo.file_close() - # save a new robot with base params + # set new name for robot with base params + base_robo.name = base_robo.name + "_base" + base_robo.set_directory() + base_robo.set_par_file_path() return symo, base_robo def compute_dynidenmodel(self): diff --git a/symoroui/layout.py b/symoroui/layout.py index 92ce88e..159f64a 100644 --- a/symoroui/layout.py +++ b/symoroui/layout.py @@ -660,28 +660,34 @@ def create_menu(self): self.SetMenuBar(menu_bar) def message_error(self, message): - wx.MessageDialog( - None, - message, - 'Error', - wx.OK | wx.ICON_ERROR - ).ShowModal() + dlg = wx.MessageDialog( + parent=None, + message=message, + caption='Error', + style=wx.OK | wx.ICON_ERROR + ) + dlg.ShowModal() + dlg.Destroy() def message_warning(self, message): - wx.MessageDialog( - None, - message, - 'Error', - wx.OK | wx.ICON_WARNING - ).ShowModal() + dlg = wx.MessageDialog( + parent=None, + message=message, + caption='Warning', + style=wx.OK | wx.ICON_WARNING + ) + dlg.ShowModal() + dlg.Destroy() def message_info(self, message): - wx.MessageDialog( - None, - message, - 'Information', - wx.OK | wx.ICON_INFORMATION - ).ShowModal() + 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") @@ -970,6 +976,12 @@ 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() From 9518a87cb4cbfb07b73e3eb2ccb26d1cbd84d2c6 Mon Sep 17 00:00:00 2001 From: aravind Date: Wed, 13 Aug 2014 00:15:50 +0200 Subject: [PATCH 272/273] Update version in `setup.py` --- setup.py | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/setup.py b/setup.py index bd25d4c..64f6a96 100644 --- a/setup.py +++ b/setup.py @@ -22,13 +22,13 @@ def apply_folder_join(item): bin_scripts = ['symoro-bin.py'] else: bin_scripts = ['symoro-bin'] -bin_scripts = map(apply_folder_join, bin_scripts) +bin_scripts = map(apply_folder_join, bin_scripts) setup( name='symoro', - version='0.1alpha', - description='SYmoblic MOdelling of RObots software', + version='0.2', + description='SYmoblic MOdelling of RObots software package', url='http://github.com/symoro/symoro', scripts=bin_scripts, packages=find_packages(), From 6bde1d59d3378958f6e501e1ba7818b40646d310 Mon Sep 17 00:00:00 2001 From: aravind Date: Mon, 18 Aug 2014 01:40:15 +0200 Subject: [PATCH 273/273] Save `*_base.par` in the same folder Save `*_base.par` robot in the same folder after base inertial computation. Modify `readpar()` function to read the robot name from the PAR file and use it during the instantiation of `Robot` class. --- pysymoro/robot.py | 29 ++++++++++++++++++++--------- symoroutils/parfile.py | 15 +++++++++++---- 2 files changed, 31 insertions(+), 13 deletions(-) diff --git a/pysymoro/robot.py b/pysymoro/robot.py index 4fc5e8c..8853ab4 100644 --- a/pysymoro/robot.py +++ b/pysymoro/robot.py @@ -38,15 +38,16 @@ class Robot(object): Also provides different representations of parameters.""" def __init__( self, name, NL=0, NJ=0, NF=0, is_floating=False, - structure=TREE, is_mobile=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 = filemgr.get_folder_path(name) + self.directory = self.set_directory(directory) """ PAR file path""" - self.par_file_path = filemgr.get_file_path(self) + 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""" @@ -121,11 +122,21 @@ def __init__( """ k - joint stiffness""" self.k = [0 for j in numj] - def set_par_file_path(self): - self.par_file_path = filemgr.get_file_path(self) + 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): - self.directory = filemgr.get_folder_path(self.name) + 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 @@ -350,8 +361,8 @@ def compute_baseparams(self): symo.file_close() # set new name for robot with base params base_robo.name = base_robo.name + "_base" - base_robo.set_directory() - base_robo.set_par_file_path() + file_path = filemgr.get_file_path(base_robo) + base_robo.set_par_file_path(file_path) return symo, base_robo def compute_dynidenmodel(self): diff --git a/symoroutils/parfile.py b/symoroutils/parfile.py index 297f7b3..b55872e 100644 --- a/symoroutils/parfile.py +++ b/symoroutils/parfile.py @@ -127,12 +127,17 @@ def readpar(robo_name, file_path): flag: indicates if any errors accured. (tools.FAIL) """ with open(file_path, 'r') as f: - #initialize the Robot instance 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: @@ -149,14 +154,16 @@ def readpar(robo_name, file_path): 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, - is_floating=is_floating, structure=tools.TYPES[d['Type']], - is_mobile=is_mobile + is_floating=is_floating, + is_mobile=is_mobile, + directory=os.path.dirname(file_path), + par_file_path=file_path ) - robo.directory = os.path.dirname(file_path) # fitting the data acc_line = '' key = ''