├── .gitignore ├── .travis.yml ├── .travis └── tex-config.ini ├── Animate.m ├── Apps ├── Polynomialspirals.mlappinstall ├── Polynomialspirals.prj ├── Triple angle visualizer.mlappinstall ├── Triple angle visualizer.prj ├── polyspiral.mlapp ├── tripleangle.fig ├── tripleangle.m ├── tripleangle_App.mlapp └── tripleangle_App_report.html ├── CITATION ├── CONTRIB ├── DEPENDENCIES ├── LICENSE ├── PGraph.m ├── PackageToolbox.prj ├── Plucker.m ├── Quaternion.m ├── README.md ├── RELEASE ├── RTBPose.m ├── SE2.m ├── SE3.m ├── SO2.m ├── SO3.m ├── SpatialAcceleration.m ├── SpatialF6.m ├── SpatialForce.m ├── SpatialInertia.m ├── SpatialM6.m ├── SpatialMath.prj ├── SpatialMomentum.m ├── SpatialVec6.m ├── SpatialVelocity.m ├── ToolboxPackagingConfiguration.prj ├── Twist.m ├── UnitQuaternion.m ├── about.m ├── angdiff.m ├── angvec2r.m ├── angvec2tr.m ├── arrow3.m ├── circle.m ├── colnorm.m ├── delta2tr.m ├── doc ├── figs │ ├── icon.png │ └── tranimate.gif ├── info.xml ├── info │ ├── acknowledgements.html │ ├── helptoc.xml │ ├── icons │ │ ├── bullet_orange.gif │ │ └── more_arrows.gif │ ├── introduction.html │ ├── license.html │ ├── release.html │ ├── robot_product_page.html │ └── style.css └── manual │ ├── Makefile │ ├── figs │ ├── CT-02-02-eps-converted-to.pdf │ ├── CT-02-02.eps │ ├── CT-02-03-eps-converted-to.pdf │ ├── CT-02-03.eps │ ├── frontcover.pdf │ ├── frontcover1.pdf │ └── plot3d.pdf │ ├── frontcover.m │ ├── right.ist │ ├── spatialmath.tex │ └── titlepage.pdf ├── e2h.m ├── eul2jac.m ├── eul2r.m ├── eul2tr.m ├── h2e.m ├── helpinfo.xml ├── homline.m ├── homtrans.m ├── ishomog.m ├── ishomog2.m ├── isrot.m ├── isrot2.m ├── isunit.m ├── isvec.m ├── lift23.m ├── numcols.m ├── numrows.m ├── oa2r.m ├── oa2tr.m ├── plot2.m ├── plot_arrow.m ├── plot_box.m ├── plot_circle.m ├── plot_ellipse.m ├── plot_homline.m ├── plot_point.m ├── plot_poly.m ├── plot_ribbon.m ├── plot_sphere.m ├── plotvol.m ├── r2t.m ├── randinit.m ├── rot2.m ├── rotx.m ├── roty.m ├── rotz.m ├── rpy2jac.m ├── rpy2r.m ├── rpy2tr.m ├── rt2tr.m ├── skew.m ├── skewa.m ├── smtbcheck.m ├── stlRead.m ├── t2r.m ├── tb_optparse.m ├── tr2angvec.m ├── tr2delta.m ├── tr2eul.m ├── tr2jac.m ├── tr2rpy.m ├── tr2rt.m ├── tranimate.m ├── tranimate2.m ├── transl.m ├── transl2.m ├── trchain.m ├── trchain2.m ├── trexp.m ├── trexp2.m ├── trinterp.m ├── trinterp2.m ├── trlog.m ├── trnorm.m ├── trot2.m ├── trotx.m ├── troty.m ├── trotz.m ├── trplot.m ├── trplot2.m ├── trprint.m ├── trprint2.m ├── trscale.m ├── unit.m ├── unit_test ├── DifferentialMotionTest.m ├── PGraphTest.m ├── PlotTest.m ├── PluckerTest.m ├── QuaternionTest.m ├── README.txt ├── RTBPoseTest.m ├── RunAllTests.m ├── SE2Test.m ├── SE3Test.m ├── SO2Test.m ├── SO3Test.m ├── SpatialTest.m ├── TODO ├── TrajectoryTest.m ├── Transformations2Test.m ├── TransformationsTest.m ├── TwistTest.m ├── UnitQuaternionTest.m ├── UtilityTest.m ├── data │ ├── 20mm_cube_ascii.stl │ └── 20mm_cube_binary.stl ├── failedtests.m ├── featherstone_test │ ├── ID.m │ ├── README.md │ ├── compare.m │ ├── idyn_roy.m │ ├── idyn_rtb.m │ ├── test_roy.m │ └── twist_test.m ├── old │ ├── arange.m │ ├── class_pgraph.m │ ├── dynamics.m │ ├── jacobian.m │ ├── kinematics.m │ ├── leg.m │ ├── loc_dr.m │ ├── loc_ekf.m │ ├── loc_map.m │ ├── loc_map2.m │ ├── loc_pf.m │ ├── loc_slam.m │ ├── mat.m │ ├── nav_bug2.m │ ├── nav_dstar.m │ ├── nav_dxform.m │ ├── nav_prm.m │ ├── nav_rrt.m │ ├── nav_rrt2.m │ ├── plots.m │ ├── quaternion.m │ ├── test.m │ ├── trajectory.m │ └── transform.m ├── plotXTest.m └── tboptparseTest.m ├── vex.m ├── vexa.m └── xyzlabel.m /.gitignore: -------------------------------------------------------------------------------- 1 | # ignore png, gimp and pdf files 2 | *.xcf 3 | *.log 4 | *.aux 5 | *.idx 6 | *.toc 7 | *.out 8 | *.xml 9 | *anaconda.asv 10 | *.synctex.gz 11 | doc/mkdocs 12 | doc/test 13 | -------------------------------------------------------------------------------- /.travis.yml: -------------------------------------------------------------------------------- 1 | ## build Spatial Math Toolbox for MATLAB 2 | # ____ _ _ _ __ __ _ _ _____ _ _ 3 | # / ___| _ __ __ _| |_(_) __ _| | \/ | __ _| |_| |__ |_ _|__ ___ | | |__ _____ __ 4 | # \___ \| '_ \ / _` | __| |/ _` | | |\/| |/ _` | __| '_ \ | |/ _ \ / _ \| | '_ \ / _ \ \/ / 5 | # ___) | |_) | (_| | |_| | (_| | | | | | (_| | |_| | | | | | (_) | (_) | | |_) | (_) > < 6 | # |____/| .__/ \__,_|\__|_|\__,_|_|_| |_|\__,_|\__|_| |_| |_|\___/ \___/|_|_.__/ \___/_/\_\ 7 | 8 | sudo: required 9 | services: docker 10 | language: matlab 11 | dist: xenial 12 | 13 | install: 14 | # install help2doc converter 15 | - git clone https://github.com/petercorke/help2doc $HOME/bin 16 | - export PATH="$(dirname "$HOME/bin"):$PATH" 17 | # install MATLAB unit test runner 18 | #- curl -sL https://storage.googleapis.com/matlabimagesus/public/install.sh | sudo -E bash 19 | 20 | before_script: 21 | ## build the doco PDF and place it in the src folder so it gets shipped to MATLAB cloud 22 | # create the LaTeX code 23 | - help2doc -l --include *.m 24 | # create the release file 25 | - printf '\\def\\release{%s}\n' `cat ./RELEASE` > release.tex 26 | - printf '\\def\\reldate{%s}\n' "`date '+%B %Y'`" >> release.tex 27 | - mv all.tex release.tex doc/manual 28 | # Compile all the LaTex code 29 | # see https://hub.docker.com/r/dxjoke/tectonic-docker 30 | - docker run --mount src=$TRAVIS_BUILD_DIR/doc/manual,target=/usr/src/tex,type=bind dxjoke/tectonic-docker /bin/sh -c "tectonic spatialmath.tex" 31 | # copy to src folder 32 | - mv doc/manual/spatialmath.pdf spatialmath-`cat ./RELEASE`.pdf 33 | ## build a minimalist Contents.m file 34 | - echo "% Spatial Math Toolbox for MATLAB" > Contents.m 35 | - echo "% Version " `cat RELEASE` `date "+%e-%b-%G"` >> Contents.m 36 | 37 | script: 38 | # run the unit tests with MATLAB on Travis cloud 39 | - matlab -sd unit_test -batch "RunAllTests" 40 | 41 | after_success: 42 | # send to codecov 43 | - bash <(curl -s https://codecov.io/bash) 44 | 45 | deploy: 46 | provider: releases 47 | api_key: $GITRELEASE 48 | file: 49 | - spatialmath-`cat ./RELEASE`.pdf 50 | - "Spatial Math Toolbox for MATLAB.mltbx" 51 | skip_cleanup: true 52 | overwrite: true 53 | -------------------------------------------------------------------------------- /.travis/tex-config.ini: -------------------------------------------------------------------------------- 1 | # build-pattern: comma separated list of paths 2 | # paths can have wild card (*). To be safe, use quotes "" 3 | # (if paths have spaces, this would be a problem) 4 | build-pattern = tests/*/main.tex 5 | # packages: comma separated list of packages 6 | packages = 7 | -------------------------------------------------------------------------------- /Apps/Polynomialspirals.mlappinstall: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/petercorke/spatialmath-matlab/6eeff4a79f14286705560b84f1fe72e0b7e0e7f7/Apps/Polynomialspirals.mlappinstall -------------------------------------------------------------------------------- /Apps/Polynomialspirals.prj: -------------------------------------------------------------------------------- 1 | 2 | 3 | Polynomialspirals 4 | Peter Corke 5 | 6 | 7 | 8 | 9 | Interactively plot cubic polynomial spirals 10 | Plot the curve whose curvature is defined as a function of length: from 0 to s_f. The user can adjust the 4 polynomial coefficients (a,b,c,d), the final value of s (s_f) and the initial heading angle (psi0). 11 | /Users/corkep/Desktop/polyspiral.png 12 | 1.0 13 | 14 | 15 | 16 | 17 | /Users/corkep/code/rvc/robotics-toolbox-matlab 18 | 3d8f2693-09b6-4b18-8886-6ea99415b6dd 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | ${PROJECT_ROOT}/polyspiral.mlapp 33 | 34 | 35 | 36 | 37 | 38 | /Users/corkep/code/rvc/robotics-toolbox-matlab 39 | 40 | 41 | 42 | /Applications/MATLAB_R2018a.app 43 | 44 | 45 | 46 | true 47 | true 48 | false 49 | false 50 | false 51 | false 52 | false 53 | false 54 | 10.13.3 55 | false 56 | true 57 | maci64 58 | true 59 | 60 | 61 | -------------------------------------------------------------------------------- /Apps/Triple angle visualizer.mlappinstall: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/petercorke/spatialmath-matlab/6eeff4a79f14286705560b84f1fe72e0b7e0e7f7/Apps/Triple angle visualizer.mlappinstall -------------------------------------------------------------------------------- /Apps/Triple angle visualizer.prj: -------------------------------------------------------------------------------- 1 | 2 | 3 | Triple angle visualizer 4 | Peter Corke 5 | rvc@petercorke.com 6 | 7 | 8 | 9 | Interactively experiment with roll-pitch-yaw or Euler angles 10 | App packaged for Introduction to Robotics MOOC - week 2 11 | 12 | 1.0 13 | 14 | MATLAB 15 | 16 | 17 | 1 18 | 19 | 20 | 9.4 21 | 22 | 23 | /Users/corkep/code/rvc/robotics-toolbox-matlab 24 | fbd722b4-a491-4bf9-9cb0-83bb714de676 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | ${PROJECT_ROOT}/tripleangle.m 36 | 37 | 38 | ${PROJECT_ROOT}/distrib/tmp/rvctools/common/stlRead.m 39 | ${PROJECT_ROOT}/distrib/tmp/rvctools/common/tb_optparse.m 40 | ${PROJECT_ROOT}/r2t.m 41 | ${PROJECT_ROOT}/rotx.m 42 | ${PROJECT_ROOT}/roty.m 43 | ${PROJECT_ROOT}/rotz.m 44 | ${PROJECT_ROOT}/tripleangle.fig 45 | 46 | 47 | ${PROJECT_ROOT}/data/gimbal-ring1.stl 48 | ${PROJECT_ROOT}/data/gimbal-ring2.stl 49 | ${PROJECT_ROOT}/data/gimbal-ring3.stl 50 | ${PROJECT_ROOT}/data/spitfire_assy-gear_up.stl 51 | 52 | 53 | 54 | /Users/corkep/code/rvc/robotics-toolbox-matlab 55 | 56 | 57 | 58 | /Applications/MATLAB_R2018a.app 59 | 60 | 61 | 62 | 63 | 64 | 65 | true 66 | 67 | 68 | 69 | 70 | true 71 | 72 | 73 | 74 | 75 | true 76 | true 77 | false 78 | false 79 | false 80 | false 81 | false 82 | false 83 | 10.13.5 84 | false 85 | true 86 | maci64 87 | true 88 | 89 | 90 | -------------------------------------------------------------------------------- /Apps/polyspiral.mlapp: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/petercorke/spatialmath-matlab/6eeff4a79f14286705560b84f1fe72e0b7e0e7f7/Apps/polyspiral.mlapp -------------------------------------------------------------------------------- /Apps/tripleangle.fig: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/petercorke/spatialmath-matlab/6eeff4a79f14286705560b84f1fe72e0b7e0e7f7/Apps/tripleangle.fig -------------------------------------------------------------------------------- /Apps/tripleangle_App.mlapp: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/petercorke/spatialmath-matlab/6eeff4a79f14286705560b84f1fe72e0b7e0e7f7/Apps/tripleangle_App.mlapp -------------------------------------------------------------------------------- /CITATION: -------------------------------------------------------------------------------- 1 | Citing the Toolbox: 2 | ------------------ 3 | 4 | If you use the Toolbox and want to cite it in a paper please use: 5 | 6 | @book{Corke17a, 7 | Author = {Peter I. Corke}, 8 | Note = {ISBN 978-3-319-54413-7}, 9 | Edition = {Second}, 10 | Publisher = {Springer}, 11 | Title = {Robotics, Vision \& Control: Fundamental Algorithms in {MATLAB}}, 12 | Year = {2017}} 13 | 14 | P.I. Corke, Robotics, Vision \& Control: Fundamental Algorithms in MATLAB. Second edition. Springer, 2017. ISBN 978-3-319-54413-7. 15 | 16 | 17 | -------------------------------------------------------------------------------- /CONTRIB: -------------------------------------------------------------------------------- 1 | The following files are fully/partly contributed by others: 2 | 3 | -------------------------------------------------------------------------------- /DEPENDENCIES: -------------------------------------------------------------------------------- 1 | arrow Matlab central 2 | findjobj Matlab central 3 | numrows 4 | numcols 5 | -------------------------------------------------------------------------------- /LICENSE: -------------------------------------------------------------------------------- 1 | MIT License 2 | 3 | Copyright (c) 2019 Peter Corke 4 | 5 | Permission is hereby granted, free of charge, to any person obtaining a copy 6 | of this software and associated documentation files (the "Software"), to deal 7 | in the Software without restriction, including without limitation the rights 8 | to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 9 | copies of the Software, and to permit persons to whom the Software is 10 | furnished to do so, subject to the following conditions: 11 | 12 | The above copyright notice and this permission notice shall be included in all 13 | copies or substantial portions of the Software. 14 | 15 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 16 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 17 | FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 18 | AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 19 | LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 20 | OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 21 | SOFTWARE. 22 | -------------------------------------------------------------------------------- /RELEASE: -------------------------------------------------------------------------------- 1 | 1.0 2 | -------------------------------------------------------------------------------- /SpatialAcceleration.m: -------------------------------------------------------------------------------- 1 | %SpatialAcceleration Spatial acceleration class 2 | % 3 | % Concrete subclass of SpatialM6 and represents the 4 | % translational and rotational acceleration of a rigid-body moving in 3D space. 5 | % 6 | % SpatialVec6 (abstract handle class) 7 | % | 8 | % +--- SpatialM6 (abstract) 9 | % | | 10 | % | +---SpatialVelocity 11 | % | +---SpatialAcceleration 12 | % | 13 | % +---SpatialF6 (abstract) 14 | % | 15 | % +---SpatialForce 16 | % +---SpatialMomentum 17 | % 18 | % Methods:: 19 | % SpatialAcceleration ^constructor invoked by subclasses 20 | % char ^convert to string 21 | % cross ^^cross product 22 | % display ^display in human readable form 23 | % double ^convert to a 6xN double 24 | % new construct new concrete class of same type 25 | % 26 | % Operators:: 27 | % + ^add spatial vectors of the same type 28 | % - ^subtract spatial vectors of the same type 29 | % - ^unary minus of spatial vectors 30 | % * ^^^premultiplication by SpatialInertia yields SpatialForce 31 | % * ^^^^premultiplication by Twist yields transformed SpatialAcceleration 32 | % 33 | % 34 | % Notes: 35 | % - ^ is inherited from SpatialVec6. 36 | % - ^^ is inherited from SpatialM6. 37 | % - ^^^ are implemented in SpatialInertia. 38 | % - ^^^^ are implemented in Twist. 39 | % 40 | % References:: 41 | % 42 | % - Robot Dynamics Algorithms, R. Featherstone, volume 22, 43 | % Springer International Series in Engineering and Computer Science, 44 | % Springer, 1987. 45 | % - A beginner's guide to 6-d vectors (part 1), R. Featherstone, 46 | % IEEE Robotics Automation Magazine, 17(3):83-94, Sep. 2010. 47 | 48 | % Copyright (C) 1993-2019 Peter I. Corke 49 | % 50 | % This file is part of The Spatial Math Toolbox for MATLAB (SMTB). 51 | % 52 | % Permission is hereby granted, free of charge, to any person obtaining a copy 53 | % of this software and associated documentation files (the "Software"), to deal 54 | % in the Software without restriction, including without limitation the rights 55 | % to use, copy, modify, merge, publish, distribute, sublicense, and/or sell copies 56 | % of the Software, and to permit persons to whom the Software is furnished to do 57 | % so, subject to the following conditions: 58 | % 59 | % The above copyright notice and this permission notice shall be included in all 60 | % copies or substantial portions of the Software. 61 | % 62 | % THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 63 | % IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS 64 | % FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR 65 | % COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER 66 | % IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN 67 | % CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. 68 | % 69 | % https://github.com/petercorke/spatial-math 70 | 71 | classdef SpatialAcceleration < SpatialM6 72 | 73 | methods 74 | function n = new(a, val) 75 | %SpatialAcceleration.new Construct a new object of the same type 76 | % 77 | % A2 = A.new(X) creates a new object of the same type as A, with the value 78 | % X (6x1). 79 | % 80 | % Notes:: 81 | % - Serves as a dynamic constructor. 82 | % - This method is polymorphic across all SpatialVec6 derived classes, and 83 | % allows easy creation of a new object of the same class as an existing 84 | % one without needing to explicitly determine its type. 85 | n = SpatialAcceleration(val); 86 | end 87 | end 88 | end 89 | -------------------------------------------------------------------------------- /SpatialF6.m: -------------------------------------------------------------------------------- 1 | %SpatialF6 Abstract spatial force class 2 | % 3 | % Abstract superclass that represents spatial force. This class has two 4 | % concrete subclasses: 5 | % 6 | % SpatialVec6 (abstract handle class) 7 | % | 8 | % +--- SpatialM6 (abstract) 9 | % | | 10 | % | +---SpatialVelocity 11 | % | +---SpatialAcceleration 12 | % | 13 | % +---SpatialF6 (abstract) 14 | % | 15 | % +---SpatialForce 16 | % +---SpatialMomentum 17 | % 18 | % Methods:: 19 | % SpatialF6 ^constructor invoked by subclasses 20 | % char ^convert to string 21 | % display ^display in human readable form 22 | % double ^convert to a 6xN double 23 | % 24 | % Operators:: 25 | % + ^add spatial vectors of the same type 26 | % - ^subtract spatial vectors of the same type 27 | % - ^unary minus of spatial vectors 28 | % 29 | % Notes: 30 | % - ^ is inherited from SpatialVec6. 31 | % - Subclass of the MATLAB handle class which means that pass by reference semantics 32 | % apply. 33 | % - Spatial vectors can be placed into arrays and indexed. 34 | % 35 | % References:: 36 | % 37 | % - Robot Dynamics Algorithms, R. Featherstone, volume 22, 38 | % Springer International Series in Engineering and Computer Science, 39 | % Springer, 1987. 40 | % - A beginner's guide to 6-d vectors (part 1), R. Featherstone, 41 | % IEEE Robotics Automation Magazine, 17(3):83-94, Sep. 2010. 42 | % 43 | % See also SpatialForce, SpatialMomentum, SpatialInertia, SpatialM6. 44 | 45 | % Copyright (C) 1993-2019 Peter I. Corke 46 | % 47 | % This file is part of The Spatial Math Toolbox for MATLAB (SMTB). 48 | % 49 | % Permission is hereby granted, free of charge, to any person obtaining a copy 50 | % of this software and associated documentation files (the "Software"), to deal 51 | % in the Software without restriction, including without limitation the rights 52 | % to use, copy, modify, merge, publish, distribute, sublicense, and/or sell copies 53 | % of the Software, and to permit persons to whom the Software is furnished to do 54 | % so, subject to the following conditions: 55 | % 56 | % The above copyright notice and this permission notice shall be included in all 57 | % copies or substantial portions of the Software. 58 | % 59 | % THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 60 | % IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS 61 | % FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR 62 | % COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER 63 | % IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN 64 | % CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. 65 | % 66 | % https://github.com/petercorke/spatial-math 67 | 68 | classdef (Abstract) SpatialF6 < SpatialVec6 69 | end 70 | 71 | -------------------------------------------------------------------------------- /SpatialForce.m: -------------------------------------------------------------------------------- 1 | %SpatialForce Spatial force class 2 | % 3 | % Concrete subclass of SpatialF6 and represents the 4 | % translational and rotational forces and torques acting on a rigid-body in 3D space. 5 | % 6 | % SpatialVec6 (abstract handle class) 7 | % | 8 | % +--- SpatialM6 (abstract) 9 | % | | 10 | % | +---SpatialVelocity 11 | % | +---SpatialAcceleration 12 | % | 13 | % +---SpatialF6 (abstract) 14 | % | 15 | % +---SpatialForce 16 | % +---SpatialMomentum 17 | % 18 | % Methods:: 19 | % SpatialForce ^constructor invoked by subclasses 20 | % char ^convert to string 21 | % display ^display in human readable form 22 | % double ^convert to a 6xN double 23 | % new construct new concrete class of same type 24 | % 25 | % Operators:: 26 | % + ^add spatial vectors of the same type 27 | % - ^subtract spatial vectors of the same type 28 | % - ^unary minus of spatial vectors 29 | % * ^^^premultiplication by SE3 yields transformed SpatialForce 30 | % * ^^^^premultiplication by Twist yields transformed SpatialForce 31 | % 32 | % Notes: 33 | % - ^ is inherited from SpatialVec6. 34 | % - ^^ is inherited from SpatialM6. 35 | % - ^^^ are implemented in RTBPose. 36 | % - ^^^^ are implemented in Twist. 37 | % 38 | % References:: 39 | % 40 | % - Robot Dynamics Algorithms, R. Featherstone, volume 22, 41 | % Springer International Series in Engineering and Computer Science, 42 | % Springer, 1987. 43 | % - A beginner's guide to 6-d vectors (part 1), R. Featherstone, 44 | % IEEE Robotics Automation Magazine, 17(3):83-94, Sep. 2010. 45 | % 46 | % See also SpatialVec6, SpatialF6, SpatialMomentum. 47 | 48 | % Copyright (C) 1993-2019 Peter I. Corke 49 | % 50 | % This file is part of The Spatial Math Toolbox for MATLAB (SMTB). 51 | % 52 | % Permission is hereby granted, free of charge, to any person obtaining a copy 53 | % of this software and associated documentation files (the "Software"), to deal 54 | % in the Software without restriction, including without limitation the rights 55 | % to use, copy, modify, merge, publish, distribute, sublicense, and/or sell copies 56 | % of the Software, and to permit persons to whom the Software is furnished to do 57 | % so, subject to the following conditions: 58 | % 59 | % The above copyright notice and this permission notice shall be included in all 60 | % copies or substantial portions of the Software. 61 | % 62 | % THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 63 | % IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS 64 | % FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR 65 | % COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER 66 | % IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN 67 | % CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. 68 | % 69 | % https://github.com/petercorke/spatial-math 70 | 71 | classdef SpatialForce < SpatialF6 72 | 73 | 74 | methods 75 | function n = new(a, val) 76 | %SpatialForce.new Construct a new object of the same type 77 | % 78 | % A2 = A.new(X) creates a new object of the same type as A, with the value 79 | % X (6x1). 80 | % 81 | % Notes:: 82 | % - Serves as a dynamic constructor. 83 | % - This method is polymorphic across all SpatialVec6 derived classes, and 84 | % allows easy creation of a new object of the same class as an existing 85 | % one without needing to explicitly determine its type. 86 | n = SpatialForce(val); 87 | end 88 | end 89 | end 90 | 91 | -------------------------------------------------------------------------------- /SpatialMath.prj: -------------------------------------------------------------------------------- 1 | 2 | 3 | -------------------------------------------------------------------------------- /SpatialMomentum.m: -------------------------------------------------------------------------------- 1 | %SpatialMomentum Spatial momentum class 2 | % 3 | % Concrete subclass of SpatialF6 and represents the 4 | % translational and rotational momentum of a rigid-body moving in 3D space. 5 | % 6 | % SpatialVec6 (abstract handle class) 7 | % | 8 | % +--- SpatialM6 (abstract) 9 | % | | 10 | % | +---SpatialVelocity 11 | % | +---SpatialAcceleration 12 | % | 13 | % +---SpatialF6 (abstract) 14 | % | 15 | % +---SpatialForce 16 | % +---SpatialMomentum 17 | % 18 | % Methods:: 19 | % SpatialMomentum ^constructor invoked by subclasses 20 | % new construct new concrete class of same type 21 | % double ^convert to a 6xN double 22 | % char ^convert to string 23 | % cross ^^cross product 24 | % display ^display in human readable form 25 | % 26 | % Operators:: 27 | % + ^add spatial vectors of the same type 28 | % - ^subtract spatial vectors of the same type 29 | % - ^unary minus of spatial vectors 30 | % 31 | % Notes: 32 | % - ^ is inherited from SpatialVec6. 33 | % - ^^ is inherited from SpatialM6. 34 | % 35 | % References:: 36 | % 37 | % - Robot Dynamics Algorithms, R. Featherstone, volume 22, 38 | % Springer International Series in Engineering and Computer Science, 39 | % Springer, 1987. 40 | % - A beginner's guide to 6-d vectors (part 1), R. Featherstone, 41 | % IEEE Robotics Automation Magazine, 17(3):83-94, Sep. 2010. 42 | % 43 | % See also SpatialVec6, SpatialF6, SpatialForce. 44 | 45 | % Copyright (C) 1993-2019 Peter I. Corke 46 | % 47 | % This file is part of The Spatial Math Toolbox for MATLAB (SMTB). 48 | % 49 | % Permission is hereby granted, free of charge, to any person obtaining a copy 50 | % of this software and associated documentation files (the "Software"), to deal 51 | % in the Software without restriction, including without limitation the rights 52 | % to use, copy, modify, merge, publish, distribute, sublicense, and/or sell copies 53 | % of the Software, and to permit persons to whom the Software is furnished to do 54 | % so, subject to the following conditions: 55 | % 56 | % The above copyright notice and this permission notice shall be included in all 57 | % copies or substantial portions of the Software. 58 | % 59 | % THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 60 | % IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS 61 | % FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR 62 | % COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER 63 | % IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN 64 | % CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. 65 | % 66 | % https://github.com/petercorke/spatial-math 67 | 68 | classdef SpatialMomentum < SpatialF6 69 | methods 70 | function n = new(a, val) 71 | %SpatialMomentum.new Construct a new object of the same type 72 | % 73 | % A2 = A.new(X) creates a new object of the same type as A, with the value 74 | % X (6x1). 75 | % 76 | % Notes:: 77 | % - Serves as a dynamic constructor. 78 | % - This method is polymorphic across all SpatialVec6 derived classes, and 79 | % allows easy creation of a new object of the same class as an existing 80 | % one without needing to explicitly determine its type. 81 | n = SpatialMomentum(val); 82 | end 83 | end 84 | end -------------------------------------------------------------------------------- /SpatialVelocity.m: -------------------------------------------------------------------------------- 1 | %SpatialVelocity Spatial velocity class 2 | % 3 | % Concrete subclass of SpatialM6 and represents the 4 | % translational and rotational velocity of a rigid-body moving in 3D space. 5 | % 6 | % SpatialVec6 (abstract handle class) 7 | % | 8 | % +--- SpatialM6 (abstract) 9 | % | | 10 | % | +---SpatialVelocity 11 | % | +---SpatialAcceleration 12 | % | 13 | % +---SpatialF6 (abstract) 14 | % | 15 | % +---SpatialForce 16 | % +---SpatialMomentum 17 | % 18 | % Methods:: 19 | % SpatialVelocity ^constructor invoked by subclasses 20 | % char ^convert to string 21 | % cross ^^cross product 22 | % display ^display in human readable form 23 | % double ^convert to a 6xN double 24 | % new construct new concrete class of same type 25 | % 26 | % Operators:: 27 | % 28 | % + ^add spatial vectors of the same type 29 | % - ^subtract spatial vectors of the same type 30 | % - ^unary minus of spatial vectors 31 | % * ^^^premultiplication by SpatialInertia yields SpatialMomentum 32 | % * ^^^^premultiplication by Twist yields transformed SpatialVelocity 33 | % 34 | % Notes: 35 | % - ^ is inherited from SpatialVec6. 36 | % - ^^ is inherited from SpatialM6. 37 | % - ^^^ are implemented in SpatialInertia. 38 | % - ^^^^ are implemented in Twist. 39 | % 40 | % References:: 41 | % 42 | % - Robot Dynamics Algorithms, R. Featherstone, volume 22, 43 | % Springer International Series in Engineering and Computer Science, 44 | % Springer, 1987. 45 | % - A beginner's guide to 6-d vectors (part 1), R. Featherstone, 46 | % IEEE Robotics Automation Magazine, 17(3):83-94, Sep. 2010. 47 | % 48 | % See also SpatialVec6, SpatialM6, SpatialAcceleration, SpatialInertia, SpatialMomentum. 49 | 50 | % Copyright (C) 1993-2019 Peter I. Corke 51 | % 52 | % This file is part of The Spatial Math Toolbox for MATLAB (SMTB). 53 | % 54 | % Permission is hereby granted, free of charge, to any person obtaining a copy 55 | % of this software and associated documentation files (the "Software"), to deal 56 | % in the Software without restriction, including without limitation the rights 57 | % to use, copy, modify, merge, publish, distribute, sublicense, and/or sell copies 58 | % of the Software, and to permit persons to whom the Software is furnished to do 59 | % so, subject to the following conditions: 60 | % 61 | % The above copyright notice and this permission notice shall be included in all 62 | % copies or substantial portions of the Software. 63 | % 64 | % THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 65 | % IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS 66 | % FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR 67 | % COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER 68 | % IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN 69 | % CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. 70 | % 71 | % https://github.com/petercorke/spatial-math 72 | 73 | classdef SpatialVelocity < SpatialM6 74 | methods 75 | 76 | function n = new(a, val) 77 | %SpatialVelocity.new Construct a new object of the same type 78 | % 79 | % A2 = A.new(X) creates a new object of the same type as A, with the value 80 | % X (6x1). 81 | % 82 | % Notes:: 83 | % - Serves as a dynamic constructor. 84 | % - This method is polymorphic across all SpatialVec6 derived classes, and 85 | % allows easy creation of a new object of the same class as an existing 86 | % one without needing to explicitly determine its type. 87 | n = SpatialVelocity(val); 88 | end 89 | end 90 | end 91 | -------------------------------------------------------------------------------- /UnitQuaternion.m: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/petercorke/spatialmath-matlab/6eeff4a79f14286705560b84f1fe72e0b7e0e7f7/UnitQuaternion.m -------------------------------------------------------------------------------- /about.m: -------------------------------------------------------------------------------- 1 | %ABOUT Compact display of variable type 2 | % 3 | % ABOUT(X) displays a compact line that describes the class and dimensions of 4 | % X. 5 | % 6 | % ABOUT X as above but this is the command rather than functional form. 7 | % 8 | % Examples:: 9 | % >> a=1; 10 | % >> about a 11 | % a [double] : 1x1 (8 bytes) 12 | % 13 | % >> a = rand(5,7); 14 | % >> about a 15 | % a [double] : 5x7 (280 bytes) 16 | % 17 | % See also WHOS. 18 | 19 | % Copyright (C) 1993-2019 Peter I. Corke 20 | % 21 | % This file is part of The Spatial Math Toolbox for MATLAB (SMTB). 22 | % 23 | % Permission is hereby granted, free of charge, to any person obtaining a copy 24 | % of this software and associated documentation files (the "Software"), to deal 25 | % in the Software without restriction, including without limitation the rights 26 | % to use, copy, modify, merge, publish, distribute, sublicense, and/or sell copies 27 | % of the Software, and to permit persons to whom the Software is furnished to do 28 | % so, subject to the following conditions: 29 | % 30 | % The above copyright notice and this permission notice shall be included in all 31 | % copies or substantial portions of the Software. 32 | % 33 | % THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 34 | % IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS 35 | % FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR 36 | % COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER 37 | % IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN 38 | % CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. 39 | % 40 | % https://github.com/petercorke/spatial-math 41 | function about(varargin) 42 | for i=1:nargin 43 | var = varargin{i}; 44 | if ischar(var) 45 | % invoked without parentheses 46 | w = evalin('caller', sprintf('whos(''%s'')', var)); 47 | varname = var; 48 | else 49 | w = whos('var'); 50 | varname = inputname(1); 51 | end 52 | 53 | if isempty(w) 54 | error('SMTB:about', ['cant find variable ' var]) 55 | end 56 | ss = sprintf('%d', w.size(1)); 57 | for i=2:length(w.size) 58 | ss = strcat(ss, sprintf('x%d', w.size(i))); 59 | end 60 | 61 | % build a string to show if complex or not 62 | if w.complex 63 | cmplx = '+complex'; 64 | else 65 | cmplx = ''; 66 | end 67 | 68 | % build a string to show size in convenient format 69 | suffix = {'bytes', 'kB', 'MB', 'GB', 'TB'}; 70 | sz = w.bytes; 71 | for i=1:numel(suffix) 72 | if sz/1000 < 1 73 | break; 74 | end 75 | sz = sz/1000; 76 | end 77 | 78 | if i==1 79 | size = sprintf('%d %s', sz, suffix{i}); 80 | else 81 | size = sprintf('%.1f %s', sz, suffix{i}); 82 | end 83 | 84 | % now display the info 85 | fprintf('%s [%s%s] : %s (%s)\n', ... 86 | varname, w.class, cmplx, ss, size); 87 | 88 | end 89 | -------------------------------------------------------------------------------- /angdiff.m: -------------------------------------------------------------------------------- 1 | %ANGDIFF Difference of two angles 2 | % 3 | % ANGDIFF(TH1, TH2) is the difference between angles TH1 and TH2, ie. TH1-TH2 4 | % on the circle. The result is in the interval [-pi pi). Either or both 5 | % arguments can be a vector: 6 | % - If TH1 is a vector, and TH2 a scalar then return a vector where TH2 is modulo 7 | % subtracted from the corresponding elements of TH1. 8 | % - If TH1 is a scalar, and TH2 a vector then return a vector where the 9 | % corresponding elements of TH2 are modulo subtracted from TH1. 10 | % - If TH1 and TH2 are vectors then return a vector whose elements are the modulo 11 | % difference of the corresponding elements of TH1 and TH2, which must be the 12 | % same length. 13 | % 14 | % ANGDIFF(TH) as above but TH=[TH1 TH2]. 15 | % 16 | % ANGDIFF(TH) is the equivalent angle to the scalar TH in the interval [-pi pi). 17 | % 18 | % Notes:: 19 | % - The MathWorks Robotics Systems Toolbox defines a function with the same name 20 | % which computes TH2-TH1 rather than TH1-TH2. 21 | % - If TH1 and TH2 are both vectors they should have the same 22 | % orientation, which the output will assume. 23 | % 24 | 25 | % Copyright (C) 1993-2019 Peter I. Corke 26 | % 27 | % This file is part of The Spatial Math Toolbox for MATLAB (SMTB). 28 | % 29 | % Permission is hereby granted, free of charge, to any person obtaining a copy 30 | % of this software and associated documentation files (the "Software"), to deal 31 | % in the Software without restriction, including without limitation the rights 32 | % to use, copy, modify, merge, publish, distribute, sublicense, and/or sell copies 33 | % of the Software, and to permit persons to whom the Software is furnished to do 34 | % so, subject to the following conditions: 35 | % 36 | % The above copyright notice and this permission notice shall be included in all 37 | % copies or substantial portions of the Software. 38 | % 39 | % THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 40 | % IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS 41 | % FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR 42 | % COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER 43 | % IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN 44 | % CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. 45 | % 46 | % https://github.com/petercorke/spatial-math 47 | 48 | function d = angdiff(th1, th2) 49 | 50 | switch nargin 51 | case 1 52 | if length(th1) == 2 53 | d = th1(1) - th1(2); 54 | else 55 | d = th1; 56 | end 57 | case 2 58 | if length(th1) > 1 && length(th2) > 1 59 | % if both arguments are vectors, they must be the same 60 | assert(all(size(th1) == size(th2)), 'SMTB:angdiff:badarg', 'vectors must be same shape'); 61 | end 62 | % th1 or th2 could be scalar 63 | d = th1 - th2; 64 | end 65 | 66 | % wrap the result into the interval [-pi pi) 67 | d = mod(d+pi, 2*pi) - pi; 68 | end 69 | 70 | % Simplistic version of the code, easy to see what it does, but slow... 71 | % 72 | % for very negative angles keep adding 2pi 73 | % while true 74 | % k = find(d < -pi); 75 | % if isempty(k) 76 | % break; 77 | % end 78 | % d(k) = d(k) + 2*pi; 79 | % end 80 | % 81 | % % for very positive angles keep subtracting 2pi 82 | % while true 83 | % k = find(d > pi); 84 | % if isempty(k) 85 | % break; 86 | % end 87 | % d(k) = d(k) - 2*pi; 88 | % end 89 | -------------------------------------------------------------------------------- /angvec2r.m: -------------------------------------------------------------------------------- 1 | %ANGVEC2R Convert angle and vector orientation to a rotation matrix 2 | % 3 | % R = ANGVEC2R(THETA, V) is an orthonormal rotation matrix (3x3) 4 | % equivalent to a rotation of THETA about the vector V. 5 | % 6 | % Notes:: 7 | % - Uses Rodrigues' formula 8 | % - If THETA == 0 then return identity matrix and ignore V. 9 | % - If THETA ~= 0 then V must have a finite length. 10 | % 11 | % See also angvec2tr, eul2r, rpy2r, tr2angvec, trexp, SO3.angvec. 12 | 13 | % Copyright (C) 1993-2019 Peter I. Corke 14 | % 15 | % This file is part of The Spatial Math Toolbox for MATLAB (SMTB). 16 | % 17 | % Permission is hereby granted, free of charge, to any person obtaining a copy 18 | % of this software and associated documentation files (the "Software"), to deal 19 | % in the Software without restriction, including without limitation the rights 20 | % to use, copy, modify, merge, publish, distribute, sublicense, and/or sell copies 21 | % of the Software, and to permit persons to whom the Software is furnished to do 22 | % so, subject to the following conditions: 23 | % 24 | % The above copyright notice and this permission notice shall be included in all 25 | % copies or substantial portions of the Software. 26 | % 27 | % THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 28 | % IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS 29 | % FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR 30 | % COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER 31 | % IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN 32 | % CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. 33 | % 34 | % https://github.com/petercorke/spatial-math 35 | function R = angvec2r(theta, v) 36 | 37 | if nargin < 2 || ~isscalar(theta) || ~isvec(v) 38 | error('SMTB:angvec2r:badarg', 'bad arguments'); 39 | end 40 | if ~isa(v, 'sym') && norm(v) < 10*eps 41 | if (abs(theta) > 0) 42 | error('SMTB:angvec2r:badarg', 'norm of direction is zero'); 43 | else 44 | R = eye(3,3); 45 | return; 46 | end 47 | end 48 | 49 | % Rodrigue's equation 50 | 51 | sk = skew( unit(v) ); 52 | R = eye(3,3) + sin(theta)*sk + (1-cos(theta))*sk^2; 53 | end 54 | -------------------------------------------------------------------------------- /angvec2tr.m: -------------------------------------------------------------------------------- 1 | %ANGVEC2TR Convert angle and vector orientation to a homogeneous transform 2 | % 3 | % T = ANGVEC2TR(THETA, V) is a homogeneous transform matrix (4x4) equivalent to a 4 | % rotation of THETA about the vector V. 5 | % 6 | % Note:: 7 | % - Uses Rodrigues' formula 8 | % - The translational part is zero. 9 | % - If THETA == 0 then return identity matrix and ignore V. 10 | % - If THETA ~= 0 then V must have a finite length. 11 | % 12 | % See also angvec2r, eul2tr, rpy2tr, angvec2r, tr2angvec, trexp, SO3.angvec. 13 | 14 | % Copyright (C) 1993-2019 Peter I. Corke 15 | % 16 | % This file is part of The Spatial Math Toolbox for MATLAB (SMTB). 17 | % 18 | % Permission is hereby granted, free of charge, to any person obtaining a copy 19 | % of this software and associated documentation files (the "Software"), to deal 20 | % in the Software without restriction, including without limitation the rights 21 | % to use, copy, modify, merge, publish, distribute, sublicense, and/or sell copies 22 | % of the Software, and to permit persons to whom the Software is furnished to do 23 | % so, subject to the following conditions: 24 | % 25 | % The above copyright notice and this permission notice shall be included in all 26 | % copies or substantial portions of the Software. 27 | % 28 | % THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 29 | % IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS 30 | % FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR 31 | % COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER 32 | % IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN 33 | % CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. 34 | % 35 | % https://github.com/petercorke/spatial-math 36 | 37 | function T = angvec2tr(theta, k) 38 | 39 | assert( nargin >= 2, 'SMTB:angvec2tr:badarg', 'two arguments required'); 40 | 41 | T = r2t( angvec2r(theta, k) ); 42 | end 43 | -------------------------------------------------------------------------------- /circle.m: -------------------------------------------------------------------------------- 1 | %CIRCLE Compute points on a circle 2 | % 3 | % CIRCLE(C, R, OPTIONS) plots a circle centred at C (1x2) with radius R on the current 4 | % axes. 5 | % 6 | % X = CIRCLE(C, R, OPTIONS) is a matrix (2xN) whose columns define the 7 | % coordinates [x,y] of points around the circumference of a circle 8 | % centred at C (1x2) and of radius R. 9 | % 10 | % C is normally 2x1 but if 3x1 then the circle is embedded in 3D, and X is Nx3. 11 | % The circle is always in the xy-plane with a z-coordinate of C(3). 12 | % 13 | % Options:: 14 | % 'n',N Specify the number of points (default 50) 15 | 16 | % Copyright (C) 1993-2019 Peter I. Corke 17 | % 18 | % This file is part of The Spatial Math Toolbox for MATLAB (SMTB). 19 | % 20 | % Permission is hereby granted, free of charge, to any person obtaining a copy 21 | % of this software and associated documentation files (the "Software"), to deal 22 | % in the Software without restriction, including without limitation the rights 23 | % to use, copy, modify, merge, publish, distribute, sublicense, and/or sell copies 24 | % of the Software, and to permit persons to whom the Software is furnished to do 25 | % so, subject to the following conditions: 26 | % 27 | % The above copyright notice and this permission notice shall be included in all 28 | % copies or substantial portions of the Software. 29 | % 30 | % THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 31 | % IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS 32 | % FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR 33 | % COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER 34 | % IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN 35 | % CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. 36 | % 37 | % https://github.com/petercorke/spatial-math 38 | function out = circle(centre, rad, varargin) 39 | 40 | opt.n = 50; 41 | 42 | [opt,arglist] = tb_optparse(opt, varargin); 43 | 44 | % compute points on circumference 45 | th = [0:opt.n-1]'/ opt.n*2*pi; 46 | x = rad*cos(th) + centre(1); 47 | y = rad*sin(th) + centre(2); 48 | 49 | % add extra row if z-coordinate is specified, but circle is always in xy plane 50 | if length(centre) > 2 51 | z = ones(size(x))*centre(3); 52 | p = [x y z]'; 53 | else 54 | p = [x y]'; 55 | end 56 | 57 | if nargout > 0 58 | % return now 59 | out = p; 60 | return; 61 | else 62 | % else plot the circle 63 | p = [p p(:,1)]; % make it close 64 | if numrows(p) > 2 65 | plot3(p(1,:), p(2,:), p(3,:), arglist{:}); 66 | else 67 | plot(p(1,:), p(2,:), arglist{:}); 68 | end 69 | end 70 | -------------------------------------------------------------------------------- /colnorm.m: -------------------------------------------------------------------------------- 1 | %COLNORM Column-wise norm of a matrix 2 | % 3 | % CN = COLNORM(A) is a vector (1xM) comprising the Euclidean norm of each column of the 4 | % matrix A (NxM). 5 | % 6 | % See also norm. 7 | 8 | % Copyright (C) 1993-2019 Peter I. Corke 9 | % 10 | % This file is part of The Spatial Math Toolbox for MATLAB (SMTB). 11 | % 12 | % Permission is hereby granted, free of charge, to any person obtaining a copy 13 | % of this software and associated documentation files (the "Software"), to deal 14 | % in the Software without restriction, including without limitation the rights 15 | % to use, copy, modify, merge, publish, distribute, sublicense, and/or sell copies 16 | % of the Software, and to permit persons to whom the Software is furnished to do 17 | % so, subject to the following conditions: 18 | % 19 | % The above copyright notice and this permission notice shall be included in all 20 | % copies or substantial portions of the Software. 21 | % 22 | % THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 23 | % IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS 24 | % FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR 25 | % COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER 26 | % IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN 27 | % CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. 28 | % 29 | % https://github.com/petercorke/spatial-math 30 | function n = colnorm(a) 31 | 32 | n = sqrt(sum(a.^2)); 33 | -------------------------------------------------------------------------------- /delta2tr.m: -------------------------------------------------------------------------------- 1 | %DELTA2TR Convert differential motion to SE(3) homogeneous transform 2 | % 3 | % T = DELTA2TR(D) is a homogeneous transform (4x4) representing differential 4 | % motion D (6x1). 5 | % 6 | % The vector D=(dx, dy, dz, dRx, dRy, dRz) represents infinitessimal translation 7 | % and rotation, and is an approximation to the instantaneous spatial velocity 8 | % multiplied by time step. 9 | % 10 | % Reference:: 11 | % - Robotics, Vision & Control: Second Edition, P. Corke, Springer 2016; p67. 12 | % 13 | % See also tr2delta, SE3.delta. 14 | 15 | % Copyright (C) 1993-2019 Peter I. Corke 16 | % 17 | % This file is part of The Spatial Math Toolbox for MATLAB (SMTB). 18 | % 19 | % Permission is hereby granted, free of charge, to any person obtaining a copy 20 | % of this software and associated documentation files (the "Software"), to deal 21 | % in the Software without restriction, including without limitation the rights 22 | % to use, copy, modify, merge, publish, distribute, sublicense, and/or sell copies 23 | % of the Software, and to permit persons to whom the Software is furnished to do 24 | % so, subject to the following conditions: 25 | % 26 | % The above copyright notice and this permission notice shall be included in all 27 | % copies or substantial portions of the Software. 28 | % 29 | % THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 30 | % IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS 31 | % FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR 32 | % COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER 33 | % IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN 34 | % CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. 35 | % 36 | % https://github.com/petercorke/spatial-math 37 | 38 | function delta = delta2tr(d) 39 | d = d(:); 40 | delta = eye(4,4) + [skew(d(4:6)) d(1:3); 0 0 0 0]; 41 | -------------------------------------------------------------------------------- /doc/figs/icon.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/petercorke/spatialmath-matlab/6eeff4a79f14286705560b84f1fe72e0b7e0e7f7/doc/figs/icon.png -------------------------------------------------------------------------------- /doc/figs/tranimate.gif: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/petercorke/spatialmath-matlab/6eeff4a79f14286705560b84f1fe72e0b7e0e7f7/doc/figs/tranimate.gif -------------------------------------------------------------------------------- /doc/info.xml: -------------------------------------------------------------------------------- 1 | 3 | 4 | 5 | 2017b 6 | Robotics Toolbox for MATLAB 7 | toolbox 8 | 9 | info 10 | 11 | -------------------------------------------------------------------------------- /doc/info/acknowledgements.html: -------------------------------------------------------------------------------- 1 | 2 | 3 | Acknowledgements 4 | 5 | 6 | 7 | 8 | 9 |

Robotics Toolbox for Matlab

10 |

Acknowledgements

11 |
12 |

13 | The following generous people have contributed code (or inspired code) to the Robotics Toolbox: 14 |

15 |

16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 |
Arturo Gil STL models used for plotting 3D robot models
Giorgio Grissetti Inspiration for the PoseGraph class
Joern Malzahn Symbolic Toolbox code
Paul Pounds Quadrotor Simulink model and graphics
Paul NewmanInspiration for the mobile robot estimation classes
Wynand SmartVarious arm robot models
Robert Biro and Gary Von McMurrayInverse kinematics for Puma560 robot
25 |

26 | 27 | -------------------------------------------------------------------------------- /doc/info/helptoc.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | Robotics Toolbox for MATLAB 8 | 9 | 10 | 11 | 12 | 13 | 14 | Functions 15 | Functions by category 16 | Functions alphabetic order 17 | 18 | 19 | 20 | Simulink 21 | 22 | 23 | Manual (PDF, ~400 pages) 24 | 25 | 26 | About 27 | 28 | 29 | Acknowledgements 30 | 31 | 32 | Release Notes & Version History 33 | 34 | 35 | Copyrights & License 36 | 37 | 38 | Web Pages 39 | Robotics Toolbox web page 40 | Robotics Toolbox discussion group 41 | Robotics, Vision & Control (the book) 42 | 43 | 44 | 45 | 46 | -------------------------------------------------------------------------------- /doc/info/icons/bullet_orange.gif: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/petercorke/spatialmath-matlab/6eeff4a79f14286705560b84f1fe72e0b7e0e7f7/doc/info/icons/bullet_orange.gif -------------------------------------------------------------------------------- /doc/info/icons/more_arrows.gif: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/petercorke/spatialmath-matlab/6eeff4a79f14286705560b84f1fe72e0b7e0e7f7/doc/info/icons/more_arrows.gif -------------------------------------------------------------------------------- /doc/info/introduction.html: -------------------------------------------------------------------------------- 1 | 2 | 3 | Introduction 4 | 5 | 6 | 7 | 8 | 9 |

Robotics Toolbox for MATLAB

10 | 11 |

Introduction

12 |

13 | Welcome to the Robotics Toolbox for MATLAB. 14 |

15 | The toolbox is based on MATLAB code developed by Peter Corke for his PhD research over the period 1990 to 1994. 16 | An early article about the toolbox was published in the 17 | IEEE Robotics & Automation magazine (Volume 3, Issue 1, March 1996). 18 |

19 | Since that time it has been periodically updated, most often, to accomodate changes to the MATLAB language. The biggest such change was the introduction of MATLAB objects which made the representation of quaternions, robots and links much more convenient. More recently the toolbox was updated to handle modified DH (Craig) as well as standard DH notation. 20 | A more recent article 21 | IEEE Robotics & Automation magazine article (Volume 14, Issue 4, Dec. 2007 Page(s):16 - 17) discusses the toolbox and its 22 | companion toolbox for machine vision. 23 |

24 | Several versions now exist and in different languages such as Python, SciLab and LabView. 25 |

26 | 27 |

28 | The Toolboxes have some important virtues. 29 | Firstly, they have been around for a long time and used by many people for many different problems so the code is entitled to some 30 | level of trust. 31 | The Toolbox provides a ``gold standard" with which to compare new algorithms or even the same algorithms coded 32 | in new languages or executing in new environments. 33 |

34 | Secondly, they allow the user to work with real problems, not trivial examples. 35 | For real robots, those with more than two links, or real images with 36 | millions of pixels the computation is beyond unaided human ability. 37 | Thirdly, they allow us to gain insight which is otherwise lost in the complexity. 38 | We can rapidly and easily experiment, play "what if" games, and depict the 39 | results graphically using MATLAB's powerful display tools such as 2D and 3D graphs and images. 40 | MATLAB that will be familiar to most engineering students and 41 |

42 | Fourthly, the Toolbox code makes many common algorithms tangible and accessible. 43 | You can read the code, you can apply it to your own problems, and you can extend it or rewrite it. 44 | At the very least it gives you a headstart. 45 |

46 | 47 | 48 | -------------------------------------------------------------------------------- /doc/info/license.html: -------------------------------------------------------------------------------- 1 | 2 | 3 | GNU GENERAL PUBLIC LICENSE 4 | 5 | 6 | 7 | 8 | 9 |

Robotics Toolbox for MATLAB

10 |

Copyrights & License

11 |

12 | The Robotics Toolbox for MATLAB is free software: you can redistribute it and/or modify 13 | it under the terms of the GNU Lesser General Public License as published by 14 | the Free Software Foundation, either version 3 of the License, or 15 | (at your option) any later version. 16 |

17 | RTB is distributed in the hope that it will be useful, 18 | but WITHOUT ANY WARRANTY; without even the implied warranty of 19 | MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 20 | GNU Lesser General Public License for more details. 21 |

22 | You should have received a copy of the GNU Leser General Public License 23 | along with RTB. If not, see 24 |

25 | 26 | 27 | -------------------------------------------------------------------------------- /doc/info/robot_product_page.html: -------------------------------------------------------------------------------- 1 | 2 | 3 | Robotic Toolbox for MATLAB 4 | 5 | 6 | 7 | 8 | 9 |

Robotics Toolbox for MATLAB

10 |
11 | Tools for robot manipulators and mobile robots 12 |

13 | This, the tenth major release of the Toolbox, representing over twenty years of continuous 14 | development and a substantial level of maturity. 15 | This version corresponds to the second edition of the book “Robotics, Vision & Control” published in 2017. 16 |

17 | This Toolbox has a rich collection of functions that are useful for the study and simulation 18 | of robots: arm-type robot manipulators and mobile robots. 19 | For robot manipulators functions include kinematics, trajectory generation, dynamics and control. 20 | For mobile robots functions path planning algorithms, kinodynamic planning, 21 | localization, map building and simultaneous localization and mapping (SLAM). 22 |

23 | The Toolbox makes strong use of classes to represent robots and such things as sensors and maps. 24 | It includes Simulink models to describe the evolution of robot state over time for a number of classical control 25 | strategies. The Toolbox also provides functions for manipulating and converting between datatypes such as vectors, homogeneous transformations and unit-quaternions which are necessary to represent position and orientation in 2- and 26 | 3-dimensions. 27 |

28 | The code is written in a straightforward manner which allows 29 | for easy understanding, perhaps at the expense of computational efficiency. 30 | If you feel strongly about computational efficiency then you can always 31 | rewrite the function to be more efficient. 32 |

33 | 34 |

© 1990-2017 Peter Corke

35 | 36 | -------------------------------------------------------------------------------- /doc/info/style.css: -------------------------------------------------------------------------------- 1 | body { 2 | padding-left: 2em; 3 | font-family: Arial, Helvetica; 4 | color: #474747; 5 | } 6 | 7 | p { 8 | font-size: 90%; 9 | } 10 | 11 | table { 12 | font-size: 90%; 13 | } 14 | -------------------------------------------------------------------------------- /doc/manual/Makefile: -------------------------------------------------------------------------------- 1 | spatialmath.pdf: titlepage.pdf spatialmath.tex all.tex 2 | printf '\\def\\release{%s}\n' `cat ../../../RELEASE` > release.tex 3 | printf '\\def\\reldate{%s}\n' "`date '+%B %Y'`" >> release.tex 4 | pdflatex spatialmath 5 | sh funcidx_make.sh 6 | pdflatex spatialmath 7 | 8 | titlepage.pdf: ../figs/tranimate.gif 9 | convert $<[10] $@ 10 | 11 | clean: 12 | -rm titlepage.pdf *.idx *.ind *.ilg *.aux *.log *.out *.toc 13 | 14 | -------------------------------------------------------------------------------- /doc/manual/figs/CT-02-02-eps-converted-to.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/petercorke/spatialmath-matlab/6eeff4a79f14286705560b84f1fe72e0b7e0e7f7/doc/manual/figs/CT-02-02-eps-converted-to.pdf -------------------------------------------------------------------------------- /doc/manual/figs/CT-02-02.eps: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/petercorke/spatialmath-matlab/6eeff4a79f14286705560b84f1fe72e0b7e0e7f7/doc/manual/figs/CT-02-02.eps -------------------------------------------------------------------------------- /doc/manual/figs/CT-02-03-eps-converted-to.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/petercorke/spatialmath-matlab/6eeff4a79f14286705560b84f1fe72e0b7e0e7f7/doc/manual/figs/CT-02-03-eps-converted-to.pdf -------------------------------------------------------------------------------- /doc/manual/figs/CT-02-03.eps: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/petercorke/spatialmath-matlab/6eeff4a79f14286705560b84f1fe72e0b7e0e7f7/doc/manual/figs/CT-02-03.eps -------------------------------------------------------------------------------- /doc/manual/figs/frontcover.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/petercorke/spatialmath-matlab/6eeff4a79f14286705560b84f1fe72e0b7e0e7f7/doc/manual/figs/frontcover.pdf -------------------------------------------------------------------------------- /doc/manual/figs/frontcover1.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/petercorke/spatialmath-matlab/6eeff4a79f14286705560b84f1fe72e0b7e0e7f7/doc/manual/figs/frontcover1.pdf -------------------------------------------------------------------------------- /doc/manual/figs/plot3d.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/petercorke/spatialmath-matlab/6eeff4a79f14286705560b84f1fe72e0b7e0e7f7/doc/manual/figs/plot3d.pdf -------------------------------------------------------------------------------- /doc/manual/frontcover.m: -------------------------------------------------------------------------------- 1 | function plot_arrow2(p1, p2, varargin) 2 | p1 = [0 0 0]; 3 | p2 = [0 0 2]; 4 | clf 5 | 6 | opt.color = 'b'; 7 | opt.edgecol = 'none'; 8 | opt.shaftdiam = 0.2; 9 | opt.headwidthscale = 2; 10 | opt.headlengthscale = 0.2; 11 | 12 | l = norm(p2-p1) 13 | 14 | h1 = arrow(3, opt, 'r'); h1.Matrix = troty(pi/2); 15 | h2 = arrow(3, opt, 'g'); h2.Matrix = trotx(-pi/2); 16 | h3 = arrow(3, opt, 'b'); 17 | 18 | ht = hgtransform() 19 | h1.Parent = ht; 20 | h2.Parent = ht; 21 | h3.Parent = ht; 22 | 23 | Tf = transl(7.5,7, 8) * rpy2tr(0.6, 0.8, 1.2); 24 | N = 50 25 | T = trinterp(Tf, N); 26 | hold on 27 | for i=1:N-1 28 | trplot(T(:,:,i), 'notext', 'color', [1 1 1]*0.3, 'perspective'); 29 | end 30 | ht.Matrix = T(:,:,end); 31 | view(30, -13) 32 | %shading interp 33 | lighting gouraud 34 | light 35 | axis equal 36 | axis([0 10 0 10 0 10]) 37 | grid on 38 | xlabel('X', 'FontSize', 16); 39 | ylabel('Y', 'FontSize', 16); 40 | zlabel('Z', 'FontSize', 16); 41 | view(158, 21) 42 | 43 | end 44 | 45 | function hg = arrow(l, opt, col) 46 | 47 | if nargin > 2 48 | opt.color = col; 49 | end 50 | 51 | hg = hgtransform() 52 | 53 | hl = opt.headlengthscale*l 54 | hw = opt.shaftdiam*opt.headwidthscale 55 | 56 | % shaft 57 | [x,y,z] = cylinder(opt.shaftdiam/2); 58 | z = z*(l-hl); % shaft length 59 | surf(x,y,z, ones(size(z)), 'FaceColor', opt.color, 'EdgeColor', opt.edgecol, 'parent', hg); 60 | 61 | % arrow head 62 | [x,y,z] = cylinder([hw 0]/2); 63 | z = z*hl + (l-hl); 64 | surf(x,y,z, ones(size(z)), 'FaceColor', opt.color, 'EdgeColor', opt.edgecol, 'parent', hg); 65 | 66 | patch('XData', x(1,:), 'YData', y(1,:), 'ZData', z(1,:), 'FaceColor', opt.color, 'EdgeColor', opt.edgecol, 'parent', hg) 67 | end -------------------------------------------------------------------------------- /doc/manual/right.ist: -------------------------------------------------------------------------------- 1 | delim_0 "\\dotfill " 2 | delim_1 "\\dotfill " 3 | delim_2 "\\dotfill " 4 | -------------------------------------------------------------------------------- /doc/manual/titlepage.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/petercorke/spatialmath-matlab/6eeff4a79f14286705560b84f1fe72e0b7e0e7f7/doc/manual/titlepage.pdf -------------------------------------------------------------------------------- /e2h.m: -------------------------------------------------------------------------------- 1 | %E2H Euclidean to homogeneous 2 | % 3 | % H = E2H(E) is the homogeneous version (K+1xN) of the Euclidean 4 | % points E (KxN) where each column represents one point in R^K. 5 | % 6 | % Reference:: 7 | % - Robotics, Vision & Control: Second Edition, P. Corke, Springer 2016; p604. 8 | % 9 | % See also H2E. 10 | 11 | % Copyright (C) 1993-2019 Peter I. Corke 12 | % 13 | % This file is part of The Spatial Math Toolbox for MATLAB (SMTB). 14 | % 15 | % Permission is hereby granted, free of charge, to any person obtaining a copy 16 | % of this software and associated documentation files (the "Software"), to deal 17 | % in the Software without restriction, including without limitation the rights 18 | % to use, copy, modify, merge, publish, distribute, sublicense, and/or sell copies 19 | % of the Software, and to permit persons to whom the Software is furnished to do 20 | % so, subject to the following conditions: 21 | % 22 | % The above copyright notice and this permission notice shall be included in all 23 | % copies or substantial portions of the Software. 24 | % 25 | % THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 26 | % IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS 27 | % FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR 28 | % COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER 29 | % IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN 30 | % CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. 31 | % 32 | % https://github.com/petercorke/spatial-math 33 | 34 | function h = e2h(e) 35 | h = [e; ones(1,numcols(e))]; 36 | -------------------------------------------------------------------------------- /eul2jac.m: -------------------------------------------------------------------------------- 1 | %EUL2JAC Euler angle rate Jacobian 2 | % 3 | % J = EUL2JAC(PHI, THETA, PSI) is a Jacobian matrix (3x3) that maps ZYZ Euler angle rates to 4 | % angular velocity at the operating point specified by the Euler angles PHI, THETA, PSI. 5 | % 6 | % J = EUL2JAC(EUL) as above but the Euler angles are passed as a vector EUL=[PHI, THETA, PSI]. 7 | % 8 | % Notes:: 9 | % - Used in the creation of an analytical Jacobian. 10 | % - Angles in radians, rates in radians/sec. 11 | % 12 | % Reference:: 13 | % - Robotics, Vision & Control: Second Edition, P. Corke, Springer 2016; p232-3. 14 | % 15 | % See also rpy2jac, eul2r, SerialLink.jacobe. 16 | 17 | % Copyright (C) 1993-2019 Peter I. Corke 18 | % 19 | % This file is part of The Spatial Math Toolbox for MATLAB (SMTB). 20 | % 21 | % Permission is hereby granted, free of charge, to any person obtaining a copy 22 | % of this software and associated documentation files (the "Software"), to deal 23 | % in the Software without restriction, including without limitation the rights 24 | % to use, copy, modify, merge, publish, distribute, sublicense, and/or sell copies 25 | % of the Software, and to permit persons to whom the Software is furnished to do 26 | % so, subject to the following conditions: 27 | % 28 | % The above copyright notice and this permission notice shall be included in all 29 | % copies or substantial portions of the Software. 30 | % 31 | % THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 32 | % IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS 33 | % FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR 34 | % COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER 35 | % IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN 36 | % CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. 37 | % 38 | % https://github.com/petercorke/spatial-math 39 | 40 | function J = eul2jac(phi, theta, psi) 41 | 42 | if length(phi) == 3 43 | % eul2jac([phi theta psi]) 44 | theta = phi(2); 45 | psi = phi(3); 46 | phi = phi(1); 47 | elseif nargin ~= 3 48 | error('SMTB:eul2jac:badarg', 'bad arguments'); 49 | end 50 | J = [ 0, -sin(phi), cos(phi)*sin(theta) 51 | 0, cos(phi), sin(phi)*sin(theta) 52 | 1, 0, cos(theta) ]; 53 | 54 | -------------------------------------------------------------------------------- /eul2r.m: -------------------------------------------------------------------------------- 1 | %EUL2R Convert Euler angles to rotation matrix 2 | % 3 | % R = EUL2R(PHI, THETA, PSI, OPTIONS) is an SO(3) orthonornal rotation 4 | % matrix (3x3) equivalent to the specified Euler angles. These correspond 5 | % to rotations about the Z, Y, Z axes respectively. If PHI, THETA, PSI are 6 | % column vectors (Nx1) then they are assumed to represent a trajectory and 7 | % R is a three-dimensional matrix (3x3xN), where the last index corresponds 8 | % to rows of PHI, THETA, PSI. 9 | % 10 | % R = EUL2R(EUL, OPTIONS) as above but the Euler angles are taken from the 11 | % vector (1x3) EUL = [PHI THETA PSI]. If EUL is a matrix (Nx3) then R is a 12 | % three-dimensional matrix (3x3xN), where the last index corresponds to 13 | % rows of RPY which are assumed to be [PHI,THETA,PSI]. 14 | % 15 | % Options:: 16 | % 'deg' Angles given in degrees (radians default) 17 | % 18 | % Note:: 19 | % - The vectors PHI, THETA, PSI must be of the same length. 20 | % 21 | % See also EUL2TR, RPY2TR, TR2EUL, SO3.eul. 22 | 23 | % Copyright (C) 1993-2019 Peter I. Corke 24 | % 25 | % This file is part of The Spatial Math Toolbox for MATLAB (SMTB). 26 | % 27 | % Permission is hereby granted, free of charge, to any person obtaining a copy 28 | % of this software and associated documentation files (the "Software"), to deal 29 | % in the Software without restriction, including without limitation the rights 30 | % to use, copy, modify, merge, publish, distribute, sublicense, and/or sell copies 31 | % of the Software, and to permit persons to whom the Software is furnished to do 32 | % so, subject to the following conditions: 33 | % 34 | % The above copyright notice and this permission notice shall be included in all 35 | % copies or substantial portions of the Software. 36 | % 37 | % THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 38 | % IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS 39 | % FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR 40 | % COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER 41 | % IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN 42 | % CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. 43 | % 44 | % https://github.com/petercorke/spatial-math 45 | 46 | function R = eul2r(phi, varargin) 47 | opt.deg = false; 48 | [opt,args] = tb_optparse(opt, varargin); 49 | 50 | % unpack the arguments 51 | if numcols(phi) == 3 52 | theta = phi(:,2); 53 | psi = phi(:,3); 54 | phi = phi(:,1); 55 | elseif nargin >= 3 56 | theta = args{1}; 57 | psi = args{2}; 58 | else 59 | error('SMTB:eul2r:badarg', 'expecting 3 inputs, 3-vector or 3-column matrix') 60 | end 61 | 62 | % optionally convert from degrees 63 | if opt.deg 64 | d2r = pi/180.0; 65 | phi = phi * d2r; 66 | theta = theta * d2r; 67 | psi = psi * d2r; 68 | end 69 | 70 | if numrows(phi) == 1 71 | R = rotz(phi) * roty(theta) * rotz(psi); 72 | else 73 | R = zeros(3,3,numrows(phi)); 74 | for i=1:numrows(phi) 75 | R(:,:,i) = rotz(phi(i)) * roty(theta(i)) * rotz(psi(i)); 76 | end 77 | end 78 | -------------------------------------------------------------------------------- /eul2tr.m: -------------------------------------------------------------------------------- 1 | %EUL2TR Convert Euler angles to homogeneous transform 2 | % 3 | % T = EUL2TR(PHI, THETA, PSI, OPTIONS) is an SE(3) homogeneous 4 | % transformation matrix (4x4) with zero translation and rotation equivalent 5 | % to the specified Euler angles. These correspond to rotations about the Z, 6 | % Y, Z axes respectively. If PHI, THETA, PSI are column vectors (Nx1) then 7 | % they are assumed to represent a trajectory and R is a three-dimensional 8 | % matrix (4x4xN), where the last index corresponds to rows of PHI, THETA, 9 | % PSI. 10 | % 11 | % R = EUL2R(EUL, OPTIONS) as above but the Euler angles are taken from the 12 | % vector (1x3) EUL = [PHI THETA PSI]. If EUL is a matrix (Nx3) then R is a 13 | % three-dimensional matrix (4x4xN), where the last index corresponds to 14 | % rows of RPY which are assumed to be [PHI,THETA,PSI]. 15 | % 16 | % Options:: 17 | % 'deg' Angles given in degrees (radians default) 18 | % 19 | % Note:: 20 | % - The vectors PHI, THETA, PSI must be of the same length. 21 | % - The translational part is zero. 22 | % 23 | % See also EUL2R, RPY2TR, TR2EUL, SE3.eul. 24 | 25 | % Copyright (C) 1993-2019 Peter I. Corke 26 | % 27 | % This file is part of The Spatial Math Toolbox for MATLAB (SMTB). 28 | % 29 | % Permission is hereby granted, free of charge, to any person obtaining a copy 30 | % of this software and associated documentation files (the "Software"), to deal 31 | % in the Software without restriction, including without limitation the rights 32 | % to use, copy, modify, merge, publish, distribute, sublicense, and/or sell copies 33 | % of the Software, and to permit persons to whom the Software is furnished to do 34 | % so, subject to the following conditions: 35 | % 36 | % The above copyright notice and this permission notice shall be included in all 37 | % copies or substantial portions of the Software. 38 | % 39 | % THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 40 | % IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS 41 | % FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR 42 | % COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER 43 | % IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN 44 | % CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. 45 | % 46 | % https://github.com/petercorke/spatial-math 47 | 48 | function T = eul2tr(phi, varargin) 49 | 50 | R = eul2r(phi, varargin{:}); 51 | T = r2t(R); 52 | -------------------------------------------------------------------------------- /h2e.m: -------------------------------------------------------------------------------- 1 | %H2E Homogeneous to Euclidean 2 | % 3 | % E = H2E(H) is the Euclidean version (K-1xN) of the homogeneous 4 | % points H (KxN) where each column represents one point in P^K. 5 | % 6 | % Reference:: 7 | % - Robotics, Vision & Control: Second Edition, P. Corke, Springer 2016; p604. 8 | % 9 | % See also E2H. 10 | 11 | % Copyright (C) 1993-2019 Peter I. Corke 12 | % 13 | % This file is part of The Spatial Math Toolbox for MATLAB (SMTB). 14 | % 15 | % Permission is hereby granted, free of charge, to any person obtaining a copy 16 | % of this software and associated documentation files (the "Software"), to deal 17 | % in the Software without restriction, including without limitation the rights 18 | % to use, copy, modify, merge, publish, distribute, sublicense, and/or sell copies 19 | % of the Software, and to permit persons to whom the Software is furnished to do 20 | % so, subject to the following conditions: 21 | % 22 | % The above copyright notice and this permission notice shall be included in all 23 | % copies or substantial portions of the Software. 24 | % 25 | % THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 26 | % IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS 27 | % FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR 28 | % COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER 29 | % IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN 30 | % CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. 31 | % 32 | % https://github.com/petercorke/spatial-math 33 | 34 | function e = h2e(h) 35 | 36 | if isvector(h) 37 | h = h(:); 38 | end 39 | e = h(1:end-1,:) ./ repmat(h(end,:), numrows(h)-1, 1); 40 | 41 | -------------------------------------------------------------------------------- /helpinfo.xml: -------------------------------------------------------------------------------- 1 | 4 | 5 | 6 | 14 7 | SpecSim (Example) 8 | toolbox 9 | 10 | specsim_example_helpfiles 11 | 12 | 13 | -------------------------------------------------------------------------------- /homline.m: -------------------------------------------------------------------------------- 1 | %HOMLINE Homogeneous line from two points 2 | % 3 | % L = HOMLINE(X1, Y1, X2, Y2) is a vector (3x1) which describes a line in 4 | % homogeneous form that contains the two Euclidean points (X1,Y1) and (X2,Y2). 5 | % 6 | % Homogeneous points X (3x1) on the line must satisfy L'*X = 0. 7 | % 8 | % See also PLOT_HOMLINE. 9 | 10 | % Copyright (C) 1993-2019 Peter I. Corke 11 | % 12 | % This file is part of The Spatial Math Toolbox for MATLAB (SMTB). 13 | % 14 | % Permission is hereby granted, free of charge, to any person obtaining a copy 15 | % of this software and associated documentation files (the "Software"), to deal 16 | % in the Software without restriction, including without limitation the rights 17 | % to use, copy, modify, merge, publish, distribute, sublicense, and/or sell copies 18 | % of the Software, and to permit persons to whom the Software is furnished to do 19 | % so, subject to the following conditions: 20 | % 21 | % The above copyright notice and this permission notice shall be included in all 22 | % copies or substantial portions of the Software. 23 | % 24 | % THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 25 | % IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS 26 | % FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR 27 | % COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER 28 | % IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN 29 | % CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. 30 | % 31 | % https://github.com/petercorke/spatial-math 32 | 33 | % TODO, probably should be part of a HomLine class. 34 | 35 | function l = homline(x1, y1, x2, y2) 36 | 37 | l = cross([x1 y1 1], [x2 y2 1]); 38 | 39 | % normalize so that the result of x*l' is the pixel distance 40 | % from the line 41 | l = l / norm(l(1:2)); 42 | -------------------------------------------------------------------------------- /homtrans.m: -------------------------------------------------------------------------------- 1 | %HOMTRANS Apply a homogeneous transformation 2 | % 3 | % P2 = HOMTRANS(T, P) applies the homogeneous transformation T to the points 4 | % stored columnwise in P. 5 | % 6 | % - If T is in SE(2) (3x3) and 7 | % - P is 2xN (2D points) they are considered Euclidean (R^2) 8 | % - P is 3xN (2D points) they are considered projective (P^2) 9 | % - If T is in SE(3) (4x4) and 10 | % - P is 3xN (3D points) they are considered Euclidean (R^3) 11 | % - P is 4xN (3D points) they are considered projective (P^3) 12 | % 13 | % P2 and P have the same number of rows, ie. if Euclidean points are given 14 | % then Euclidean points are returned, if projective points are given then 15 | % projective points are returned. 16 | % 17 | % TP = HOMTRANS(T, T1) applies homogeneous transformation T to the 18 | % homogeneous transformation T1, that is TP=T*T1. If T1 is a 3-dimensional 19 | % transformation then T is applied to each plane as defined by the first two 20 | % dimensions, ie. if T is NxN and T1 is NxNxM then the result is NxNxM. 21 | % 22 | % Notes:: 23 | % - If T is a homogeneous transformation defining the pose of {B} with respect to {A}, 24 | % then the points are defined with respect to frame {B} and are transformed to be 25 | % with respect to frame {A}. 26 | % 27 | % See also E2H, H2E, RTBPose.mtimes. 28 | 29 | % Copyright (C) 1993-2019 Peter I. Corke 30 | % 31 | % This file is part of The Spatial Math Toolbox for MATLAB (SMTB). 32 | % 33 | % Permission is hereby granted, free of charge, to any person obtaining a copy 34 | % of this software and associated documentation files (the "Software"), to deal 35 | % in the Software without restriction, including without limitation the rights 36 | % to use, copy, modify, merge, publish, distribute, sublicense, and/or sell copies 37 | % of the Software, and to permit persons to whom the Software is furnished to do 38 | % so, subject to the following conditions: 39 | % 40 | % The above copyright notice and this permission notice shall be included in all 41 | % copies or substantial portions of the Software. 42 | % 43 | % THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 44 | % IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS 45 | % FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR 46 | % COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER 47 | % IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN 48 | % CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. 49 | % 50 | % https://github.com/petercorke/spatial-math 51 | function pt = homtrans(T, p) 52 | 53 | if numrows(p) == numrows(T) 54 | if ndims(p) == 3 55 | % homtrans(T1, T2) 56 | pt = []; 57 | for i=1:size(p,3) 58 | pt = cat(3, pt, T*p(:,:,i)); 59 | end 60 | else 61 | % points are in projective coordinates 62 | pt = T * p; 63 | end 64 | elseif (numrows(T)-numrows(p)) == 1 65 | % points are in Euclidean coordinates, promote to homogeneous 66 | pt = h2e( T * e2h(p) ); 67 | else 68 | error('SMTB:homtrans:badarg', 'matrices and point data do not conform') 69 | end 70 | -------------------------------------------------------------------------------- /ishomog.m: -------------------------------------------------------------------------------- 1 | %ISHOMOG Test if SE(3) homogeneous transformation matrix 2 | % 3 | % ISHOMOG(T) is true (1) if the argument T is of dimension 4x4 or 4x4xN, else 4 | % false (0). 5 | % 6 | % ISHOMOG(T, 'check') as above, but also checks the validity of the rotation 7 | % sub-matrix. 8 | % 9 | % Notes:: 10 | % - A valid rotation sub-matrix has determinant of 1. 11 | % - The first form is a fast, but incomplete, test for a transform is SE(3). 12 | % 13 | % See also ISROT, ISHOMOG2, ISVEC. 14 | 15 | % Copyright (C) 1993-2019 Peter I. Corke 16 | % 17 | % This file is part of The Spatial Math Toolbox for MATLAB (SMTB). 18 | % 19 | % Permission is hereby granted, free of charge, to any person obtaining a copy 20 | % of this software and associated documentation files (the "Software"), to deal 21 | % in the Software without restriction, including without limitation the rights 22 | % to use, copy, modify, merge, publish, distribute, sublicense, and/or sell copies 23 | % of the Software, and to permit persons to whom the Software is furnished to do 24 | % so, subject to the following conditions: 25 | % 26 | % The above copyright notice and this permission notice shall be included in all 27 | % copies or substantial portions of the Software. 28 | % 29 | % THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 30 | % IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS 31 | % FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR 32 | % COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER 33 | % IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN 34 | % CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. 35 | % 36 | % https://github.com/petercorke/spatial-math 37 | 38 | function h = ishomog(T, rtest) 39 | h = false; 40 | d = size(T); 41 | 42 | if ndims(T) >= 2 43 | if ~(all(d(1:2) == [4 4])) 44 | return %false 45 | end 46 | 47 | if nargin > 1 48 | for i = 1:size(T,3) 49 | % check rotational part 50 | R = T(1:3,1:3,i); 51 | e = R'*R - eye(3,3); 52 | if norm(e) > 10*eps 53 | return %false 54 | end 55 | e = abs(det(R) - 1); 56 | if norm(e) > 10*eps 57 | return %false 58 | end 59 | % check bottom row 60 | if ~all(T(4,:,i) == [0 0 0 1]) 61 | return %false 62 | end 63 | end 64 | end 65 | end 66 | 67 | h = true; 68 | end -------------------------------------------------------------------------------- /ishomog2.m: -------------------------------------------------------------------------------- 1 | %ISHOMOG2 Test if SE(2) homogeneous transformation matrix 2 | % 3 | % ISHOMOG2(T) is true (1) if the argument T is of dimension 3x3 or 3x3xN, else 4 | % false (0). 5 | % 6 | % ISHOMOG2(T, 'check') as above, but also checks the validity of the rotation 7 | % sub-matrix. 8 | % 9 | % Notes:: 10 | % - A valid rotation sub-matrix has determinant of 1. 11 | % - The first form is a fast, but incomplete, test for a transform in SE(3). 12 | % 13 | % See also ISHOMOG, ISROT2, ISVEC. 14 | 15 | % Copyright (C) 1993-2019 Peter I. Corke 16 | % 17 | % This file is part of The Spatial Math Toolbox for MATLAB (SMTB). 18 | % 19 | % Permission is hereby granted, free of charge, to any person obtaining a copy 20 | % of this software and associated documentation files (the "Software"), to deal 21 | % in the Software without restriction, including without limitation the rights 22 | % to use, copy, modify, merge, publish, distribute, sublicense, and/or sell copies 23 | % of the Software, and to permit persons to whom the Software is furnished to do 24 | % so, subject to the following conditions: 25 | % 26 | % The above copyright notice and this permission notice shall be included in all 27 | % copies or substantial portions of the Software. 28 | % 29 | % THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 30 | % IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS 31 | % FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR 32 | % COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER 33 | % IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN 34 | % CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. 35 | % 36 | % https://github.com/petercorke/spatial-math 37 | 38 | function h = ishomog2(T, rtest) 39 | h = false; 40 | d = size(T); 41 | 42 | if ndims(T) >= 2 43 | if ~(all(d(1:2) == [3 3])) 44 | return %false 45 | end 46 | 47 | if nargin > 1 48 | for i = 1:size(T,3) 49 | % check rotational part 50 | R = T(1:2,1:2,i); 51 | e = R'*R - eye(2,2); 52 | if norm(e) > 10*eps 53 | return %false 54 | end 55 | e = abs(det(R) - 1); 56 | if norm(e) > 10*eps 57 | return %false 58 | end 59 | % check bottom row 60 | if ~all(T(3,:,i) == [0 0 1]) 61 | return %false 62 | end 63 | end 64 | end 65 | end 66 | 67 | h = true; 68 | end -------------------------------------------------------------------------------- /isrot.m: -------------------------------------------------------------------------------- 1 | %ISROT Test if SO(3) rotation matrix 2 | % 3 | % ISROT(R) is true (1) if the argument is of dimension 3x3 or 3x3xN, else false (0). 4 | % 5 | % ISROT(R, 'check') as above, but also checks the validity of the rotation 6 | % matrix. 7 | % 8 | % Notes:: 9 | % - A valid rotation matrix has determinant of 1. 10 | % 11 | % See also ISHOMOG, ISROT2, ISVEC. 12 | 13 | % Copyright (C) 1993-2019 Peter I. Corke 14 | % 15 | % This file is part of The Spatial Math Toolbox for MATLAB (SMTB). 16 | % 17 | % Permission is hereby granted, free of charge, to any person obtaining a copy 18 | % of this software and associated documentation files (the "Software"), to deal 19 | % in the Software without restriction, including without limitation the rights 20 | % to use, copy, modify, merge, publish, distribute, sublicense, and/or sell copies 21 | % of the Software, and to permit persons to whom the Software is furnished to do 22 | % so, subject to the following conditions: 23 | % 24 | % The above copyright notice and this permission notice shall be included in all 25 | % copies or substantial portions of the Software. 26 | % 27 | % THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 28 | % IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS 29 | % FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR 30 | % COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER 31 | % IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN 32 | % CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. 33 | % 34 | % https://github.com/petercorke/spatial-math 35 | 36 | function h = isrot(R, rtest) 37 | 38 | h = false; 39 | d = size(R); 40 | 41 | if ndims(R) >= 2 42 | if ~(all(d(1:2) == [3 3])) 43 | return %false 44 | end 45 | 46 | if nargin > 1 47 | for i = 1:size(R,3) 48 | RR = R(:,:,i); 49 | e = RR'*RR - eye(3,3); 50 | if norm(e) > 10*eps 51 | return %false 52 | end 53 | e = abs(det(RR) - 1); 54 | if norm(e) > 10*eps 55 | return %false 56 | end 57 | end 58 | end 59 | end 60 | 61 | h = true; 62 | end -------------------------------------------------------------------------------- /isrot2.m: -------------------------------------------------------------------------------- 1 | %ISROT2 Test if SO(2) rotation matrix 2 | % 3 | % ISROT2(R) is true (1) if the argument is of dimension 2x2 or 2x2xN, else false (0). 4 | % 5 | % ISROT2(R, 'check') as above, but also checks the validity of the rotation 6 | % matrix. 7 | % 8 | % Notes:: 9 | % - A valid rotation matrix has determinant of 1. 10 | % 11 | % See also ISROT, ISHOMOG2, ISVEC. 12 | 13 | % Copyright (C) 1993-2019 Peter I. Corke 14 | % 15 | % This file is part of The Spatial Math Toolbox for MATLAB (SMTB). 16 | % 17 | % Permission is hereby granted, free of charge, to any person obtaining a copy 18 | % of this software and associated documentation files (the "Software"), to deal 19 | % in the Software without restriction, including without limitation the rights 20 | % to use, copy, modify, merge, publish, distribute, sublicense, and/or sell copies 21 | % of the Software, and to permit persons to whom the Software is furnished to do 22 | % so, subject to the following conditions: 23 | % 24 | % The above copyright notice and this permission notice shall be included in all 25 | % copies or substantial portions of the Software. 26 | % 27 | % THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 28 | % IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS 29 | % FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR 30 | % COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER 31 | % IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN 32 | % CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. 33 | % 34 | % https://github.com/petercorke/spatial-math 35 | 36 | function h = isrot2(R, rtest) 37 | 38 | h = false; 39 | d = size(R); 40 | 41 | if ndims(R) >= 2 42 | if ~(all(d(1:2) == [2 2])) 43 | return %false 44 | end 45 | 46 | if nargin > 1 47 | for i = 1:size(R,3) 48 | RR = R(:,:,i); 49 | e = RR'*RR - eye(2,2); 50 | if norm(e) > 10*eps 51 | return %false 52 | end 53 | e = abs(det(RR) - 1); 54 | if norm(e) > 10*eps 55 | return %false 56 | end 57 | end 58 | end 59 | end 60 | 61 | h = true; 62 | end -------------------------------------------------------------------------------- /isunit.m: -------------------------------------------------------------------------------- 1 | %ISUNIT Test if vector has unit length 2 | % 3 | % ISUNIT(V) is true if the vector has unit length. 4 | % 5 | % Notes:: 6 | % - A tolerance of 100eps is used. 7 | 8 | % Copyright (C) 1993-2019 Peter I. Corke 9 | % 10 | % This file is part of The Spatial Math Toolbox for MATLAB (SMTB). 11 | % 12 | % Permission is hereby granted, free of charge, to any person obtaining a copy 13 | % of this software and associated documentation files (the "Software"), to deal 14 | % in the Software without restriction, including without limitation the rights 15 | % to use, copy, modify, merge, publish, distribute, sublicense, and/or sell copies 16 | % of the Software, and to permit persons to whom the Software is furnished to do 17 | % so, subject to the following conditions: 18 | % 19 | % The above copyright notice and this permission notice shall be included in all 20 | % copies or substantial portions of the Software. 21 | % 22 | % THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 23 | % IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS 24 | % FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR 25 | % COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER 26 | % IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN 27 | % CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. 28 | % 29 | % https://github.com/petercorke/spatial-math 30 | function s = isunit(v) 31 | s = abs(norm(v)-1) < 100*eps; 32 | end 33 | -------------------------------------------------------------------------------- /isvec.m: -------------------------------------------------------------------------------- 1 | %ISVEC Test if vector 2 | % 3 | % ISVEC(V) is true (1) if the argument V is a 3-vector, either a 4 | % row- or column-vector. Otherwise false (0). 5 | % 6 | % ISVEC(V, L) is true (1) if the argument V is a vector of length L, 7 | % either a row- or column-vector. Otherwise false (0). 8 | % 9 | % Notes:: 10 | % - Differs from MATLAB builtin function ISVECTOR which returns true 11 | % for the case of a scalar, ISVEC does not. 12 | % - Gives same result for row- or column-vector, ie. 3x1 or 1x3 gives true. 13 | % - Works for a symbolic math symfun. 14 | % 15 | % See also ISHOMOG, ISROT. 16 | 17 | % Copyright (C) 1993-2019 Peter I. Corke 18 | % 19 | % This file is part of The Spatial Math Toolbox for MATLAB (SMTB). 20 | % 21 | % Permission is hereby granted, free of charge, to any person obtaining a copy 22 | % of this software and associated documentation files (the "Software"), to deal 23 | % in the Software without restriction, including without limitation the rights 24 | % to use, copy, modify, merge, publish, distribute, sublicense, and/or sell copies 25 | % of the Software, and to permit persons to whom the Software is furnished to do 26 | % so, subject to the following conditions: 27 | % 28 | % The above copyright notice and this permission notice shall be included in all 29 | % copies or substantial portions of the Software. 30 | % 31 | % THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 32 | % IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS 33 | % FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR 34 | % COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER 35 | % IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN 36 | % CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. 37 | % 38 | % https://github.com/petercorke/spatial-math 39 | 40 | function h = isvec(v, l) 41 | if nargin == 1 42 | l = 3; 43 | end 44 | if isa(v, 'symfun') 45 | h = logical( length(formula(v)) == l); 46 | else 47 | d = size(v); 48 | h = logical( length(d) == 2 && min(d) == 1 && numel(v) == l ); 49 | end 50 | end 51 | 52 | -------------------------------------------------------------------------------- /lift23.m: -------------------------------------------------------------------------------- 1 | %SE3 Lift SE(2) transform to SE(3) 2 | % 3 | % T3 = SE3(T2) returns a homogeneous transform (4x4) that represents 4 | % the same X,Y translation and Z rotation as does T2 (3x3). 5 | % 6 | % See also SE2, SE2.SE3, TRANSL, ROTX. 7 | 8 | % Copyright (C) 1993-2019 Peter I. Corke 9 | % 10 | % This file is part of The Spatial Math Toolbox for MATLAB (SMTB). 11 | % 12 | % Permission is hereby granted, free of charge, to any person obtaining a copy 13 | % of this software and associated documentation files (the "Software"), to deal 14 | % in the Software without restriction, including without limitation the rights 15 | % to use, copy, modify, merge, publish, distribute, sublicense, and/or sell copies 16 | % of the Software, and to permit persons to whom the Software is furnished to do 17 | % so, subject to the following conditions: 18 | % 19 | % The above copyright notice and this permission notice shall be included in all 20 | % copies or substantial portions of the Software. 21 | % 22 | % THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 23 | % IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS 24 | % FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR 25 | % COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER 26 | % IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN 27 | % CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. 28 | % 29 | % https://github.com/petercorke/spatial-math 30 | function T = se3(x) 31 | 32 | if all(size(x) == [3 3]) 33 | T = [x(1:2,1:2) [0 0]' x(1:2,3); 0 0 1 0; 0 0 0 1]; 34 | end 35 | -------------------------------------------------------------------------------- /numcols.m: -------------------------------------------------------------------------------- 1 | %NUMCOLS Number of columns in matrix 2 | % 3 | % NC = NUMCOLS(M) is the number of columns in the matrix M. 4 | % 5 | % Notes:: 6 | % - Readable shorthand for SIZE(M,2); 7 | % 8 | % See also NUMROWS, SIZE. 9 | 10 | % Copyright (C) 1993-2019 Peter I. Corke 11 | % 12 | % This file is part of The Spatial Math Toolbox for MATLAB (SMTB). 13 | % 14 | % Permission is hereby granted, free of charge, to any person obtaining a copy 15 | % of this software and associated documentation files (the "Software"), to deal 16 | % in the Software without restriction, including without limitation the rights 17 | % to use, copy, modify, merge, publish, distribute, sublicense, and/or sell copies 18 | % of the Software, and to permit persons to whom the Software is furnished to do 19 | % so, subject to the following conditions: 20 | % 21 | % The above copyright notice and this permission notice shall be included in all 22 | % copies or substantial portions of the Software. 23 | % 24 | % THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 25 | % IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS 26 | % FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR 27 | % COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER 28 | % IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN 29 | % CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. 30 | % 31 | % https://github.com/petercorke/spatial-math 32 | 33 | function c = numcols(m) 34 | c = size(m,2); 35 | -------------------------------------------------------------------------------- /numrows.m: -------------------------------------------------------------------------------- 1 | %NUMROWS Number of rows in matrix 2 | % 3 | % NR = NUMROWS(M) is the number of rows in the matrix M. 4 | % 5 | % Notes:: 6 | % - Readable shorthand for SIZE(M,1); 7 | % 8 | % See also NUMCOLS, SIZE. 9 | 10 | % Copyright (C) 1993-2019 Peter I. Corke 11 | % 12 | % This file is part of The Spatial Math Toolbox for MATLAB (SMTB). 13 | % 14 | % Permission is hereby granted, free of charge, to any person obtaining a copy 15 | % of this software and associated documentation files (the "Software"), to deal 16 | % in the Software without restriction, including without limitation the rights 17 | % to use, copy, modify, merge, publish, distribute, sublicense, and/or sell copies 18 | % of the Software, and to permit persons to whom the Software is furnished to do 19 | % so, subject to the following conditions: 20 | % 21 | % The above copyright notice and this permission notice shall be included in all 22 | % copies or substantial portions of the Software. 23 | % 24 | % THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 25 | % IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS 26 | % FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR 27 | % COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER 28 | % IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN 29 | % CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. 30 | % 31 | % https://github.com/petercorke/spatial-math 32 | 33 | function r = numrows(m) 34 | 35 | r = size(m, 1); 36 | -------------------------------------------------------------------------------- /oa2r.m: -------------------------------------------------------------------------------- 1 | %OA2R Convert orientation and approach vectors to rotation matrix 2 | % 3 | % R = OA2R(O, A) is an SO(3) rotation matrix (3x3) for the specified 4 | % orientation and approach vectors (3x1) formed from 3 vectors such that R 5 | % = [N O A] and N = O x A. 6 | % 7 | % Notes:: 8 | % - The matrix is guaranteed to be orthonormal so long as O and A 9 | % are not parallel. 10 | % - The vectors O and A are parallel to the Y- and Z-axes of the coordinate 11 | % frame respectively. 12 | % 13 | % References:: 14 | % - Robot manipulators: mathematics, programming and control 15 | % Richard Paul, MIT Press, 1981. 16 | % 17 | % See also RPY2R, EUL2R, OA2TR, SO3.oa. 18 | 19 | % Copyright (C) 1993-2019 Peter I. Corke 20 | % 21 | % This file is part of The Spatial Math Toolbox for MATLAB (SMTB). 22 | % 23 | % Permission is hereby granted, free of charge, to any person obtaining a copy 24 | % of this software and associated documentation files (the "Software"), to deal 25 | % in the Software without restriction, including without limitation the rights 26 | % to use, copy, modify, merge, publish, distribute, sublicense, and/or sell copies 27 | % of the Software, and to permit persons to whom the Software is furnished to do 28 | % so, subject to the following conditions: 29 | % 30 | % The above copyright notice and this permission notice shall be included in all 31 | % copies or substantial portions of the Software. 32 | % 33 | % THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 34 | % IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS 35 | % FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR 36 | % COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER 37 | % IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN 38 | % CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. 39 | % 40 | % https://github.com/petercorke/spatial-math 41 | 42 | function R = oa2r(o, a) 43 | 44 | assert( nargin >= 2 && isvec(o) && isvec(a), 'SMTB:oa2r:badarg', 'bad arguments'); 45 | 46 | o = o(:); a = a(:); 47 | n = cross(o, a); 48 | o = cross(a, n); 49 | R = [unit(n(:)) unit(o(:)) unit(a(:))]; 50 | -------------------------------------------------------------------------------- /oa2tr.m: -------------------------------------------------------------------------------- 1 | %OA2TR Convert orientation and approach vectors to homogeneous transformation 2 | % 3 | % T = OA2TR(O, A) is an SE(3) homogeneous tranformation (4x4) for the 4 | % specified orientation and approach vectors (3x1) formed from 3 vectors 5 | % such that R = [N O A] and N = O x A. 6 | % 7 | % Notes:: 8 | % - The rotation submatrix is guaranteed to be orthonormal so long as O and A 9 | % are not parallel. 10 | % - The vectors O and A are parallel to the Y- and Z-axes of the coordinate 11 | % frame respectively. 12 | % - The translational part is zero. 13 | % 14 | % References:: 15 | % - Robot manipulators: mathematics, programming and control 16 | % Richard Paul, MIT Press, 1981. 17 | % 18 | % See also RPY2TR, EUL2TR, OA2R, SE3.oa. 19 | 20 | % Copyright (C) 1993-2019 Peter I. Corke 21 | % 22 | % This file is part of The Spatial Math Toolbox for MATLAB (SMTB). 23 | % 24 | % Permission is hereby granted, free of charge, to any person obtaining a copy 25 | % of this software and associated documentation files (the "Software"), to deal 26 | % in the Software without restriction, including without limitation the rights 27 | % to use, copy, modify, merge, publish, distribute, sublicense, and/or sell copies 28 | % of the Software, and to permit persons to whom the Software is furnished to do 29 | % so, subject to the following conditions: 30 | % 31 | % The above copyright notice and this permission notice shall be included in all 32 | % copies or substantial portions of the Software. 33 | % 34 | % THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 35 | % IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS 36 | % FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR 37 | % COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER 38 | % IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN 39 | % CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. 40 | % 41 | % https://github.com/petercorke/spatial-math 42 | 43 | function r = oa2tr(o, a) 44 | assert( nargin >= 2 && isvec(o) && isvec(a), 'SMTB:oa2tr:badarg', 'bad arguments'); 45 | 46 | n = cross(o, a); 47 | o = cross(a, n); 48 | r = [unit(n(:)) unit(o(:)) unit(a(:)) zeros(3,1); 0 0 0 1]; 49 | -------------------------------------------------------------------------------- /plot2.m: -------------------------------------------------------------------------------- 1 | %PLOT2 Plot trajectories 2 | % 3 | % Convenience function for plotting 2D or 3D trajectory data stored in a 4 | % matrix with one row per time step. 5 | % 6 | % PLOT2(P) plots a line with coordinates taken from successive rows of P(Nx2 or Nx3). 7 | % 8 | % If P has three dimensions, ie. Nx2xM or Nx3xM then the M trajectories are 9 | % overlaid in the one plot. 10 | % 11 | % PLOT2(P, LS) as above but the line style arguments LS are passed to plot. 12 | % 13 | % See also MPLOT, PLOT. 14 | 15 | % Copyright (C) 1993-2019 Peter I. Corke 16 | % 17 | % This file is part of The Spatial Math Toolbox for MATLAB (SMTB). 18 | % 19 | % Permission is hereby granted, free of charge, to any person obtaining a copy 20 | % of this software and associated documentation files (the "Software"), to deal 21 | % in the Software without restriction, including without limitation the rights 22 | % to use, copy, modify, merge, publish, distribute, sublicense, and/or sell copies 23 | % of the Software, and to permit persons to whom the Software is furnished to do 24 | % so, subject to the following conditions: 25 | % 26 | % The above copyright notice and this permission notice shall be included in all 27 | % copies or substantial portions of the Software. 28 | % 29 | % THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 30 | % IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS 31 | % FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR 32 | % COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER 33 | % IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN 34 | % CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. 35 | % 36 | % https://github.com/petercorke/spatial-math 37 | function h = plot2(p1, varargin) 38 | 39 | if ndims(p1) == 2 40 | switch numcols(p1) 41 | case 3 42 | hh = plot3(p1(:,1), p1(:,2), p1(:,3), varargin{:}); 43 | case 2 44 | hh = plot(p1(:,1), p1(:,2), varargin{:}); 45 | otherwise 46 | error('SMTB:plot2:badarg', 'Data must have 2 or 3 columns'); 47 | end 48 | if nargout == 1 49 | h = hh; 50 | end 51 | else 52 | clf 53 | hold on 54 | for i=1:size(p1,2) 55 | plot2( squeeze(p1(:,:,i)) ); 56 | end 57 | end 58 | -------------------------------------------------------------------------------- /plot_arrow.m: -------------------------------------------------------------------------------- 1 | %PLOT_ARROW Draw an arrow in 2D or 3D 2 | % 3 | % PLOT_ARROW(P1, P2, OPTIONS) draws an arrow from P1 to P2 (2x1 or 3x1). For 3D 4 | % case the arrow head is a cone. 5 | % 6 | % PLOT_ARROW(P, OPTIONS) as above where the columns of P (2x2 or 3x2) define the 7 | % start and end points, ie. P=[P1 P2]. 8 | % 9 | % H = PLOT_ARROW(...) as above but returns the graphics handle of the arrow. 10 | % 11 | % Options:: 12 | % - All options are passed through to arrow3. 13 | % - MATLAB LineSpec such as 'r' or 'b--' 14 | % 15 | % Example:: 16 | % plot_arrow([0 0 0]', [1 2 3]', 'r') % a red arrow 17 | % plot_arrow([0 0 0], [1 2 3], 'r--3', 4) % dashed red arrow big head 18 | % 19 | % Notes:: 20 | % - Requires https://www.mathworks.com/matlabcentral/fileexchange/14056-arrow3 21 | % - ARROW3 attempts to preserve the appearance of existing axes. In 22 | % particular, ARROW3 will not change XYZLim, View, or CameraViewAngle. 23 | % 24 | % See also ARROW3. 25 | 26 | % Copyright (C) 1993-2019 Peter I. Corke 27 | % 28 | % This file is part of The Spatial Math Toolbox for MATLAB (SMTB). 29 | % 30 | % Permission is hereby granted, free of charge, to any person obtaining a copy 31 | % of this software and associated documentation files (the "Software"), to deal 32 | % in the Software without restriction, including without limitation the rights 33 | % to use, copy, modify, merge, publish, distribute, sublicense, and/or sell copies 34 | % of the Software, and to permit persons to whom the Software is furnished to do 35 | % so, subject to the following conditions: 36 | % 37 | % The above copyright notice and this permission notice shall be included in all 38 | % copies or substantial portions of the Software. 39 | % 40 | % THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 41 | % IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS 42 | % FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR 43 | % COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER 44 | % IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN 45 | % CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. 46 | % 47 | % https://github.com/petercorke/spatial-math 48 | function h = plot_arrow(p1, varargin) 49 | if isvec(p1, 2) || isvec(p1, 3) 50 | p1 = p1(:)'; % force row vector 51 | else 52 | error('SMTB:plot_arrow', 'P1 must have 2 or 3 elements') 53 | end 54 | 55 | if nargin > 1 56 | p2 = varargin{1}; 57 | if isnumeric(p2) 58 | if isvec(p2, 2) || isvec(p2, 3) 59 | p2 = p2(:)'; % force row vector 60 | varargin = varargin(2:end); 61 | else 62 | error('SMTB:plot_arrow', 'P2 must have 2 or 3 elements') 63 | end 64 | end 65 | end 66 | assert(numcols(p1) == numcols(p2), 'SMTB:plot_arrow', 'P1 and P2 must be the same length'); 67 | 68 | hh = arrow3(p1, p2, varargin{:}); 69 | 70 | if nargout > 0 71 | h = hh; 72 | end 73 | end 74 | -------------------------------------------------------------------------------- /plot_homline.m: -------------------------------------------------------------------------------- 1 | %PLOT_HOMLINE Draw a line in homogeneous form 2 | % 3 | % PLOT_HOMLINE(L) draws a 2D line in the current plot defined in homogenous 4 | % form ax + by + c = 0 where L (3x1) is L = [a b c]. 5 | % The current axis limits are used to determine the endpoints of 6 | % the line. If L (3xN) then N lines are drawn, one per column. 7 | % 8 | % PLOT_HOMLINE(L, LS) as above but the MATLAB line specification LS is given. 9 | % 10 | % H = PLOT_HOMLINE(...) as above but returns a vector of graphics handles for the lines. 11 | % 12 | % Notes:: 13 | % - The line(s) is added to the current plot. 14 | % - The line(s) can be drawn in 3D axes but will always lie in the 15 | % xy-plane. 16 | % 17 | % Example:: 18 | % L = homline([1 2]', [3 1]'); % homog line from (1,2) to (3,1) 19 | % plot_homline(L, 'k--'); % plot dashed black line 20 | % 21 | % See also PLOT_BOX, PLOT_POLY, HOMLINE. 22 | 23 | % Copyright (C) 1993-2019 Peter I. Corke 24 | % 25 | % This file is part of The Spatial Math Toolbox for MATLAB (SMTB). 26 | % 27 | % Permission is hereby granted, free of charge, to any person obtaining a copy 28 | % of this software and associated documentation files (the "Software"), to deal 29 | % in the Software without restriction, including without limitation the rights 30 | % to use, copy, modify, merge, publish, distribute, sublicense, and/or sell copies 31 | % of the Software, and to permit persons to whom the Software is furnished to do 32 | % so, subject to the following conditions: 33 | % 34 | % The above copyright notice and this permission notice shall be included in all 35 | % copies or substantial portions of the Software. 36 | % 37 | % THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 38 | % IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS 39 | % FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR 40 | % COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER 41 | % IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN 42 | % CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. 43 | % 44 | % https://github.com/petercorke/spatial-math 45 | 46 | 47 | function handles = plot_homline(lines, varargin) 48 | 49 | % get plot limits from current graph 50 | xlim = get(gca, 'XLim'); 51 | ylim = get(gca, 'YLim'); 52 | 53 | ish = ishold; 54 | hold on; 55 | 56 | if min(size(lines)) == 1 57 | lines = lines(:); 58 | end 59 | 60 | assert(numrows(lines) == 3, 'SMTB:plot_homline:badarg', 'Input must be a 3-vector or 3xN matrix'); 61 | 62 | h = []; 63 | % for all input lines (columns 64 | for l=lines 65 | if abs(l(2)) > abs(l(1)) 66 | y = (-l(3) - l(1)*xlim) / l(2); 67 | hh = plot(xlim, y, varargin{:}); 68 | else 69 | x = (-l(3) - l(2)*ylim) / l(1); 70 | hh = plot(x, ylim, varargin{:}); 71 | end 72 | h = [h; hh]; 73 | end 74 | 75 | if ~ish 76 | hold off 77 | end 78 | 79 | if nargout > 0, 80 | handles = h; 81 | end 82 | -------------------------------------------------------------------------------- /plot_sphere.m: -------------------------------------------------------------------------------- 1 | %PLOT_SPHERE Draw sphere 2 | % 3 | % PLOT_SPHERE(C, R, LS) draws spheres in the current plot. C is the 4 | % centre of the sphere (3x1), R is the radius and LS is an optional MATLAB 5 | % ColorSpec, either a letter or a 3-vector. 6 | % 7 | % PLOT_SPHERE(C, R, COLOR, ALPHA) as above but ALPHA specifies the opacity 8 | % of the sphere where 0 is transparant and 1 is opaque. The default is 1. 9 | % 10 | % If C (3xN) then N sphhere are drawn and H is Nx1. If R (1x1) then all 11 | % spheres have the same radius or else R (1xN) to specify the radius of 12 | % each sphere. 13 | % 14 | % H = PLOT_SPHERE(...) as above but returns the handle(s) for the 15 | % spheres. 16 | % 17 | % Notes:: 18 | % - The sphere is always added, irrespective of figure hold state. 19 | % - The number of vertices to draw the sphere is hardwired. 20 | % 21 | % Example:: 22 | % plot_sphere( mkgrid(2, 1), .2, 'b'); % Create four spheres 23 | % lighting gouraud % full lighting model 24 | % light 25 | % 26 | % See also: plot_point, plot_box, plot_circle, plot_ellipse, plot_poly. 27 | 28 | % Copyright (C) 1993-2019 Peter I. Corke 29 | % 30 | % This file is part of The Spatial Math Toolbox for MATLAB (SMTB). 31 | % 32 | % Permission is hereby granted, free of charge, to any person obtaining a copy 33 | % of this software and associated documentation files (the "Software"), to deal 34 | % in the Software without restriction, including without limitation the rights 35 | % to use, copy, modify, merge, publish, distribute, sublicense, and/or sell copies 36 | % of the Software, and to permit persons to whom the Software is furnished to do 37 | % so, subject to the following conditions: 38 | % 39 | % The above copyright notice and this permission notice shall be included in all 40 | % copies or substantial portions of the Software. 41 | % 42 | % THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 43 | % IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS 44 | % FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR 45 | % COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER 46 | % IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN 47 | % CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. 48 | % 49 | % https://github.com/petercorke/spatial-math 50 | 51 | % TODO 52 | % inconsistant call format compared to other plot_xxx functions. 53 | 54 | function out = plot_sphere(c, r, varargin) 55 | 56 | opt.color = 'b'; 57 | opt.alpha = 1; 58 | opt.mesh = 'none'; 59 | opt.n = 40; 60 | 61 | [opt,args] = tb_optparse(opt, varargin); 62 | 63 | % backward compatibility with RVC 64 | if length(args) > 0 65 | opt.color = args{1}; 66 | end 67 | if length(args) > 1 68 | opt.alpha = args{2}; 69 | end 70 | 71 | daspect([1 1 1]) 72 | hold_on = ishold; 73 | hold on 74 | [xs,ys,zs] = sphere(opt.n); 75 | 76 | if isvec(c,3) 77 | c = c(:); 78 | end 79 | if size(r) == 1 80 | r = r * ones(numcols(c),1); 81 | end 82 | 83 | if nargin < 4 84 | alpha = 1; 85 | end 86 | 87 | % transform the sphere 88 | for i=1:numcols(c) 89 | x = r(i)*xs + c(1,i); 90 | y = r(i)*ys + c(2,i); 91 | z = r(i)*zs + c(3,i); 92 | 93 | % the following displays a nice smooth sphere with glint! 94 | h = surf(x,y,z, ones(size(z)), 'FaceColor', opt.color, 'EdgeColor', opt.mesh, 'FaceAlpha', opt.alpha); 95 | % camera patches disappear when shading interp is on 96 | %h = surfl(x,y,z) 97 | end 98 | %lighting gouraud 99 | %light 100 | if ~hold_on 101 | hold off 102 | end 103 | if nargout > 0 104 | out = h; 105 | end 106 | -------------------------------------------------------------------------------- /plotvol.m: -------------------------------------------------------------------------------- 1 | %PLOTVOL Set the bounds for a 2D or 3D plot 2 | % 3 | % 2D plots:: 4 | % 5 | % PLOTVOL([WX WY]) creates a new axis, and sets the bounds for a 2D plot, 6 | % with X spanning [-WX, WX] and Y spanning [-WY,WY]. 7 | % 8 | % PLOTVOL([XMIN XMAX YMIN YMAX]) as above but the X and Y axis limits are explicitly provided. 9 | % 10 | % 3D plots:: 11 | % 12 | % PLOTVOL(W) creates a new axis, and sets the bounds for a 3D plot with X, Y and Z spanning 13 | % the interval -W to W. 14 | % 15 | % PLOTVOL([WX WY WZ]) as above with X spanning [-WX, WX], Y spanning [-WY, WY] and Z 16 | % spanning [-WZ, WZ]. 17 | % 18 | % Notes:: 19 | % - The axes are labelled, grid is enabled, aspect ratio set to 1:1. 20 | % - Hold is enabled for subsequent plots. 21 | % 22 | % See also: axis. 23 | 24 | % 25 | % PLOTVOL([XMIN XMAX YMIN YMAX ZMIN ZMAX]) as above but the X, Y and Z axis limits are 26 | % explicitly provided. 27 | % 28 | % See also axis, xaxis, yaxis. 29 | 30 | % Copyright (C) 1993-2019 Peter I. Corke 31 | % 32 | % This file is part of The Spatial Math Toolbox for MATLAB (SMTB). 33 | % 34 | % Permission is hereby granted, free of charge, to any person obtaining a copy 35 | % of this software and associated documentation files (the "Software"), to deal 36 | % in the Software without restriction, including without limitation the rights 37 | % to use, copy, modify, merge, publish, distribute, sublicense, and/or sell copies 38 | % of the Software, and to permit persons to whom the Software is furnished to do 39 | % so, subject to the following conditions: 40 | % 41 | % The above copyright notice and this permission notice shall be included in all 42 | % copies or substantial portions of the Software. 43 | % 44 | % THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 45 | % IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS 46 | % FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR 47 | % COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER 48 | % IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN 49 | % CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. 50 | % 51 | % https://github.com/petercorke/spatial-math 52 | 53 | function plotvol(bounds) 54 | 55 | assert(any(length(bounds) == [1 2 3 4 6]), 'SMTB:plotvol:badarg', 'expecting vector of length 1, 2, 3, 4 or 6'); 56 | 57 | 58 | clf 59 | axis equal 60 | 61 | switch length(bounds) 62 | case 1 63 | bounds = bounds * [-1 1 -1 1 -1 1]; 64 | case 3 65 | bounds = bounds(:) * [-1 1]; 66 | bounds = bounds'; 67 | bounds = bounds(:)'; 68 | case 6 69 | % % 3D case 70 | % set(gca, 'XTick', floor(bounds(1)):floor(bounds(2))) 71 | % set(gca, 'YTick', floor(bounds(3)):floor(bounds(4))) 72 | % set(gca, 'ZTick', floor(bounds(5)):floor(bounds(6))) 73 | bounds = bounds(:)'; 74 | case 2 75 | bounds = bounds(:) * [-1 1]; 76 | bounds = bounds'; 77 | bounds = bounds(:)'; 78 | case 4 79 | bounds = bounds(:)'; 80 | end 81 | 82 | axis(bounds) 83 | if length(bounds) == 6 84 | view(3) 85 | end 86 | 87 | % common code 88 | grid on; 89 | hold on 90 | 91 | 92 | xyzlabel 93 | end 94 | -------------------------------------------------------------------------------- /r2t.m: -------------------------------------------------------------------------------- 1 | %R2T Convert rotation matrix to a homogeneous transform 2 | % 3 | % T = R2T(R) is an SE(2) or SE(3) homogeneous transform equivalent to an 4 | % SO(2) or SO(3) orthonormal rotation matrix R with a zero translational 5 | % component. Works for T in either SE(2) or SE(3): 6 | % - if R is 2x2 then T is 3x3, or 7 | % - if R is 3x3 then T is 4x4. 8 | % 9 | % Notes:: 10 | % - Translational component is zero. 11 | % - For a rotation matrix sequence (KxKxN) returns a homogeneous transform 12 | % sequence (K+1xK+1xN). 13 | % 14 | % See also T2R. 15 | 16 | % Copyright (C) 1993-2019 Peter I. Corke 17 | % 18 | % This file is part of The Spatial Math Toolbox for MATLAB (SMTB). 19 | % 20 | % Permission is hereby granted, free of charge, to any person obtaining a copy 21 | % of this software and associated documentation files (the "Software"), to deal 22 | % in the Software without restriction, including without limitation the rights 23 | % to use, copy, modify, merge, publish, distribute, sublicense, and/or sell copies 24 | % of the Software, and to permit persons to whom the Software is furnished to do 25 | % so, subject to the following conditions: 26 | % 27 | % The above copyright notice and this permission notice shall be included in all 28 | % copies or substantial portions of the Software. 29 | % 30 | % THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 31 | % IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS 32 | % FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR 33 | % COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER 34 | % IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN 35 | % CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. 36 | % 37 | % https://github.com/petercorke/spatial-math 38 | 39 | function T = r2t(R) 40 | 41 | % check dimensions: R is SO(2) or SO(3) 42 | d = size(R); 43 | assert(d(1) == d(2), 'SMTB:r2t:badarg', 'matrix must be square'); 44 | assert(any(d(1) == [2 3]), 'SMTB:r2t:badarg', 'argument is not a 2x2 or 3x3 rotation matrix (sequence)'); 45 | 46 | n = size(R,2); 47 | Z = zeros(n,1); 48 | B = [zeros(1,n) 1]; 49 | 50 | if numel(d) == 2 51 | % single matrix case 52 | T = [R Z; B]; 53 | else 54 | % matrix sequence case 55 | T = zeros(n+1,n+1,d(3)); 56 | for i=1:d(3) 57 | T(:,:,i) = [R(:,:,i) Z; B]; 58 | end 59 | end 60 | end 61 | -------------------------------------------------------------------------------- /randinit.m: -------------------------------------------------------------------------------- 1 | %RANDINIT Reset random number generator 2 | % 3 | % RANDINIT resets the defaul random number stream. For example: 4 | % 5 | % >> rand 6 | % ans = 7 | % 0.8147 8 | % >> rand 9 | % ans = 10 | % 0.9058 11 | % >> rand 12 | % ans = 13 | % 0.1270 14 | % >> randinit 15 | % >> rand 16 | % ans = 17 | % 0.8147 18 | 19 | % 20 | % See also RandStream. 21 | 22 | % Copyright (C) 1993-2019 Peter I. Corke 23 | % 24 | % This file is part of The Spatial Math Toolbox for MATLAB (SMTB). 25 | % 26 | % Permission is hereby granted, free of charge, to any person obtaining a copy 27 | % of this software and associated documentation files (the "Software"), to deal 28 | % in the Software without restriction, including without limitation the rights 29 | % to use, copy, modify, merge, publish, distribute, sublicense, and/or sell copies 30 | % of the Software, and to permit persons to whom the Software is furnished to do 31 | % so, subject to the following conditions: 32 | % 33 | % The above copyright notice and this permission notice shall be included in all 34 | % copies or substantial portions of the Software. 35 | % 36 | % THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 37 | % IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS 38 | % FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR 39 | % COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER 40 | % IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN 41 | % CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. 42 | % 43 | % https://github.com/petercorke/spatial-math 44 | 45 | function randinit(seed) 46 | 47 | stream = RandStream.getGlobalStream; 48 | stream.reset() 49 | 50 | -------------------------------------------------------------------------------- /rot2.m: -------------------------------------------------------------------------------- 1 | %ROT2 SO(2) rotation matrix 2 | % 3 | % R = ROT2(THETA) is an SO(2) rotation matrix (2x2) representing a rotation of THETA 4 | % radians. 5 | % 6 | % R = ROT2(THETA, 'deg') as above but THETA is in degrees. 7 | % 8 | % See also TROT2, ISROT2, TRPLOT2, ROTX, ROTY, ROTZ, SO2. 9 | 10 | % Copyright (C) 1993-2019 Peter I. Corke 11 | % 12 | % This file is part of The Spatial Math Toolbox for MATLAB (SMTB). 13 | % 14 | % Permission is hereby granted, free of charge, to any person obtaining a copy 15 | % of this software and associated documentation files (the "Software"), to deal 16 | % in the Software without restriction, including without limitation the rights 17 | % to use, copy, modify, merge, publish, distribute, sublicense, and/or sell copies 18 | % of the Software, and to permit persons to whom the Software is furnished to do 19 | % so, subject to the following conditions: 20 | % 21 | % The above copyright notice and this permission notice shall be included in all 22 | % copies or substantial portions of the Software. 23 | % 24 | % THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 25 | % IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS 26 | % FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR 27 | % COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER 28 | % IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN 29 | % CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. 30 | % 31 | % https://github.com/petercorke/spatial-math 32 | 33 | function R = rot2(t, deg) 34 | 35 | if nargin > 1 && strcmp(deg, 'deg') 36 | t = t *pi/180; 37 | end 38 | 39 | ct = cos(t); 40 | st = sin(t); 41 | R = [ 42 | ct -st 43 | st ct 44 | ]; 45 | -------------------------------------------------------------------------------- /rotx.m: -------------------------------------------------------------------------------- 1 | %ROTX SO(3) rotation about X axis 2 | % 3 | % R = ROTX(THETA) is an SO(3) rotation matrix (3x3) representing a rotation of THETA 4 | % radians about the x-axis. 5 | % 6 | % R = ROTX(THETA, 'deg') as above but THETA is in degrees. 7 | % 8 | % See also TROTX, ROTY, ROTZ, ANGVEC2R, ROT2, SO3.Rx. 9 | 10 | % Copyright (C) 1993-2019 Peter I. Corke 11 | % 12 | % This file is part of The Spatial Math Toolbox for MATLAB (SMTB). 13 | % 14 | % Permission is hereby granted, free of charge, to any person obtaining a copy 15 | % of this software and associated documentation files (the "Software"), to deal 16 | % in the Software without restriction, including without limitation the rights 17 | % to use, copy, modify, merge, publish, distribute, sublicense, and/or sell copies 18 | % of the Software, and to permit persons to whom the Software is furnished to do 19 | % so, subject to the following conditions: 20 | % 21 | % The above copyright notice and this permission notice shall be included in all 22 | % copies or substantial portions of the Software. 23 | % 24 | % THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 25 | % IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS 26 | % FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR 27 | % COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER 28 | % IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN 29 | % CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. 30 | % 31 | % https://github.com/petercorke/spatial-math 32 | 33 | function R = rotx(t, deg) 34 | 35 | assert((isreal(t) & isscalar(t)) | isa(t, 'sym'), ... 36 | 'SMTB:rotx:badarg', 'theta must be a real scalar'); 37 | 38 | if nargin > 1 && strcmp(deg, 'deg') 39 | t = t *pi/180; 40 | end 41 | 42 | ct = cos(t); 43 | st = sin(t); 44 | 45 | % make almost zero elements exactly zero 46 | if ~isa(t, 'sym') 47 | if abs(st) < eps 48 | st = 0; 49 | end 50 | if abs(ct) < eps 51 | ct = 0; 52 | end 53 | end 54 | 55 | % create the rotation matrix 56 | R = [ 57 | 1 0 0 58 | 0 ct -st 59 | 0 st ct 60 | ]; 61 | end 62 | -------------------------------------------------------------------------------- /roty.m: -------------------------------------------------------------------------------- 1 | %ROTY SO(3) rotation about Y axis 2 | % 3 | % R = ROTY(THETA) is an SO(3) rotation matrix (3x3) representing a rotation of THETA 4 | % radians about the y-axis. 5 | % 6 | % R = ROTY(THETA, 'deg') as above but THETA is in degrees. 7 | % 8 | % See also TROTY, ROTX, ROTZ, ANGVEC2R, ROT2, SO3.Ry. 9 | 10 | % Copyright (C) 1993-2019 Peter I. Corke 11 | % 12 | % This file is part of The Spatial Math Toolbox for MATLAB (SMTB). 13 | % 14 | % Permission is hereby granted, free of charge, to any person obtaining a copy 15 | % of this software and associated documentation files (the "Software"), to deal 16 | % in the Software without restriction, including without limitation the rights 17 | % to use, copy, modify, merge, publish, distribute, sublicense, and/or sell copies 18 | % of the Software, and to permit persons to whom the Software is furnished to do 19 | % so, subject to the following conditions: 20 | % 21 | % The above copyright notice and this permission notice shall be included in all 22 | % copies or substantial portions of the Software. 23 | % 24 | % THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 25 | % IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS 26 | % FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR 27 | % COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER 28 | % IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN 29 | % CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. 30 | % 31 | % https://github.com/petercorke/spatial-math 32 | 33 | function R = roty(t, deg) 34 | 35 | assert((isreal(t) & isscalar(t)) | isa(t, 'sym'), ... 36 | 'SMTB:roty:badarg', 'theta must be a real scalar'); 37 | 38 | if nargin > 1 && strcmp(deg, 'deg') 39 | t = t *pi/180; 40 | end 41 | 42 | ct = cos(t); 43 | st = sin(t); 44 | 45 | % make almost zero elements exactly zero 46 | if ~isa(t, 'sym') 47 | if abs(st) < eps 48 | st = 0; 49 | end 50 | if abs(ct) < eps 51 | ct = 0; 52 | end 53 | end 54 | 55 | % create the rotation matrix 56 | R = [ 57 | ct 0 st 58 | 0 1 0 59 | -st 0 ct 60 | ]; 61 | end 62 | -------------------------------------------------------------------------------- /rotz.m: -------------------------------------------------------------------------------- 1 | %ROTZ SO(3) rotation about Z axis 2 | % 3 | % R = ROTZ(THETA) is an SO(3) rotation matrix (3x3) representing a rotation of THETA 4 | % radians about the z-axis. 5 | % 6 | % R = ROTZ(THETA, 'deg') as above but THETA is in degrees. 7 | % 8 | % See also TROTZ, ROTX, ROTY, ANGVEC2R, ROT2, SO3.Rx. 9 | 10 | % Copyright (C) 1993-2019 Peter I. Corke 11 | % 12 | % This file is part of The Spatial Math Toolbox for MATLAB (SMTB). 13 | % 14 | % Permission is hereby granted, free of charge, to any person obtaining a copy 15 | % of this software and associated documentation files (the "Software"), to deal 16 | % in the Software without restriction, including without limitation the rights 17 | % to use, copy, modify, merge, publish, distribute, sublicense, and/or sell copies 18 | % of the Software, and to permit persons to whom the Software is furnished to do 19 | % so, subject to the following conditions: 20 | % 21 | % The above copyright notice and this permission notice shall be included in all 22 | % copies or substantial portions of the Software. 23 | % 24 | % THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 25 | % IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS 26 | % FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR 27 | % COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER 28 | % IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN 29 | % CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. 30 | % 31 | % https://github.com/petercorke/spatial-math 32 | 33 | function R = rotz(t, deg) 34 | 35 | assert((isreal(t) & isscalar(t)) | isa(t, 'sym'), ... 36 | 'SMTB:rotz:badarg', 'theta must be a real scalar or symbolic'); 37 | 38 | if nargin > 1 && strcmp(deg, 'deg') 39 | t = t *pi/180; 40 | end 41 | 42 | ct = cos(t); 43 | st = sin(t); 44 | 45 | % make almost zero elements exactly zero 46 | if ~isa(t, 'sym') 47 | if abs(st) < eps 48 | st = 0; 49 | end 50 | if abs(ct) < eps 51 | ct = 0; 52 | end 53 | end 54 | 55 | % create the rotation matrix 56 | R = [ 57 | ct -st 0 58 | st ct 0 59 | 0 0 1 60 | ]; 61 | end 62 | -------------------------------------------------------------------------------- /rpy2tr.m: -------------------------------------------------------------------------------- 1 | %RPY2TR Roll-pitch-yaw angles to SE(3) homogeneous transform 2 | % 3 | % T = RPY2TR(ROLL, PITCH, YAW, OPTIONS) is an SE(3) homogeneous 4 | % transformation matrix (4x4) with zero translation and rotation equivalent 5 | % to the specified roll, pitch, yaw angles angles. These correspond to 6 | % rotations about the Z, Y, X axes respectively. If ROLL, PITCH, YAW are 7 | % column vectors (Nx1) then they are assumed to represent a trajectory and 8 | % R is a three-dimensional matrix (4x4xN), where the last index corresponds 9 | % to rows of ROLL, PITCH, YAW. 10 | % 11 | % T = RPY2TR(RPY, OPTIONS) as above but the roll, pitch, yaw angles are 12 | % taken from the vector (1x3) RPY=[ROLL,PITCH,YAW]. If RPY is a matrix 13 | % (Nx3) then R is a three-dimensional matrix (4x4xN), where the last index 14 | % corresponds to rows of RPY which are assumed to be 15 | % ROLL,PITCH,YAW]. 16 | % 17 | % Options:: 18 | % 'deg' Compute angles in degrees (radians default) 19 | % 'xyz' Rotations about X, Y, Z axes (for a robot gripper) 20 | % 'zyx' Rotations about Z, Y, X axes (for a mobile robot, default) 21 | % 'yxz' Rotations about Y, X, Z axes (for a camera) 22 | % 'arm' Rotations about X, Y, Z axes (for a robot arm) 23 | % 'vehicle' Rotations about Z, Y, X axes (for a mobile robot) 24 | % 'camera' Rotations about Y, X, Z axes (for a camera) 25 | % 26 | % Note:: 27 | % - Toolbox rel 8-9 has the reverse angle sequence as default. 28 | % - ZYX order is appropriate for vehicles with direction of travel in the X 29 | % direction. XYZ order is appropriate if direction of travel is in the Z 30 | % direction. 31 | % - 'arm', 'vehicle', 'camera' are synonyms for 'xyz', 'zyx' and 'yxz' 32 | % respectively. 33 | % 34 | % See also TR2RPY, RPY2R, EUL2TR. 35 | 36 | % Copyright (C) 1993-2019 Peter I. Corke 37 | % 38 | % This file is part of The Spatial Math Toolbox for MATLAB (SMTB). 39 | % 40 | % Permission is hereby granted, free of charge, to any person obtaining a copy 41 | % of this software and associated documentation files (the "Software"), to deal 42 | % in the Software without restriction, including without limitation the rights 43 | % to use, copy, modify, merge, publish, distribute, sublicense, and/or sell copies 44 | % of the Software, and to permit persons to whom the Software is furnished to do 45 | % so, subject to the following conditions: 46 | % 47 | % The above copyright notice and this permission notice shall be included in all 48 | % copies or substantial portions of the Software. 49 | % 50 | % THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 51 | % IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS 52 | % FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR 53 | % COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER 54 | % IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN 55 | % CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. 56 | % 57 | % https://github.com/petercorke/spatial-math 58 | 59 | function T = rpy2tr(roll, varargin) 60 | 61 | R = rpy2r(roll, varargin{:}); 62 | T = r2t(R); 63 | -------------------------------------------------------------------------------- /rt2tr.m: -------------------------------------------------------------------------------- 1 | %RT2TR Convert rotation and translation to homogeneous transform 2 | % 3 | % TR = RT2TR(R, t) is a homogeneous transformation matrix (N+1xN+1) formed 4 | % from an orthonormal rotation matrix R (NxN) and a translation vector t 5 | % (Nx1). Works for R in SO(2) or SO(3): 6 | % - If R is 2x2 and t is 2x1, then TR is 3x3 7 | % - If R is 3x3 and t is 3x1, then TR is 4x4 8 | % 9 | % For a sequence R (NxNxK) and t (NxK) results in a transform sequence (N+1xN+1xK). 10 | % 11 | % Notes:: 12 | % - The validity of R is not checked 13 | % 14 | % See also T2R, R2T, TR2RT. 15 | 16 | % Copyright (C) 1993-2019 Peter I. Corke 17 | % 18 | % This file is part of The Spatial Math Toolbox for MATLAB (SMTB). 19 | % 20 | % Permission is hereby granted, free of charge, to any person obtaining a copy 21 | % of this software and associated documentation files (the "Software"), to deal 22 | % in the Software without restriction, including without limitation the rights 23 | % to use, copy, modify, merge, publish, distribute, sublicense, and/or sell copies 24 | % of the Software, and to permit persons to whom the Software is furnished to do 25 | % so, subject to the following conditions: 26 | % 27 | % The above copyright notice and this permission notice shall be included in all 28 | % copies or substantial portions of the Software. 29 | % 30 | % THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 31 | % IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS 32 | % FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR 33 | % COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER 34 | % IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN 35 | % CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. 36 | % 37 | % https://github.com/petercorke/spatial-math 38 | 39 | function T = rt2tr(R, t) 40 | 41 | assert( size(R,1) == size(R,2), 'SMTB:rt2tr:badarg', 'R must be square'); 42 | 43 | n = size(R,2); 44 | B = [zeros(1,n) 1]; 45 | 46 | if size(R,3) > 1 47 | % vector case 48 | assert( size(R,1) == size(t,1), 'SMTB:rt2tr:badarg', 'R and t must have conforming dimensions') 49 | assert( size(R,3) == size(t,2), 'SMTB:rt2tr:badarg', 'For sequence size(R,3) must equal size(t,2)'); 50 | 51 | T = zeros(n+1,n+1,size(R,3)); 52 | for i=1:size(R,3) 53 | T(:,:,i) = [R(:,:,i) t(:,i); B]; 54 | end 55 | else 56 | % scalar case 57 | assert( isvec(t, size(R,1)), 'SMTB:rt2tr:badarg', 'R and t must have conforming dimensions') 58 | T = [R t(:); B]; 59 | end 60 | end 61 | 62 | -------------------------------------------------------------------------------- /skew.m: -------------------------------------------------------------------------------- 1 | %SKEW Create skew-symmetric matrix 2 | % 3 | % S = SKEW(V) is a skew-symmetric matrix formed from V. 4 | % 5 | % If V (1x1) then S = 6 | % 7 | % | 0 -v | 8 | % | v 0 | 9 | % 10 | % and if V (1x3) then S = 11 | % 12 | % | 0 -vz vy | 13 | % | vz 0 -vx | 14 | % |-vy vx 0 | 15 | % 16 | % 17 | % Notes:: 18 | % - This is the inverse of the function VEX(). 19 | % - These are the generator matrices for the Lie algebras so(2) and so(3). 20 | % 21 | % References:: 22 | % - Robotics, Vision & Control: Second Edition, Chap 2, 23 | % P. Corke, Springer 2016. 24 | % 25 | % See also SKEWA, VEX. 26 | 27 | % Copyright (C) 1993-2019 Peter I. Corke 28 | % 29 | % This file is part of The Spatial Math Toolbox for MATLAB (SMTB). 30 | % 31 | % Permission is hereby granted, free of charge, to any person obtaining a copy 32 | % of this software and associated documentation files (the "Software"), to deal 33 | % in the Software without restriction, including without limitation the rights 34 | % to use, copy, modify, merge, publish, distribute, sublicense, and/or sell copies 35 | % of the Software, and to permit persons to whom the Software is furnished to do 36 | % so, subject to the following conditions: 37 | % 38 | % The above copyright notice and this permission notice shall be included in all 39 | % copies or substantial portions of the Software. 40 | % 41 | % THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 42 | % IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS 43 | % FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR 44 | % COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER 45 | % IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN 46 | % CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. 47 | % 48 | % https://github.com/petercorke/spatial-math 49 | 50 | function S = skew(v) 51 | if isvec(v,3) 52 | % SO(3) case 53 | S = [ 0 -v(3) v(2) 54 | v(3) 0 -v(1) 55 | -v(2) v(1) 0]; 56 | elseif isvec(v,1) 57 | % SO(2) case 58 | S = [0 -v; v 0]; 59 | else 60 | error('SMTB:skew:badarg', 'argument must be a 1- or 3-vector'); 61 | end 62 | -------------------------------------------------------------------------------- /skewa.m: -------------------------------------------------------------------------------- 1 | %SKEWA Create augmented skew-symmetric matrix 2 | % 3 | % S = SKEWA(V) is an augmented skew-symmetric matrix formed from V. 4 | % 5 | % If V (1x3) then S = 6 | % 7 | % | 0 -v3 v1 | 8 | % | v3 0 v2 | 9 | % | 0 0 0 | 10 | % 11 | % and if V (1x6) then S = 12 | % 13 | % | 0 -v6 v5 v1 | 14 | % | v6 0 -v4 v2 | 15 | % |-v5 v4 0 v3 | 16 | % | 0 0 0 0 | 17 | % 18 | % Notes:: 19 | % - This is the inverse of the function VEXA(). 20 | % - These are the generator matrices for the Lie algebras se(2) and se(3). 21 | % - Map twist vectors in 2D and 3D space to se(2) and se(3). 22 | % 23 | % References:: 24 | % - Robotics, Vision & Control: Second Edition, Chap 2, 25 | % P. Corke, Springer 2016. 26 | % 27 | % See also SKEW, VEX, Twist. 28 | 29 | % Copyright (C) 1993-2019 Peter I. Corke 30 | % 31 | % This file is part of The Spatial Math Toolbox for MATLAB (SMTB). 32 | % 33 | % Permission is hereby granted, free of charge, to any person obtaining a copy 34 | % of this software and associated documentation files (the "Software"), to deal 35 | % in the Software without restriction, including without limitation the rights 36 | % to use, copy, modify, merge, publish, distribute, sublicense, and/or sell copies 37 | % of the Software, and to permit persons to whom the Software is furnished to do 38 | % so, subject to the following conditions: 39 | % 40 | % The above copyright notice and this permission notice shall be included in all 41 | % copies or substantial portions of the Software. 42 | % 43 | % THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 44 | % IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS 45 | % FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR 46 | % COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER 47 | % IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN 48 | % CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. 49 | % 50 | % https://github.com/petercorke/spatial-math 51 | 52 | function Omega = skewa(s) 53 | s = s(:); 54 | switch length(s) 55 | case 3 56 | Omega = [skew(s(3)) s(1:2); 0 0 0]; 57 | 58 | case 6 59 | Omega = [skew(s(4:6)) s(1:3); 0 0 0 0]; 60 | 61 | otherwise 62 | error('SMTB:skewa:badarg', 'expecting a 3- or 6-vector'); 63 | end 64 | end 65 | -------------------------------------------------------------------------------- /t2r.m: -------------------------------------------------------------------------------- 1 | %T2R Rotational submatrix 2 | % 3 | % R = T2R(T) is the orthonormal rotation matrix component of homogeneous 4 | % transformation matrix T. Works for T in SE(2) or SE(3) 5 | % - If T is 4x4, then R is 3x3. 6 | % - If T is 3x3, then R is 2x2. 7 | % 8 | % Notes:: 9 | % - For a homogeneous transform sequence (KxKxN) returns a rotation matrix 10 | % sequence (K-1xK-1xN). 11 | % - The validity of rotational part is not checked 12 | % 13 | % See also R2T, TR2RT, RT2TR. 14 | 15 | %## 2d 3d homogeneous 16 | 17 | % Copyright (C) 1993-2019 Peter I. Corke 18 | % 19 | % This file is part of The Spatial Math Toolbox for MATLAB (SMTB). 20 | % 21 | % Permission is hereby granted, free of charge, to any person obtaining a copy 22 | % of this software and associated documentation files (the "Software"), to deal 23 | % in the Software without restriction, including without limitation the rights 24 | % to use, copy, modify, merge, publish, distribute, sublicense, and/or sell copies 25 | % of the Software, and to permit persons to whom the Software is furnished to do 26 | % so, subject to the following conditions: 27 | % 28 | % The above copyright notice and this permission notice shall be included in all 29 | % copies or substantial portions of the Software. 30 | % 31 | % THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 32 | % IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS 33 | % FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR 34 | % COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER 35 | % IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN 36 | % CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. 37 | % 38 | % https://github.com/petercorke/spatial-math 39 | 40 | function R = t2r(T) 41 | 42 | assert(isfloat(T), 'SMTB:t2r:badarg', 'expecting real matrix argument'); 43 | 44 | % check dimensions: T is SE(2) or SE(3) 45 | d = size(T); 46 | assert(d(1) == d(2), 'SMTB:t2r:badarg', 'matrix must be square'); 47 | assert(any(d(1) == [3 4]), 'SMTB:t2r:badarg', 'argument is not a homogeneous transform (sequence)'); 48 | 49 | n = d(1); % works for SE(2) or SE(3) 50 | 51 | if numel(d) == 2 52 | % single matrix case 53 | R = T(1:n-1,1:n-1); 54 | else 55 | % matrix sequence case 56 | R = zeros(n-1,n-1,d(3)); 57 | for i=1:d(3) 58 | R(:,:,i) = T(1:n-1,1:n-1,i); 59 | end 60 | end 61 | end 62 | -------------------------------------------------------------------------------- /tr2delta.m: -------------------------------------------------------------------------------- 1 | %TR2DELTA Convert SE(3) homogeneous transform to differential motion 2 | % 3 | % D = TR2DELTA(T0, T1) is the differential motion (6x1) corresponding to 4 | % infinitessimal motion (in the T0 frame) from pose T0 to T1 which are homogeneous 5 | % transformations (4x4) or SE3 objects. 6 | % 7 | % The vector D=(dx, dy, dz, dRx, dRy, dRz) represents infinitessimal translation 8 | % and rotation, and is an approximation to the instantaneous spatial velocity 9 | % multiplied by time step. 10 | % 11 | % D = TR2DELTA(T) as above but the motion is from the world frame to the SE3 12 | % pose T. 13 | % 14 | % Notes:: 15 | % - D is only an approximation to the motion T, and assumes 16 | % that T0 ~ T1 or T ~ eye(4,4). 17 | % - Can be considered as an approximation to the effect of spatial velocity over a 18 | % a time interval, average spatial velocity multiplied by time. 19 | % 20 | % Reference:: 21 | % - Robotics, Vision & Control: Second Edition, P. Corke, Springer 2016; p67. 22 | % 23 | % See also DELTA2TR, SKEW, SE3.todelta. 24 | 25 | %## 3d differential homogeneous 26 | 27 | % Copyright (C) 1993-2019 Peter I. Corke 28 | % 29 | % This file is part of The Spatial Math Toolbox for MATLAB (SMTB). 30 | % 31 | % Permission is hereby granted, free of charge, to any person obtaining a copy 32 | % of this software and associated documentation files (the "Software"), to deal 33 | % in the Software without restriction, including without limitation the rights 34 | % to use, copy, modify, merge, publish, distribute, sublicense, and/or sell copies 35 | % of the Software, and to permit persons to whom the Software is furnished to do 36 | % so, subject to the following conditions: 37 | % 38 | % The above copyright notice and this permission notice shall be included in all 39 | % copies or substantial portions of the Software. 40 | % 41 | % THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 42 | % IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS 43 | % FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR 44 | % COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER 45 | % IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN 46 | % CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. 47 | % 48 | % https://github.com/petercorke/spatial-math 49 | 50 | function delta = tr2delta(A, B) 51 | 52 | % handle arguments, convert all to 4x4 matrices 53 | if nargin > 0 54 | if isa(A, 'SE3') 55 | T1 = A.double; 56 | elseif ishomog(A) 57 | T1 = A; 58 | else 59 | error('SMTB:tr2delta:badarg', 'T1 should be a homogeneous transformation'); 60 | end 61 | T0 = eye(4,4); 62 | end 63 | if nargin > 1 64 | T0 = T1; 65 | if isa(B, 'SE3') 66 | T1 = B.double; 67 | elseif ishomog(B) 68 | T1 = B; 69 | else 70 | error('SMTB:tr2delta:badarg', 'T0 should be a homogeneous transformation'); 71 | end 72 | end 73 | 74 | % compute incremental transformation from T0 to T1 in the T0 frame 75 | TD = inv(T0) * T1; 76 | 77 | % build the delta vector 78 | delta = [transl(TD); vex(t2r(TD) - eye(3,3))]; 79 | 80 | 81 | % R0 = t2r(T0); R1 = t2r(T1); 82 | % % in world frame 83 | % %[th,vec] = tr2angvec(R1*R0'); 84 | % dR = vex(R1*R0'); 85 | % %delta = [ (T1(1:3,4)-T0(1:3,4)); th*vec' ]; 86 | % delta = [ (T1(1:3,4)-T0(1:3,4)); dR]; 87 | 88 | % same as above but more complex 89 | % delta = [ T1(1:3,4)-T0(1:3,4); 90 | % 0.5*( cross(T0(1:3,1), T1(1:3,1)) + ... 91 | % cross(T0(1:3,2), T1(1:3,2)) + ... 92 | % cross(T0(1:3,3), T1(1:3,3)) ... 93 | % )]; 94 | end 95 | 96 | -------------------------------------------------------------------------------- /tr2eul.m: -------------------------------------------------------------------------------- 1 | %TR2EUL Convert SO(3) or SE(3) matrix to Euler angles 2 | % 3 | % EUL = TR2EUL(T, OPTIONS) are the ZYZ Euler angles (1x3) corresponding to 4 | % the rotational part of a homogeneous transform T (4x4). The 3 angles 5 | % EUL=[PHI,THETA,PSI] correspond to sequential rotations about the Z, Y and 6 | % Z axes respectively. 7 | % 8 | % EUL = TR2EUL(R, OPTIONS) as above but the input is an orthonormal 9 | % rotation matrix R (3x3). 10 | % 11 | % If R (3x3xK) or T (4x4xK) represent a sequence then each row of EUL 12 | % corresponds to a step of the sequence. 13 | % 14 | % Options:: 15 | % 'deg' Compute angles in degrees (radians default) 16 | % 'flip' Choose first Euler angle to be in quadrant 2 or 3. 17 | % 18 | % Notes:: 19 | % - There is a singularity for the case where THETA=0 in which case PHI is arbitrarily 20 | % set to zero and PSI is the sum (PHI+PSI). 21 | % - Translation component is ignored. 22 | % 23 | % See also EUL2TR, TR2RPY. 24 | 25 | %## 3d rotation homogeneous 26 | 27 | % Copyright (C) 1993-2019 Peter I. Corke 28 | % 29 | % This file is part of The Spatial Math Toolbox for MATLAB (SMTB). 30 | % 31 | % Permission is hereby granted, free of charge, to any person obtaining a copy 32 | % of this software and associated documentation files (the "Software"), to deal 33 | % in the Software without restriction, including without limitation the rights 34 | % to use, copy, modify, merge, publish, distribute, sublicense, and/or sell copies 35 | % of the Software, and to permit persons to whom the Software is furnished to do 36 | % so, subject to the following conditions: 37 | % 38 | % The above copyright notice and this permission notice shall be included in all 39 | % copies or substantial portions of the Software. 40 | % 41 | % THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 42 | % IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS 43 | % FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR 44 | % COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER 45 | % IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN 46 | % CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. 47 | % 48 | % https://github.com/petercorke/spatial-math 49 | 50 | function euler = tr2eul(RR, varargin) 51 | 52 | assert(isrot(RR) || ishomog(RR), 'SMTB:tr2eul:badarg', 'argument must be a 3x3 or 4x4 matrix'); 53 | 54 | opt.deg = false; 55 | opt.flip = false; 56 | opt = tb_optparse(opt, varargin); 57 | 58 | euler = zeros(size(RR,3),3); 59 | 60 | for i=1:size(RR,3) 61 | 62 | R = RR(:,:,i); 63 | 64 | % Method as per Paul, p 69. 65 | % euler = [phi theta psi] 66 | % 67 | 68 | if abs(R(1,3)) < eps && abs(R(2,3)) < eps 69 | % singularity 70 | eul(1) = 0; 71 | sp = 0; 72 | cp = 1; 73 | eul(2) = atan2(cp*R(1,3) + sp*R(2,3), R(3,3)); 74 | eul(3) = atan2(-sp * R(1,1) + cp * R(2,1), -sp*R(1,2) + cp*R(2,2)); 75 | else 76 | % non singular 77 | 78 | % Only positive phi is returned. 79 | if opt.flip 80 | eul(1) = atan2(-R(2,3), -R(1,3)); 81 | else 82 | eul(1) = atan2(R(2,3), R(1,3)); 83 | end 84 | sp = sin(eul(1)); 85 | cp = cos(eul(1)); 86 | eul(2) = atan2(cp*R(1,3) + sp*R(2,3), R(3,3)); 87 | eul(3) = atan2(-sp * R(1,1) + cp * R(2,1), -sp*R(1,2) + cp*R(2,2)); 88 | end 89 | 90 | euler(i,:) = eul; 91 | end 92 | 93 | if opt.deg 94 | euler = euler * 180/pi; 95 | end 96 | end 97 | -------------------------------------------------------------------------------- /tr2jac.m: -------------------------------------------------------------------------------- 1 | %TR2JAC Jacobian for differential motion 2 | % 3 | % J = TR2JAC(TAB) is a Jacobian matrix (6x6) that maps spatial velocity or 4 | % differential motion from frame {A} to frame {B} where the pose of {B} 5 | % relative to {A} is represented by the homogeneous transform TAB (4x4). 6 | % 7 | % J = TR2JAC(TAB, 'samebody') is a Jacobian matrix (6x6) that maps spatial 8 | % velocity or differential motion from frame {A} to frame {B} where both 9 | % are attached to the same moving body. The pose of {B} relative to {A} is 10 | % represented by the homogeneous transform TAB (4x4). 11 | % 12 | % See also WTRANS, TR2DELTA, DELTA2TR, SE3.velxform. 13 | 14 | %## 3d homogeneous differential 15 | 16 | % Copyright (C) 1993-2019 Peter I. Corke 17 | % 18 | % This file is part of The Spatial Math Toolbox for MATLAB (SMTB). 19 | % 20 | % Permission is hereby granted, free of charge, to any person obtaining a copy 21 | % of this software and associated documentation files (the "Software"), to deal 22 | % in the Software without restriction, including without limitation the rights 23 | % to use, copy, modify, merge, publish, distribute, sublicense, and/or sell copies 24 | % of the Software, and to permit persons to whom the Software is furnished to do 25 | % so, subject to the following conditions: 26 | % 27 | % The above copyright notice and this permission notice shall be included in all 28 | % copies or substantial portions of the Software. 29 | % 30 | % THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 31 | % IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS 32 | % FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR 33 | % COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER 34 | % IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN 35 | % CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. 36 | % 37 | % https://github.com/petercorke/spatial-math 38 | 39 | function J = tr2jac(T, varargin) 40 | 41 | opt.samebody = false; 42 | 43 | opt = tb_optparse(opt, varargin); 44 | 45 | R = t2r(T); 46 | 47 | if opt.samebody 48 | J = [ 49 | R' (skew(transl(T))*R)' 50 | zeros(3,3) R' 51 | ]; 52 | else 53 | J = [ 54 | R' zeros(3,3) 55 | zeros(3,3) R' 56 | ]; 57 | end 58 | -------------------------------------------------------------------------------- /tr2rt.m: -------------------------------------------------------------------------------- 1 | %TR2RT Convert homogeneous transform to rotation and translation 2 | % 3 | % [R,t] = TR2RT(TR) splits a homogeneous transformation matrix (NxN) into an 4 | % orthonormal rotation matrix R (MxM) and a translation vector t (Mx1), where 5 | % N=M+1. 6 | % 7 | % Works for TR in SE(2) or SE(3) 8 | % - If TR is 4x4, then R is 3x3 and T is 3x1. 9 | % - If TR is 3x3, then R is 2x2 and T is 2x1. 10 | % 11 | % A homogeneous transform sequence TR (NxNxK) is split into rotation matrix 12 | % sequence R (MxMxK) and a translation sequence t (KxM). 13 | % 14 | % 15 | % Notes:: 16 | % - The validity of R is not checked. 17 | % 18 | % See also RT2TR, R2T, T2R. 19 | 20 | %## 2d 3d homogeneous rotation translation 21 | 22 | % Copyright (C) 1993-2019 Peter I. Corke 23 | % 24 | % This file is part of The Spatial Math Toolbox for MATLAB (SMTB). 25 | % 26 | % Permission is hereby granted, free of charge, to any person obtaining a copy 27 | % of this software and associated documentation files (the "Software"), to deal 28 | % in the Software without restriction, including without limitation the rights 29 | % to use, copy, modify, merge, publish, distribute, sublicense, and/or sell copies 30 | % of the Software, and to permit persons to whom the Software is furnished to do 31 | % so, subject to the following conditions: 32 | % 33 | % The above copyright notice and this permission notice shall be included in all 34 | % copies or substantial portions of the Software. 35 | % 36 | % THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 37 | % IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS 38 | % FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR 39 | % COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER 40 | % IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN 41 | % CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. 42 | % 43 | % https://github.com/petercorke/spatial-math 44 | 45 | function [R,t] = tr2rt(T) 46 | assert(size(T,1) == size(T,2), 'T must be square'); 47 | 48 | n = size(T,2)-1; 49 | 50 | if size(T,3) > 1 51 | R = zeros(n,n,size(T,3)); 52 | t = zeros(size(T,3), n); 53 | for i=1:size(T,3) 54 | R(1:n,1:n,i) = T(1:n,1:n,i); 55 | t(i,:) = T(1:n,n+1,i)'; 56 | end 57 | else 58 | R = T(1:n,1:n); 59 | t = T(1:n,n+1); 60 | end 61 | -------------------------------------------------------------------------------- /transl2.m: -------------------------------------------------------------------------------- 1 | %TRANSL2 SE(2) translational homogeneous transform 2 | % 3 | % Create a translational SE(2) matrix:: 4 | % 5 | % T = TRANSL2(X, Y) is an SE(2) homogeneous transform (3x3) representing a 6 | % pure translation. 7 | % 8 | % T = TRANSL2(P) is a homogeneous transform representing a translation or 9 | % point P=[X,Y]. P (Mx2) represents a sequence and T (3x3xM) is a 10 | % sequence of homogenous transforms such that T(:,:,i) corresponds to the 11 | % i'th row of P. 12 | % 13 | % Extract the translational part of an SE(2) matrix:: 14 | % 15 | % P = TRANSL2(T) is the translational part of a homogeneous transform as a 16 | % 2-element column vector. T (3x3xM) is a homogeneous transform 17 | % sequence and the rows of P (Mx2) are the translational component of the 18 | % corresponding transform in the sequence. 19 | % 20 | % Notes:: 21 | % - Somewhat unusually, this function performs a function and its inverse. An 22 | % historical anomaly. 23 | % 24 | % See also SE2.t, ROT2, ISHOMOG2, TRPLOT2, TRANSL. 25 | 26 | %## 2d homogeneous translation 27 | 28 | % Copyright (C) 1993-2019 Peter I. Corke 29 | % 30 | % This file is part of The Spatial Math Toolbox for MATLAB (SMTB). 31 | % 32 | % Permission is hereby granted, free of charge, to any person obtaining a copy 33 | % of this software and associated documentation files (the "Software"), to deal 34 | % in the Software without restriction, including without limitation the rights 35 | % to use, copy, modify, merge, publish, distribute, sublicense, and/or sell copies 36 | % of the Software, and to permit persons to whom the Software is furnished to do 37 | % so, subject to the following conditions: 38 | % 39 | % The above copyright notice and this permission notice shall be included in all 40 | % copies or substantial portions of the Software. 41 | % 42 | % THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 43 | % IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS 44 | % FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR 45 | % COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER 46 | % IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN 47 | % CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. 48 | % 49 | % https://github.com/petercorke/spatial-math 50 | 51 | function T = transl2(x, y) 52 | if nargin == 1 53 | if ishomog2(x) 54 | if ndims(x) == 3 55 | % transl(T) -> P, trajectory case 56 | T = squeeze(x(1:2,3,:))'; 57 | else 58 | % transl(T) -> P 59 | T = x(1:2,3); 60 | end 61 | elseif all(size(x) == [3 3]) 62 | T = x(1:2,3); 63 | elseif length(x) == 2 64 | % transl(P) -> T 65 | t = x(:); 66 | T = [eye(2) t(:); 67 | 0 0 1]; 68 | else 69 | % transl(P) -> T, trajectory case 70 | n = size(x,1); 71 | T = repmat(eye(3,3), [1 1 n]); 72 | T(1:2,3,:) = x'; 73 | end 74 | elseif nargin == 2 75 | % transl(x,y) -> T 76 | t = [x; y]; 77 | T = [ eye(2) t; 0 0 1]; 78 | end 79 | end 80 | 81 | % one less function to upload for Cody/LTI assessments 82 | 83 | function h = ishomog2(tr, rtest) 84 | d = size(tr); 85 | if ndims(tr) >= 2 86 | h = all(d(1:2) == [3 3]); 87 | 88 | if h && nargin > 1 89 | h = abs(det(tr(1:2,1:2)) - 1) < eps; 90 | end 91 | else 92 | h = false; 93 | end 94 | end 95 | -------------------------------------------------------------------------------- /trnorm.m: -------------------------------------------------------------------------------- 1 | %TRNORM Normalize an SO(3) or SE(3) matrix 2 | % 3 | % TRNORM(R) is guaranteed to be a proper orthogonal matrix rotation 4 | % matrix (3x3) which is "close" to the input matrix R (3x3). If R 5 | % = [N,O,A] the O and A vectors are made unit length and the normal vector 6 | % is formed from N = O x A, and then we ensure that O and A are orthogonal 7 | % by O = A x N. 8 | % 9 | % TRNORM(T) as above but the rotational submatrix of the homogeneous 10 | % transformation T (4x4) is normalised while the translational part is 11 | % unchanged. 12 | % 13 | % If R (3x3xK) or T (4x4xK) representing a sequence then the normalisation 14 | % is performed on each of the K planes. 15 | % 16 | % Notes:: 17 | % - Only the direction of A (the z-axis) is unchanged. 18 | % - Used to prevent finite word length arithmetic causing transforms to 19 | % become `unnormalized'. 20 | % - There is no Toolbox function for SO(2) or SE(2). 21 | % 22 | % See also OA2TR, SO3.trnorm, SE3.trnorm. 23 | 24 | %## 3d homogeneous 25 | 26 | % Copyright (C) 1993-2019 Peter I. Corke 27 | % 28 | % This file is part of The Spatial Math Toolbox for MATLAB (SMTB). 29 | % 30 | % Permission is hereby granted, free of charge, to any person obtaining a copy 31 | % of this software and associated documentation files (the "Software"), to deal 32 | % in the Software without restriction, including without limitation the rights 33 | % to use, copy, modify, merge, publish, distribute, sublicense, and/or sell copies 34 | % of the Software, and to permit persons to whom the Software is furnished to do 35 | % so, subject to the following conditions: 36 | % 37 | % The above copyright notice and this permission notice shall be included in all 38 | % copies or substantial portions of the Software. 39 | % 40 | % THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 41 | % IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS 42 | % FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR 43 | % COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER 44 | % IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN 45 | % CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. 46 | % 47 | % https://github.com/petercorke/spatial-math 48 | 49 | function TR = trnorm(T) 50 | 51 | assert(ishomog(T) || isrot(T), 'SMTB:trnorm:badarg', 'expecting 3x3xN or 4x4xN hom xform'); 52 | 53 | if ndims(T) == 3 54 | % recurse for transform sequence 55 | n = size(T); 56 | TR = zeros(n); 57 | for i=1:n(3) 58 | TR(:,:,i) = trnorm(T(:,:,i)); 59 | end 60 | return 61 | end 62 | 63 | o = T(1:3,2); a = T(1:3,3); 64 | n = cross(o, a); % N = O x A 65 | o = cross(a, n); % O = A x N 66 | R = [unit(n) unit(o) unit(a)]; 67 | 68 | if ishomog(T) 69 | TR = rt2tr( R, T(1:3,4) ); 70 | elseif isrot(T) 71 | TR = R; 72 | end 73 | 74 | -------------------------------------------------------------------------------- /trot2.m: -------------------------------------------------------------------------------- 1 | %TROT2 SE(2) rotation matrix 2 | % 3 | % T = TROT2(THETA) is a homogeneous transformation (3x3) representing a rotation 4 | % of THETA radians. 5 | % 6 | % T = TROT2(THETA, 'deg') as above but THETA is in degrees. 7 | % 8 | % Notes:: 9 | % - Translational component is zero. 10 | % 11 | % See also rot2, transl2, ishomog2, trplot2, trotx, troty, trotz, SE2. 12 | 13 | %## 2d homogeneous rotation 14 | 15 | % Copyright (C) 1993-2019 Peter I. Corke 16 | % 17 | % This file is part of The Spatial Math Toolbox for MATLAB (SMTB). 18 | % 19 | % Permission is hereby granted, free of charge, to any person obtaining a copy 20 | % of this software and associated documentation files (the "Software"), to deal 21 | % in the Software without restriction, including without limitation the rights 22 | % to use, copy, modify, merge, publish, distribute, sublicense, and/or sell copies 23 | % of the Software, and to permit persons to whom the Software is furnished to do 24 | % so, subject to the following conditions: 25 | % 26 | % The above copyright notice and this permission notice shall be included in all 27 | % copies or substantial portions of the Software. 28 | % 29 | % THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 30 | % IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS 31 | % FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR 32 | % COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER 33 | % IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN 34 | % CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. 35 | % 36 | % https://github.com/petercorke/spatial-math 37 | 38 | function T = trot2(t, varargin) 39 | T = [rot2(t, varargin{:}) [0 0]'; 0 0 1]; 40 | -------------------------------------------------------------------------------- /trotx.m: -------------------------------------------------------------------------------- 1 | %TROTX SE(3) rotation about X axis 2 | % 3 | % T = TROTX(THETA) is a homogeneous transformation (4x4) representing a rotation 4 | % of THETA radians about the x-axis. 5 | % 6 | % T = TROTX(THETA, 'deg') as above but THETA is in degrees. 7 | % 8 | % Notes:: 9 | % - Translational component is zero. 10 | % 11 | % See also rotx, troty, trotz, trot2, SE3.Rx. 12 | 13 | %## 3d rotation 14 | 15 | % Copyright (C) 1993-2019 Peter I. Corke 16 | % 17 | % This file is part of The Spatial Math Toolbox for MATLAB (SMTB). 18 | % 19 | % Permission is hereby granted, free of charge, to any person obtaining a copy 20 | % of this software and associated documentation files (the "Software"), to deal 21 | % in the Software without restriction, including without limitation the rights 22 | % to use, copy, modify, merge, publish, distribute, sublicense, and/or sell copies 23 | % of the Software, and to permit persons to whom the Software is furnished to do 24 | % so, subject to the following conditions: 25 | % 26 | % The above copyright notice and this permission notice shall be included in all 27 | % copies or substantial portions of the Software. 28 | % 29 | % THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 30 | % IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS 31 | % FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR 32 | % COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER 33 | % IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN 34 | % CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. 35 | % 36 | % https://github.com/petercorke/spatial-math 37 | 38 | function T = trotx(t, varargin) 39 | T = [rotx(t, varargin{:}) [0 0 0]'; 0 0 0 1]; 40 | -------------------------------------------------------------------------------- /troty.m: -------------------------------------------------------------------------------- 1 | %troty SE(3) rotation about Y axis 2 | % 3 | % T = troty(THETA) is a homogeneous transformation (4x4) representing a rotation 4 | % of THETA radians about the y-axis. 5 | % 6 | % T = troty(THETA, 'deg') as above but THETA is in degrees. 7 | % 8 | % Notes:: 9 | % - Translational component is zero. 10 | % 11 | % See also roty, trotx, trotz, trot2, SE3.Ry. 12 | 13 | %## 3d homogeneous rotation 14 | 15 | 16 | % Copyright (C) 1993-2019 Peter I. Corke 17 | % 18 | % This file is part of The Spatial Math Toolbox for MATLAB (SMTB). 19 | % 20 | % Permission is hereby granted, free of charge, to any person obtaining a copy 21 | % of this software and associated documentation files (the "Software"), to deal 22 | % in the Software without restriction, including without limitation the rights 23 | % to use, copy, modify, merge, publish, distribute, sublicense, and/or sell copies 24 | % of the Software, and to permit persons to whom the Software is furnished to do 25 | % so, subject to the following conditions: 26 | % 27 | % The above copyright notice and this permission notice shall be included in all 28 | % copies or substantial portions of the Software. 29 | % 30 | % THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 31 | % IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS 32 | % FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR 33 | % COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER 34 | % IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN 35 | % CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. 36 | % 37 | % https://github.com/petercorke/spatial-math 38 | 39 | function T = troty(t, varargin) 40 | T = [roty(t, varargin{:}) [0 0 0]'; 0 0 0 1]; 41 | -------------------------------------------------------------------------------- /trotz.m: -------------------------------------------------------------------------------- 1 | %trotz SE(3) rotation about Z axis 2 | % 3 | % T = trotz(THETA) is a homogeneous transformation (4x4) representing a rotation 4 | % of THETA radians about the z-axis. 5 | % 6 | % T = trotz(THETA, 'deg') as above but THETA is in degrees. 7 | % 8 | % Notes:: 9 | % - Translational component is zero. 10 | % 11 | % See also rotz, trotx, troty, trot2, SE3.Rz. 12 | 13 | %## 3d homogeneous rotation 14 | 15 | % Copyright (C) 1993-2019 Peter I. Corke 16 | % 17 | % This file is part of The Spatial Math Toolbox for MATLAB (SMTB). 18 | % 19 | % Permission is hereby granted, free of charge, to any person obtaining a copy 20 | % of this software and associated documentation files (the "Software"), to deal 21 | % in the Software without restriction, including without limitation the rights 22 | % to use, copy, modify, merge, publish, distribute, sublicense, and/or sell copies 23 | % of the Software, and to permit persons to whom the Software is furnished to do 24 | % so, subject to the following conditions: 25 | % 26 | % The above copyright notice and this permission notice shall be included in all 27 | % copies or substantial portions of the Software. 28 | % 29 | % THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 30 | % IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS 31 | % FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR 32 | % COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER 33 | % IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN 34 | % CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. 35 | % 36 | % https://github.com/petercorke/spatial-math 37 | 38 | function T = trotz(t, varargin) 39 | T = [rotz(t, varargin{:}) [0 0 0]'; 0 0 0 1]; 40 | -------------------------------------------------------------------------------- /trscale.m: -------------------------------------------------------------------------------- 1 | %TRSCALE Homogeneous transformation for pure scale 2 | % 3 | % T = TRSCALE(S) is a homogeneous transform (4x4) corresponding to a pure 4 | % scale change. If S is a scalar the same scale factor is used for x,y,z, 5 | % else it can be a 3-vector specifying scale in the x-, y- and 6 | % z-directions. 7 | % 8 | % Note:: 9 | % - This matrix does not belong to SE(3) and if compounded with 10 | % any SE(3) matrix the result will not be in SE(3). 11 | % 12 | 13 | %## 3d homogeneous 14 | 15 | % Copyright (C) 1993-2019 Peter I. Corke 16 | % 17 | % This file is part of The Spatial Math Toolbox for MATLAB (SMTB). 18 | % 19 | % Permission is hereby granted, free of charge, to any person obtaining a copy 20 | % of this software and associated documentation files (the "Software"), to deal 21 | % in the Software without restriction, including without limitation the rights 22 | % to use, copy, modify, merge, publish, distribute, sublicense, and/or sell copies 23 | % of the Software, and to permit persons to whom the Software is furnished to do 24 | % so, subject to the following conditions: 25 | % 26 | % The above copyright notice and this permission notice shall be included in all 27 | % copies or substantial portions of the Software. 28 | % 29 | % THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 30 | % IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS 31 | % FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR 32 | % COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER 33 | % IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN 34 | % CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. 35 | % 36 | % https://github.com/petercorke/spatial-math 37 | 38 | function t = trscale(sx, sy, sz) 39 | 40 | if length(sx) > 1 41 | s = sx; 42 | else 43 | if nargin == 1 44 | s = [sx sx sx]; 45 | else 46 | s = [sx sy sz]; 47 | end 48 | end 49 | t = r2t(diag(s)); 50 | -------------------------------------------------------------------------------- /unit.m: -------------------------------------------------------------------------------- 1 | %UNIT Unitize a vector 2 | % 3 | % VN = UNIT(V) is a unit-vector parallel to V. 4 | % 5 | % Note:: 6 | % - Reports error for the case where V is non-symbolic and norm(V) is zero 7 | 8 | % Copyright (C) 1993-2019 Peter I. Corke 9 | % 10 | % This file is part of The Spatial Math Toolbox for MATLAB (SMTB). 11 | % 12 | % Permission is hereby granted, free of charge, to any person obtaining a copy 13 | % of this software and associated documentation files (the "Software"), to deal 14 | % in the Software without restriction, including without limitation the rights 15 | % to use, copy, modify, merge, publish, distribute, sublicense, and/or sell copies 16 | % of the Software, and to permit persons to whom the Software is furnished to do 17 | % so, subject to the following conditions: 18 | % 19 | % The above copyright notice and this permission notice shall be included in all 20 | % copies or substantial portions of the Software. 21 | % 22 | % THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 23 | % IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS 24 | % FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR 25 | % COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER 26 | % IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN 27 | % CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. 28 | % 29 | % https://github.com/petercorke/spatial-math 30 | 31 | function u = unit(v) 32 | n = norm(v, 'fro'); 33 | assert( isa(v, 'sym') || n > eps , 'SMTB:unit:zero_norm', 'vector has zero norm'); 34 | 35 | u = v / n; 36 | end 37 | -------------------------------------------------------------------------------- /unit_test/PGraphTest.m: -------------------------------------------------------------------------------- 1 | function tests = PGraphTest 2 | tests = functiontests(localfunctions); 3 | end 4 | function PGraph2_test(testCase) 5 | 6 | stream = RandStream.getGlobalStream; 7 | stream.reset() 8 | 9 | g = PGraph(2); 10 | 11 | % add first component, nodes 1-5 12 | g.add_node( rand(2,1)); 13 | g.add_node( rand(2,1)); 14 | g.add_node( rand(2,1)); 15 | g.add_node( rand(2,1)); 16 | g.add_node( rand(2,1)); 17 | 18 | g.add_edge(1,2); 19 | g.add_edge(1,3); 20 | g.add_edge(1,4); 21 | g.add_edge(2,3); 22 | g.add_edge(2,4); 23 | g.add_edge(4,5); 24 | 25 | verifyEqual(testCase, g.n, 5); 26 | verifyEqual(testCase, g.ne, 6); 27 | verifyEqual(testCase, g.nc, 1); 28 | verifyEqual(testCase, g.neighbours(2), [3 4 1]); 29 | z = g.coord(1); 30 | %verifyEqual(testCase, g.distance(1,2), 0.6878, 'absTol',1e-4) 31 | % verifyEqual(testCase, g.distances([0 0]), ... 32 | % [0.6137 0.6398 0.9222 1.2183 1.3593], 'absTol',1e-4) 33 | 34 | D = g.degree(); 35 | I = g.incidence(); 36 | A = g.adjacency(); 37 | L = g.laplacian(); 38 | 39 | verifyEqual(testCase, D, diag([3 3 2 3 1])); 40 | verifyEqual(testCase, sum(I), [2 2 2 2 2 2]); 41 | verifyEqual(testCase, sum(I'), [3 3 2 3 1]); 42 | verifyEqual(testCase, max(max(A-A')), 0); 43 | 44 | verifyEqual(testCase, g.edgedir(1,3), 1); 45 | verifyEqual(testCase, g.edgedir(3,1), -1); 46 | verifyEqual(testCase, g.edgedir(2,5), 0); 47 | verifyEqual(testCase, g.neighbours_d(2), [-1 3 4]); 48 | 49 | 50 | s = g.char(); 51 | 52 | % test node data 53 | g.setvdata(1, [1 2 3]); 54 | d = g.vdata(1); 55 | verifyEqual(testCase, d, [1 2 3]); 56 | 57 | % test edge data 58 | g.setedata(1, [4 5 6 7]); 59 | d = g.edata(1); 60 | verifyEqual(testCase, d, [4 5 6 7]); 61 | 62 | % add second component, nodes 6-8 63 | 64 | g.add_node( rand(2,1)); 65 | g.add_node( rand(2,1), 6, 1); 66 | g.add_node( rand(2,1), 7); 67 | g.add_edge(6,7); 68 | g.add_edge(6,8); 69 | 70 | verifyEqual(testCase, g.nc, 2); 71 | 72 | clf 73 | g.plot('labels') 74 | g.plot('edgelabels') 75 | g.plot('componentcolor') 76 | 77 | [path,cost]=g.Astar(3,5); 78 | g.highlight_path(path); 79 | %verifyEqual(testCase, path, [3 2 4 5]); 80 | %verifyEqual(testCase, cost, 2.1536, 'absTol',1e-4) 81 | 82 | %verifyEqual(testCase, g.closest([0 0]), 4); 83 | 84 | g.setcost(1, 99); 85 | verifyEqual(testCase, g.cost(1), 99); 86 | 87 | g.Astar(3, 5); 88 | %verifyEqual(testCase, path, [3 2 4 5]); 89 | end 90 | 91 | function PGraph3_test(testCase) 92 | 93 | stream = RandStream.getGlobalStream; 94 | stream.reset() 95 | 96 | g = PGraph(3); 97 | 98 | g.add_node( rand(3,1)); 99 | g.add_node( rand(3,1)); 100 | g.add_node( rand(3,1)); 101 | g.add_node( rand(3,1)); 102 | g.add_node( rand(3,1)); 103 | 104 | g.add_edge(1,2); 105 | g.add_edge(1,3); 106 | g.add_edge(1,4); 107 | g.add_edge(2,3); 108 | g.add_edge(2,4); 109 | g.add_edge(4,5); 110 | 111 | clf 112 | g.plot('labels') 113 | g.plot('edgelabels') 114 | end 115 | -------------------------------------------------------------------------------- /unit_test/README.txt: -------------------------------------------------------------------------------- 1 | To run unit tests from MATLAB: 2 | 3 | 4 | Ensure that SMTB is on your path, then 5 | 6 | ```matlab 7 | >> RunAllTests 8 | ``` 9 | 10 | will also generate a [Cobertura](http://cobertura.github.io/cobertura/) format coverage report which is viewable using services like [codecov](https://codecov.io/gh/petercorke/spatial-math). 11 | 12 | This file is invoked from Travis CI via the `matlab-runner`. 13 | -------------------------------------------------------------------------------- /unit_test/RTBPoseTest.m: -------------------------------------------------------------------------------- 1 | function tests = RTBPoseTest 2 | tests = functiontests(localfunctions); 3 | clc 4 | end 5 | % we will assume that the primitives rotx,trotx, etc. all work 6 | 7 | 8 | function constructor_test(testCase) 9 | q = Quaternion([1 2 3 4]); 10 | verifyEqual(testCase, q.double(),[1 2 3 4],'absTol',1e-4); 11 | 12 | % native 13 | end 14 | 15 | function staticconstructors(testCase) 16 | end 17 | 18 | 19 | function multiply_test(testCase) 20 | % scalar x scalar 21 | 22 | % vector x vector 23 | 24 | % scalar x vector 25 | 26 | % vector x scalar 27 | end 28 | 29 | function divide(testCase) 30 | 31 | % scalar x scalar 32 | 33 | % vector x vector 34 | 35 | % scalar x vector 36 | 37 | % vector x scalar 38 | end 39 | 40 | function angle(testCase) 41 | end 42 | 43 | function conversions(testCase) 44 | % double 45 | % tr2rt convert to rotation matrix and translation vector 46 | % t2r convert to rotation matrix 47 | % trprint print single line representation 48 | % trprint2 print single line representation 49 | % trplot plot coordinate frame 50 | % trplot2 plot coordinate frame 51 | % tranimate aimate coordinate frame 52 | end 53 | 54 | function interp_test(tc) 55 | 56 | interp1(tc, 'SO2'); 57 | interp1(tc, 'SE2'); 58 | interp1(tc, 'SO3'); 59 | interp1(tc, 'SE3') 60 | 61 | function interp1(tc, cn) 62 | a1 = eval(sprintf('%s.rand', cn)); 63 | a2 = eval(sprintf('%s.rand', cn)); 64 | 65 | verifyEqual(tc, double(a1.interp(a2, 0)), double(a1), 'absTol',1e-10); 66 | verifyEqual(tc, double(a1.interp(a2, 1)), double(a2), 'absTol',1e-10); 67 | 68 | errmsg = sprintf('SMTB:%s:interp:badarg', cn); 69 | verifyError(tc, @() a1.interp(a2, -1), errmsg ); 70 | verifyError(tc, @() a1.interp(a2, 1.2), errmsg ); 71 | end 72 | end 73 | 74 | function tests_test(tc) 75 | a = SO2; 76 | verifyFalse(tc, isrot(a) ); 77 | verifyTrue(tc, isrot2(a) ); 78 | verifyFalse(tc, ishomog(a) ); 79 | verifyFalse(tc, ishomog2(a) ); 80 | 81 | verifyFalse(tc, isSE(a) ); 82 | verifyEqual(tc, dim(a), 2); 83 | verifyFalse(tc, issym(a) ); 84 | 85 | a = SE2; 86 | verifyFalse(tc, isrot(a) ); 87 | verifyFalse(tc, isrot2(a) ); 88 | verifyFalse(tc, ishomog(a) ); 89 | verifyTrue(tc, ishomog2(a) ); 90 | 91 | verifyTrue(tc, isSE(a) ); 92 | verifyEqual(tc, dim(a), 3); 93 | verifyFalse(tc, issym(a) ); 94 | 95 | a = SO3; 96 | verifyTrue(tc, isrot(a) ); 97 | verifyFalse(tc, isrot2(a) ); 98 | verifyFalse(tc, ishomog(a) ); 99 | verifyFalse(tc, ishomog2(a) ); 100 | 101 | verifyFalse(tc, isSE(a) ); 102 | verifyEqual(tc, dim(a), 3); 103 | verifyFalse(tc, issym(a) ); 104 | 105 | a = SE3; 106 | verifyFalse(tc, isrot(a) ); 107 | verifyFalse(tc, isrot2(a) ); 108 | verifyTrue(tc, ishomog(a) ); 109 | verifyFalse(tc, ishomog2(a) ); 110 | 111 | verifyTrue(tc, isSE(a) ); 112 | verifyEqual(tc, dim(a), 4); 113 | verifyFalse(tc, issym(a) ); 114 | 115 | syms theta x y z 116 | a = SE3.Rx(theta); 117 | verifyTrue(tc, issym(a) ); 118 | 119 | a = SE3(x, y, z); 120 | verifyTrue(tc, issym(a) ); 121 | 122 | a = SO2(theta); 123 | verifyTrue(tc, issym(a) ); 124 | 125 | a = SE2(x, y, theta); 126 | verifyTrue(tc, issym(a) ); 127 | end 128 | -------------------------------------------------------------------------------- /unit_test/RunAllTests.m: -------------------------------------------------------------------------------- 1 | % run the tests for Travis CI 2 | 3 | import matlab.unittest.plugins.CodeCoveragePlugin 4 | import matlab.unittest.plugins.codecoverage.CoberturaFormat 5 | import matlab.unittest.TestRunner 6 | 7 | suite = testsuite('IncludeSubfolders', false); 8 | runner = TestRunner.withTextOutput; 9 | 10 | % add a coverage report 11 | reportFile = fullfile('..', 'coverage.xml'); 12 | reportFormat = CoberturaFormat(reportFile); 13 | plugin = CodeCoveragePlugin.forFolder('..', 'Producing',reportFormat); 14 | runner.addPlugin(plugin); 15 | 16 | %% setup the path 17 | addpath .. 18 | 19 | %% Run all unit tests in my repository 20 | 21 | % Run all unit tests in my repository. 22 | results = runner.run(suite); 23 | 24 | % Assert no tests failed. 25 | assert(all(~[results.Failed])); 26 | 27 | %% Build the toolbox distribution file 28 | fprintf('---------------------------------- Build the MLTBX file ------------------------------------\n') 29 | cd .. 30 | matlab.addons.toolbox.packageToolbox('PackageToolbox.prj') 31 | -------------------------------------------------------------------------------- /unit_test/TODO: -------------------------------------------------------------------------------- 1 | pose classes: test with symbolic constructors, multidimensional matrices, conversions, .R, .t 2 | 3 | graft all the RPY formats into tests 4 | 5 | test skew, vex 6 | 7 | Plucker 8 | -------------------------------------------------------------------------------- /unit_test/TrajectoryTest.m: -------------------------------------------------------------------------------- 1 | %% This is for testing the Trajectory Generation functions in the robotics Toolbox 2 | function tests = TrajectoryTest 3 | tests = functiontests(localfunctions); 4 | clc 5 | end 6 | 7 | function teardownOnce(tc) 8 | close all 9 | end 10 | 11 | function trinterp2_test(tc) 12 | T0 = transl2(1, 2) * trot2(20, 'deg'); 13 | T1 = transl2(3, 6) * trot2(60, 'deg'); 14 | Tm = transl2(2, 4) * trot2(40, 'deg'); 15 | 16 | T = trinterp2(T0, T1, [0.5 0 1]); 17 | tc.verifyEqual(size(T), [3 3 3]); 18 | tc.verifyEqual(T(:,:,1), Tm, 'abstol', 1e-10); 19 | tc.verifyEqual(T(:,:,2), T0, 'abstol', 1e-10); 20 | tc.verifyEqual(T(:,:,3), T1, 'abstol', 1e-10); 21 | end 22 | 23 | %%TODO trinterp 24 | 25 | -------------------------------------------------------------------------------- /unit_test/data/20mm_cube_ascii.stl: -------------------------------------------------------------------------------- 1 | solid model 2 | facet normal 0.0 0.0 -1.0 3 | outer loop 4 | vertex 20.0 0.0 0.0 5 | vertex 0.0 -20.0 0.0 6 | vertex 0.0 0.0 0.0 7 | endloop 8 | endfacet 9 | facet normal 0.0 0.0 -1.0 10 | outer loop 11 | vertex 0.0 -20.0 0.0 12 | vertex 20.0 0.0 0.0 13 | vertex 20.0 -20.0 0.0 14 | endloop 15 | endfacet 16 | facet normal -0.0 -1.0 -0.0 17 | outer loop 18 | vertex 20.0 -20.0 20.0 19 | vertex 0.0 -20.0 0.0 20 | vertex 20.0 -20.0 0.0 21 | endloop 22 | endfacet 23 | facet normal -0.0 -1.0 -0.0 24 | outer loop 25 | vertex 0.0 -20.0 0.0 26 | vertex 20.0 -20.0 20.0 27 | vertex 0.0 -20.0 20.0 28 | endloop 29 | endfacet 30 | facet normal 1.0 0.0 0.0 31 | outer loop 32 | vertex 20.0 0.0 0.0 33 | vertex 20.0 -20.0 20.0 34 | vertex 20.0 -20.0 0.0 35 | endloop 36 | endfacet 37 | facet normal 1.0 0.0 0.0 38 | outer loop 39 | vertex 20.0 -20.0 20.0 40 | vertex 20.0 0.0 0.0 41 | vertex 20.0 0.0 20.0 42 | endloop 43 | endfacet 44 | facet normal -0.0 -0.0 1.0 45 | outer loop 46 | vertex 20.0 -20.0 20.0 47 | vertex 0.0 0.0 20.0 48 | vertex 0.0 -20.0 20.0 49 | endloop 50 | endfacet 51 | facet normal -0.0 -0.0 1.0 52 | outer loop 53 | vertex 0.0 0.0 20.0 54 | vertex 20.0 -20.0 20.0 55 | vertex 20.0 0.0 20.0 56 | endloop 57 | endfacet 58 | facet normal -1.0 0.0 0.0 59 | outer loop 60 | vertex 0.0 0.0 20.0 61 | vertex 0.0 -20.0 0.0 62 | vertex 0.0 -20.0 20.0 63 | endloop 64 | endfacet 65 | facet normal -1.0 0.0 0.0 66 | outer loop 67 | vertex 0.0 -20.0 0.0 68 | vertex 0.0 0.0 20.0 69 | vertex 0.0 0.0 0.0 70 | endloop 71 | endfacet 72 | facet normal -0.0 1.0 0.0 73 | outer loop 74 | vertex 0.0 0.0 20.0 75 | vertex 20.0 0.0 0.0 76 | vertex 0.0 0.0 0.0 77 | endloop 78 | endfacet 79 | facet normal -0.0 1.0 0.0 80 | outer loop 81 | vertex 20.0 0.0 0.0 82 | vertex 0.0 0.0 20.0 83 | vertex 20.0 0.0 20.0 84 | endloop 85 | endfacet 86 | endsolid model 87 | -------------------------------------------------------------------------------- /unit_test/data/20mm_cube_binary.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/petercorke/spatialmath-matlab/6eeff4a79f14286705560b84f1fe72e0b7e0e7f7/unit_test/data/20mm_cube_binary.stl -------------------------------------------------------------------------------- /unit_test/failedtests.m: -------------------------------------------------------------------------------- 1 | function fails = failedtests(results) 2 | 3 | tr = table(results); 4 | 5 | f = string( tr(tr.Failed,:).Name ); 6 | 7 | f = arrayfun( @(x) extractBefore(x, "/"), f); 8 | 9 | fails = unique(f); 10 | end -------------------------------------------------------------------------------- /unit_test/featherstone_test/ID.m: -------------------------------------------------------------------------------- 1 | % inverse dynamics (recursive Newton-Euler) using spatial vector notation 2 | function tau = ID( model, q, qd, qdd ) 3 | 4 | a_grav = SpatialAcceleration([0;0;-9.81;0;0;0]); 5 | 6 | for i = 1:model.NB 7 | [ XJ, S(:,i) ] = jcalc( model.pitch(i), q(i) ); 8 | XJ 9 | S(:,i) 10 | vJ = SpatialVelocity(S(:,i)*qd(i)); 11 | Xup(i) = XJ*model.Xtree(i); 12 | if model.parent(i) == 0 13 | v(i) = vJ; 14 | a(i) = Xup(i)*(-a_grav) + SpatialAcceleration(S(:,i)*qdd(i)); 15 | a 16 | else 17 | v(i) = Xup(i)*v(model.parent(i)) + vJ; 18 | a(i) = Xup(i)*a(model.parent(i)) ... 19 | + SpatialAcceleration(S(:,i)*qdd(i)) ... 20 | + cross(v(i),vJ); 21 | end 22 | f(i) = model.I(i)*a(i) + cross( v(i), model.I(i)*v(i) ); 23 | end 24 | 25 | v 26 | a 27 | f 28 | 29 | for i = model.NB:-1:1 30 | tau(i,1) = S(:,i)' * double(f(i)); 31 | if model.parent(i) ~= 0 32 | f(model.parent(i)) = f(model.parent(i)) + Xup(i)*f(i); 33 | end 34 | end 35 | end 36 | 37 | function [Xj,S] = jcalc( pitch, q ) %FIXED VW ORDER 38 | 39 | % jcalc Calculate joint transform and motion subspace. 40 | % [Xj,S]=jcalc(pitch,q) calculates the joint transform and motion subspace 41 | % matrices for a revolute (pitch==0), prismatic (pitch==inf) or helical 42 | % (pitch==any other value) joint. For revolute and helical joints, q is 43 | % the joint angle. For prismatic joints, q is the linear displacement. 44 | 45 | if pitch == 0 % revolute joint 46 | Xj = Twist(SE3.Rz(q)); 47 | S = [0;0;0;0;0;1]; 48 | elseif pitch == inf % prismatic joint 49 | Xj = Twist(SE3([0 0 q])); 50 | S = [0;0;1;0;0;0]; 51 | else % helical joint 52 | Xj = Twist(SE3.Rz(q) * SE3([0 0 q*pitch])); 53 | S = [0;0;pitch0;0;1;]; 54 | end 55 | end 56 | -------------------------------------------------------------------------------- /unit_test/featherstone_test/README.md: -------------------------------------------------------------------------------- 1 | assume that spatial math is in your path 2 | 3 | ID.m inverse dynamics using spatial vector notation 4 | test_roy.m test script for ID.m 5 | 6 | compare.m compare results of RTB RNE and spatial vector for planar 2 link, uses: 7 | idyn_roy.m ID for spatial vector 8 | idyn_rtb.m ID for RNE 9 | -------------------------------------------------------------------------------- /unit_test/featherstone_test/compare.m: -------------------------------------------------------------------------------- 1 | q = [90 -90] * pi/180 2 | qd = [2 -3] 3 | qdd = [3 4] 4 | 5 | [idyn_rtb(q, qd, qdd); idyn_roy(q, qd, qdd)] -------------------------------------------------------------------------------- /unit_test/featherstone_test/idyn_roy.m: -------------------------------------------------------------------------------- 1 | %% create robot model 2 | function tau = idyn_roy(q, qd, qdd) 3 | % copied from planarN 4 | n = 2; 5 | robot.NB = n; 6 | robot.parent = [0:n-1]; 7 | robot.pitch = zeros(1,n); % revolute joints 8 | 9 | 10 | robot.Xtree(1) = Twist(inv(SE3.Rx(pi/2))); % joint 1 at origin, z in horizontal plane 11 | robot.Xtree(2) = Twist(inv(SE3([1 0 0]))); 12 | 13 | robot.I(1) = SpatialInertia( 1, [0.5 0 0], diag([0 0 0]) ); 14 | robot.I(2) = SpatialInertia( 1, [0.5 0 0], diag([0 0 0]) ); 15 | 16 | robot.Xtree(1) 17 | %robot.I(1) 18 | robot.Xtree(2) 19 | %robot.I(2) 20 | 21 | %% compute inverse dynamics 22 | tau = ID( robot, q, qd, qdd)' %, f_ext, grav_accn ) 23 | end -------------------------------------------------------------------------------- /unit_test/featherstone_test/idyn_rtb.m: -------------------------------------------------------------------------------- 1 | function tau = idyn_rtb(q, qd, qdd) 2 | twolink = []; 3 | mdl_twolink 4 | 5 | twolink.fast = false 6 | 7 | tau = twolink.rne(q, qd, qdd) 8 | end -------------------------------------------------------------------------------- /unit_test/featherstone_test/test_roy.m: -------------------------------------------------------------------------------- 1 | n = 2; 2 | clear robot 3 | robot.NB = n; 4 | robot.parent = [0:n-1]; 5 | robot.pitch = zeros(1,n); % revolute joints 6 | 7 | robot.Xtree(1) = Twist(inv(SE3.Rx(pi/2))); % joint 1 at origin, z in horizontal plane 8 | robot.Xtree(2) = Twist(inv(SE3([1 0 0]))); 9 | 10 | robot.I(1) = SpatialInertia( 1, [0.5 0 0], diag([0 0 0]) ); 11 | robot.I(2) = SpatialInertia( 1, [0.5 0 0], diag([0 0 0]) ); 12 | 13 | q = [0 0]; qd = [0 0]; qdd = [0 0]; 14 | 15 | tau = ID( robot, q, qd, qdd)' 16 | 17 | -------------------------------------------------------------------------------- /unit_test/featherstone_test/twist_test.m: -------------------------------------------------------------------------------- 1 | a = Twist(SE3([1 2 3])) 2 | %a.SE 3 | % a.Ad 4 | x = inv(a.SE); 5 | x.Ad 6 | 7 | Xtrans([1 2 3]) 8 | 9 | a = Twist(SE3.Rx(pi/2)) 10 | %a.SE 11 | x = inv(a.SE); 12 | x.Ad 13 | 14 | Xrotx(pi/2) -------------------------------------------------------------------------------- /unit_test/old/arange.m: -------------------------------------------------------------------------------- 1 | function r = arange(start, stop, step) 2 | r = [start:step:stop]'; 3 | -------------------------------------------------------------------------------- /unit_test/old/class_pgraph.m: -------------------------------------------------------------------------------- 1 | %clear classes 2 | randinit 3 | clf 4 | g = PGraph(); 5 | g 6 | 7 | v = g.add_node( rand(2,1) ); 8 | v 9 | v = g.add_node( rand(2,1) ); 10 | v = g.add_node( rand(2,1) ); 11 | v = g.add_node( rand(2,1) ); 12 | v = g.add_node( rand(2,1) ); 13 | v 14 | g 15 | g.plot() 16 | pause 17 | 18 | % add nodes 1-4 19 | g.add_edge(1,2); 20 | g.add_edge(1,3); 21 | g.add_edge(1,4); 22 | g.add_edge(2,3); 23 | g.add_edge(2,4); 24 | g.add_edge(4,5); 25 | 26 | g 27 | g.plot('labels') 28 | 29 | pause 30 | 31 | g.neighbours(1) 32 | g.neighbours(4) 33 | 34 | g.neighbours2(1) 35 | g.neighbours2(4) 36 | 37 | pause 38 | 39 | % add nodes 5-8, all connected to node 5 40 | v1 = g.add_node( rand(2,1) ); 41 | v = g.add_node( rand(2,1), v1 ); 42 | v = g.add_node( rand(2,1), v1 ); 43 | v = g.add_node( rand(2,1), v1 ); 44 | g 45 | g.plot('labels') 46 | pause 47 | 48 | g.add_edge(4,7) 49 | g 50 | g.plot() 51 | pause 52 | 53 | % test path finding 54 | g.goal(7) 55 | g.path(1) 56 | 57 | -------------------------------------------------------------------------------- /unit_test/old/dynamics.m: -------------------------------------------------------------------------------- 1 | q1 = mat(ones(1,6)) 2 | 3 | % test the RNE primitive for DH 4 | p560 5 | rne(p560, qz, qz, qz) 6 | rne(p560, qz, qz, qz, [1,2,3]) 7 | rne(p560, qz, qz, qz, [0,0,9.81], [1,2,3,0,0,0]) 8 | rne(p560, qz, 0.2*q1, qz) 9 | rne(p560, qz, 0.2*q1, 0.2*q1) 10 | 11 | z = [qr qz qz] 12 | rne(p560, z, [1,2,3]) 13 | rne(p560, z, [0,0,9.81], [1,2,3,0,0,0]) 14 | rne(p560, [z;z;z]) 15 | 16 | % test the RNE primitive for MDH 17 | puma560akb 18 | rne(p560m, qz, qz, qz) 19 | rne(p560m, qz, qz, qz, [1,2,3]) 20 | rne(p560m, qz, qz, qz, [0,0,9.81], [1,2,3,0,0,0]) 21 | rne(p560m, qz, 0.2*q1, qz) 22 | rne(p560m, qz, 0.2*q1, 0.2*q1) 23 | 24 | z = [qr qz qz] 25 | rne(p560m, z, [1,2,3]) 26 | rne(p560m, z, [0,0,9.81], [1,2,3,0,0,0]) 27 | rne(p560m, [z;z;z]) 28 | 29 | % at zero pose 30 | gravload(p560, qz) 31 | gravload(p560, qz, [9,0,0]) 32 | gravload(p560, [qz;qz;qz]) 33 | 34 | inertia(p560, qz) 35 | inertia(p560, [qz;qr]) 36 | 37 | accel(p560, qz, 0.2*q1, 0.5*q1) 38 | 39 | % need to pick a non-singular configuration for cinertia() 40 | cinertia(p560, qn) 41 | 42 | coriolis(p560, qn, 0.5*q1) 43 | 44 | % along trajectory 45 | [q,qd,qdd] = jtraj(qz, qr, 20) 46 | rne(p560, q, qd, qdd) 47 | -------------------------------------------------------------------------------- /unit_test/old/jacobian.m: -------------------------------------------------------------------------------- 1 | % Test Jacobian primitives 2 | 3 | p560 4 | 5 | jacob0(p560, qz) 6 | jacob0(p560, qr) 7 | jacob0(p560, qn) 8 | 9 | jacobn(p560, qz) 10 | jacobn(p560, qr) 11 | jacobn(p560, qn) 12 | 13 | t = fkine(p560, qn) 14 | tr2jac(t) 15 | -------------------------------------------------------------------------------- /unit_test/old/kinematics.m: -------------------------------------------------------------------------------- 1 | mdl_p560 2 | 3 | % at zero pose 4 | t = p560.fkine(qz) 5 | t 6 | q = p560.ikine560(t) 7 | q 8 | p560.fkine(q) 9 | p560.ikine560(t, 'r') 10 | p560.ikine560(t, 'rn') 11 | 12 | p560.ikine(t) 13 | 14 | % at nominal pose 15 | qn 16 | t = p560.fkine(qn) 17 | t 18 | %q = ikine560(t) 19 | q 20 | p560.fkine(q) 21 | p560.ikine(t, [0, 0.7, 3, 0, 0.7, 0]) 22 | 23 | % along trajectory 24 | [q,qd,qdd] = jtraj(qz, qr, 20) 25 | fkine(q) 26 | 27 | t1 = p560.fkine(qz) 28 | t2 = p560.fkine(qr) 29 | traj = ctraj(t1, t2, 5) 30 | p560.ikine(traj) 31 | -------------------------------------------------------------------------------- /unit_test/old/leg.m: -------------------------------------------------------------------------------- 1 | dh = 'Rz(q1).Rx(q2).Ty(L1).Rx(q3).Tz(L2)'; 2 | dh = dhfactor(dh); 3 | dh 4 | dh.base 5 | dh.tool 6 | dh.dh 7 | dh.offset 8 | dh.command('leg') 9 | -------------------------------------------------------------------------------- /unit_test/old/loc_dr.m: -------------------------------------------------------------------------------- 1 | clear classes 2 | 3 | V = diag([0.005, 0.5*pi/180].^2); 4 | P0 = diag([0.005, 0.005, 0.001].^2); 5 | 6 | randinit 7 | v = Vehicle(V); 8 | r = RandomPath(v, 10); 9 | 10 | ekf = EKF(v, V, P0); 11 | 12 | randinit 13 | ekf.run(1000); 14 | 15 | f1 16 | clf 17 | ekf.plot_xy 18 | hold on 19 | v.plot_xy('r') 20 | grid on 21 | xyzlabel 22 | 23 | ekf.plot_ellipse([], 'g') 24 | 25 | p0 = ekf.plot_P; 26 | 27 | ekf = EKF(v, 2*V, P0); 28 | randinit 29 | ekf.run(1000); 30 | pb = ekf.plot_P; 31 | 32 | ekf = EKF(v, 0.5*V, P0); 33 | randinit 34 | ekf.run(1000); 35 | ps = ekf.plot_P; 36 | 37 | f2 38 | clf 39 | plot([p0 pb ps]); 40 | legend('1', '2', '0.5'); 41 | grid 42 | -------------------------------------------------------------------------------- /unit_test/old/loc_ekf.m: -------------------------------------------------------------------------------- 1 | clear classes 2 | 3 | fprintf('********************* DR test\n'); 4 | V = diag([0.005, 0.5*pi/180].^2); 5 | P0 = diag([0.005, 0.005, 0.001].^2); 6 | 7 | randinit 8 | v = Vehicle(V); 9 | v.add_driver( RandomPath(10) ); 10 | 11 | ekf = EKF(v, V, P0); 12 | 13 | randinit 14 | ekf.run(1000); 15 | 16 | f1 17 | clf 18 | ekf.plot_xy 19 | hold on 20 | v.plot_xy('r') 21 | grid on 22 | xyzlabel 23 | 24 | ekf.plot_ellipse([], 'g') 25 | 26 | f2 27 | ekf.plot_P() 28 | 29 | pause 30 | fprintf('********************* mapnav test\n'); 31 | W = diag([0.1, 1*pi/180].^2); 32 | P0 = diag([0.005, 0.005, 0.001].^2); 33 | V = diag([0.005, 0.5*pi/180].^2); 34 | 35 | randinit 36 | map = Map(20, 10); 37 | veh = Vehicle(V); 38 | veh.add_driver( RandomPath(10) ); 39 | sensor = RangeBearingSensor(veh, map, W); 40 | sensor.interval = 5; 41 | ekf = EKF(veh, W, P0, sensor, W); 42 | 43 | randinit 44 | ekf.run(1000); 45 | 46 | 47 | 48 | f1 49 | clf 50 | map.visualize() 51 | veh.plot_xy('b'); 52 | ekf.plot_xy('r'); 53 | ekf.plot_ellipse([], 'k') 54 | grid on 55 | xyzlabel 56 | 57 | f2 58 | clf 59 | ekf.plot_P() 60 | 61 | pause 62 | fprintf('********************* mapping test\n'); 63 | W = diag([0.1, 1*pi/180].^2); 64 | P0 = diag([0.005, 0.005, 0.001].^2); 65 | V = diag([0.005, 0.5*pi/180].^2); 66 | 67 | randinit 68 | map = Map(20, 10); 69 | veh = Vehicle(V); 70 | veh.add_driver( RandomPath(10) ); 71 | sensor = RangeBearingSensor(veh, map, W); 72 | sensor.interval = 5; 73 | ekf = EKF(veh, [], P0, sensor, W, []); 74 | ekf.verbose = true; 75 | 76 | randinit 77 | ekf.run(1000); 78 | 79 | f1 80 | clf 81 | map.visualize() 82 | veh.plot_xy('b'); 83 | ekf.plot_map('g'); 84 | grid on 85 | xyzlabel 86 | 87 | pause 88 | fprintf('********************* slam test\n'); 89 | W = diag([0.1, 1*pi/180].^2); 90 | P0 = diag([0.005, 0.005, 0.001].^2); 91 | V = diag([0.005, 0.5*pi/180].^2); 92 | 93 | randinit 94 | map = Map(20, 10); 95 | veh = Vehicle(V); 96 | veh.add_driver( RandomPath(10) ); 97 | sensor = RangeBearingSensor(veh, map, W); 98 | sensor.interval = 1; 99 | ekf = EKF(veh, V, P0, sensor, W, []); 100 | ekf.verbose = true; 101 | 102 | ekf.run(1000); 103 | 104 | f1 105 | clf 106 | map.visualize() 107 | veh.plot_xy('b'); 108 | ekf.plot_xy('r'); 109 | ekf.plot_ellipse([], 'k') 110 | grid on 111 | xyzlabel 112 | 113 | f2 114 | clf 115 | ekf.plot_P() 116 | 117 | f3 118 | map.visualize(); 119 | ekf.plot_map(5,'g'); 120 | -------------------------------------------------------------------------------- /unit_test/old/loc_map.m: -------------------------------------------------------------------------------- 1 | clear classes 2 | 3 | W = diag([0.1, 1*pi/180].^2); 4 | P0 = diag([0.005, 0.005, 0.001].^2); 5 | V = diag([0.005, 0.5*pi/180].^2); 6 | 7 | randinit 8 | map = Map(20, 10); 9 | veh = Vehicle(V); 10 | RandomPath(veh, map.dim); 11 | sensor = RangeBearingSensor(veh, map, W); 12 | sensor.interval = 5; 13 | ekf = EKF(veh, W, P0, sensor, W); 14 | 15 | randinit 16 | ekf.run(1000); 17 | 18 | 19 | 20 | f1 21 | clf 22 | map.visualize() 23 | veh.plot_xy('b'); 24 | ekf.plot_xy('r'); 25 | ekf.plot_ellipse([], 'k') 26 | grid on 27 | xyzlabel 28 | 29 | f2 30 | clf 31 | ekf.plot_P() 32 | -------------------------------------------------------------------------------- /unit_test/old/loc_map2.m: -------------------------------------------------------------------------------- 1 | clear classes 2 | 3 | W = diag([0.1, 1*pi/180].^2); 4 | P0 = diag([0.005, 0.005, 0.001].^2); 5 | V = diag([0.005, 0.5*pi/180].^2); 6 | 7 | randinit 8 | map = Map(20, 10); 9 | veh = Vehicle(V); 10 | RandomPath(veh, map.dim); 11 | sensor = RangeBearingSensor(veh, map, W); 12 | sensor.interval = 5; 13 | ekf = EKF(veh, [], P0, sensor, W, []); 14 | ekf.verbose = true; 15 | 16 | randinit 17 | ekf.run(1000); 18 | 19 | f1 20 | clf 21 | map.visualize() 22 | veh.plot_xy('b'); 23 | ekf.plot_map(); 24 | grid on 25 | xyzlabel 26 | -------------------------------------------------------------------------------- /unit_test/old/loc_pf.m: -------------------------------------------------------------------------------- 1 | clear 2 | clf 3 | randinit 4 | map = Map(20); 5 | 6 | W = diag([0.1, 1*pi/180].^2); 7 | v = Vehicle(W); 8 | v.add_driver( RandomPath(10) ); 9 | V = diag([0.005, 0.5*pi/180].^2); 10 | sensor = RangeBearingSensor(v, map, V); 11 | 12 | Q = diag([0.1, 0.1, 1*pi/180]).^2; 13 | L = diag([0.1 0.1]) 14 | pf = ParticleFilter(v, sensor, Q, L, 1000); 15 | pf.run(1000); 16 | pause 17 | plot(pf.std) 18 | xlabel('time step') 19 | ylabel('standard deviation') 20 | legend('x', 'y', '\theta') 21 | grid 22 | -------------------------------------------------------------------------------- /unit_test/old/loc_slam.m: -------------------------------------------------------------------------------- 1 | clear classes 2 | 3 | W = diag([0.1, 1*pi/180].^2); 4 | P0 = diag([0.005, 0.005, 0.001].^2); 5 | V = diag([0.005, 0.5*pi/180].^2); 6 | 7 | randinit 8 | map = Map(20, 10); 9 | veh = Vehicle(V); 10 | RandomPath(veh, map.dim); 11 | sensor = RangeBearingSensor(veh, map, W); 12 | sensor.interval = 5; 13 | ekf = EKF(veh, V, P0, sensor, W, []); 14 | ekf.verbose = true; 15 | 16 | randinit 17 | ekf.run(1000); 18 | 19 | f1 20 | clf 21 | map.visualize() 22 | veh.plot_xy('b'); 23 | ekf.plot_xy('r'); 24 | ekf.plot_ellipse([], 'k') 25 | grid on 26 | xyzlabel 27 | 28 | f2 29 | clf 30 | ekf.plot_P() 31 | -------------------------------------------------------------------------------- /unit_test/old/mat.m: -------------------------------------------------------------------------------- 1 | function x = mat(y) 2 | x = y; 3 | -------------------------------------------------------------------------------- /unit_test/old/nav_bug2.m: -------------------------------------------------------------------------------- 1 | function nav_bug2 2 | 3 | goal = [50,30]; 4 | start = [20, 10]; 5 | load map1 6 | 7 | b = bug2(map); 8 | b.goal = goal; 9 | 10 | b.path(start); 11 | p = b.path(start); 12 | b.visualize(p); 13 | -------------------------------------------------------------------------------- /unit_test/old/nav_dstar.m: -------------------------------------------------------------------------------- 1 | function nav_dstar 2 | 3 | % load the world 4 | load map1 5 | goal = [50,30]; 6 | start = [20, 10]; 7 | 8 | % create a planner 9 | ds = Dstar(map); 10 | ds 11 | 12 | % plan path to goal 13 | ds.plan(goal); 14 | ds 15 | 16 | % execute it 17 | ds.path(start); 18 | 19 | pause(2) 20 | 21 | % add a swamp 22 | for r=78:85 23 | for c=12:45 24 | ds.modify_cost([c,r], 2); 25 | end 26 | end 27 | 28 | % replan 29 | ds.plan(); 30 | 31 | % show new path 32 | ds.path(start) 33 | 34 | p = ds.path(start); 35 | ds.visualize(p); 36 | -------------------------------------------------------------------------------- /unit_test/old/nav_dxform.m: -------------------------------------------------------------------------------- 1 | function nav_dxform 2 | goal = [50,30]; 3 | start = [20, 10]; 4 | load map1 5 | 6 | dx = DXform(map); 7 | dx 8 | 9 | dx.plan(goal); 10 | dx 11 | 12 | dx.path(start); 13 | 14 | p = dx.path(start); 15 | dx.visualize(p); 16 | 17 | -------------------------------------------------------------------------------- /unit_test/old/nav_prm.m: -------------------------------------------------------------------------------- 1 | function nav_prm 2 | load map1 3 | randinit 4 | prm = PRM(map) 5 | goal = [50,30] 6 | start=[20,10] 7 | 8 | prm.plan(); 9 | prm 10 | prm.visualize(); 11 | 12 | pause(4) 13 | 14 | prm.path(start, goal) 15 | p = prm.path(start, goal) 16 | prm.visualize(p); 17 | -------------------------------------------------------------------------------- /unit_test/old/nav_rrt.m: -------------------------------------------------------------------------------- 1 | clear classes 2 | 3 | %load map1 4 | %goal = [50,30]; 5 | %start = [20, 10]; 6 | 7 | map = zeros(100,100); 8 | goal = [0,0]; 9 | start = [20, 10]; 10 | 11 | randinit 12 | rrt = RRT([], goal) 13 | rrt.xrange = [-5 5] 14 | rrt.yrange = [-5 5] 15 | rrt.verbose = true; 16 | 17 | rrt.plan 18 | 19 | %rrt.visualize 20 | -------------------------------------------------------------------------------- /unit_test/old/nav_rrt2.m: -------------------------------------------------------------------------------- 1 | randinit 2 | clf 3 | hold on 4 | g = PGraph(3); 5 | 6 | sim_time = 2; 7 | 8 | xinit = [0, 0, 0]; 9 | plot(xinit(1), xinit(2), 'go'); 10 | rho = 1; 11 | 12 | g.add_node(xinit); 13 | t =2; 14 | 15 | for k=1:500 16 | % Step 3 17 | % find random state x,y in [-10, 10], th in [0 2pi] 18 | x = (rand-0.5)*20; 19 | y = (rand-0.5)*20; 20 | theta = rand*2*pi; 21 | xrand = [x, y, theta]'; 22 | xrand' 23 | 24 | % Step 4 25 | % find the existing node closest in state space 26 | dmin = Inf; 27 | for v=1:g.n 28 | xv = g.coord(v); 29 | d = sum( (xv(1:2)-xrand(1:2)).^2 ) + t*angdiff(xv(3)-xrand(3))^2; 30 | if d < dmin 31 | xnear = xv; 32 | vnear = v; 33 | dmin = d; 34 | end 35 | end 36 | 37 | % Step 5 38 | % figure how to drive the robot from xnear to xrand 39 | x0 = xnear'; 40 | xg = xrand'; 41 | %set_param('sl_movepoint2/Bicycle', 'x0', sprintf('[%f,%f,%f]', xnear) ); 42 | %set_param('sl_movepoint2/Goal pose', 'Value', sprintf('trans2([%f,%f,0])', xrand(1:2)) ); 43 | 44 | r = sim('sl_drivepose', sim_time); 45 | t = r.find('tout'); 46 | y = r.find('yout'); 47 | plot2(y) 48 | drawnow 49 | xnew = y(end,:)'; 50 | plot2(xnew', 'go'); 51 | 52 | % ensure that the path is collision free 53 | 54 | % Step 7,8 55 | % add xnew to the graph, with an edge to xnear 56 | g.add_node(xnew, vnear); 57 | end 58 | 59 | grid on 60 | xyzlabel 61 | zlabel('\theta'); 62 | axis([-10 10 -10 10]) 63 | iprint('rrt_paths'); 64 | view(0, 0); 65 | iprint('rrt_paths_xt'); 66 | -------------------------------------------------------------------------------- /unit_test/old/plots.m: -------------------------------------------------------------------------------- 1 | puma560 2 | p1 = p560; 3 | p2 = p560; 4 | p2.name = 'puma #2' 5 | p2.base = transl(0.5,-0.5,0); 6 | 7 | clearfigs 8 | 9 | disp('create single Puma and move it') 10 | 11 | p1.plot(qz) 12 | pause(1) 13 | p1.plot(qn); 14 | 15 | pause 16 | 17 | disp('add another Puma to this figure and move both') 18 | 19 | hold on 20 | p2.plot(qn); 21 | hold off 22 | pause(1) 23 | p1.plot(qz) 24 | p2.plot(qz) 25 | pause(1) 26 | p1.plot(qn) 27 | p2.plot(qn) 28 | 29 | pause 30 | 31 | disp('create a new figure and draw puma 1 in it') 32 | figure 33 | p1.plot(qz) 34 | for i=1:5 35 | p1.plot(qn); 36 | pause(0.5); 37 | p1.plot(qz); 38 | pause(0.5); 39 | end 40 | 41 | pause 42 | 43 | disp('add puma 2 to second figure') 44 | figure(2) 45 | hold on 46 | p2.plot(qz) 47 | hold off 48 | for i=1:5 49 | p1.plot(qn); 50 | p2.plot(qn); 51 | pause(0.5); 52 | p1.plot(qz); 53 | p2.plot(qz); 54 | pause(0.5); 55 | end 56 | -------------------------------------------------------------------------------- /unit_test/old/quaternion.m: -------------------------------------------------------------------------------- 1 | q = Quaternion() 2 | q2 = Quaternion([1 0 0 0]); 3 | if q ~= q2 4 | error('should match') 5 | end 6 | 7 | q = Quaternion(rotx(0.3)) 8 | q2 = Quaternion(trotx(0.3)) 9 | if q ~= q2 10 | error('should match') 11 | 12 | q.R(); 13 | q.T(); 14 | 15 | q+q2 16 | q*q2 17 | % test the values 18 | q/q2 19 | q^3 20 | q.inv() 21 | q.norm() 22 | q.unit() 23 | q.scale(0) 24 | q.scale(1) 25 | test these equal qi and q 26 | q.dot([1 2 3]); 27 | q*0.1 28 | q/0.1 29 | q*[1 2 3]; 30 | q*[1 2 3]'; 31 | q.plot() 32 | 33 | % quaternions 34 | 35 | quaternion(0.1) 36 | quaternion( mat([1,2,3]), 0.1 ) 37 | quaternion( rotx(0.1) ) 38 | quaternion( trotx(0.1) ) 39 | quaternion( quaternion(0.1) ) 40 | quaternion( mat([1,2,3,4]) ) 41 | quaternion( [1,2,3,4] ) 42 | quaternion( mat([1,2,3]), 0.1 ) 43 | 44 | q1 = quaternion( rotx(0.1) ); 45 | q1.norm() 46 | q1.unit() 47 | q1.norm() 48 | q1.double() 49 | q1.r() 50 | q1.tr() 51 | 52 | q1 = quaternion( rotx(0.1) ); 53 | q2 = quaternion( roty(0.2) ); 54 | q1_t = q1 55 | q2_t = q2 56 | q1*2 57 | 2*q1 58 | q1^1 59 | q1^2 60 | q1*q1 61 | q1+q2 62 | q1-q2 63 | q1*q2 64 | q2.inv() 65 | inv(q2*q2) 66 | q2*q2^(-1) 67 | q1/q2 68 | q1*v1 69 | q1*v1' 70 | 71 | q1 72 | q2 73 | qinterp(q1, q2, 0) 74 | qinterp(q1, q2, 1) 75 | qinterp(q1, q2, 0.5) 76 | qinterp(q1, q2, [0, .2, .5, 1]) 77 | 78 | q1-q1_t 79 | q2-q2_t 80 | 81 | -------------------------------------------------------------------------------- /unit_test/old/test.m: -------------------------------------------------------------------------------- 1 | disp('================================== transform ==============================') 2 | transform 3 | 4 | disp('================================== trajectory ==============================') 5 | trajectory 6 | 7 | disp('================================== jacobian ==============================') 8 | jacobian 9 | 10 | disp('================================== kinematics ==============================') 11 | kinematics 12 | 13 | disp('================================== dynamics ==============================') 14 | dynamics 15 | 16 | disp('================================== demos ==============================') 17 | pause off 18 | rttrdemo 19 | rtfkdemo 20 | rtikdemo 21 | rttgdemo 22 | rtandemo 23 | rtjademo 24 | rtfddemo 25 | rtidemo 26 | -------------------------------------------------------------------------------- /unit_test/old/trajectory.m: -------------------------------------------------------------------------------- 1 | % Test trajectory primitives 2 | 3 | mdl_puma560 4 | 5 | [q,qd,qdd] = jtraj(qz, qr, 20) 6 | q 7 | qd 8 | qdd 9 | 10 | [q,qd,qdd] = jtraj(qz, qr, 20, 0.1*mat(ones(1,6)), -0.1*mat(ones(1,6)) ) 11 | q 12 | qd 13 | qdd 14 | 15 | [q,qd,qdd] = jtraj(qz, qr, [0:0.2:10]) 16 | q 17 | 18 | t1 = trotx(0.1) * transl(0.2, 0.3, 0.4) 19 | t1 20 | t2 = troty(-0.3) * transl(-0.2, -0.3, 0.6) 21 | t2 22 | ctraj(t1, t2, 5) 23 | ctraj(t1, t2, [0:0.1:1]) 24 | -------------------------------------------------------------------------------- /unit_test/old/transform.m: -------------------------------------------------------------------------------- 1 | 2 | % utility 3 | v1 = mat([0, 1, 0]); 4 | v2 = mat([0, 0, 1]); 5 | unit(v1+v2) 6 | 7 | 8 | m = mat(zeros( 3,4 )); 9 | numcols(m) 10 | numrows(m) 11 | 12 | % transform primitives 13 | rotx(.1) 14 | roty(.1) 15 | rotz(.1) 16 | 17 | r = rotz(0.1) 18 | r2t(r) 19 | 20 | 21 | trotx(.1) 22 | troty(.1) 23 | trotz(.1) 24 | 25 | t = trotz(0.1) 26 | t2r(t) 27 | 28 | t1 = trotx(.1) 29 | t2 = troty(.2) 30 | trinterp(t1, t2, 0) 31 | trinterp(t1, t2, 1) 32 | trinterp(t1, t2, 0.5) 33 | 34 | % predicates 35 | isvec(v1) 36 | isvec(r) 37 | isvec(t) 38 | 39 | isrot(v1) 40 | isrot(r) 41 | isrot(t) 42 | 43 | ishomog(v1) 44 | ishomog(r) 45 | ishomog(t) 46 | det(t) 47 | 48 | % translational matrices 49 | transl(0.1, 0.2, 0.3) 50 | transl( [0.1, 0.2, 0.3] ) 51 | transl( mat([0.1, 0.2, 0.3]) ) 52 | t = transl(0.1, 0.2, 0.3) 53 | transl(t) 54 | 55 | % angle conversions 56 | eul2r(.1, .2, .3) 57 | eul2r( [.1, .2, .3] ) 58 | eul2r( mat([.1, .2, .3; .4, .5, .6]) ) 59 | eul2r( mat([.1, .2, .3]) ) 60 | eul2tr(.1, .2, .3) 61 | eul2tr( [.1, .2, .3] ) 62 | eul2tr( mat([.1, .2, .3]) ) 63 | te = eul2tr( mat([.1, .2, .3]) ) 64 | tr2eul(te) 65 | 66 | rpy2r(.1, .2, .3) 67 | rpy2r( [.1, .2, .3] ) 68 | rpy2r( mat([.1, .2, .3]) ) 69 | rpy2r( mat([.1, .2, .3; .4, .5, .6]) ) 70 | rpy2tr(.1, .2, .3) 71 | rpy2tr( [.1, .2, .3] ) 72 | rpy2tr( mat([.1, .2, .3]) ) 73 | tr = rpy2tr( mat([.1, .2, .3]) ) 74 | tr2rpy(tr) 75 | 76 | oa2r(v1, v2) 77 | oa2tr(v1, v2) 78 | t = oa2tr(v1, v2) 79 | trnorm(t) 80 | 81 | angvec2r( 0.1, mat([1,2,3]) ) 82 | 83 | % special matrices 84 | %diff2tr( mat([.1, .2 ,.3]) ) 85 | %m = diff2tr( mat([.1, .2 ,.3]) ) 86 | %diff2tr(m) 87 | m = delta2tr( [.1, .2 ,.3, .4, .5, .6] ) 88 | tr2delta(m) 89 | 90 | R = rotx(0.2); t = [1 2 3]'; 91 | T = rt2tr(R, t); 92 | assertEqual(T, [R t; 0 0 0 1]); 93 | 94 | [RR,tt] = tr2rt(T); 95 | assertEqual(RR, R); 96 | assertEqual(tt, t); 97 | 98 | T = se2(1, 2, .5); 99 | [RR,tt] = tr2rt(T); 100 | TT = rt2tr(RR, tt); 101 | assertEqual(TT, T); 102 | 103 | -------------------------------------------------------------------------------- /vex.m: -------------------------------------------------------------------------------- 1 | %VEX Convert skew-symmetric matrix to vector 2 | % 3 | % V = VEX(S) is the vector which has the corresponding skew-symmetric 4 | % matrix S. 5 | % 6 | % In the case that S (2x2) = 7 | % 8 | % | 0 -v | 9 | % | v 0 | 10 | % 11 | % then V = [v]. In the case that S (3x3) = 12 | % 13 | % | 0 -vz vy | 14 | % | vz 0 -vx | 15 | % |-vy vx 0 | 16 | % 17 | % then V = [vx; vy; vz]. 18 | % 19 | % Notes:: 20 | % - This is the inverse of the function SKEW(). 21 | % - Only rudimentary checking (zero diagonal) is done to ensure that the 22 | % matrix is actually skew-symmetric. 23 | % - The function takes the mean of the two elements that correspond to 24 | % each unique element of the matrix. 25 | % - The matrices are the generator matrices for so(2) and so(3). 26 | % 27 | % References:: 28 | % - Robotics, Vision & Control: Second Edition, P. Corke, Springer 2016; p25+43. 29 | % 30 | % See also SKEW, VEXA. 31 | 32 | % Copyright (C) 1993-2019 Peter I. Corke 33 | % 34 | % This file is part of The Spatial Math Toolbox for MATLAB (SMTB). 35 | % 36 | % Permission is hereby granted, free of charge, to any person obtaining a copy 37 | % of this software and associated documentation files (the "Software"), to deal 38 | % in the Software without restriction, including without limitation the rights 39 | % to use, copy, modify, merge, publish, distribute, sublicense, and/or sell copies 40 | % of the Software, and to permit persons to whom the Software is furnished to do 41 | % so, subject to the following conditions: 42 | % 43 | % The above copyright notice and this permission notice shall be included in all 44 | % copies or substantial portions of the Software. 45 | % 46 | % THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 47 | % IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS 48 | % FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR 49 | % COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER 50 | % IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN 51 | % CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. 52 | % 53 | % https://github.com/petercorke/spatial-math 54 | 55 | function v = vex(S) 56 | % if trace(abs(S)) > 10*eps 57 | % error('SMTB:vex:badarg', 'argument is not skew symmetric tr=%g', trace(abs(S))); 58 | % end 59 | if all(size(S) == [3 3]) 60 | v = 0.5*[S(3,2)-S(2,3); S(1,3)-S(3,1); S(2,1)-S(1,2)]; 61 | elseif all(size(S) == [2 2]) 62 | v = 0.5*(S(2,1)-S(1,2)); 63 | else 64 | error('SMTB:vex:badarg', 'argument must be a 2x2 or 3x3 matrix'); 65 | end 66 | -------------------------------------------------------------------------------- /vexa.m: -------------------------------------------------------------------------------- 1 | %VEXA Convert augmented skew-symmetric matrix to vector 2 | % 3 | % V = VEXA(S) is the vector which has the corresponding augmented skew-symmetric 4 | % matrix S. 5 | % 6 | % In the case that S (3x3) = 7 | % 8 | % | 0 -v3 v1 | 9 | % | v3 0 v2 | 10 | % | 0 0 0 | 11 | % 12 | % then V = [v1; v2; v3]. In the case that S (6x6) = 13 | % 14 | % 15 | % | 0 -v6 v5 v1 | 16 | % | v6 0 -v4 v2 | 17 | % |-v5 v4 0 v3 | 18 | % | 0 0 0 0 | 19 | % 20 | % then V = [v1; v2; v3; v4; v5; v6]. 21 | % 22 | % Notes:: 23 | % - This is the inverse of the function SKEWA(). 24 | % - The matrices are the generator matrices for se(2) and se(3). The elements 25 | % comprise the equivalent twist vector. 26 | % 27 | % References:: 28 | % - Robotics, Vision & Control: Second Edition, Chap 2, 29 | % P. Corke, Springer 2016. 30 | % 31 | % See also SKEWA, VEX, Twist. 32 | 33 | % Copyright (C) 1993-2019 Peter I. Corke 34 | % 35 | % This file is part of The Spatial Math Toolbox for MATLAB (SMTB). 36 | % 37 | % Permission is hereby granted, free of charge, to any person obtaining a copy 38 | % of this software and associated documentation files (the "Software"), to deal 39 | % in the Software without restriction, including without limitation the rights 40 | % to use, copy, modify, merge, publish, distribute, sublicense, and/or sell copies 41 | % of the Software, and to permit persons to whom the Software is furnished to do 42 | % so, subject to the following conditions: 43 | % 44 | % The above copyright notice and this permission notice shall be included in all 45 | % copies or substantial portions of the Software. 46 | % 47 | % THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 48 | % IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS 49 | % FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR 50 | % COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER 51 | % IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN 52 | % CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. 53 | % 54 | % https://github.com/petercorke/spatial-math 55 | 56 | function s = vexa(Omega) 57 | 58 | if all(size(Omega) == [4 4]) 59 | s = [transl(Omega); vex(Omega(1:3,1:3))]; 60 | elseif all(size(Omega) == [3 3 ]) 61 | s = [transl2(Omega); vex(Omega(1:2,1:2))]; 62 | else 63 | error('SMTB:vexa:badarg', 'argument must be a 3x3 or 4x4 matrix'); 64 | end 65 | 66 | 67 | end 68 | -------------------------------------------------------------------------------- /xyzlabel.m: -------------------------------------------------------------------------------- 1 | %XYZLABEL Label X, Y and Z axes 2 | % 3 | % XYZLABEL() label the x-, y- and z-axes with 'X', 'Y', and 'Z' 4 | % respectiveley. 5 | % 6 | % XYZLABEL(FMT) as above but pass in a format string where %s is substituted 7 | % for the axis label, eg. 8 | % 9 | % xyzlabel('This is the %s axis') 10 | % 11 | % See also xlabel, ylabel, zlabel, sprintf. 12 | 13 | % Copyright (C) 1993-2019 Peter I. Corke 14 | % 15 | % This file is part of The Spatial Math Toolbox for MATLAB (SMTB). 16 | % 17 | % Permission is hereby granted, free of charge, to any person obtaining a copy 18 | % of this software and associated documentation files (the "Software"), to deal 19 | % in the Software without restriction, including without limitation the rights 20 | % to use, copy, modify, merge, publish, distribute, sublicense, and/or sell copies 21 | % of the Software, and to permit persons to whom the Software is furnished to do 22 | % so, subject to the following conditions: 23 | % 24 | % The above copyright notice and this permission notice shall be included in all 25 | % copies or substantial portions of the Software. 26 | % 27 | % THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 28 | % IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS 29 | % FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR 30 | % COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER 31 | % IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN 32 | % CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. 33 | % 34 | % https://github.com/petercorke/spatial-math 35 | function xyzlabel(fmt) 36 | if nargin < 1 37 | fmt = '%s'; 38 | end 39 | xlabel(sprintf(fmt, 'X')); 40 | ylabel(sprintf(fmt, 'Y')); 41 | zlabel(sprintf(fmt, 'Z')); 42 | --------------------------------------------------------------------------------